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() 可以直接将一个坐标系下的位姿转换到另一个坐标系的位姿。