Obtaining nav_msgs/Odometry from a laser_scan (eg. with laser_scan_matcher)

asked 2011-12-30 03:00:54 -0500

1079 ●39 ●55 ●65

updated 2011-12-30 19:53:01 -0500

Is it possible to obtain nav_msgs/Odometry message expressed in base_link basing on laser measurements with any existing package? I'd like to feed it to base_local_planner in the future, and laser measurements is all I have to estimate odometry.

E.g. the laser_scan_matcher package publishes a base_link->odom tf and an equivalent message geometry_msgs/Pose2D expressed in odom frame. As laser_scan_matcher's outcome is of a cumulative nature, I'm guessing twist (velocity) information is calculated internally as first. So maybe, with some tweaking, it would be possible to make it publish nav_msgs/Odometry expressed in base_link, too?

Or is there another way of solving this issue?

edit retag flag offensive close merge delete

add a comment

3 Answers

Sort by » oldest newest most voted

answered 2011-12-31 13:06:08 -0500

ahendrix

47576 ●179 ●367 ●662 http://namniart.com/

It looks like the scan matcher isn't measuring velocity directly, but it is computing the delta in the laser position, which you could combine with the time between laser scans to produce a rough estimate of velocity, subject to jitter in your laser frequency and any errors in the scan matching.

Since laser_scan_matcher doesn't publish stamped poses, you'll have to modify laser_scan_matcher directly to compute this output.

LaserScan has a timestamp in the message header that you can use to compute the time between scans.

edit flag offensive delete link

Comments

Thanks, that's exactly what I did.

tom ( 2012-01-04 01:34:36 -0500 )edit

@tom: Could you publish your changes somewhere? I'd like to use it, too.

Cav ( 2012-01-23 23:34:24 -0500 )edit

Here you are, hope it's the working version :). http://pastebin.com/Nd4ekeuj http://pastebin.com/WgBpiaqH

tom ( 2012-01-29 02:59:42 -0500 )edit

awesome, thank you!

Cav ( 2012-01-30 21:52:50 -0500 )edit

just came upon this thread. @tom - your code was very helpful, thanks a lot!. There were a few bugs which I fixed and it worked fine. If someone needs the code do leave a message I will be happy to share!

mpai227 ( 2015-07-08 16:15:06 -0500 )edit

@mpai227 Can you share your code please? I want to get the odometry msg from laser_scan_matcher. Thank you!

lisayoung ( 2015-07-22 04:51:31 -0500 )edit

@mpai227 Yes can you share it, that's for me a fondamental piece of code. It should be an optionnal param of laser_scan_matcher :)

TomSon ( 2015-09-21 03:48:07 -0500 )edit

@mpai227 Many thanks to your code!

Clack ( 2016-07-09 04:17:16 -0500 )edit

see more comments

answered 2012-01-01 04:27:00 -0500

phil0stine
682 ●15 ●24 ●33

I would write a wrapper node that uses the laser_scan_matcher nodelet to get the cumulative odom estimate, then calculate the Twist information and publish an Odometry msg.

I was going to suggest looking at robot_pose_ekf, (since it's a Kalman Filter I assumed there would be calculated Velocities), but I realized that the node only outputs a Pose msg and not a full Odom msg.

edit flag offensive delete link

add a comment

answered 2015-07-22 08:55:37 -0500

mpai227
21 ●1 ●3 http://www.frc.ri.cmu.edu/

updated 2015-07-24 09:49:11 -0500

Tom's fixed code is here http://pastebin.com/ebtxMGAD http://pastebin.com/hY5pFWsY . Paste these in the /src and /include directory of your laser_scan_matcher package. Also dont forget to modify your CMakelists.txt file. Add the following lines

add_library(laser_scan_matcher_odom src/laser_scan_matcher_odom.cpp)
target_link_libraries( laser_scan_matcher_odom ${catkin_LIBRARIES} ${csm_LIBRARIES})
add_dependencies(laser_scan_matcher_odom ${csm_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(laser_scan_matcher_node src/laser_scan_matcher_node.cpp)
target_link_libraries( laser_scan_matcher_node laser_scan_matcher_odom )

edit flag offensive delete link

Comments

Thanks for your code. I have tried it with my robot. I have set the "fixed frame" to "odom" and "publish_tf" to "true". Then I get the tf tree like this: odom->base_footprint->base_link->laser_link. (I don't have the odom frame before. ) But the result in rviz truns out not good.

lisayoung ( 2015-07-22 13:48:24 -0500 )edit

When I give the robot a velocity vx, the robot get stuck at the place about 5 meters away. Instead the scans move backward. After that the pose and orientation of the robot are false. Do you have similar problems? If not, what have you done with the frames? Or other parameters should be set? Thanks!

lisayoung ( 2015-07-22 14:00:18 -0500 )edit

Quick question. What frame are you giving your robot velocity in? Also the velocity from odom in the laser_scan_matcher_odom is in the base frame and not in the laser frame, hope you dint transform this further. Have you given a static transformation from the base frame to laser frame?

mpai227 ( 2015-07-22 15:09:33 -0500 )edit

I'm working with gazebo. I use keyboard to give the robot velocity. I checked the topic /keyboard and there isn't a frame_id. Is this necessary to give the velocity a frame_id as base frame? I havn't given a static transformaion from base frame to laser frame.

lisayoung ( 2015-07-22 16:09:50 -0500 )edit

Do I still need a static transformation, when the laser frame is already in the tf tree?

lisayoung ( 2015-07-22 16:11:44 -0500 )edit

Is there a rotational and transnational offset between your base frame and laser frame? If there is you should publish a static transform between the two.

mpai227 ( 2015-07-23 08:04:00 -0500 )edit

You can add something like this in your launch file <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="1 0 0 0 0 0 1 base_frame_name laser_frame_name 100"/> Replace {1 0 0 0 0 0 1} with the transform of your robot's base to laser frame.

mpai227 ( 2015-07-23 08:17:30 -0500 )edit

Thanks a lot! I tried. It works well in a room. But in a long hallway the same problem occurs. I think the reason is probably that the laser scan can't recognise its location in a long hallway. Do you have any suggestions when used in a long hallway?

lisayoung ( 2015-07-23 09:29:32 -0500 )edit

see more comments

Obtaining nav_msgs/Odometry from a laser_scan (eg. with laser_scan_matcher)

3 Answers

Comments

Comments

Question Tools

Stats

Related questions

Obtaining nav_msgs/Odometry from a laser_scan (eg. with laser_scan_matcher) edit

3 Answers

Comments

Comments

Question Tools

Stats

Related questions

Obtaining nav_msgs/Odometry from a laser_scan (eg. with laser_scan_matcher)