0
$\begingroup$

Rosanswers logo

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 http://img5.imageshack.us/img5/2325/kof.gif

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 http://img32.imageshack.us/img32/4708/l48y.png

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. Whatever, the parameter "inverted_laser" takes no effect.


Originally posted by FlashErase on ROS Answers with karma: 39 on 2013-09-14

Post score: 2


Original comments

Comment by Ben_S on 2013-09-16:
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...

Comment by FlashErase on 2013-09-16:
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.

Comment by Ben_S on 2013-09-16:
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...

Comment by FlashErase on 2013-09-16:
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".

Comment by FlashErase on 2013-09-16:
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".

Comment by FlashErase on 2013-09-16:
This is hector_slam mapping the uploaded test-bag. http://img19.imageshack.us/img19/6443/u5o.gif

Comment by dornhege on 2013-09-18:
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.

Comment by FlashErase on 2013-09-18:
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.

Comment by dornhege on 2013-09-18:
I was more hinting at the fact that the algorithms work that fine that they will work even if the data isn't correctly arriving. The fixes by @Ben_S working indicate that that happened.

Comment by Ben_S on 2013-09-19:
Well, in this case there was a real problem with GMapping and how it handles incoming data. There is no way, that this would have worked with the data "as is". :)

Comment by FlashErase on 2013-09-19:
Ben_S, if I cut out the frames during the pitch of the robot, how many seconds gmapping approximately can cope the loss of data without losing the position. I think no data are better than false data.

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

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 helps. If this is working under all conditions i think this should be included in the git-version. But this needs further testing. :)


Originally posted by Ben_S with karma: 2510 on 2013-09-18

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by FlashErase on 2013-09-18:
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

Comment by FlashErase on 2013-09-18:
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.

Comment by Ben_S on 2013-09-18:
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.

Comment by FlashErase on 2013-09-18:
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

Comment by FlashErase on 2013-09-18:
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.

Comment by dornhege on 2013-09-18:
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.

Comment by dornhege on 2013-09-18:
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.

Comment by dornhege on 2013-09-18:
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.

Comment by FlashErase on 2013-09-19:
Unless someone find a serial port on the new PCB rev. 64 it's not able me to get additional odometric data. Connecting such a port via bluetooth to the PC, there were a lot of informations for mapping: http://www.robotreviews.com/chat/viewtopic.php?f=20&t=14008 . Hope, that it will not take too long

Comment by FlashErase on 2013-09-19:
On weekend I'll try to increase the low intensity values in the datastream (only them). In the long hallways the end is to seen with the lidar but seems to be ignored by gmapping because of the low intensity. I'll report the results.

Comment by dornhege on 2013-09-20:
Do you mean intensity values as in field "intensity" in the message? I'm pretty sure gmapping ignores that. However, gmapping will grasp to the "density" of points as it matches readings to the map. If there are just a couple at the end mismatching, this won't really matter in the error measure.

Comment by FlashErase on 2013-09-20:
You're right. Increasing the intensity values in the scan showed no effect. And neither gmapping nor hector_slam tolerates gaps in the data stream. Removing the pitch scans from the data not was helpful too. So it seems that it is impossible me to generate maps and paths with gmapping or hector_slam

Comment by dornhege on 2013-09-21:
Do you measure the robot's attitude? One way to try might be transform the scans in 3D with the attitude and remove those ranges that are below a certain z-height.

Comment by FlashErase on 2013-09-22:
Because for the new revision of PCB there till now no known serial ports to connect to a bluetooth module at the moment I can only monitor the raw lidar data. Odometry data like acceleration sensors and motor states are not available. So the lidar data must be enough.

$\endgroup$
0
$\begingroup$

Rosanswers logo

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.


Originally posted by tfoote with karma: 58457 on 2013-09-14

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by FlashErase on 2013-09-14:
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.

Comment by FlashErase on 2013-09-16:
OK, you're right, the direction referring to the world is wrong too. Can this be caused by inverted laser data?

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.