ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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. :)