ROS Q&A | Merge Laser Scans into single PointCloud
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
