devbox
weixin_40725706
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

ROS消息过滤器之 message_filters::Synchronizer 使用详解_synchronizer sync

作者：weixin_40725706 | 2024-07-12 11:42:50

赞

踩

synchronizer sync

        在ROS中，当我们有多个传感器发布的数据需要同步时，message_filters::Synchronizer 是一个非常有用的工具。它可以确保多个消息在时间上是同步的，以便更有效地处理数据。

1.什么是ROS消息过滤器？

ROS消息过滤器是一种用于处理ROS消息的工具，允许我们对消息进行过滤、同步和组合。这对于在机器人感知中整合不同传感器数据或在控制中同步多个输入源非常重要。

2.message_filters::Synchronizer 简介

        message_filters::Synchronizer 是ROS中消息过滤器的一部分，专门用于同步多个消息。它可以确保多个发布者发布的消息在时间上是同步的，以便在回调函数中处理这些消息。

3.使用场景

        当你有多个传感器发布的数据，需要确保这些数据在同一时刻可用时，message_filters::Synchronizer 就派上用场了。例如，在处理相机图像和激光雷达扫描数据时，你可能希望它们在相同的时间戳上可用，以便更准确地进行感知。

4.示例代码

工程结构：

4.1 发布者节点 (image_publisher.cpp)

  1. #include <ros/ros.h>
  2. #include <sensor_msgs/Image.h>
  3. #include <cv_bridge/cv_bridge.h>
  4. #include <opencv2/highgui/highgui.hpp>
  5.  
  6. #include <ros/ros.h>
  7. #include <sensor_msgs/Image.h>
  8. #include <sensor_msgs/PointCloud2.h>
  9. #include <cv_bridge/cv_bridge.h>
  10. #include <pcl_conversions/pcl_conversions.h>
  11. #include <pcl/point_cloud.h>
  12. #include <pcl/point_types.h>
  13. #include <pcl/io/pcd_io.h>
  14.  
  15. int main(int argc, char** argv)
  16. {
  17.     ros::init(argc, argv, "image_and_pointcloud_publisher");
  18.     ros::NodeHandle nh;
  19.  
  20.     // 创建一个发布者，发布图像消息到 "/image_topic" 话题，缓存队列大小为 10
  21.     ros::Publisher image_pub = nh.advertise<sensor_msgs::Image>("/image_topic", 10);
  22.  
  23.     // 创建一个发布者，发布点云消息到 "/pointcloud_topic" 话题，缓存队列大小为 10
  24.     ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/pointcloud_topic", 10);
  25.  
  26.     // 创建图像消息
  27.     sensor_msgs::Image image_msg;
  28.  
  29.     // 设置图像消息的头部信息
  30.     image_msg.header.stamp = ros::Time::now();
  31.     image_msg.header.frame_id = "camera_frame";
  32.  
  33.     // 填充图像数据（这里假设你已经有一个图像数据，例如使用OpenCV加载的图像）
  34.     cv::Mat image = cv::imread("/home/guo/Pictures/1.png", cv::IMREAD_COLOR);
  35.  
  36.     // 使用cv_bridge将OpenCV图像转换为ROS图像消息
  37.     cv_bridge::CvImage cv_image;
  38.     cv_image.image = image;
  39.     cv_image.encoding = "bgr8"; // 8-bit, 3 channel
  40.     image_msg = *cv_image.toImageMsg();
  41.  
  42.     // 创建点云消息
  43.     sensor_msgs::PointCloud2 pcl_msg;
  44.  
  45.     // 填充点云数据（这里假设你已经有一个点云数据，例如使用PCL库生成的点云）
  46.     pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
  47.     // Populate your point cloud data here...
  48.  
  49.     pcl::io::loadPCDFile("/home/guo/Downloads/pcd/1.pcd", *cloud);
  50.  
  51.     // 使用PCL库将点云转换为ROS点云消息
  52.     pcl::toROSMsg(*cloud, pcl_msg);
  53.  
  54.  
  55.  
  56.     pcl_msg.header.stamp = ros::Time::now();
  57.     pcl_msg.header.frame_id = "camera_frame";
  58.  
  59.     ros::Rate loop_rate(1); // 发布频率为1Hz
  60.  
  61.     while (ros::ok())
  62.     {
  63.         // 更新时间戳
  64.         image_msg.header.stamp = ros::Time::now();
  65.         pcl_msg.header.stamp = ros::Time::now();
  66.  
  67.         // 发布图像消息
  68.         image_pub.publish(image_msg);
  69.  
  70.         // 发布点云消息
  71.         pcl_pub.publish(pcl_msg);
  72.  
  73.         // 循环等待
  74.         ros::spinOnce();
  75.         loop_rate.sleep();
  76.     }
  77.  
  78.     return 0;
  79. }
  80.  
  81.

