Hi, I have been playing around with the laser_assembler 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:
- I create a package that will
handle all the logic
- On the
launch file of that package, I
launch the laser_assembler properly
configured and my own node
- My
node subscribes to the service of
the laser_assembler that provides
point clouds
- Then it calls the
service, gets the response, and then
publishes on a topic as a single
PointCloud2 message
How to do it ===========
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 here
Do you need a single scan as points or multiple merged ones? The latter is what laser_assembler does. Converting a single scan is rather simple. For visualization in rviz, just use a LaserScan display.
I don't need merged ones. Just the actual ones, but not as polar coordinates. I need them as Cartesian coordinates. I know, I can visualize just the laserscan topic in Rviz. But I need them as a pointcloud. In a later approach I have to detect circles in the pointcloud, thats why I need Cartesian coordinates.
In that case I'd suggest to just to that manually in the code, unless that is something that you cannot change and needs pointcloud input. Then you could just write a simple node that does the conversion.
Ok, I managed to run the assembler and the periodic snapshotter. But it only gets new points after about 4sec. But I cant find the variable to change, which makes the snapshotter publish the points more often.