Ask Your Question
2

Creating a map with gmapping using lidar data of a quick spinning robot

asked 2013-09-14 09:38:28 -0500

FlashErase gravatar image

updated 2014-01-28 17:17:57 -0500

ngrennan gravatar image

Using the lidar data of a neato vacuum cleaning robot to create a map during vacuuming was till now not possible with hector_slam. hector_slam loses very often the position of the robot during its fast spins.
So I tried to use gmapping in combination with the laser_scan_matcher instead. After a lot of tests it was able me to feed gmapping with my unusual data. Unfortunately the laser_scan_matcher calculates a position about twice as far from the map center as the real position:

image description

Is there a trick to correct this behaviour of the scan-matcher? Here the lauch file for this demo:

<!-- 
Example launch file: uses laser_scan_matcher together with
slam_gmapping 
-->

<launch>

  #### set up data playback from bag #############################

  <param name="/use_sim_time" value="true"/>

  <node pkg="rosbag" type="play" name="play" 
    args="$(find laser_scan_matcher)/demo/vr100.bag -r 3 --delay=3 --clock"/>

  #### publish an example base_link -> laser transform ###########

  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" 
    args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /neato_laser 100" />

  #### start rviz ################################################

  <node pkg="rviz" type="rviz" name="rviz" 
    args="-d $(find laser_scan_matcher)/demo/demo_vr100.vcg"/>

  #### start the laser scan_matcher ##############################

  <node pkg="laser_scan_matcher" type="laser_scan_matcher_node" 
    name="laser_scan_matcher_node" output="screen">

    <param name="fixed_frame" value = "odom"/>
    <param name="use_alpha_beta" value="true"/>
    <param name="max_iterations" value="10"/>

  </node>

  #### start gmapping ############################################

  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
    <param name="map_udpate_interval" value="1.0"/>
    <param name="maxUrange" value="5.0"/>
    <param name="sigma" value="0.1"/>
    <param name="kernelSize" value="1"/>
    <param name="lstep" value="0.15"/>
    <param name="astep" value="0.15"/>
    <param name="iterations" value="1"/>
    <param name="lsigma" value="0.1"/>
    <param name="ogain" value="3.0"/>
    <param name="lskip" value="1"/>
    <param name="srr" value="0.1"/>
    <param name="srt" value="0.2"/>
    <param name="str" value="0.1"/>
    <param name="stt" value="0.2"/>
    <param name="linearUpdate" value="1.0"/>
    <param name="angularUpdate" value="0.5"/>
    <param name="temporalUpdate" value="0.4"/>
    <param name="resampleThreshold" value="0.5"/>
    <param name="particles" value="10"/>
    <param name="xmin" value="-5.0"/>
    <param name="ymin" value="-5.0"/>
    <param name="xmax" value="5.0"/>
    <param name="ymax" value="5.0"/>
    <param name="delta" value="0.02"/>
    <param name="llsamplerange" value="0.01"/>
    <param name="llsamplestep" value="0.05"/>
    <param name="lasamplerange" value="0.05"/>
    <param name="lasamplestep" value="0.05"/>
    <param name="inverted_laser" value="true"/>
  </node>

</launch>


Appendix 1:
It is possible, that the neato-laser is a little bit special? It is a 360° lidar, built in right side up and rotating with 300 rpm CCW. It delivers 360 range/intensity pairs per revolution. I can't find out, if gmapping or the scan matcher have a problem with this laser and the created data header (angle increment):

image description

There where so a lot of discussions about gmapping with inverted lasers that I don't know now if my laser is normal or inverted and if gmapping and the scan-matcher can handle the delivered data or if there are some necessary patches to do ... (more)

edit retag flag offensive close merge delete

Comments

Could you upload the bag file somewhere? I would love to take a closer look. I wrote the last patch regarding gmapping and inverted (and rear facing) lasers. Try checking out the last gmapping versions for your ros-version from the git-repo. I dont know if the binary already includes the patches...

Ben_S gravatar image Ben_S  ( 2013-09-16 03:39:02 -0500 )edit

Hi Ben, thank you for your interest. I uploded the first minute of the bag here: http://flasherase.ohost.de/demo.bag . Hope you can find the reason of my problem.

FlashErase gravatar image FlashErase  ( 2013-09-16 08:13:48 -0500 )edit

I will be able to look into it on wednesday. Are you using the latest version from Github? GMapping in general has problems with non-symmetrical scanners. (e.g. min_angle != -max_angle) This will lead to problems like rotation by 180 degrees in your case...

