ROS中点云学习（五）：同时接收点云和图像

为了更好地理解场景，需要同时接收点云消息和图像消息。

主要思想：目前，我还不知道点云和图像的对应关系，所以不能修改点云中的每个点的颜色，所以把点云颜色弄成一样的，这个颜色由图像上某一点确定（100,250）。这样点云就随着图像而改变颜色了。

和上一篇类似，为了主程序的简洁，我们建立一个头文件myPointType.h，在这里面我们定义我们所需要的点云类型，具体代码如下：

#ifndef PCL_NO_PRECOMPILE
#define PCL_NO_PRECOMPILE


#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

/*
    *一个具有XYZ、intensity、ring的点云类型
    */
struct PointXYZIR
{
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    uint16_t ring;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR,  
                                   (float, x, x) (float, y, y)
                                   (float, z, z) (float, intensity, intensity)
                                   (uint16_t, ring, ring)
)


/*
    * 一个具有XYZ、RGB、intensity、ring的点云类型
    */
struct PointXYZRGBIR
{
    PCL_ADD_POINT4D;
    PCL_ADD_RGB;
    PCL_ADD_INTENSITY;
    uint16_t ring;
    uint16_t label;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZRGBIR,  
                            (float, x, x)
                            (float, y, y)
                            (float, z, z)
                            (float, rgb, rgb)
                            (float, intensity, intensity)
                            (uint16_t, label, label)
                            (uint16_t, ring, ring)
)
 
#endif  //PCL_NO_PRECOMPILE

主函数pcl_image.cpp，具体如下

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include "myPointType.h"

cv::Vec3b color_raw;  //全局变量都能访问，图像修改，点云获取

class pcl_image
{
private:
  ros::NodeHandle n;
  sensor_msgs::PointCloud2 msg;  //接收到的点云消息
  sensor_msgs::PointCloud2 color_msg;  //等待发送的点云消息

public:
  ros::Subscriber subCloud = n.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 1, &pcl_image::getcloud, this); //接收velodyne点云数据，进入回调函数getcloud()
  ros::Publisher pubCloud = n.advertise<sensor_msgs::PointCloud2>("/color_cloud", 1);  //建立了一个发布器，主题是/adjustd_cloud，方便之后发布加入颜色之后的点云
  
  //点云回调函数
  void getcloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
    pcl::PointCloud<PointXYZRGBIR>::Ptr  color_pcl_ptr (new pcl::PointCloud<PointXYZRGBIR>);   //放在这里是因为，每次都需要重新初始化，舍弃了原有但没用的两个通道intensity、ring
    pcl::PointCloud<PointXYZIR>::Ptr   raw_pcl_ptr (new pcl::PointCloud<PointXYZIR>);   //VLP-16的点云消息包含xyz和intensity、ring的，这里没有加ring不知道为啥也可以，需要的话要自己定义一个点类型PointXYZIR
    pcl::fromROSMsg(*laserCloudMsg, *raw_pcl_ptr);  //把msg消息指针转化为点云指正
    for (int i = 0; i <  raw_pcl_ptr->points.size(); i++)
    {
      PointXYZRGBIR  p;
      p.x=raw_pcl_ptr->points[i].x;
      p.y=raw_pcl_ptr->points[i].y;
      p.z=raw_pcl_ptr->points[i].z;
      //点云颜色由图像上某一点确定
      p.b = color_raw[0];
      p.g = color_raw[1];
      p.r = color_raw[2];
      p.label = (rand() % (10+1));  //设置10个标签，标签随机
      p.intensity = raw_pcl_ptr->points[i].intensity;  //继承之前点云的intensity
      p.ring = raw_pcl_ptr->points[i].ring;  //继承之前点云的ring
      color_pcl_ptr->points.push_back(p);
    }
    color_pcl_ptr->width = 1;
    color_pcl_ptr->height = raw_pcl_ptr->points.size();
    pcl::toROSMsg( *color_pcl_ptr, color_msg);  //将点云转化为消息才能发布
    color_msg.header.frame_id = "velodyne";//帧id改成和velodyne一样的
    pubCloud.publish( color_msg); //发布调整之后的点云数据，主题为/adjustd_cloud
  }

};

 //图像回调函数
void imageCallback(const sensor_msgs::ImageConstPtr& msg){
  try{
    cv::Mat image  = cv_bridge::toCvShare(msg, "bgr8")->image; //image_raw就是我们得到的图像了
    cv::circle(image,cv::Point(100,250),5,cv::Scalar(0,0,255),3); //注意先列后行
    cv::Vec3b color =  (cv::Vec3b)image.at<cv::Vec3b>(250, 100);  //注意先行后列
    color_raw[0] = color[0];
    color_raw[1] = color[1];
    color_raw[2] = color[2];
    cv::imshow("view", image);
  }
  catch (cv_bridge::Exception& e){
  ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "pcl_image");
  cv::namedWindow("view");
  cv::startWindowThread();
  pcl_image pi;
  ros::NodeHandle n;
  image_transport::ImageTransport it(n); //用之前声明的节点句柄初始化it，其实这里的it和nh的功能基本一样，可以像之前一样使用it来发布和订阅相消息。
  image_transport::Subscriber sub = it.subscribe("/realsense/color/image_raw", 1, &imageCallback);
  
  ros::spin();
  cv::destroyWindow("view");
}

CMakeLists.txt文为：

cmake_minimum_required(VERSION 2.8)
project(MY_GRAND_PROJECT)

set(PACKAGE_DEPENDENCIES
  roscpp
  sensor_msgs
  pcl_ros
  pcl_conversions
  std_srvs
  message_generation 
  std_msgs
  cv_bridge
  image_transport 
)

find_package(OpenCV REQUIRED)
find_package(PCL 1.3 REQUIRED COMPONENTS common io)
find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
include_directories(${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} )
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(pcl_image pcl_image.cpp)
target_link_libraries(pcl_image ${PCL_LIBRARIES}  ${catkin_LIBRARIES}  ${OpenCV_LIBS})

图像会自动显示，点云可以在rviz查看。我们可以发现点云的颜色完全收到图像中红圈的原点的颜色决定。
成功！！！