ROS多消息同步与多消息回调

举报
Hermit_Rabbit 发表于 2022/09/21 20:35:11 2022/09/21
1.9k+ 0 0
【摘要】 0.存在的问题多传感器数据融合的时候,由于各个传感器采集数据的频率的不同,例如odom 50Hz、Imu 100Hz、camera 25Hz,需要将传感器数据进行时间同步后才能进行融合。这时就需要我们使用数据融合。当然只有多个主题都有数据的时候才可以触发回调函数。如果其中一个主题的发布节点崩溃了,则整个回调函数永远无法触发回调。当多个主题频率一致的时候也无法保证回调函数的频率等于订阅主题的...

0.存在的问题

多传感器数据融合的时候,由于各个传感器采集数据的频率的不同,例如odom 50Hz、Imu 100Hz、camera 25Hz,需要将传感器数据进行时间同步后才能进行融合。这时就需要我们使用数据融合。当然只有多个主题都有数据的时候才可以触发回调函数。如果其中一个主题的发布节点崩溃了,则整个回调函数永远无法触发回调。当多个主题频率一致的时候也无法保证回调函数的频率等于订阅主题的频率,一般会很低。实际测试订阅两个需要同步的主题,odom 50Hz、imu 50Hz,而回调函数的频率只有24Hz左右。

1. 全局变量形式 : TimeSynchronizer

步骤:

  1. message_filter ::subscriber 分别订阅不同的输入topic
  2. TimeSynchronizer<Image,CameraInfo> 定义时间同步器;
  3. sync.registerCallback 同步回调
  4. void callback(const ImageConstPtr&image,const CameraInfoConstPtr& cam_info) 带多消息的消息同步自定义回调函数

相应的API message_filters::TimeSynchronizer

//wiki参考demo http://wiki.ros.org/message_filters
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
 
using namespace sensor_msgs;
using namespace message_filters;
 
void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)  //回调中包含多个消息
{
  // Solve all of perception here...
}
 
int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");
 
  ros::NodeHandle nh;
 
  message_filters::Subscriber<Image> image_sub(nh, "image", 1);             // topic1 输入
  message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);   // topic2 输入
  TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);       // 同步
  sync.registerCallback(boost::bind(&callback, _1, _2));                   // 回调
 
  ros::spin();
 
  return 0;
}

参考连接:http://wiki.ros.org/message_filters

2.类成员的形式 message_filters::Synchronizer

另外,ros还有一种Policy-Based的消息同步机制,本质与上述Time Synchronizer类似。它分为两种,ExactTime Policy 和ApproximateTime Policy。

修改一下CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  sensor_msgs
  message_filters //ADD
  OpenCV
  cv_bridge 
  image_transport
)

find_package(PCL 1.7 REQUIRED)
find_package(OpenCV REQUIRED)

target_link_libraries (syn ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_LIBRARIES})

include_directories(include ${catkin_INCLUDE_DIRS})
include_directories(include ${OpenCV_INCLUDE_DIRS})
include_directories(include ${PCL_INCLUDE_DIRS})

add_executable(hw_1 src/hw_1.cpp)
target_link_libraries (hw_1 ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})

使用时间戳相近对齐

#include "ros/ros.h"    
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <iostream>

using namespace std;
using namespace sensor_msgs;
using namespace message_filters;

ros::Publisher PointCloudInfo_pub;
ros::Publisher ImageInfo_pub;

PointCloud2 syn_pointcloud;
Image syn_iamge;

void Syncallback(const PointCloud2ConstPtr& ori_pointcloud,const ImageConstPtr& ori_image)
{
    cout << "\033[1;32m Syn! \033[0m" << endl;
    syn_pointcloud = *ori_pointcloud;
    syn_iamge = *ori_image;
    cout << "syn pointcloud' timestamp : " << syn_pointcloud.header.stamp << endl;
    cout << "syn image's timestamp : " << syn_iamge.header.stamp << endl;
    PointCloudInfo_pub.publish(syn_pointcloud);
    ImageInfo_pub.publish(syn_iamge);
}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "hw1");
    ros::NodeHandle node;

    cout << "\033[1;31m hw1! \033[0m" << endl;

    // 建立需要订阅的消息对应的订阅器
    message_filters::Subscriber<PointCloud2> PointCloudInfo_sub(node, "/rslidar_points", 1);
    message_filters::Subscriber<Image> ImageInfo_sub(node, "/zed/zed_node/left/image_rect_color", 1);
    
    typedef sync_policies::ApproximateTime<PointCloud2, Image> MySyncPolicy; 
    
    Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), PointCloudInfo_sub, ImageInfo_sub); //queue size=10
    sync.registerCallback(boost::bind(&Syncallback, _1, _2));
    PointCloudInfo_pub = node.advertise<PointCloud2>("/djq_pc", 10);
    ImageInfo_pub = node.advertise<Image>("/djq_image", 10);

    ros::spin();
    return 0;
}

同步结果
在这里插入图片描述
在这里插入图片描述

3. 其他详解链接

https://blog.csdn.net/qq_43145072/article/details/102602602
https://www.coder.work/article/3156449
https://blog.csdn.net/zyh821351004/article/details/47758433

【声明】本内容来自华为云开发者社区博主,不代表华为云及华为云开发者社区的观点和立场。转载时必须标注文章的来源(华为云社区)、文章链接、文章作者等基本信息,否则作者和本社区有权追究责任。如果您发现本社区中有涉嫌抄袭的内容,欢迎发送邮件进行举报,并提供相关证据,一经查实,本社区将立刻删除涉嫌侵权内容,举报邮箱: cloudbbs@huaweicloud.com
  • 点赞
  • 收藏
  • 关注作者

作者其他文章

评论(0

抱歉,系统识别当前为高风险访问,暂不支持该操作

    全部回复

    上滑加载中

    设置昵称

    在此一键设置昵称,即可参与社区互动!

    *长度不超过10个汉字或20个英文字符,设置后3个月内不可修改。

    *长度不超过10个汉字或20个英文字符,设置后3个月内不可修改。