Ask Your Question
5

How to turn laser scan to point cloud map

asked 2011-09-16 03:27:54 -0500

tyler258 gravatar image

updated 2011-09-25 07:52:42 -0500

kwc gravatar image

Hello,

I'm fairly new to ROS and was wondering if there was a way to convert a ".bag" laser scan file into a point cloud map. I have successfully run the "laser.bag" scan file, provided by ROS, in Rviz. Now I am curious to know how to turn this "laser.bag" file into a 3D map.

Thanks you for your time,

Tyler

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
16

answered 2011-09-16 03:33:21 -0500

DimitriProsser gravatar image

updated 2011-09-23 07:52:43 -0500

One quick way to do this would be to use the laser_geometry node. It provides functionality for converting laser scans directly into PointCloud or PointCloud2 messages in the frame of your choice. That's what I use, and it's quite simple to do. I hope this helps.

EDIT: To use this functionality, you could do something like this:

In the class definition:

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>

class My_Filter {
     public:
        My_Filter();
        void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
     private:
        ros::NodeHandle node_;
        laser_geometry::LaserProjection projector_;
        tf::TransformListener tfListener_;

        ros::Publisher point_cloud_publisher_;
        ros::Subscriber scan_sub_;
};

My_Filter::My_Filter(){
        scan_sub_ = node_.subscribe<sensor_msgs::LaserScan> ("/scan", 100, &My_Filter::scanCallback, this);
        point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud2> ("/cloud", 100, false);
        tfListener_.setExtrapolationLimit(ros::Duration(0.1));
}

In the callback function:

void My_Filter::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
    sensor_msgs::PointCloud2 cloud;
    projector_.transformLaserScanToPointCloud("base_link", *scan, cloud, tfListener_);
    point_cloud_publisher_.publish(cloud);
}

And the main:

int main(int argc, char** argv)
{
    ros::init(argc, argv, "my_filter");

    My_Filter filter;

    ros::spin();

    return 0;
}

Where "base_link" is the frame that you'd like to see all the scans in, *scan is the reference to the incoming laser scan, cloud is the sensor_msgs::PointCloud2 that you'd like to be your output (this can also be a regular sensor_msgs::PointCloud), and tfListener_ is your transform listener.

You must also add the following to your manifest.xml:

<depend package="laser_geometry"/>
<depend package="tf"/>
<depend package="sensor_msgs" />

That's how I build my LaserScans into point clouds. I can provide a more specific example if you need it.

EDIT: I have updated the code snippets such that if you paste all of the code into a .cpp file, build the file, and run it, it will work 100%. You should change "/scan" to the topic that your laser publishes, and you should change "/cloud" to the topic you wish to view the cloud on. In Rviz, add a new PointCloud2 visualizer and point it to the topic you chose for "/cloud" and it should work. This node also assumes that there is a tf from the frame of the laser to the target frame ("base_link" in this example).

edit flag offensive delete link more

Comments

Could you explain how to call the transformLaserScanToPointCloud() function in the laser_gemoetry class?
tyler258 gravatar imagetyler258 ( 2011-09-21 05:29:12 -0500 )edit
Would you mind providing the more specific example I would appreciate it. Right now I have my .cpp file set up, more or less, just like the example you provided. Now once I run the .cpp file, I run "rosbag play my_laser_data.bag" but I can't anything to show up in rviz.
tyler258 gravatar imagetyler258 ( 2011-09-23 04:37:57 -0500 )edit
Okay, I got the code running but I'm still having trouble publishing it in Rviz. The laser scan data I have publishes to tilt_scan. So I replaced "/scan" with "/tilt_scan" and I replaced "/cloud" with "/base_link" but this is still not working. Also, how do I get access to this new map?
tyler258 gravatar imagetyler258 ( 2011-09-26 05:10:06 -0500 )edit
Just some advice, I wouldn't rename "/cloud" to "/base_link" because that could get confusing. You want "/cloud" to be the name of data stream, not the frame that it's in. To see it in Rviz, add a new PointCloud2 and link it to "/cloud" or whatever you've named it.
DimitriProsser gravatar imageDimitriProsser ( 2011-09-26 06:33:18 -0500 )edit
How a tf from based_laser to based_link can be achieved?
acp gravatar imageacp ( 2011-12-05 18:45:16 -0500 )edit
2

Running 12.04 and groovy, the following error comes up: /opt/ros/groovy/include/laser_geometry/laser_geometry.h:46:22: fatal error: Eigen/Core: No such file or directory

Any help?

AG1617 gravatar imageAG1617 ( 2013-05-14 04:53:52 -0500 )edit

I had the same problem as you AG1617, you'll have to edit the file manually and fill in the correct path, usually the file is somewhere here /usr/include/eigen3/Eigen/core

Bernhard gravatar imageBernhard ( 2013-06-05 03:36:13 -0500 )edit

what is the difference between the target frame and frame id ?? the frame id for the /scan topic in my case is laser0_frame. So should it be the target frame or not ?

RSA_kustar gravatar imageRSA_kustar ( 2014-07-20 06:36:00 -0500 )edit
1

answered 2017-07-24 06:27:37 -0500

R. Tellez gravatar image

updated 2017-10-09 03:49:12 -0500

Hi,

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 ( https://youtu.be/GvilxcePD64 ) showing how to use the laser_geometry with Python with a practical example.

Hope it helps.

edit flag offensive delete link more

Comments

3

As always with videos, please add a short description describing the main points / steps to your answer. If your video ever disappears, your answer becomes useless.

gvdhoorn gravatar imagegvdhoorn ( 2017-07-24 07:02:19 -0500 )edit

This glosses over the meat of the question: how to turn a laser scan into a point cloud. Instead, you still direct people to look at your video. @DimitriProsser's answer is much more detailed and actually answers the question.

jayess gravatar imagejayess ( 2017-10-02 14:18:10 -0500 )edit

Thanks @jayess for stating what already was stated in the previous comment. As you can see in this answer I learnt the lesson, so no need to keep pushing. Use that time instead to answer yourself the question

R. Tellez gravatar imageR. Tellez ( 2017-10-03 14:31:33 -0500 )edit
1

@R. Tellez: I'm commenting on the current version of this answer which was updated since the first comment. So, even if you have started giving more info on other answers this answer still glosses over relevant information and makes the user watch a video instead of getting an answer.

jayess gravatar imagejayess ( 2017-10-03 14:35:04 -0500 )edit

Modified to include everything

R. Tellez gravatar imageR. Tellez ( 2017-10-09 03:49:29 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2011-09-16 03:27:54 -0500

Seen: 17,449 times

Last updated: Oct 09 '17