ROS进⾏多传感器融合时的时间对齐和时间同步解决⽅法⽂章⽬录
1.引⾔
本⼩节针对在ROS节点中需要订阅两个及两个以上的话题时，需要保持对这两个话题数据的同步，且需要同时接收数据⼀起处理然后当做参数传⼊到另⼀个函数中。
研究背景：realnT265 和 realn D435 都有IMU数据，但是这两个传感器都将imu的数据拆开进⾏发布了，区分了线加速度和⾓加速，⽽在有⼀些场合我们需要合并使⽤。
2.⽅法⼀：利⽤全局变量TimeSynchronizer
1 . message_filter ::subscriber 分别订阅不同的输⼊topic
2 . TimeSynchronizer<Image,CameraInfo> 定义时间同步器;
3 . isterCallback 同步回调
4 . void callback(const ImageConstPtr&image,const CameraInfoConstPtr& cam_info) 带多消息的消息同步⾃定义回调函数
5 .相关s/api/message_filters/html/c++/classmessage__filters_1_1TimeSynchronizer.html
#include <boost/thread/thread.hpp>
screaming
using namespace message_filters;
void imu_callback(const nsor_msgs::ImuConstPtr &imu_msg_accel, const nsor_msgs::ImuConstPtr &imu_msg_gyro)
{
double t = imu_msg_accel->Sec();
double dx = imu_msg_accel->linear_acceleration.x;
double dy = imu_msg_accel->linear_acceleration.y;
double dz = imu_msg_accel->linear_acceleration.z;
double rx = imu_msg_gyro->angular_velocity.x;desrt怎么读>suffer的用法
ltemdouble ry = imu_msg_gyro->angular_velocity.y;
double rz = imu_msg_gyro->angular_velocity.z;
Vector3d gyr(rx, ry, rz);
Vector3d acc(dx, dy, dz);
/**
处理函数 ......
*/
}
int main(int argc, char** argv)maroon5主唱
{
// 需要⽤message_filter容器对两个话题的数据发布进⾏初始化，这⾥不能指定回调函数
message_filters::Subscriber<nsor_msgs::Imu> sub_imu_accel(n,IMU_TOPIC_ACCEL,2000,ros::TransportHints().tcpNoDelay()); message_filters::Subscriber<nsor_msgs::Imu> sub_imu_gyro(n,IMU_TOPIC_GYRO,2000,ros::TransportHints().tcpNoDelay());
// 将两个话题的数据进⾏同步
typedef sync_policies::ApproximateTime<nsor_msgs::Imu, nsor_msgs::Imu> syncPolicy; //近似同步，⾮严格同步
Synchronizer<syncPolicy> sync(syncPolicy(10), sub_imu_accel, sub_imu_gyro);
// 指定⼀个回调函数，就可以实现两个话题数据的同步获取
ros::spin();
return 0;
}
3. 和 l添加ROS包
<下添加：
find_package(catkin REQUIRED COMPONENTS
....
image_transport
....
)
<build_depend>image_transport</build_depend>
西班牙语学习
edon
<exec_depend>image_transport</exec_depend>
参考连接:s/message_filters
补充:⽤ TimeSynchronizer 改写成类形式中间出现了⼀点问题.后就改写成message_filters::Synchronizer的形式4.⽅法⼆: 利⽤类成员message_filters::Synchronizer
1 . 头⽂件
2 . 定义消息同步机制和成员变量
typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry,nsor_msgs::Image> slamSyncPolicy;
message_filters::Subscriber<nav_msgs::Odometry>* odom_sub_ ; // topic1 输⼊
message_filters::Subscriber<nsor_msgs::Image>* img_sub_; // topic2 输⼊
message_filters::Synchronizer<slamSyncPolicy>* sync_;
3 .类构造函数中开辟空间new
odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(ar_handle, "/odom", 1);
img_sub_ = new message_filters::Subscriber<nsor_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));
4.类成员函数回调处理
void QrSlam::combineCallback(const nav_msgs::Odometry::ConstPtr& pOdom, const nsor_msgs::ImageConstPtr& pImg) //回调中包含多个消息{reading是什么意思
//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 = fal;
if(FINISH_INIT_ODOM_STATIC)
{
atmp
ekfslam(robot_odom_);
}
幼稚是什么意思el if(is_img_update_)
{
if(addInitVectorFull())
{
computerCoordinate();
FINISH_INIT_ODOM_STATIC = true;
}
}
}
