How to use a PCL tutorial in ROS
Contents
Create a ROS package
$ roscreate-pkg my_pcl_tutorial pcl pcl_ros roscpp sensor_msgs
$ catkin_create_pkg my_pcl_tutorial pcl pcl_ros roscpp sensor_msgs
This will create a new ROS package with the necessary dependencies.
Create the code skeleton
Create an empty file called src/example.cpp and paste the following code in it:
1 #include <ros/ros.h>
2 #include <sensor_msgs/PointCloud2.h>
3 // PCL specific includes
4 #include <pcl/ros/conversions.h>
5 #include <pcl/point_cloud.h>
6 #include <pcl/point_types.h>
7
8 ros::Publisher pub;
9
10 void
11 cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
12 {
13 // ... do data processing
14
15 sensor_msgs::PointCloud2 output;
16 // Publish the data
17 pub.publish (output);
18 }
19
20 int
21 main (int argc, char** argv)
22 {
23 // Initialize ROS
24 ros::init (argc, argv, "my_pcl_tutorial");
25 ros::NodeHandle nh;
26
27 // Create a ROS subscriber for the input point cloud
28 ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);
29
30 // Create a ROS publisher for the output point cloud
31 pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);
32
33 // Spin
34 ros::spin ();
35 }
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:
rosbuild_add_executable (example src/example.cpp)
add_executable(example src/example.cpp)
target_link_libraries(example ${catkin_LIBRARIES})
Download the source code from the PCL tutorial
All PCL versions prior to 2.0, have two different ways of representing point cloud data:
through a sensor_msgs/PointCloud2 ROS message
through a pcl/PointCloud<T> data structure
The following two code examples will discuss both formats.
sensor_msgs/PointCloud2
If you'd like to save yourself some copying and pasting, you can download the source file for this example here.
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. Copy these lines, in the code snippet above, by modifying the callback function as follows:
1 #include <pcl/filters/voxel_grid.h>
2
3 ...
4
5 void
6 cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud)
7 {
8 sensor_msgs::PointCloud2 cloud_filtered;
9
10 // Perform the actual filtering
11 pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
12 sor.setInputCloud (cloud);
13 sor.setLeafSize (0.01, 0.01, 0.01);
14 sor.filter (cloud_filtered);
15
16 // Publish the data
17 pub.publish (cloud_filtered);
18 }
Note
Since different tutorials will often use different variable names for their inputs and outputs, remember that you may need to modify the code slightly when integrating the tutorial code into your own ROS node. In this case, notice that we had to change the variable name input to cloud, and output to cloud_filtered in order to match up with the code from the tutorial we copied.
Save the output file then build:
$ rosmake my_pcl_tutorial
$ cd %TOP_DIR_YOUR_CATKIN_HOME% $ catkin_make
Then run:
$ 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
pcl/PointCloud<T>
As with the previous example, if you want to skip a few steps, you can download the source file for this example here.
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:
1 pcl::ModelCoefficients coefficients;
2 pcl::PointIndices inliers;
3 // Create the segmentation object
4 pcl::SACSegmentation<pcl::PointXYZ> seg;
5 // Optional
6 seg.setOptimizeCoefficients (true);
7 // Mandatory
8 seg.setModelType (pcl::SACMODEL_PLANE);
9 seg.setMethodType (pcl::SAC_RANSAC);
10 seg.setDistanceThreshold (0.01);
11
12 seg.setInputCloud (cloud.makeShared ());
13 seg.segment (inliers, coefficients);
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:
1 #include <pcl/sample_consensus/model_types.h>
2 #include <pcl/sample_consensus/method_types.h>
3 #include <pcl/segmentation/sac_segmentation.h>
4
5 ...
6
7 void
8 cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
9 {
10 // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
11 pcl::PointCloud<pcl::PointXYZ> cloud;
12 pcl::fromROSMsg (*input, cloud);
13
14 pcl::ModelCoefficients coefficients;
15 pcl::PointIndices inliers;
16 // Create the segmentation object
17 pcl::SACSegmentation<pcl::PointXYZ> seg;
18 // Optional
19 seg.setOptimizeCoefficients (true);
20 // Mandatory
21 seg.setModelType (pcl::SACMODEL_PLANE);
22 seg.setMethodType (pcl::SAC_RANSAC);
23 seg.setDistanceThreshold (0.01);
24
25 seg.setInputCloud (cloud.makeShared ());
26 seg.segment (inliers, coefficients);
27
28 // Publish the model coefficients
29 pub.publish (coefficients);
30 }
Notice that we added a conversion step from sensor_msgs/PointCloud2 to pcl/PointCloud<T> on the first two lines, and we also changed the variable that we publish from output to coefficients.
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::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