Ben_S gravatar image Ben_S  ( 2013-09-16 13:06:57 -0500 )edit

Yes, I'm using the latest version. Built it fresh last week. I wrote a mapper let me modifying the scan header and data in a wide range. It transforms topic "scan" frame "neato_laser" to topic "scan_fix" frame "laser".

FlashErase gravatar image FlashErase  ( 2013-09-16 13:26:19 -0500 )edit

Unfortunately it was not able me, to feed gmapping with this data till now because tf and laser_scan_matcher seems to be fixed to get the data from tipc "scan".

FlashErase gravatar image FlashErase  ( 2013-09-16 13:26:29 -0500 )edit

This is hector_slam mapping the uploaded test-bag. http://img19.imageshack.us/img19/6443/u5o.gif

FlashErase gravatar image FlashErase  ( 2013-09-16 14:43:34 -0500 )edit

In general before testing any SLAM algorithm, make sure that your data is correct (but noisy), i.e. test http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide#Odometry - The SLAM algorithms might seem to work "not so good" because they actually achieve to correct bad data.

dornhege gravatar image dornhege  ( 2013-09-18 03:39:57 -0500 )edit

I wanted not to test the algorithms (they work fine). I was interested in processing my noisy data by this algorithms. Ben_S already found a possibility to improve the stability of this process. Because my data transferred wireless I can't guarantee for gapless data streams. I'll filter them now.

FlashErase gravatar image FlashErase  ( 2013-09-18 06:12:41 -0500 )edit

2 Answers

Sort by » oldest newest most voted
2

answered 2013-09-18 03:24:06 -0500

Ben_S gravatar image

I have made two changes to GMapping so that it looks fine to me playing your bag:

diff --git a/gmapping/src/slam_gmapping.cpp b/gmapping/src/slam_gmapping.cpp
index c97356c..6049a6f 100644
--- a/gmapping/src/slam_gmapping.cpp
+++ b/gmapping/src/slam_gmapping.cpp
@@ -350,7 +350,10 @@ SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
   gsp_laser_angle_increment_ = orientationFactor * scan.angle_increment;
   ROS_DEBUG("Laser angles top down in laser-frame: min: %.3f max: %.3f inc: %.3f", angle_min_, angle_max_, gsp_laser_angle_increment_);

-  GMapping::OrientedPoint gmap_pose(0, 0, 0);
+  double theta = angle_min_ + (gsp_laser_beam_count_ * gsp_laser_angle_increment_ / 2);
+  ROS_DEBUG("Theta for gmap_pose: %.3f", theta);
+
+  GMapping::OrientedPoint gmap_pose(0, 0, theta);

   // setting maxRange and maxUrange here so we can set a reasonable default
   ros::NodeHandle private_nh_("~");
@@ -542,7 +545,8 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
   boost::mutex::scoped_lock(map_mutex_);
   GMapping::ScanMatcher matcher;
   double* laser_angles = new double[scan.ranges.size()];
