ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
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. :)