ROS Q&A | Merge Laser Scans into single PointCloud

Written by Ricardo Tellez

22/08/2017

 

I have been playing around with the laser_assembler ROS package and managed to make it work for assembling several laser scans into a single point cloud, and publish that point cloud on a topic.

What I have done is the following:
1- I created a package that will handle all the logic
2- On the launch file of that package, I launch the laser_assembler properly configured and my own node
3- My node subscribes to the service of the laser_assembler that provides point clouds
4- Then it calls the service, gets the response, and then publishes on a topic as a single PointCloud2 message

 

Using Python to do the conversion simplifies a lot. Let’s see how to do that using the laser_geometry package.

First, you need to launch the laser_geometry package from a launch file, properly configured. Here a suggested launch file:

<launch>
<node type="laser_scan_assembler" pkg="laser_assembler"
    name="my_assembler">
<remap from="scan" to="laser_scan"/>
<param name="max_scans" type="int" value="400" />
<param name="fixed_frame" type="string" value="my_robot_link_base" />
 </node>
  <node type ="laser2pc.py" pkg="laser_to_pcl" name="laser2pc"/>
</launch>

That file launches the laser_geometry package indicating that the frame from which the transform will be done is the my_robot_link_base (that is whatever tf link of your robot you want the point cloud to be refered from). Another parameter is the max_scans which indicates the max number of last scans that will be take into account for the computation of the point cloud ( this is useful in case your laser is rotating, so you can generate a full 3D cloud).

The launch file above also launches a second node that is the one you have to create to get the data and publish it into your selected topic. What should be the code of such a node? Check the following:

#!/usr/bin/env python

import rospy 
from laser_assembler.srv import AssembleScans2
from sensor_msgs.msg import PointCloud2

rospy.init_node("assemble_scans_to_cloud")
rospy.wait_for_service("assemble_scans2")
assemble_scans = rospy.ServiceProxy('assemble_scans2', AssembleScans2)
pub = rospy.Publisher ("/laser_pointcloud", PointCloud2, queue_size=1)

r = rospy.Rate (1)

while (True):
    try:
        resp = assemble_scans(rospy.Time(0,0), rospy.get_rostime())
        print "Got cloud with %u points" % len(resp.cloud.data)
        pub.publish (resp.cloud)

    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

    r.sleep()

The code shows how to subscribe to the assemble_scans2 service. That is the one that will provide the point cloud on its answer. The code also shows that the point cloud will be published on a topic of type PoinCloud2. basically, the while loop what it does is, first, call to the service, then publish the answer of the service into a topic.

I have created a video describing step by step and code solution. In case you are interested, you can find it below

 

This post was created as an answer to the following question in ROS Answers Forum: Laserscan to PointCloud

Topics:
Masterclass 2023 batch2 blog banner

Check Out These Related Posts

129. ros2ai

129. ros2ai

I would like to dedicate this episode to all the ROS Developers who believe that ChatGPT or...

read more

4 Comments

  1. steve

    How do I do this using my real laser scanner without using any demo robots?

    Reply
    • Ricardo Tellez

      Using that code on a real robot works exactly the same way as with the simulated robots of the video.

      First, you need the robot to have ROS support. I’m assuming that you already have that in your robot.
      Then you just apply the same steps as in the video but adapting the code to your robot topic names for laser (you must know this in advance for your robot).

      Reply
  2. Anonymous

    I am working with SICK TIM 561 LIDAR and it’s a 2D lidar.
    How do I collect several scans and convert into 3D point Cloud ?

    Reply
    • Anonymous

      LoL we have the exact same problem

      Reply

Submit a Comment

Your email address will not be published.

This site uses Akismet to reduce spam. Learn how your comment data is processed.

Pin It on Pinterest

Share This