ROS中点云学习（二）：使用PCL接收点云，操作之后重新发送

在上一篇ROS中点云学习（一）：使用PCL生成动态随机彩色点云，并发布使用rviz查看中，我们生成了动态的彩色点云，并发布了出去。
这次我们接收点云，并对它进行操作，然后发布一个新的点云。

主要思想：自己写一个发布器和接收器，接收器的回调函数是最重要的部分。在回调函数中我们把接收到的点云颜色进行修改，然后重新发布一个主题。具体注释详见代码。

主程序为pcl_sub.cpp，这次写的规范了很多，使用了类。具体代码如下：

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

class pcl_sub
{
private:
  ros::NodeHandle n;
  ros::Subscriber subCloud;
  ros::Publisher pubCloud;
  sensor_msgs::PointCloud2 msg;  //接收到的点云消息
  sensor_msgs::PointCloud2 adjust_msg;  //等待发送的点云消息
  pcl::PointCloud<pcl::PointXYZRGB> adjust_pcl;   //建立了一个pcl的点云，作为中间过程

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

  //回调函数
  void getcloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr adjust_pcl_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);   //放在这里是因为，每次都需要重新初始化
    pcl::fromROSMsg(*laserCloudMsg, *adjust_pcl_ptr);  //把msg消息转化为点云
    adjust_pcl = *adjust_pcl_ptr;  
    for (int i = 0; i < adjust_pcl.points.size(); i++)
    //把接收到的点云位置不变，颜色全部变为绿色
    {
      adjust_pcl.points[i].r = 0;
      adjust_pcl.points[i].g = 255;
      adjust_pcl.points[i].b = 0;
    }
    pcl::toROSMsg(adjust_pcl, adjust_msg);  //将点云转化为消息才能发布
    pubCloud.publish(adjust_msg); //发布调整之后的点云数据，主题为/adjustd_cloud
  }

  ~pcl_sub(){}

};

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

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

  pcl_sub ps;

  ros::spin();
  return 0;
}

CMakeLists.txt文件和上一次的差不多，具体为：

cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
project(pcl_sub)

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_sub pcl_sub.cpp)
target_link_libraries(pcl_sub ${PCL_LIBRARIES}  ${catkin_LIBRARIES} )

按照同样的方法，cmake编译之后，运行./pcl_sub
这需要同时运行之前的./pcl_pub
这样，我们打开两个rviz，可以看到操作之后的效果，如下（8倍速之后）