ros消息时间同步与回调
2015-08-18 20:51
2286 查看
最近做ekfslam,有两个数据输入/odom 与/ img信息,刚开始这两个都是通过rosbag包回放,分别有两个toptic订阅回调函数触发不同的处理.
这在后面处理的时候又需要对这两数据进行相应的时间同步,我之前采用的是odom建立vector向量表存储(频率比图像快),后面以img数据为准,
从img的时间戳搜索最近的odom. 然后再将这两数据传入ekfslam进行预测与更新. 而我这种处理方式又必须得考虑类成员与多线程(topic消息机制)
存在成员变量在过程中强行覆盖的问题.
今天与根哥讨论,建议我用消息同步的方式进行回调同步处理. 我这就参考wiki试着开干了.
通过对多输入通道的输入topic的时间戳分析,进行同步,将同步的所有消息以一个回调的方式触发,
方式一: 全局变量形式 : 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
参考连接:http://wiki.ros.org/message_filters
方式二: 类成员的形式 message_filters::Synchronizer
说明: 我用 TimeSynchronizer 改写成类形式中间出现了一点问题.后就改写成message_filters::Synchronizer的形式.
1. 头文件
2. 定义消息同步机制
3. 定义类成员变量
4.类构造函数中开辟空间new
5. 类成员函数回调处理
这在后面处理的时候又需要对这两数据进行相应的时间同步,我之前采用的是odom建立vector向量表存储(频率比图像快),后面以img数据为准,
从img的时间戳搜索最近的odom. 然后再将这两数据传入ekfslam进行预测与更新. 而我这种处理方式又必须得考虑类成员与多线程(topic消息机制)
存在成员变量在过程中强行覆盖的问题.
今天与根哥讨论,建议我用消息同步的方式进行回调同步处理. 我这就参考wiki试着开干了.
通过对多输入通道的输入topic的时间戳分析,进行同步,将同步的所有消息以一个回调的方式触发,
方式一: 全局变量形式 : 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
方式二: 类成员的形式 message_filters::Synchronizer
说明: 我用 TimeSynchronizer 改写成类形式中间出现了一点问题.后就改写成message_filters::Synchronizer的形式.1. 头文件
#include <message_filters/subscriber.h> #include <message_filters/synchronizer.h> #include <message_filters/sync_policies/approximate_time.h>
2. 定义消息同步机制
typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry,sensor_msgs::Image> slamSyncPolicy;
3. 定义类成员变量
message_filters::Subscriber<nav_msgs::Odometry>* odom_sub_ ; // topic1 输入 message_filters::Subscriber<sensor_msgs::Image>* img_sub_; // topic2 输入 message_filters::Synchronizer<slamSyncPolicy>* sync_;
4.类构造函数中开辟空间new
odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(ar_handle, "/odom", 1); img_sub_ = new message_filters::Subscriber<sensor_msgs::Image>(ar_handle, "/usb_cam/image_raw", 1); sync_ = new message_filters::Synchronizer<slamSyncPolicy>(slamSyncPolicy(10), *odom_sub_, *img_sub_); sync_->registerCallback(boost::bind(&QrSlam::combineCallback,this, _1, _2));
5. 类成员函数回调处理
void QrSlam::combineCallback(const nav_msgs::Odometry::ConstPtr& pOdom, const sensor_msgs::ImageConstPtr& pImg) //回调中包含多个消息 { //TODO fStampAll<<pOdom->header.stamp<<" "<<pImg->header.stamp<<endl; getOdomData(pOdom); // is_img_update_ = getImgData(pImg); // 像素值 cout << "stamp x y theta v w " << robot_odom_.stamp<<" "<<robot_odom_.x << " "<< robot_odom_.y << " " << robot_odom_.theta << " " << robot_odom_.v << " " << robot_odom_.w << std::endl; fOdom << "stamp x y theta v w " << robot_odom_.stamp<<" "<<robot_odom_.x << " "<< robot_odom_.y << " " << robot_odom_.theta << " " << robot_odom_.v << " " << robot_odom_.w << std::endl; pixDataToMetricData(); static bool FINISH_INIT_ODOM_STATIC = false; if(FINISH_INIT_ODOM_STATIC) { ekfslam(robot_odom_); } else if(is_img_update_) { if(addInitVectorFull()) { computerCoordinate(); FINISH_INIT_ODOM_STATIC = true; } } }
相关文章推荐
- 笔记本wifi共享给手机 连接连笔记本wifi
- java 引用类型 和 基本类型的理解
- Project2013 界面目录清单
- frame,iframe,frameset用法和区别
- Codeforces Gym 100650D Queens, Knights and Pawns 暴力
- scanf详解
- POJ1703 简单的种类并查集
- 转:理解C++11的模板类型推导
- 白酒香型
- [HDU 1281] 棋盘游戏 最大匹配
- 2015 Multi-University Training Contest 9(区间dp)
- StringGrid右击选中表格(发消息给句柄模拟点击,右键点击也是MouseDown)
- ZKW费用流修正
- 织梦DedeCms Runphp 标签中调用其他变量的方法
- linux curl 命令
- 编程技巧 - 3
- Redis介绍
- autolayout的使用原理及代码实现
- CODEVS 1132 瑞士轮
- .NET破解之PDFdo转换器