ROS中点云学习（三）：使用PCL接收点云，给没有颜色的点云增加颜色变为彩色点云

转载自祢豆子
在上一篇ROS中点云学习（二）：使用PCL接收点云，操作之后重新发送中，我们接收了动态的彩色点云，并进行了颜色修改的操作。

但是这是针对同一类型的点云而言的，如果原本接收到的点云就没有颜色，我们应该如何给它上色呢？

所以就有了这一篇，点云上色。

主要思想：使用点云消息的接收，接收之后，建立一个PointXYZRGB类型的点，把接收到的点云的XYZ信息给这个点，然后颜色可以自己设置。设置完成点之后，把点放入预先设置好的点云里，再把点云主题发布出去。

这里我使用的原始点云为Velodyne VLP-16采集到的数据，数据点击这里下载。

链接：https://pan.baidu.com/s/1LZpQMWw1quG4Dr7dZmFLrQ 
提取码：xueu

原始数据中，包含XYZ信息，intensity(反射强度)和ring(第几圈)信息。但是这里的intensity和ring对我们没有太大用途，所以直接舍弃。
增加RGB的信息，变为一个PointXYZRGB类型的点云。

主函数pcl_colored.cpp具体代码如下：

#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

class pcl_colored
{
private:
  ros::NodeHandle n;
  ros::Subscriber subCloud;
  ros::Publisher pubCloud;
  sensor_msgs::PointCloud2 msg;  //接收到的点云消息
  sensor_msgs::PointCloud2 colored_msg;  //等待发送的点云消息

public:
  pcl_colored():
    n("~"){
    subCloud = n.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 1, &pcl_colored::getcloud, this); //接收velodyne点云数据，进入回调函数getcloud()
    pubCloud = n.advertise<sensor_msgs::PointCloud2>("/colored_cloud", 1000);  //建立了一个发布器，主题是/adjustd_cloud，方便之后发布加入颜色之后的点云
  }

  //回调函数
  void getcloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr  colored_pcl_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);   //放在这里是因为，每次都需要重新初始化，舍弃了原有但没用的两个通道intensity、ring
    pcl::PointCloud<pcl::PointXYZI>::Ptr   raw_pcl_ptr (new pcl::PointCloud<pcl::PointXYZI>);   //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++)
    //把接收到的点云位置不变，颜色设置为红色![在这里插入图片描述](https://img-blog.csdnimg.cn/20210218123024696.gif#pic_center)

    {
      pcl::PointXYZRGB  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.r =  255;
      p.g =  0;
      p.b =  0; 
      colored_pcl_ptr->points.push_back(p);
    }
    colored_pcl_ptr->width = 1;
    colored_pcl_ptr->height = raw_pcl_ptr->points.size();
    pcl::toROSMsg( *colored_pcl_ptr,  colored_msg);  //将点云转化为消息才能发布
    colored_msg.header.frame_id = "velodyne";//帧id改成和velodyne一样的
    pubCloud.publish( colored_msg); //发布调整之后的点云数据，主题为/adjustd_cloud
  }

  ~pcl_colored(){}

};

int main(int argc, char** argv) {

  ros::init(argc, argv, "colored");  //初始化了一个节点，名字为colored

  pcl_colored  pc;

  ros::spin();
  return 0;
}

Cmakelist.txt

cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
project(pcl_colored)

set(PACKAGE_DEPENDENCIES
  roscpp
  sensor_msgs
  pcl_ros
  pcl_conversions
  std_srvs
  message_generation 
  std_msgs
)

find_package(PCL 1.3 REQUIRED COMPONENTS common io)
find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
include_directories(${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(pcl_colored pcl_colored.cpp)
target_link_libraries(pcl_colored ${PCL_LIBRARIES}  ${catkin_LIBRARIES} )

按照同样的方法，cmake编译之后，运行./pcl_colored

于此同时打开下载的bag文件的文件夹，在终端打开，输入

$ rosbag play ***.bag

打开rviz，即可查看发布出来的消息。值得注意的是，这两个消息的frame_id均为velodyne，需要在rviz中把Fixed_Frame修改过来。