ROS：一节点读取发布点云，另一节点订阅点云后进行滤波处理、切割处理然后发布至ros

在ROS中，一节点读取发布点云，另一节点订阅点云后进行滤波处理、切割处理然后发布至ros

---

前言

通过简单的for循环和if语句实现点云的切割

---

一、功能实现步骤详情

（1）创建两个节点，voxel 和 pubpcd；

第一个节点pubpcd在代码pubpcb.cpp中读取点云进行话题发布；
第二个节点voxel在代码example_voxelgrid_pcl_types.cpp 中读取图像点云message然后进行滤波切割处理;

二、节点工作环境

在工作空间～home/catkin_ws/src下新建文件夹my_pcl_tutorial，在此文件夹下新建src（文件夹）、CMakeList.cpp（构建配置文件），package.xml（依赖），
如图所示：

1.src文件夹下的文件

两个节点文件pubpcb.cpp 和 example_voxelgrid_pcl_types.cpp，table_scene_lms400.pcd点云文件。

2.CMakeLists.txt

添加并修改代码如下：

add_executable(voxel src/example_voxelgrid_pcl_types.cpp) 
add_executable(pubpcd src/pubpcb.cpp) 

3.package.xml
添加并修改代码如下：

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>libpcl-all-dev</build_depend>
  <build_depend>pcl_conversions</build_depend>
  <build_depend>pcl_ros</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <run_depend>libpcl-all</run_depend>
  <run_depend>pcl_conversions</run_depend>
  <run_depend>pcl_ros</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>sensor_msgs</run_depend>

  <build_depend>cv_bridge</build_depend>
  <build_depend>image_transport</build_depend>
  <run_depend>cv_bridge</run_depend>
  <run_depend>image_transport</run_depend>

三、两节点代码详情

1.pubpcb.cpp

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
//#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
sensor_msgs::PointCloud2 cloud;

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

    ros::init (argc,argv,"pub_pcb");
    ros::NodeHandle nh;
    ros::Publisher pub; 
    
    pub = nh.advertise<sensor_msgs::PointCloud2>("pcbpub",1); 

    
  // Replace the path below with the path where you saved your file
  //  pcl::io::loadPCDFile ("/home/ros/catkin_ws/src/my_pcl_tutorial/src/table_scene_lms400.pcd", cloud); // Remember to download the file first!
    pcl::io::loadPCDFile ("/home/ros/catkin_ws/src/my_pcl_tutorial/src/table_scene_lms400.pcd", cloud); // Remember to download the file first!
    cloud.header.stamp=ros::Time::now();
    cloud.header.frame_id="/base_link";  //坐标

    ros::Rate loop_rate(10);

    while(ros::ok())
    {
        pub.publish(cloud);

        loop_rate.sleep();
    }
  return (0);
}

2.example_voxelgrid_pcl_types.cpp

