navigation/Tutorials/RobotSetup/Sensors

http://ftp.isr.ist.utl.pt/pub/roswiki/navigation(2f)Tutorials(2f...

navigation/Tutorials/RobotSetup/Sensors
[Documentation] [TitleIndex] [WordIndex] 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..

1. 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

2. 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 1. Publishing Sensor Streams over ROS 2. ROS Message Headers 3. Publishing LaserScans over ROS 1. The LaserScan Message 2. Writing Code to Publish a LaserScan Message 4. Publishing PointClouds over ROS 1. The PointCloud Message 2. Writing Code to Publish a PointCloud Message

1 of 8

05/20/2013 05:11 PM

navigation/Tutorials/RobotSetup/Sensors

http://ftp.isr.ist.utl.pt/pub/roswiki/navigation(2f)Tutorials(2f...

3. 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

4. Publishing LaserScans over ROS

4.1 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 float32 angle_max float32 angle_increment float32 time_increment # # # # start angle of the scan [rad] end angle of the scan [rad] angular distance between measurements [rad] time between measurements [seconds]

2 of 8

05/20/2013 05:11 PM

navigation/Tutorials/RobotSetup/Sensors

http://ftp.isr.ist.utl.pt/pub/roswiki/navigation(2f)Tutorials(2f...

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.

4.2 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.
Toggle line numbers
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.set_ranges_size(num_readings); 37 scan.set_intensities_size(num_readings); 38 for(unsigned int i = 0; i < num_readings; ++i){

3 of 8

05/20/2013 05:11 PM

navigation/Tutorials/RobotSetup/Sensors 39 40 41 42 43 44 45 46 } 47 } 48

http://ftp.isr.ist.utl.pt/pub/roswiki/navigation(2f)Tutorials(2f...

scan.ranges[i] = ranges[i]; scan.intensities[i] = intensities[i];

scan_pub.publish(scan); ++count; r.sleep();

Now we'll break the code above down step by step.

Toggle line numbers
2 #include <sensor_msgs/LaserScan.h> 3

Here, we include the sensor_msgs/LaserScan message that we want to send over the wire.
Toggle line numbers
8 ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan> ("scan", 50); 9

This code creates a ros::Publisher that is later used to send LaserScan messages using ROS.
Toggle line numbers
10 11 12 13 14 unsigned int num_readings = 100; double laser_frequency = 40; double ranges[num_readings]; double intensities[num_readings];

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.
Toggle line numbers
18 19 20 21 22 23 24 //generate some fake data for our laser scan for(unsigned int i = 0; i < num_readings; ++i){ ranges[i] = count; intensities[i] = 100 + count; } ros::Time scan_time = ros::Time::now();

Populate the dummy laser data with values that increase by one every second.

4 of 8

05/20/2013 05:11 PM

navigation/Tutorials/RobotSetup/Sensors

http://ftp.isr.ist.utl.pt/pub/roswiki/navigation(2f)Tutorials(2f...

Toggle line numbers

25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 //populate the LaserScan message sensor_msgs::LaserScan scan; scan.header.stamp = scan_time; scan.header.frame_id = "laser_frame"; scan.angle_min = -1.57; scan.angle_max = 1.57; scan.angle_increment = 3.14 / num_readings; scan.time_increment = (1 / laser_frequency) / (num_readings); scan.range_min = 0.0; scan.range_max = 100.0; scan.set_ranges_size(num_readings); scan.set_intensities_size(num_readings); for(unsigned int i = 0; i < num_readings; ++i){ scan.ranges[i] = ranges[i]; scan.intensities[i] = intensities[i]; }

Create a scan_msgs::LaserScan message and fill it with the data that we've generated in preparation to send it over the wire.
Toggle line numbers
43 44 scan_pub.publish(scan);

Publish the message over ROS.

5. Publishing PointClouds over ROS

5.1 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

5 of 8

05/20/2013 05:11 PM

navigation/Tutorials/RobotSetup/Sensors correspond 1:1 with each point

http://ftp.isr.ist.utl.pt/pub/roswiki/navigation(2f)Tutorials(2f...

5.2 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.
Toggle line numbers
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.set_points_size(num_points); 20 21 //we'll also add an intensity channel to the cloud 22 cloud.set_channels_size(1); 23 cloud.channels[0].name = "intensities"; 24 cloud.channels[0].set_values_size(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 } 39

Now we'll break it down step by step.

Toggle line numbers
2 #include <sensor_msgs/PointCloud.h>

6 of 8

05/20/2013 05:11 PM

navigation/Tutorials/RobotSetup/Sensors 3

http://ftp.isr.ist.utl.pt/pub/roswiki/navigation(2f)Tutorials(2f...

Includes the sensor_msgs/PointCloud message.

Toggle line numbers
8 ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud> ("cloud", 50); 9

Creates the ros::Publisher that we'll use to send PointCloud messages out over the wire.
Toggle line numbers
15 16 17 18 sensor_msgs::PointCloud cloud; cloud.header.stamp = ros::Time::now(); cloud.header.frame_id = "sensor_frame";

Fill in the header of the PointCloud message that we'll send out with the relevant frame and timestamp information.
Toggle line numbers
19 20 cloud.set_points_size(num_points);

Set the number of points in the point cloud so that we can populate them with dummy data.
Toggle line numbers
21 22 23 24 25 //we'll also add an intensity channel to the cloud cloud.set_channels_size(1); cloud.channels[0].name = "intensities"; cloud.channels[0].set_values_size(num_points);

Adds a channel called "intensity" to the PointCloud and sizes it to match the number of points that we'll have in the cloud.
Toggle line numbers
26 27 28 29 30 31 32 33 //generate some fake data for our point cloud for(unsigned int i = 0; i < num_points; ++i){ cloud.points[i].x = 1 + count; cloud.points[i].y = 2 + count; cloud.points[i].z = 3 + count; cloud.channels[0].values[i] = 100 + count; }

7 of 8

05/20/2013 05:11 PM

navigation/Tutorials/RobotSetup/Sensors

http://ftp.isr.ist.utl.pt/pub/roswiki/navigation(2f)Tutorials(2f...

Populates the PointCloud message with some dummy data. Also, populates the intensity channel with dummy data.
Toggle line numbers
34 35 cloud_pub.publish(cloud);

Publishes the PointCloud using ROS. 2011-11-19 12:27

8 of 8

05/20/2013 05:11 PM