Ask Your Question
4

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

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

tom gravatar image

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

3 Answers

Sort by ยป oldest newest most voted
5

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

ahendrix gravatar image

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 more

Comments

Thanks, that's exactly what I did.
tom gravatar imagetom ( 2012-01-04 01:34:36 -0500 )edit
@tom: Could you publish your changes somewhere? I'd like to use it, too.
Cav gravatar imageCav ( 2012-01-23 23:34:24 -0500 )edit
Here you are, hope it's the working version :). http://pastebin.com/Nd4ekeujhttp://pastebin.com/WgBpiaqH
tom gravatar imagetom ( 2012-01-29 02:59:42 -0500 )edit
awesome, thank you!
Cav gravatar imageCav ( 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 gravatar imagempai227 ( 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 gravatar imagelisayoung ( 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 gravatar imageTomSon ( 2015-09-21 03:48:07 -0500 )edit

@mpai227 Many thanks to your code!

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

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

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 more
0

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

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 more

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 gravatar imagelisayoung ( 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 gravatar imagelisayoung ( 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 gravatar imagempai227 ( 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 gravatar imagelisayoung ( 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 gravatar imagelisayoung ( 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 gravatar imagempai227 ( 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 gravatar imagempai227 ( 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 gravatar imagelisayoung ( 2015-07-23 09:29:32 -0500 )edit

Long featureless corridors seem to be a problem for laser scan matching and Slam in general. If you are just doing simulations you could consider adding some features or reducing the corridor length so that it is well within the laser scan range.

mpai227 gravatar imagempai227 ( 2015-07-23 10:12:38 -0500 )edit

Hey, I got this error using the corrected code and the odom get an error on angular Z:

:err:   : before trimming, only 8 correspondences. 
:err: icp: ICP failed for some reason.
TomSon gravatar imageTomSon ( 2015-09-22 08:02:47 -0500 )edit

Hi, all may I know how you implement all these? I'm new to all theses and figuring out the navigation stack withRobot_Setup tutorial. How can I write a node to replace odom node (2.2 in the tutorial) with laser scan matcher then?

IvyKK gravatar imageIvyKK ( 2018-10-26 03:56:19 -0500 )edit

Hi! I'm unable to do it. I'm getting error with CMakeLists and there seems to be some errors in laser_scan_matcher_odom.cpp. Please help

parzival gravatar imageparzival ( 2019-08-16 06:17:13 -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

5 followers

Stats

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

Seen: 3,614 times

Last updated: Jul 11 '17