| Note: This tutorial assumes that you have completed the Transform Configuration tutorial. All the code for this tutorial is available in the laser_scan_publisher_tutorial and point_cloud_publisher_tutorial packages.. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Publishing Sensor Streams Over ROS
Description: This tutorial provides examples of sending two types of sensor streams, sensor_msgs/LaserScan messages and sensor_msgs/PointCloud messages over ROS.Tutorial Level: BEGINNER
Publishing Sensor Streams over ROS
Publishing data correctly from sensors over ROS is important for the Navigation Stack to operate safely. If the Navigation Stack receives no information from a robot's sensors, then it will be driving blind and, most likely, hit things. There are many sensors that can be used to provide information to the Navigation Stack: lasers, cameras, sonar, infrared, bump sensors, etc. However, the current navigation stack only accepts sensor data published using either the sensor_msgs/LaserScan Message type or the sensor_msgs/PointCloud Message type. The following tutorial will provide examples of typical setup and use for both types of messages.
Related: tf frame setup for the Navigation Stack
Contents
ROS Message Headers
Both the sensor_msgs/LaserScan and sensor_msgs/PointCloud message types, like many other messages sent over ROS, contain tf frame and time dependent information. To standardize how this information is sent, the Header message type is used as a field in all such messages.
The three fields in the Header type are shown below. The seq field corresponds to an identifier that automatically increases as Messages are sent from a given publisher. The stamp field stores time information that should be associated with data in a Message. In the case of a laser scan, for example, the stamp might correspond to the time at which the scan was taken. The frame_id field stores tf frame information that should be associated with data in a message. In the case of a laser scan, this would be set to the frame in which the scan was taken.
#Standard metadata for higher-level flow data types #sequence ID: consecutively increasing ID uint32 seq #Two-integer timestamp that is expressed as: # * stamp.secs: seconds (stamp_secs) since epoch # * stamp.nsecs: nanoseconds since stamp_secs # time-handling sugar is provided by the client library time stamp #Frame this data is associated with # 0: no frame # 1: global frame string frame_id
Publishing LaserScans over ROS
The LaserScan Message
For robots with laser scanners, ROS provides a special Message type in the sensor_msgs package called LaserScan to hold information about a given scan. LaserScan Messages make it easy for code to work with virtually any laser as long as the data coming back from the scanner can be formatted to fit into the message. Before we talk about how to generate and publish these messages, let's take a look at the message specification itself:
# # Laser scans angles are measured counter clockwise, with 0 facing forward # (along the x-axis) of the device frame # Header header float32 angle_min # start angle of the scan [rad] float32 angle_max # end angle of the scan [rad] float32 angle_increment # angular distance between measurements [rad] float32 time_increment # time between measurements [seconds] float32 scan_time # time between scans [seconds] float32 range_min # minimum range value [m] float32 range_max # maximum range value [m] float32[] ranges # range data [m] (Note: values < range_min or > range_max should be discarded) float32[] intensities # intensity data [device-specific units]
Hopefully, the names/comments above make most of the fields in the Message clear. To make things concrete, let's write a simple laser scan publisher to demonstrate how things work.
Writing Code to Publish a LaserScan Message
Publishing a LaserScan message over ROS is fairly straightforward. We'll first provide sample code below to do it and then break the code down line by line afterwards.
1 #include <ros/ros.h>
2 #include <sensor_msgs/LaserScan.h>
3
4 int main(int argc, char** argv){
5 ros::init(argc, argv, "laser_scan_publisher");
6
7 ros::NodeHandle n;
8 ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);
9
10 unsigned int num_readings = 100;
11 double laser_frequency = 40;
12 double ranges[num_readings];
13 double intensities[num_readings];
14
15 int count = 0;
16 ros::Rate r(1.0);
17 while(n.ok()){
18 //generate some fake data for our laser scan
19 for(unsigned int i = 0; i < num_readings; ++i){
20 ranges[i] = count;
21 intensities[i] = 100 + count;
22 }
23 ros::Time scan_time = ros::Time::now();
24
25 //populate the LaserScan message
26 sensor_msgs::LaserScan scan;
27 scan.header.stamp = scan_time;
28 scan.header.frame_id = "laser_frame";
29 scan.angle_min = -1.57;
30 scan.angle_max = 1.57;
31 scan.angle_increment = 3.14 / num_readings;
32 scan.time_increment = (1 / laser_frequency) / (num_readings);
33 scan.range_min = 0.0;
34 scan.range_max = 100.0;
35
36 scan.ranges.resize(num_readings);
37 scan.intensities.resize(num_readings);
38 for(unsigned int i = 0; i < num_readings; ++i){
39 scan.ranges[i] = ranges[i];
40 scan.intensities[i] = intensities[i];
41 }
42
43 scan_pub.publish(scan);
44 ++count;
45 r.sleep();
46 }
47 }
Now we'll break the code above down step by step.
Here, we include the sensor_msgs/LaserScan message that we want to send over the wire.
8 ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);
This code creates a ros::Publisher that is later used to send LaserScan messages using ROS.
Here we're just setting up storage for the dummy data that we're going to use to populate our scan. A real application would pull this data from their laser driver.
Populate the dummy laser data with values that increase by one every second.
25 //populate the LaserScan message
26 sensor_msgs::LaserScan scan;
27 scan.header.stamp = scan_time;
28 scan.header.frame_id = "laser_frame";
29 scan.angle_min = -1.57;
30 scan.angle_max = 1.57;
31 scan.angle_increment = 3.14 / num_readings;
32 scan.time_increment = (1 / laser_frequency) / (num_readings);
33 scan.range_min = 0.0;
34 scan.range_max = 100.0;
35
36 scan.ranges.resize(num_readings);
37 scan.intensities.resize(num_readings);
38 for(unsigned int i = 0; i < num_readings; ++i){
39 scan.ranges[i] = ranges[i];
40 scan.intensities[i] = intensities[i];
41 }
Create a scan_msgs::LaserScan message and fill it with the data that we've generated in preparation to send it over the wire.
43 scan_pub.publish(scan);
Publish the message over ROS.
Publishing PointClouds over ROS
The PointCloud Message
For storing and sharing data about a number of points in the world, ROS provides a sensor_msgs/PointCloud message. This message, as shown below, is meant to support arrays of points in three dimensions along with any associated data stored as a channel. For example, a PointCloud could be sent over the wire with an "intensity" channel that holds information about the intensity value of each point in the cloud. We'll explore an example of sending a PointCloud using ROS in the next section.
#This message holds a collection of 3d points, plus optional additional information about each point. #Each Point32 should be interpreted as a 3d point in the frame given in the header Header header geometry_msgs/Point32[] points #Array of 3d points ChannelFloat32[] channels #Each channel should have the same number of elements as points array, and the data in each channel should correspond 1:1 with each point
Writing Code to Publish a PointCloud Message
Publishing a PointCloud with ROS is fairly straightforward. We'll show a simple example in its entirety below and then discuss the important parts in greater detail afterwards.
1 #include <ros/ros.h>
2 #include <sensor_msgs/PointCloud.h>
3
4 int main(int argc, char** argv){
5 ros::init(argc, argv, "point_cloud_publisher");
6
7 ros::NodeHandle n;
8 ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);
9
10 unsigned int num_points = 100;
11
12 int count = 0;
13 ros::Rate r(1.0);
14 while(n.ok()){
15 sensor_msgs::PointCloud cloud;
16 cloud.header.stamp = ros::Time::now();
17 cloud.header.frame_id = "sensor_frame";
18
19 cloud.points.resize(num_points);
20
21 //we'll also add an intensity channel to the cloud
22 cloud.channels.resize(1);
23 cloud.channels[0].name = "intensities";
24 cloud.channels[0].values.resize(num_points);
25
26 //generate some fake data for our point cloud
27 for(unsigned int i = 0; i < num_points; ++i){
28 cloud.points[i].x = 1 + count;
29 cloud.points[i].y = 2 + count;
30 cloud.points[i].z = 3 + count;
31 cloud.channels[0].values[i] = 100 + count;
32 }
33
34 cloud_pub.publish(cloud);
35 ++count;
36 r.sleep();
37 }
38 }
Now we'll break it down step by step.
Includes the sensor_msgs/PointCloud message.
8 ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);
Creates the ros::Publisher that we'll use to send PointCloud messages out over the wire.
Fill in the header of the PointCloud message that we'll send out with the relevant frame and timestamp information.
19 cloud.points.resize(num_points);
Set the number of points in the point cloud so that we can populate them with dummy data.
Adds a channel called "intensity" to the PointCloud and sizes it to match the number of points that we'll have in the cloud.
Populates the PointCloud message with some dummy data. Also, populates the intensity channel with dummy data.
34 cloud_pub.publish(cloud);
Publishes the PointCloud using ROS.
