Demo: Using PCL Nodelets

This is a simple package that uses a few nodelets from the pcl_ros ROS package. The basic idea is that we’ve constructed a pipeline of pointcloud processing steps where the overall goal is to extract a few objects sitting on a tabletop into separate point clouds and calculate the centroid of each of these clouds. Most of the processing steps use nodelets and the nodelets operate on sensor_msgs/PointCloud2 messages (that might come from an Intel D435i) In this demo, you can either use live data from a device, or the package includes a PCD file that can be used if you don’t have access to a device.

High Level Description

The majority of the package is implemented in a single launch file called nodelet_table_extraction.launch. At a high level, the pipeline follows these steps:

1. Either a depth sensor device or a PCD file is used to publish a sensor_msgs/PointCloud2 on the /camera/depth/points topic.
2. A pcl/CropBox filter nodelet is used to throw away all points that exist outside of a cubic volume described by maximum and minimum bounds in x,y,z coordinate axes.
3. The output of the pcl/CropBox filter is fed into a pcl/VoxelGrid filter nodelet that downsamples the point cloud to a coarser point cloud that uses less data.
4. The output of the pcl/VoxelGrid nodelet is fed into a pcl/SACSegmentation nodelet nodelet that is used to extract a planar model from the point cloud. If your pcl/CropBox filter parameters have been tuned correctly, this plane should be the surface of the table top. This nodelet produces two things (i) a pcl_msgs/PointIndices message containing the indices of the points in the input cloud that belong to the plane, and (ii) the model parameters of the plane published as a pcl_msgs/ModelCoefficients (the plane is parameterized as ax+by+cz+d=0). These parameters represent a least-squares plane that best fits the points that belong to the plane.
5. The indices are fed into two pcl/ExtractIndices filter nodelets that are used to create point clouds (as opposed to lists of indices) of the points that belong to the plane, and the points that don’t belong to the plane. These point clouds are not strictly necessary, but they do help visualization.
6. The table_cutoff_settings.py node subscribes to the model coefficients from the plane extraction. Every time a client calls the std_srvs/Empty service that this node offers (called /table_cutoff/update_table_model), this node updates its internal model of where the table is. It provides this information to the ROS world by broadcasting a /tf transform from the frame the original cloud is expressed in (camera_depth_optical_frame) to a frame called table_frame.
7. A pcl/PassThrough filter nodelet is then used to throw away all points that are at or below the plane extracted during plane extraction. This is done by setting the input_frame private parameter for the nodelet to be table_frame. Thus, if the table_cutoff_settings.py node has provided the transform to the table_frame, then the limits of the pass through filter are easy to calculate.
8. The output of the pcl/PassThrough nodelet is fed into a pcl/StatisticalOutlierRemoval filter nodelet that helps to remove random stray points.
9. The output of the pcl/StatisticalOutlierRemoval filter is fed into the cluster_extraction node. This simple C++ node subscribes to the output of the StatisticalOutlierRemoval filter, it converts this cloud into a PCL datatype, and then it calls pcl::EuclideanClusterExtraction<pcl::PointXYZ>.extract to produce a std::vector<pcl::PointIndices>. Each element in this std_vector is of type pcl::PointIndices, and the values in the pcl::PointIndices entries represent the points in the original cloud that belong to each cluster detected during cluster extraction. This data is used to construct point clouds for up to MAX_CLUSTERS and publish them on separate topics (named /cluster_X_cloud). The centroids of each of these cluster clouds are also computed and published as geometry_msgs/PointStamped messages (on the /cluster_X_point topics) and sent as /tf data.

Usage

PCD Data

To run using the included PCD file simply run

	roslaunch nodelet_pcl_demo nodelet_table_extraction.launch pcd:=true

This will use the pcd_to_pointcloud node from the pcl_ros package to read the PCD file, and repeatedly publish it on the /camera/depth/points topic (at approx. 20Hz). The included PCD file is a slightly modified version of the table_scene_lms400.pcd file used in the PCL Euclidean Cluster Extraction Tutorial. The only difference is that I’ve rotated the raw data by π radians about the x axis. This was to ensure that the coordinate systems when using the PCD data agreed with the data one would get when using openni_launch or openni2_launch.

Once all of the nodes/nodelets are up-and-running, you can call the /table_cutoff/update_table_model service to update the location of the table’s plane in the ROS world. This can be done by running

	rosservice call /table_cutoff/update_table_model "{}"

In rviz you should be able to see the output of all of the steps of the pipeline described above (all steps have labeled displays in the rviz config that is automatically loaded).

Live Data

	roslaunch nodelet_pcl_demo nodelet_table_extraction.launch source:=<source> 

The source parameter determines where the sensor_msgs/PointCloud2 data comes from.

1. Use “openni” for an openni device such as a ASUS Xtion PRO LIVE.
2. Use “realsense” for an Intel RealSense
3. Use “pcd” to open the test data.
4. Use “none” to launch your own camera source. For example,
   • Kinect for Box 360 using freenect_launch
   • Kinect for Xbox One using iai_kinect2
5. The min_z argument sets the minimum distance between table and camera. Default is 0.5m

You need to call the /table_cutoff/update_table_model service to update the geometry of the table. When using live data, it makes sense to look at the PlanarInlierCloud in rviz to ensure that you are accurately fitting the desired plane before calling this service. As before, the service can be called at the command line using

	rosservice call /table_cutoff/update_table_model "{}"

When using live data, you may have to tune some of the parameters in the launch file to achieve the desired performance. Note that many of the parameters can be tuned in real time using rqt_reconfigure.