ROS机器人程序设计（原书第2版）补充资料 （陆） 第六章 点云 PCL

How to use a PCL tutorial in ROS

Create a ROS package

Create the code skeleton

Create an empty file called src/example.cpp and paste the following code in it:

The code above does nothing but initialize ROS, create a subscriber and a publisher for PointCloud2 data.

Add the source file to CMakeLists.txt

Edit the CMakeLists.txt file in your newly created package and add:

Download the source code from the PCL tutorial

PCL has about four different ways of representing point cloud data, so it can get a bit confusing, but we'll try to keep it simple for you. The types are:

• sensor_msgs::PointCloud — ROS message (deprecated)

• sensor_msgs::PointCloud2 — ROS message

• pcl::PCLPointCloud2 — PCL data structure mostly for compatibility with ROS (I think)

• pcl::PointCloud<T> — standard PCL data structure

In the following code examples we will focus on the ROS message (sensor_msgs::PointCloud2) and the standard PCL data structure (pcl::PointCloud<T>). However, you should also note that pcl::PCLPointCloud2 is an important and useful type as well: you can directly subscribe to nodes using that type and it will be automatically serialized to/from the sensor_msgs type. See this example to try PCLPointCloud2 yourself.

sensor_msgs/PointCloud2

The sensor_msgs/PointCloud2 format was designed as a ROS message, and is the preferred choice for ROS applications. In the following example, we downsample a PointCloud2 structure using a 3D grid, thus reducing the number of points in the input dataset considerably.

To add this capability to the code skeleton above, perform the following steps:

• visit http://www.pointclouds.org/documentation/, click on Tutorials, then navigate to the Downsampling a PointCloud using a VoxelGrid filter tutorial (http://www.pointclouds.org/documentation/tutorials/voxel_grid.php)

• read the code and the explanations provided there. You will notice that the code breaks down essentially in 3 parts: 
  • load the cloud (lines 9-19)
  • process the cloud (lines 20-24)
  • save the output (lines 25-32)
• since we use ROS subscribers and publishers in our code snippet above, we can ignore the loading and saving of point cloud data using the PCD format. Thus, the only relevant part in the tutorial remains lines 20-24 that create the PCL object, pass the input data, and perform the actual computation:

• In these lines, the input dataset is named cloud, and the output dataset is called cloud_filtered. We can copy this work, but remember from earlier that we said we wanted to work with the sensor_msgs class, not the pcl class. In order to do this, we're going to have to do a little bit of extra work to convert the ROS message to the PCL type. Modify the callback function as follows:

Save the output file then build:

Then run:

$ rosrun my_pcl_tutorial example input:=/narrow_stereo_textured/points2

or, if you're running an OpenNI-compatible depth sensor, try:

$ roslaunch openni_launch openni.launch
$ rosrun my_pcl_tutorial example input:=/camera/depth/points

You can visualize the result by running RViz:

$ rosrun rviz rviz

and adding a "PointCloud2" display. Select camera_depth_frame for the Fixed Frame (or whatever frame is appropriate for your sensor) and select output for the PointCloud2 topic. You should see a highly downsampled point cloud. For comparison, you can view the /camera/depth/points topic and see how much it has been downsampled.

pcl/PointCloud<T>

The pcl/PointCloud<T> format represents the internal PCL point cloud format. For modularity and efficiency reasons, the format is templated on the point type, and PCL provides a list of templated common types which are SSE aligned. In the following example, we estimate the planar coefficients of the largest plane found in a scene.

To add this capability to the code skeleton above, perform the following steps:

• visit http://www.pointclouds.org/documentation/, click on Tutorials, then navigate to the Planar model segmentation tutorial (http://www.pointclouds.org/documentation/tutorials/planar_segmentation.php)

• read the code and the explanations provided there. You will notice that the code breaks down essentially in 3 parts: 
  • create a cloud and populate it with values (lines 12-30)
  • process the cloud (38-56)
  • write the coefficients (58-68)
• since we use ROS subscribers in our code snippet above, we can ignore the first step, and just process the cloud received on the callback directly. Thus, the only relevant part in the tutorial remains lines 38-56 that create the PCL object, pass the input data, and perform the actual computation:

• In these lines, the input dataset is named cloud and is of type pcl::PointCloud<pcl::PointXYZ>, and the output is represented by a set of point indices that contain the plane together with the plane coefficients. cloud.makeShared()creates a boost shared_ptr object for the object cloud (see the pcl::PointCloud API documentation).

Copy these lines, in the code snippet above, by modifying the callback function as follows:

In addition, since we're now publishing the planar model coefficients found rather than point cloud data, we have to change our publisher type from:

// Create a ROS publisher for the output point cloud
pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);

to:

// Create a ROS publisher for the output model coefficients
pub = nh.advertise<pcl_msgs::ModelCoefficients> ("output", 1);

Save the output file, then compile and run the code above:

$ rosrun my_pcl_tutorial example input:=/narrow_stereo_textured/points2

or, if you're running an OpenNI-compatible depth sensor, try:

$ rosrun my_pcl_tutorial example input:=/camera/depth/points

See the output with

$ rostopic echo output