ROS编程: 重要的代码优化知识点记录(1)

订阅多个话题并对其进行同步处理

本小节针对在ROS节点中需要订阅两个及两个以上的话题时，需要保持对这两个话题数据的同步，且需要同时接收数据一起处理然后当做参数传入到另一个函数中；

研究背景：realsenseT265 和 realsense D435 都有IMU数据，但是这两个传感器都将imu的数据拆开进行发布了，区分了线加速度和角加速，而在有一些场合我们需要合并使用；

详细的代码如下：

#include <message_filters/subscriber.h>

#include <message_filters/synchronizer.h>

#include <message_filters/sync_policies/approximate_time.h>

#include <boost/thread/thread.hpp>

using namespace message_filters;

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg_accel, const sensor_msgs::ImuConstPtr &imu_msg_gyro)

{

        double t = imu_msg_accel->header.stamp.toSec();

        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;

        double 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)

{

    // 需要用message_filter容器对两个话题的数据发布进行初始化，这里不能指定回调函数

    message_filters::Subscriber<sensor_msgs::Imu> sub_imu_accel(n,IMU_TOPIC_ACCEL,2000,ros::TransportHints().tcpNoDelay());

    message_filters::Subscriber<sensor_msgs::Imu> sub_imu_gyro(n,IMU_TOPIC_GYRO,2000,ros::TransportHints().tcpNoDelay());

    // 将两个话题的数据进行同步

    typedef sync_policies::ApproximateTime<sensor_msgs::Imu, sensor_msgs::Imu> syncPolicy;

    Synchronizer<syncPolicy> sync(syncPolicy(10), sub_imu_accel, sub_imu_gyro);

    // 指定一个回调函数，就可以实现两个话题数据的同步获取

    sync.registerCallback(boost::bind(&imu_callback, _1, _2));

    ros::spin();

    return 0;

}

巴特西

ROS编程: 重要的代码优化知识点记录(1)

订阅多个话题并对其进行同步处理

最新文章

热门文章