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

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

• 详细的代码如下：

  #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;
  
  }