Overview
This package provides interfaces and tools for bridging a running ROS system to the Point Cloud Library. These include ROS nodelets, nodes, and C++ interfaces.
ROS nodelets
pcl_ros includes several PCL filters packaged as ROS nodelets. These links provide details for using those interfaces:
ROS C++ interface
pcl_ros extends the ROS C++ client library to support message passing with PCL native data types. Simply add the following include to your ROS node source code:
#include <pcl_ros/point_cloud.h>
This header allows you to publish and subscribe pcl::PointCloud<T> objects as ROS messages. These appear to ROS as sensor_msgs/PointCloud2 messages, offering seamless interoperability with non-PCL-using ROS nodes. For example, you may publish a pcl::PointCloud<T> in one of your nodes and visualize it in rviz using a Point Cloud2 display. It works by hooking into the roscpp serialization infrastructure.
The old format sensor_msgs/PointCloud is not supported in PCL.
Publishing point clouds
You may publish PCL point clouds using the standard ros::Publisher:
1 #include <ros/ros.h>
2 #include <pcl_ros/point_cloud.h>
3 #include <pcl/point_types.h>
4 #include <pcl_conversions/pcl_conversions.h>
5
6 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
7
8 int main(int argc, char** argv)
9 {
10 ros::init (argc, argv, "pub_pcl");
11 ros::NodeHandle nh;
12 ros::Publisher pub = nh.advertise<PointCloud> ("points2", 1);
13
14 PointCloud::Ptr msg (new PointCloud);
15 msg->header.frame_id = "some_tf_frame";
16 msg->height = msg->width = 1;
17 msg->points.push_back (pcl::PointXYZ(1.0, 2.0, 3.0));
18
19 ros::Rate loop_rate(4);
20 while (nh.ok())
21 {
22 pcl_conversions::toPCL(ros::Time::now(), msg->header.stamp);
23 pub.publish (*msg);
24 ros::spinOnce ();
25 loop_rate.sleep ();
26 }
27 }
Subscribing to point clouds
You may likewise subscribe to PCL point clouds using the standard ros::Subscriber:
1 #include <ros/ros.h>
2 #include <pcl_ros/point_cloud.h>
3 #include <pcl/point_types.h>
4 #include <boost/foreach.hpp>
5
6 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
7
8 void callback(const PointCloud::ConstPtr& msg)
9 {
10 printf ("Cloud: width = %d, height = %d\n", msg->width, msg->height);
11 BOOST_FOREACH (const pcl::PointXYZ& pt, msg->points)
12 printf ("\t(%f, %f, %f)\n", pt.x, pt.y, pt.z);
13 }
14
15 int main(int argc, char** argv)
16 {
17 ros::init(argc, argv, "sub_pcl");
18 ros::NodeHandle nh;
19 ros::Subscriber sub = nh.subscribe<PointCloud>("points2", 1, callback);
20 ros::spin();
21 }
ROS nodes
Several tools run as ROS nodes. They convert ROS messages or bags to and from Point Cloud Data (PCD) file format.
bag_to_pcd
Reads a bag file, saving all ROS point cloud messages on a specified topic as PCD files.
Usage
$ rosrun pcl_ros bag_to_pcd <input_file.bag> <topic> <output_directory>
Where:
<input_file.bag> is the bag file name to read.
<topic> is the topic in the bag file containing messages to save.
<output_directory> is the directory on disk in which to create PCD files from the point cloud messages.
Example
Read messages from the /laser_tilt_cloud topic in data.bag, saving a PCD file for each message into the ./pointclouds subdirectory.
$ rosrun pcl_ros bag_to_pcd data.bag /laser_tilt_cloud ./pointclouds
convert_pcd_to_image
Loads a PCD file, publishing it as a ROS image message five times a second.Published Topics
output (sensor_msgs/Image)- A stream of images generated from the PCD file.
Usage
$ rosrun pcl_ros convert_pcd_to_image <cloud.pcd>
Read the point cloud in <cloud.pcd> and publish it in ROS image messages at 5Hz.
convert_pointcloud_to_image
Subscribes to a ROS point cloud topic and republishes image messages.Subscribed Topics
input (sensor_msgs/PointCloud2)- A stream of point clouds to convert to images.
Published Topics
output (sensor_msgs/Image)- Corresponding stream of images.
Examples
Subscribe to the /my_cloud topic and republish each message on the /my_image topic.
$ rosrun pcl_ros convert_pointcloud_to_image input:=/my_cloud output:=/my_image
To view the images created by the previous command, use image_view.
$ rosrun image_view image_view image:=/my_image
pcd_to_pointcloud
Loads a PCD file, publishing it one or more times as a ROS point cloud message.Published Topics
cloud_pcd (sensor_msgs/PointCloud2)- A stream of point clouds generated from the PCD file.
Parameters
~frame_id (str, default: /base_link)- Transform frame ID for published data.
Usage
$ rosrun pcl_ros pcd_to_pointcloud <file.pcd> [ <interval> ]
Where:
<file.pcd> is the (required) file name to read.
<interval> is the (optional) number of seconds to sleep between messages. If <interval> is zero or not specified the message is published once.
Examples
Publish the contents of point_cloud_file.pcd once in the /base_link frame of reference.
$ rosrun pcl_ros pcd_to_pointcloud point_cloud_file.pcd
Publish the contents of cloud_file.pcd approximately ten times a second in the /odom frame of reference.
$ rosrun pcl_ros pcd_to_pointcloud cloud_file.pcd 0.1 _frame_id:=/odom
pointcloud_to_pcd
Subscribes to a ROS topic and saves point cloud messages to PCD files. Each message is saved to a separate file, names are composed of an optional prefix parameter, the ROS time of the message, and the .pcd extension.Subscribed Topics
input (sensor_msgs/PointCloud2)- A stream of point clouds to save as PCD files.
Parameters
~prefix (str)- Prefix for PCD file names created.
- If set, the transform from the fixed frame to the frame of the point cloud is written to the VIEWPOINT entry of the pcd file.
- Output the pcd file in binary form.
- In case that binary output format is set, use binary compressed output.
Examples
Subscribe to the /velodyne/pointcloud2 topic and save each message in the current directory. File names look like 1285627014.833100319.pcd, the exact names depending on the message time stamps.
$ rosrun pcl_ros pointcloud_to_pcd input:=/velodyne/pointcloud2
Set the prefix parameter in the current namespace, save messages to files with names like /tmp/pcd/vel_1285627015.132700443.pcd.
$ rosrun pcl_ros pointcloud_to_pcd input:=/my_cloud _prefix:=/tmp/pcd/vel_
More examples
We have more examples on http://wiki.ros.org/pcl_ros/Tutorials page