ROS MessageFilter订阅多个激光雷达话题进行同步处理

0 MessageFilter

http://wiki.ros.org/message_filters
message_filters消息过滤器类似一个消息缓存,当消息到达消息过滤器的时候,可能并不会立即输出,需要满足一定条件才输出,比如有最简单的直接输出消息,还有常用的时间同步器,tf转换等.

1 消息的订阅与回调

message_filters::Subscriber<std_msgs::UInt32> topic_sub(nh, "topic_name", 1);
topic_sub.registerCallback(Callback);

等同于:

ros::Subscriber topic_sub = nh.subscribe("topic_name", 1, Callback);

2 时间同步

在ROS节点中需要订阅两个及两个以上的话题时，这些话题的频率不一致,需要保持对这些话题数据的同步，且需要同时接收数据一起处理然后当做参数传入到另一个函数中。
个人这里是两个激光雷达的发送频率不一致,需要同步时间戳对数据进行处理.

2.1 常规写法

#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const sensor_msgs::LaserScan::ConstPtr &msg,
              const sensor_msgs::LaserScan::ConstPtr &msg1) {
  //sensor_msgs::LaserScan s = *msg;
  //sensor_msgs::LaserScan s1 = *msg1;
  printf("scan stamp:%f\n", msg->header.stamp.toSec());
  printf("scan1 stamp:%f\n", msg1->header.stamp.toSec());
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "demo");
  ros::NodeHandle nh;
  //①分别接收两个topic
  message_filters::Subscriber<sensor_msgs::LaserScan> sub_scan(
      nh, "/scan", 1, ros::TransportHints().tcpNoDelay());
  message_filters::Subscriber<sensor_msgs::LaserScan> sub_scan1(
      nh, "/scan1", 1, ros::TransportHints().tcpNoDelay());

  // ②将两个topic的数据进行同步
  typedef sync_policies::ApproximateTime<sensor_msgs::LaserScan,
                                         sensor_msgs::LaserScan>
      syncPolicy;
  Synchronizer<syncPolicy> sync(syncPolicy(10), sub_scan, sub_scan1);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}

2.2 利用类成员写法

#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

using namespace sensor_msgs;
using namespace message_filters;

typedef sync_policies::ApproximateTime<sensor_msgs::LaserScan,
                                       sensor_msgs::LaserScan>
    syncPolicy;
    
class Demo{
public:
    Demo();
    ~Demo();
    void Callback(const sensor_msgs::LaserScan::ConstPtr &msg,
                  const sensor_msgs::LaserScan::ConstPtr &msg1);
    ros::NodeHandle nh_;
private:
    message_filters::Subscriber<sensor_msgs::LaserScan> *sub_scan_;
    message_filters::Subscriber<sensor_msgs::LaserScan> *sub_scan1_;
    message_filters::Synchronizer<syncPolicy> *sync_;
};

Demo::Demo(){
    sub_scan_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(
        nh_, "/scan", 1, ros::TransportHints().tcpNoDelay());
    sub_scan1_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(
        nh_, "/scan1", 1, ros::TransportHints().tcpNoDelay());
        
    sync_ = new message_filters::Synchronizer<syncPolicy>(
        syncPolicy(10), *sub_scan_, *sub_scan1_);
    sync_->registerCallback(boost::bind(&Demo::Callback, this, _1, _2));
}
Demo::~Demo(){
    delete sub_scan_;
    delete sub_scan1_;
    delete sync_;
}

void Demo::Callback(const sensor_msgs::LaserScan::ConstPtr &msg,
                    const sensor_msgs::LaserScan::ConstPtr &msg1){
    //sensor_msgs::LaserScan s = *msg;
    //sensor_msgs::LaserScan s1 = *msg1;
    printf("scan stamp:%f\n", msg->header.stamp.toSec());
    printf("scan1 stamp:%f\n", msg1->header.stamp.toSec());
}

int main(int argc, char **argv){
    ros::init(argc, argv, "demo");
    Demo demo;
    ros::spin();
    return 0;
}

2.3 注意

• 时间同步是按频率最低的topic来进行,比如scan 是15Hz, scan1是10Hz,那么时间同步后,接收两个topic的频率是10Hz,有点像木桶理论;
• 只有两个topic都有接收到topic,才会调用共同的callback函数;
• 这里的callback函数形参是ConstPtr类型,这是一种SharePoint,目的是无需对topic数据进行复制.所以sensor_msgs::LaserScan::ConstPtr与sensor_msgs::LaserScan是有区别的,注意使用方法,可以取地址内容直接赋予临时变量,也可以取地址某一内容单独赋予变量.

3 tf转换

tf::MessageFilter可以订阅任何的ROS消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进入回调函数进行相应的处理.
tf::MessageFilter的初始化需要message_filters::Subscriber，tf转换，目标坐标系，等待时间。当message_filters::Subscriber的消息能够由tf转换到目标坐标系时，调用回调函数，回调函数由tf::MessageFilter::registerCallback()进行注册。
示例AMCL:

tf_ = new TransformListenerWrapper();
message_filters::Subscriber<sensor_msgs::LaserScan>* laser_scan_sub_;
tf::MessageFilter<sensor_msgs::LaserScan>* laser_scan_filter_;
 
laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);
laser_scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_, *tf_, odom_frame_id_, 100);
 
laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived, this, _1));
 
void AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan){
    this->tf_->transformPose(base_frame_id_, ident, laser_pose);//这个函数的意思是:将ident在其原frame下的表示,转换为在base_frame_id下的表示,得laser_pose
}

tf2_ros::Buffer::transform() 可以直接将一个坐标系下的位姿转换到另一个坐标系的位姿。

参考

tf之 MessageFilter 与 tf::MessageFilter理解与应用

ROS之订阅多个话题并对其进行同步处理(多传感器融合)