4.2 订阅者节点 (synchronized_subscriber.cpp)

  1. #include <ros/ros.h>
  2. #include <sensor_msgs/Image.h>
  3. #include <sensor_msgs/PointCloud2.h>
  4. #include <message_filters/subscriber.h>
  5. #include <message_filters/synchronizer.h>
  6. #include <message_filters/sync_policies/approximate_time.h>
  7.  
  8. void callback(const sensor_msgs::Image::ConstPtr& image_msg, const sensor_msgs::PointCloud2::ConstPtr& pcl_msg)
  9. {
  10.     // 处理同步的消息
  11.     // image_msg 和 pcl_msg 在相同时间戳下
  12.     ROS_INFO("Received synchronized the message of       image_msg at time %f", image_msg->header.stamp.toSec());
  13.     ROS_INFO("Received synchronized the message of pcl_msg message at time %f", pcl_msg->header.stamp.toSec());
  14.  
  15.     ROS_INFO("------------------------------------------------------------------------");
  16. }
  17.  
  18. int main(int argc, char** argv)
  19. {
  20.     ros::init(argc, argv, "synchronized_subscriber");
  21.     ros::NodeHandle nh;
  22.  
  23.     // 创建两个订阅者
  24.     message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "/image_topic", 1);
  25.     message_filters::Subscriber<sensor_msgs::PointCloud2> pcl_sub(nh, "/pointcloud_topic", 1);
  26.  
  27.     // 定义同步策略为 ApproximateTime
  28.     typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> MySyncPolicy;
  29.     
  30.     // 创建同步器对象
  31.     message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, pcl_sub);
  32.     
  33.     // 注册回调函数
  34.     sync.registerCallback(boost::bind(&callback, _1, _2));
  35.  
  36.     ros::spin();
  37.  
  38.     return 0;
  39. }

 4.3 CMakeLists.txt

  1. cmake_minimum_required(VERSION 3.0.2)
  2. project(synchronizer)
  3.  
  4. ## Compile as C++11, supported in ROS Kinetic and newer
  5. # add_compile_options(-std=c++11)
  6.  
  7. ## Find catkin macros and libraries
  8. ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
  9. ## is used, also find other catkin packages
  10. find_package(catkin REQUIRED COMPONENTS
  11.   cv_bridge
  12.   roscpp
  13.   sensor_msgs
  14.   std_msgs
  15.   message_filters
  16.   pcl_conversions
  17. )
  18.  
  19. find_package(OpenCV REQUIRED)
  20. include_directories(${OpenCV_INCLUDE_DIRS})
  21. message(STATUS "version: ${OpenCV_VERSION}")
  22.  
  23. find_package(PCL REQUIRED QUIET)
  24. include_directories(${PCL_INCLUDE_DIRS})
  25. ## System dependencies are found with CMake's conventions
  26. # find_package(Boost REQUIRED COMPONENTS system)
  27.  
  28.  
  29. ## Uncomment this if the package has a setup.py. This macro ensures
  30. ## modules and global scripts declared therein get installed
  31. ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
  32. # catkin_python_setup()
  33.  
  34. ################################################
  35. ## Declare ROS messages, services and actions ##
  36. ################################################
  37.  
  38. ## To declare and build messages, services or actions from within this
  39. ## package, follow these steps:
  40. ## * Let MSG_DEP_SET be the set of packages whose message types you use in
  41. ##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
  42. ## * In the file package.xml:
  43. ##   * add a build_depend tag for "message_generation"
  44. ##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
  45. ##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
  46. ##     but can be declared for certainty nonetheless:
  47. ##     * add a exec_depend tag for "message_runtime"
  48. ## * In this file (CMakeLists.txt):
  49. ##   * add "message_generation" and every package in MSG_DEP_SET to
  50. ##     find_package(catkin REQUIRED COMPONENTS ...)
  51. ##   * add "message_runtime" and every package in MSG_DEP_SET to
  52. ##     catkin_package(CATKIN_DEPENDS ...)
  53. ##   * uncomment the add_*_files sections below as needed
  54. ##     and list every .msg/.srv/.action file to be processed
  55. ##   * uncomment the generate_messages entry below
  56. ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
  58. ## Generate messages in the 'msg' folder
  59. # add_message_files(
  60. #   FILES
  61. #   Message1.msg
  62. #   Message2.msg
  63. # )
  65. ## Generate services in the 'srv' folder
  66. # add_service_files(
  67. #   FILES
  68. #   Service1.srv
  69. #   Service2.srv
  70. # )
  72. ## Generate actions in the 'action' folder
  73. # add_action_files(
  74. #   FILES
  75. #   Action1.action
  76. #   Action2.action
  77. # )
  79. ## Generate added messages and services with any dependencies listed here
  80. # generate_messages(
  81. #   DEPENDENCIES
  82. #   sensor_msgs#   std_msgs
  83. # )
  85. ################################################
  86. ## Declare ROS dynamic reconfigure parameters ##
  87. ################################################
  89. ## To declare and build dynamic reconfigure parameters within this
  90. ## package, follow these steps:
  91. ## * In the file package.xml:
  92. ##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
  93. ## * In this file (CMakeLists.txt):
  94. ##   * add "dynamic_reconfigure" to
  95. ##     find_package(catkin REQUIRED COMPONENTS ...)
  96. ##   * uncomment the "generate_dynamic_reconfigure_options" section below
  97. ##     and list every .cfg file to be processed
  99. ## Generate dynamic reconfigure parameters in the 'cfg' folder
  100. # generate_dynamic_reconfigure_options(
  101. #   cfg/DynReconf1.cfg
  102. #   cfg/DynReconf2.cfg
  103. # )
  105. ###################################
  106. ## catkin specific configuration ##
  107. ###################################
  108. ## The catkin_package macro generates cmake config files for your package
  109. ## Declare things to be passed to dependent projects
  110. ## INCLUDE_DIRS: uncomment this if your package contains header files
  111. ## LIBRARIES: libraries you create in this project that dependent projects also need
  112. ## CATKIN_DEPENDS: catkin_packages dependent projects also need
  113. ## DEPENDS: system dependencies of this project that dependent projects also need
  114. catkin_package(
  115. #  INCLUDE_DIRS include
  116. #  LIBRARIES synchronizer
  117. #  CATKIN_DEPENDS cv_bridge roscpp sensor_msgs std_msgs
  118. #  DEPENDS system_lib
  119. # DEPENDS OpenCV
  120. )
  122. ###########
  123. ## Build ##
  124. ###########
  126. ## Specify additional locations of header files
  127. ## Your package locations should be listed before other locations
  128. include_directories(
  129. # include
  130.   ${catkin_INCLUDE_DIRS}
  131. )
  132.  
  133. ## Declare a C++ library
  134. # add_library(image_publisher
  135. #   src/image_publisher.cpp
  136. # )
  137.  
  138. ## Add cmake target dependencies of the library
  139. ## as an example, code may need to be generated before libraries
  140. ## either from message generation or dynamic reconfigure
  141. # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
  142.  
  143. ## Declare a C++ executable
  144. ## With catkin_make all packages are built within a single CMake context
  145. ## The recommended prefix ensures that target names across packages don't collide
  146. add_executable(image_publisher /home/guo/Downloads/syn_ws/src/synchronizer/src/image_publisher.cpp)
  147. add_executable(synchronized_subscriber /home/guo/Downloads/syn_ws/src/synchronizer/src/synchronized_subscriber.cpp)
  149. ## Rename C++ executable without prefix
  150. ## The above recommended prefix causes long target names, the following renames the
  151. ## target back to the shorter version for ease of user use
  152. ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
  153. # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
  155. ## Add cmake target dependencies of the executable
  156. ## same as for the library above
  157. # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
  159. ## Specify libraries to link a library or executable target against
  160. target_link_libraries(image_publisher
  161.   ${catkin_LIBRARIES}
  162.   ${OpenCV_INCLUDE_DIRS}
  163.   ${PCL_INCLUDE_DIRS}
  164. )
  166. target_link_libraries(synchronized_subscriber
  167.   ${catkin_LIBRARIES}
  168. )
  170. #############
  171. ## Install ##
  172. #############
  174. # all install targets should use catkin DESTINATION variables
  175. # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
  177. ## Mark executable scripts (Python etc.) for installation
  178. ## in contrast to setup.py, you can choose the destination
  179. # catkin_install_python(PROGRAMS
  180. #   scripts/my_python_script
  181. #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
  182. # )
  183.  
  184. ## Mark executables for installation
  185. ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
  186. # install(TARGETS ${PROJECT_NAME}_node
  187. #   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
  188. # )
  189.  
  190. ## Mark libraries for installation
  191. ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
  192. # install(TARGETS ${PROJECT_NAME}
  193. #   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  194. #   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  195. #   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
  196. # )
  197.  
  198. ## Mark cpp header files for installation
  199. # install(DIRECTORY include/${PROJECT_NAME}/
  200. #   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
  201. #   FILES_MATCHING PATTERN "*.h"
  202. #   PATTERN ".svn" EXCLUDE
  203. # )
  204.  
  205. ## Mark other files for installation (e.g. launch and bag files, etc.)
  206. # install(FILES
  207. #   # myfile1
  208. #   # myfile2
  209. #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
  210. # )
  211.  
  212. #############
  213. ## Testing ##
  214. #############
  215.  
  216. ## Add gtest based cpp test target and link libraries
  217. # catkin_add_gtest(${PROJECT_NAME}-test test/test_synchronizer.cpp)
  218. # if(TARGET ${PROJECT_NAME}-test)
  219. #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
  220. # endif()
  221.  
  222. ## Add folders to be run by python nosetests
  223. # catkin_add_nosetests(test)

4.4 launch文件

  1. <launch>
  2.  
  3.   <!-- Start the image_publisher_node -->
  4.   <node name="synchronizer1" pkg="synchronizer" type="image_publisher" output="screen"/>
  5.  
  6.   <!-- Start the synchronized_subscriber_node -->
  7.   <node name="synchronizer2" pkg="synchronizer" type="synchronized_subscriber" output="screen"/>
  8.  
  9. </launch>

4.5 代码解释

  • 引入必要的ROS和消息过滤器头文件。
  • 定义回调函数 callback，用于处理同步的图像和激光雷达数据。
  • 在 main 函数中创建 message_filters::Subscriber 对象，分别订阅相机图像和激光雷达扫描数据。
  • 使用 message_filters::sync_policies::ApproximateTime 创建 message_filters::Synchronizer 对象，设置时间同步策略为近似同步。
  • 注册回调函数到 Synchronizer 对象中，它将在图像和激光雷达数据近似同步时调用。

5. 运行和测试

        确保ROS环境已经启动，然后运行launch文件：

        运行结果：

图像和点云的时间辍实现同步。 

 

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/weixin_40725706/article/detail/813972
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号