-  double theta = angle_min_;
+  // this is how GMapping internally calculates the min_angle
+  double theta = -gsp_laser_angle_increment_ * scan.ranges.size() / 2;
   for(unsigned int i=0; i<scan.ranges.size(); i++)
   {
     if (gsp_laser_angle_increment_ < 0)

I also played around with the launch file a bit to improve the performance of the laser_scan_matcher and GMapping:

<!-- 
Example launch file: uses laser_scan_matcher together with
slam_gmapping 
-->

<launch>

  #### set up data playback from bag #############################

  <param name="/use_sim_time" value="true"/>

  <node pkg="rosbag" type="play" name="play" 
    args="$(find laser_scan_matcher)/demo/vr100.bag -r 3 --delay=3 --clock"/>

  #### publish an example base_link -> laser transform ###########

  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" 
    args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /neato_laser 100" />

  #### start rviz ################################################
  <node pkg="rviz" type="rviz" name="rviz" 
    args="-d $(find laser_scan_matcher)/demo/demo_vr100.vcg"/>

  #### start the laser scan_matcher ##############################

  <node pkg="laser_scan_matcher" type="laser_scan_matcher_node" 
    name="laser_scan_matcher_node" output="screen">

    <param name="fixed_frame" value = "odom"/>
    <param name="use_imu" value="false"/>
    <param name="use_odom" value="false"/>
    <param name="use_alpha_beta" value="false"/>
    <param name="max_iterations" value="20"/>
    <param name="kf_dist_linear" value="0.5"/>
    <param name="kf_dist_angular" value="1.0"/>

  </node>

  #### start gmapping ############################################

  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
    <param name="map_udpate_interval" value="1.0"/>
    <param name="maxUrange" value="5.0"/>
    <param name="sigma" value="0.1"/>
    <param name="kernelSize" value="1"/>
    <param name="lstep" value="0.15"/>
    <param name="astep" value="0.15"/>
    <param name="iterations" value="5"/>
    <param name="lsigma" value="0.1"/>
    <param name="ogain" value="3.0"/>
    <param name="lskip" value="1"/>
    <param name="srr" value="0.1"/>
    <param name="srt" value="0.2"/>
    <param name="str" value="0.1"/>
    <param name="stt" value="0.2"/>
    <param name="linearUpdate" value="0.3"/>
    <param name="angularUpdate" value="0.3"/>
    <param name="temporalUpdate" value="0.5"/>
    <param name="resampleThreshold" value="0.5"/>
    <param name="particles" value="50"/>
    <param name="xmin" value="-5.0"/>
    <param name="ymin" value="-5.0"/>
    <param name="xmax" value="5.0"/>
    <param name="ymax" value="5.0"/>
    <param name="delta" value="0.02"/>
    <param name="llsamplerange" value="0.01"/>
    <param name="llsamplestep" value="0.05"/>
    <param name="lasamplerange" value="0.05"/>
    <param name="lasamplestep" value="0.05"/>
  </node>

</launch>

I hope this ... (more)

edit flag offensive delete link more

Comments

Great! This dramatically improves the stability of the mappig process. Independent from the results with my noisy data files this should be checked in. Now I have to find out, why almost hickups occure. If you want to play with the whole bag I uploaded it here: http://speedy.sh/tgRjD/vr100.zip

FlashErase gravatar image FlashErase  ( 2013-09-18 06:07:34 -0500 )edit

It's important do play the bag in realtime. Speeding up the player causes more mapping errors. I hope I'll find out, what kind of data errors is the reason of the furthermore occurent hickups.

FlashErase gravatar image FlashErase  ( 2013-09-18 06:16:00 -0500 )edit

One thing i saw in the small bag is that the robot sometimes seems to roll or tilt, so that a part of the floor is seen by the laser. This may confuse pure 2d slam approaches as they may interpret that measurement as a wall.

Ben_S gravatar image Ben_S  ( 2013-09-18 06:31:35 -0500 )edit

Yes there are some doorsills in the way of the robot causing tilt the laser rolling over them. In the second room lies a small but higher carpet with the same effect. I think, the only way for me to fix that is watching the time stamp when tilts occure and remove them from the bag with rosbag filter

FlashErase gravatar image FlashErase  ( 2013-09-18 06:40:57 -0500 )edit

Is there a timing parameter to make gmapping less sensitive for such tilts? Another issue is that because this is no longrange lidar in long and small passages not the base_link moves in the map but the map regarding home. Here the map center should stay longer on the old position to prevent this.

FlashErase gravatar image FlashErase  ( 2013-09-18 08:36:12 -0500 )edit

Unless you have additional sensor sources besides the laser, I don't see a way to distinguish ground from wall. I looks the same to the laser.

dornhege gravatar image dornhege  ( 2013-09-18 23:15:03 -0500 )edit
1

The long hallway is a common problem, if the laser doesn't see the end and there is no odometry. If you look at the raw laser data and it stays the same, it's not really fixable. You could add some features, e.g. put a trashcan somewhere.

dornhege gravatar image dornhege  ( 2013-09-18 23:16:35 -0500 )edit

The only somewhat proper thing I could image would be to make some fake odometry that includes velocity (e.g. a Kalman filter) that is seeded from the gmapping output, assuming velocity doesn't change without inputs.

dornhege gravatar image dornhege  ( 2013-09-18 23:17:15 -0500 )edit
1

answered 2013-09-14 11:21:51 -0500

tfoote gravatar image

If you're going twice as far in the wrong direction it suggests that you have something with the wrong sign or direction. Something that should be canceling is doubling.

edit flag offensive delete link more

Comments

The direction is right, only the calculated distance is too big. I can't imagine what reason causes such a behaviour. Also I found no parameters for the scan matcher such as factors to play with. The hector_slam calculates with the same bag correct odometry data.

FlashErase gravatar image FlashErase  ( 2013-09-14 22:53:57 -0500 )edit

OK, you're right, the direction referring to the world is wrong too. Can this be caused by inverted laser data?

FlashErase gravatar image FlashErase  ( 2013-09-16 00:36:24 -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: 2013-09-14 09:38:28 -0500

Seen: 4,412 times

Last updated: Sep 18 '13