melodic noetic   Show EOL distros: 

Overview

The laser_geometry package contains a single C++ class: LaserProjection. There is no ROS API.

This class has two relevant functions for transforming from sensor_msgs/LaserScan to sensor_msgs/PointCloud or sensor_msgs/PointCloud2.

• projectLaser() is simple and fast. It does not change the frame of your data.

• transformLaserScanToPointCloud() is slower but more precise. It makes uses of tf and the sensor_msgs/LaserScan time increment to transform each individual ray appropriately. This is the recommended means of transformation for tilting laser scanners or moving robots.

Both of these functions have a final optional argument that augments the sensor_msgs/PointCloud which is created to include extra channels. These channels may include intensities, distances, timestamps, the index or thew viewpoint from the original laser array.

There is a simple Python implementation here (https://github.com/ros-perception/laser_geometry/blob/indigo-devel/src/laser_geometry/laser_geometry.py).

C++ Usage

Simple projection

The method projectLaser() does the simplest possible projection of the laser. Each ray is simply projected out along the appropriate angle according to:

The appropriate sine and cosine values are cached, making this a very efficient operation. However, the generated sensor_msgs/PointCloud is in the same frame as the original sensor_msgs/LaserScan. While this has the advantage that it does not require an instance of a tf::transformer or message notifier, it does not hold up in situations where the laser is moving and skew needs to be accounted for.

Please consult the API Documentation for full usage details.

Example: To convert a sensor_msgs/LaserScan to a sensor_msgs/PointCloud

• Toggle line numbers

     1 laser_geometry::LaserProjection projector_;
     2 
     3 void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
     4 {
     5   sensor_msgs::PointCloud cloud;
     6   projector_.projectLaser(*scan_in, cloud);
     7 
     8   // Do something with cloud.
     9 }
  

High fidelity projection

The transformLaserScanToPointCloud() method does a more advanced projection, but requires that you have set up a tf transform listener. (If you are unfamiliar with tf, it is recommended you go through the tf/Tutorials first.)

Since we are gathering data across the time of the scan, it is often important that the chosen target_frame is actually a fixed frame.

Because the stamp of a sensor_msgs/LaserScan is the time of the first measurement, one cannot simply wait for a transform to target_frame at this stamp. Instead one also has to wait for a transform at the last measurement of the scan.

Please consult the API Documentation for full usage details.

Example: To convert a sensor_msgs/LaserScan to a sensor_msgs/PointCloud in the base_link frame, using a high fidelity transform:

• Toggle line numbers

     1 laser_geometry::LaserProjection projector_;
     2 tf::TransformListener listener_;
     3 
     4 void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
     5 {
     6   if(!listener_.waitForTransform(
     7         scan_in->header.frame_id,
     8         "/base_link",
     9         scan_in->header.stamp + ros::Duration().fromSec(scan_in->ranges.size()*scan_in->time_increment),
    10         ros::Duration(1.0))){
    11      return;
    12   }
    13 
    14   sensor_msgs::PointCloud cloud;
    15   projector_.transformLaserScanToPointCloud("/base_link",*scan_in,
    16           cloud,listener_);
    17 
    18   // Do something with cloud.
    19 }
  

Python Usage

Simple projection

The method projectLaser() projects a single laser scan from a linear array into a 3D sensor_msgs/PointCloud2. The generated cloud will be in the same frame as the original laser scan.

   1 import sensor_msgs.point_cloud2 as pc2
   2 import rospy
   3 from sensor_msgs.msg import PointCloud2, LaserScan
   4 import laser_geometry.laser_geometry as lg
   5 import math
   6 
   7 rospy.init_node("laserscan_to_pointcloud")
   8 
   9 lp = lg.LaserProjection()
  10 
  11 pc_pub = rospy.Publisher("converted_pc", PointCloud2, queue_size=1)
  12 
  13 def scan_cb(msg):
  14     # convert the message of type LaserScan to a PointCloud2
  15     pc2_msg = lp.projectLaser(msg)
  16 
  17     # now we can do something with the PointCloud2 for example:
  18     # publish it
  19     pc_pub.publish(pc2_msg)
  20     
  21     # convert it to a generator of the individual points
  22     point_generator = pc2.read_points(pc2_msg)
  23     
  24 
  25     # we can access a generator in a loop
  26     sum = 0.0
  27     num = 0
  28     for point in point_generator:
  29         if not math.isnan(point[2])
  30             sum += point[2]
  31             num += 1
  32     # we can calculate the average z value for example
  33     print(str(sum/num))
  34 
  35     # or a list of the individual points which is less efficient
  36     point_list = pc2.read_points_list(pc2_msg)
  37 
  38     # we can access the point list with an index, each element is a namedtuple
  39     # we can access the elements by name, the generator does not yield namedtuples!
  40     # if we convert it to a list and back this possibility is lost
  41     print(point_list[len(point_list)/2].x)
  42 
  43 
  44 
  45 rospy.Subscriber("/scan", LaserScan, scan_cb, queue_size=1)
  46 rospy.spin()

High fidelity projection

Currently there is no Python version of the high fidelity projection