ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org
Ask Your Question
4

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

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

tom gravatar image

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

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 -0600

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 image tom  ( 2012-01-04 01:34:36 -0600 )edit
@tom: Could you publish your changes somewhere? I'd like to use it, too.
Cav gravatar image Cav  ( 2012-01-23 23:34:24 -0600 )edit
Here you are, hope it's the working version :). http://pastebin.com/Nd4ekeujhttp://pastebin.com/WgBpiaqH
tom gravatar image tom  ( 2012-01-29 02:59:42 -0600 )edit
awesome, thank you!
Cav gravatar image Cav  ( 2012-01-30 21:52:50 -0600 )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 image mpai227  ( 2015-07-08 16:15:06 -0600 )edit

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

lisayoung gravatar image lisayoung  ( 2015-07-22 04:51:31 -0600 )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 image TomSon  ( 2015-09-21 03:48:07 -0600 )edit

@mpai227 Many thanks to your code!

Clack gravatar image Clack  ( 2016-07-09 04:17:16 -0600 )edit
1

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

phil0stine gravatar image

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 -0600

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

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 image lisayoung  ( 2015-07-22 13:48:24 -0600 )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 image lisayoung  ( 2015-07-22 14:00:18 -0600 )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 image mpai227  ( 2015-07-22 15:09:33 -0600 )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 image lisayoung  ( 2015-07-22 16:09:50 -0600 )edit

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

lisayoung gravatar image lisayoung  ( 2015-07-22 16:11:44 -0600 )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 image mpai227  ( 2015-07-23 08:04:00 -0600 )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 image mpai227  ( 2015-07-23 08:17:30 -0600 )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 image lisayoung  ( 2015-07-23 09:29:32 -0600 )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

6 followers

Stats

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

Seen: 4,904 times

Last updated: Jul 11 '17