ROS下使用PCL处理点云话题

 1.在工作空间创建一个ROS软件包

catkin_create_pkg my_pcl pcl_conversions pcl_ros pcl_msgs sensor_msg

cd my_pcl

mkdir src

cd src

2.ros话题读取

在/my_pcl/src中创建pcl_writecpp

 touch pcl_writecpp

#include <ros/ros.h>

#include <sensor_msgs/PointCloud2.h>

#include <pcl_conversions/pcl_conversions.h>

#include <pcl/point_cloud.h>

#include <pcl/point_types.h>

#include <pcl/filters/voxel_grid.h>

#include <pcl/filters/conditional_removal.h>

#include <pcl/conversions.h>

#include <pcl/filters/passthrough.h>  

ros::Publisher pub;

void

cloud_cb (const sensor_msgs::PointCloud2& cloud_msg)

{

  pcl::PointCloud<pcl::PointXYZI>::Ptr result(new pcl::PointCloud<pcl::PointXYZI>);

  //将ros点云转换为pcl点云

  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);

  pcl::fromROSMsg(cloud_msg, *cloud);

  //处理操作

  //将处理后的pcl点云转换为ros点云并发布

  sensor_msgs::PointCloud2 output;

  pcl::toROSMsg(*result,output);

  pub.publish (output);

}

int

main (int argc, char** argv)

{

  // Initialize ROS

  ros::init (argc, argv, "my_pcl");

  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud

  ros::Subscriber sub = nh.subscribe ("/livox/lidar", 1000, cloud_cb);

  // Create a ROS publisher for the output point cloud

  pub = nh.advertise<sensor_msgs::PointCloud2> ("/pcl_output", 1000);

  // Spin

  ros::spin ();

}

3.修改CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)

project(my_pcl)

find_package(catkin REQUIRED COMPONENTS

  pcl_conversions

  pcl_msgs

  pcl_ros

  sensor_msgs

)

find_package(PCL REQUIRED)

include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})

link_directories(${PCL_LIBRARY_DIRS})

add_executable(pcl_write src/pcl_write.cpp)

target_link_libraries(pcl_write ${catkin_LIBRARIES} ${PCL_LIBRARIES})

4.编译运行指令

catkin build my_pcl

/XXX_ws/src路径下

source devel/setup.bash

roscore

rosrun my_pcl pcl_write

5.ROS和PCL的四种数据类型

pcl::PointCloud<pcl::PointXYZ>

pcl::PCLPointCloud2

sensor_msgs::PointCloud2

sensor_msgs::PointCloud

6.转换函数

1. sensor_msgs::PCLPointCloud2 <=> pcl::PCLPointCloud2

void pcl_conversions::toPCL(const sensor_msgs::PointCloud2 &, pcl::PCLPointCloud2 &)

void pcl_conversions::moveFromPCL(const pcl::PCLPointCloud2 &, const sensor_msgs::PointCloud2 &);

2. pcl::PCLPointCloud2 <=> pcl::PointCloud<pcl::PointXYZ>

void pcl::fromPCLPointCloud2(const pcl::PCLPointCloud2 &, pcl::PointCloud<pointT> &);

void pcl::toPCLPointCloud2(const pcl::PointCloud<pointT> &, pcl::PCLPointCloud2 &);

3. sensor_msgs::PCLPointCloud2 <=> pcl::PointCloud<pcl::PointXYZ>

void pcl::fromROSMsg(const sensor_msgs::PointCloud2 &, pcl::PointCloud<T> &);

void pcl::toROSMsg(const pcl::PointCloud<T> &, sensor_msgs::PointCloud2 &);

4. sensor_msgs::PointCloud2 <=> sensor_msgs::PointCloud

static bool sensor_msgs::convertPointCloudToPointCloud2(const sensor_msgs::PointCloud & input,sensor_msgs::PointCloud2 & output);

static bool sensor_msgs::convertPointCloud2ToPointCloud(const sensor_msgs::PointCloud2 & input,sensor_msgs::PointCloud & output);

参考

C++点云PCL基础ROS代码

 pcl点云与ros之间的数据类型转换