ROS下同时接收多个话题并实现相机和雷达的数据融合

更多干货请关注公众号[3D视觉工坊]~

前言

最近要在ROS下做激光雷达和相机的数据融合，而且要同步，搜了网上一大堆，没有找到特别明确的答案，最终，还是搞出来啦～～～

这里主要是完成雷达和相机同步映射，需要相机的内参和雷达相机标定的外参。关于雷达和相机的标定请参考我的另一篇博文：雷达和相机的联合标定

代码

把要处理的数据声明为类的私有变量，data_fusion（）为数据融合函数～，这里相机的更新频率要低于雷达，相机大约为3fps，而雷达为10Hz，所以暂时把data_fusion放在相机的回调函数中进行处理，后期会通过加锁来保证数据的同步性。

#include <ros/ros.h>
#include <std_msgs/String.h>
#include<car_sensor/image_detect.h>
#include<string>
#include <boost/thread.hpp>
#include <sstream>
#include<sensor_msgs/PointCloud2.h>
#include<opencv2/opencv.hpp>
#include<cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
//pcl and pointcloud2 to handle the lidar_point data
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <sensor_msgs/PointCloud2.h>

using namespace std;
using namespace cv;

class SubscribeAndPublish
{

private:
  cv::Mat image;
  ros::NodeHandle n;
  //ros::Publisher pub_;
  ros::Subscriber sub_camera;
  ros::Subscriber sub_lidar;
  //std_msgs::String output;
  //int count;
  std::vector<string> roi_label;
  std::vector<int> roi_leftX;
  std::vector<int> roi_leftY;
  std::vector<int&