在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);