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

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

文章目录

• 在ROS中，一节点读取发布点云，另一节点订阅点云后进行滤波处理、切割处理然后发布至ros
• 前言
• 一、功能实现步骤详情
• 二、节点工作环境
• • 1.src文件夹下的文件
  • 2.CMakeLists.txt
• 三、两节点代码详情
• • 1.pubpcb.cpp
  • 2.example_voxelgrid_pcl_types.cpp
• 四、编译，并运行节点
• • 1.在工作空间打开一个终端，输入指令：catkin_make
  • 2.打开新终端，运行roscore
  • 3. 打开新终端，运行第一个节点pubpcd
  • 4.打开新终端，运行第二个节点voxel
  • 5.将处理后的点云以坐标形式显示
  • 6.在rviz查看运行结果
  • 7.添加话题：
  • 8.最终切割后效果图：
  • 9.原图：
• 五、总结
• • • 重点为ROS到PCL的互相转化，以及代码切割

---

前言

通过简单的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
添加并修改代码如下：

  catkin
  libpcl-all-dev
  pcl_conversions
  pcl_ros
  roscpp
  sensor_msgs
  libpcl-all
  pcl_conversions
  pcl_ros
  roscpp
  sensor_msgs

  cv_bridge
  image_transport
  cv_bridge
  image_transport

三、两节点代码详情 1.pubpcb.cpp

#include 
#include 
#include 
#include 
#include 
//#include 
#include 
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("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 
// PCL specific includes
//#include 
//#include 

#include 
#include 
#include 

#include 

typedef pcl::PointCloud 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*cloudPXYZ1(new pcl::PointCloud);
  pcl::PointCloud*cloudPXYZ2(new pcl::PointCloud);

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


  //pcl::VoxelGrid 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 ("output", 1);
  pub = nh.advertise ("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);