```cpp
#include <ros/ros.h>
// PCL specific includes
//#include <pcl_conversions/pcl_conversions.h>
//#include <pcl/point_cloud.h>

#include <pcl/ros/conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/filters/voxel_grid.h>

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

ros::Publisher pub;

void cloud_cb (const pcl::PCLPointCloud2ConstPtr& cloud)
//void cloud_cb (const  PointCloud::ConstPtr& cloud)
{

  // PointCloud cloud_filtered;
  pcl::PCLPointCloud2 cloud_filtered;
  pcl::PointCloud<pcl::PointXYZ>*cloudPXYZ1(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>*cloudPXYZ2(new pcl::PointCloud<pcl::PointXYZ>);

  // Perform the actual filtering// 创建滤波器对象
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;


  //pcl::VoxelGrid<PointCloud> sor;
  sor.setInputCloud (cloud);
  sor.setLeafSize (0.01, 0.01, 0.01);//设置滤波时创建的体素体积为 10cm*10cm*10cm的立方体
  sor.filter (cloud_filtered);

  //转换为模板，点云实现从ROS到PCL中的数据的转化，同时也可以直接使用PCL库实现可视化
  pcl::fromPCLPointCloud2(cloud_filtered,*cloudPXYZ1);  //PCL转换点云
  ROS_INFO_STREAM("pointxyz size: "<<(*cloudPXYZ1).size());

  
  (*cloudPXYZ2).resize((*cloudPXYZ1).size());

  //for循环 进行切割
  for(unsigned long int i=0;i<(*cloudPXYZ1).size();++i)
  { 
    //矩形切割
    if((*cloudPXYZ1).at(i).x>-0.3&&(*cloudPXYZ1).at(i).y>0.2&&(*cloudPXYZ1).at(i).z>-2)
        //满足if条件的点云存放到×cloudPXYZ2
        (*cloudPXYZ2).push_back((*cloudPXYZ1).at(i)); 
  
  }
  //pcl到ros的转换
  pcl::toPCLPointCloud2(*cloudPXYZ2,cloud_filtered);

  //
  pcl_conversions::toPCL(ros::Time::now(),cloud_filtered.header.stamp);
  cloud_filtered.header.frame_id = "/base_link";
  
  // Publish the data
  pub.publish (cloud_filtered);
}

int main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
  //pub = nh.advertise<pcl::PCLPointCloud2> ("output", 1);
  pub = nh.advertise<PointCloud> ("output", 1);
  // Spin
  ros::spin ();
}

四、编译，并运行节点

1.在工作空间打开一个终端，输入指令：catkin_make

ros@ubuntu:~/catkin_ws$ catkin_make

2.打开新终端，运行roscore

输入指令：roscore

ros@ubuntu:~$ roscore

3. 打开新终端，运行第一个节点pubpcd

输入指令：rosrun my_pcl_tutorial pubpcd

ros@ubuntu:~/catkin_ws/$ rosrun my_pcl_tutorial pubpcd

4.打开新终端，运行第二个节点voxel

输入指令：rosrun my_pcl_tutorial voxel input:=/pcbpub

ros@ubuntu:~/catkin_ws/$ rosrun my_pcl_tutorial voxel input:=/pcbpub

5.将处理后的点云以坐标形式显示

输入指令：rosrun tf static_transform_publisher 0 0 0 0 0 0 1 map base_link 30

ros@ubuntu:~/catkin_ws/$ rosrun rosrun tf static_transform_publisher 0 0 0 0 0 0 1  map base_link 30

6.在rviz查看运行结果

在新终端输入指令：rviz

ros@ubuntu:~$ rviz

7.添加话题：

8.最终切割后效果图：

9.原图：

五、总结

重点为ROS到PCL的互相转化，以及代码切割

ROS到PCL

  //转换为模板，点云实现从ROS到PCL中的数据的转化，同时也可以直接使用PCL库实现可视化
  pcl::fromPCLPointCloud2(cloud_filtered,*cloudPXYZ1);  //PCL转换点云
  ROS_INFO_STREAM("pointxyz size: "<<(*cloudPXYZ1).size());
  
  (*cloudPXYZ2).resize((*cloudPXYZ1).size());

“切割”代码部分

//for循环 进行切割
  for(unsigned long int i=0;i<(*cloudPXYZ1).size();++i)
  { 
    //矩形切割
    if((*cloudPXYZ1).at(i).x>-0.3&&(*cloudPXYZ1).at(i).y>0.2&&(*cloudPXYZ1).at(i).z>-2)
        //满足if条件的点云存放到×cloudPXYZ2
        (*cloudPXYZ2).push_back((*cloudPXYZ1).at(i)); 
  
  }

PCL到ROS

  //pcl到ros的转换
  pcl::toPCLPointCloud2(*cloudPXYZ2,cloud_filtered);

  //
  pcl_conversions::toPCL(ros::Time::now(),cloud_filtered.header.stamp);
  cloud_filtered.header.frame_id = "/base_link";
  
  // Publish the data
  pub.publish (cloud_filtered);