AMCL and Stage

[oscar-lima]

A student had problems with the setup of AMCL in Stage, here I post some launch files and code for it

[oscar-lima]

Map building with gmapping:

Before you can use AMCL you have to create a map using gmapping:

You can use the following code:

<?xml version="1.0"?>
<launch>

  <!-- gmapping will publish the map in this topic -->
  <arg name="map_topic" default="/map" />

  <arg name="scan_topic"      default="/robot_0/base_scan_1"/> 
  <arg name="odom_frame_id"   default="robot_0/odom"/>
  <arg name="base_frame_id"   default="robot_0/base_link"/>
  <arg name="global_frame_id" default="map"/>

  <!-- launch gmapping SLAM -->
  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping">
    <remap from="scan" to="$(arg scan_topic)"/>
    <remap from="map" to="$(arg map_topic)"/>
    <remap from="map_metadata" to="$(arg map_topic)_metadata"/>
    <param name="base_frame" value="$(arg base_frame_id)"/>
    <param name="map_frame" value="$(arg global_frame_id)"/>
    <param name="odom_frame" value="$(arg odom_frame_id)"/>
    
    <param name="map_update_interval" value="5.0"/>
    <param name="maxUrange" value="6.0"/>
    <param name="maxRange" value="8.0"/>
    <param name="sigma" value="0.05"/>
    <param name="kernelSize" value="1"/>
    <param name="lstep" value="0.05"/>
    <param name="astep" value="0.05"/>
    <param name="iterations" value="5"/>
    <param name="lsigma" value="0.075"/>
    <param name="ogain" value="3.0"/>
    <param name="lskip" value="0"/>
    <param name="minimumScore" value="200"/>
    <param name="srr" value="0.01"/>
    <param name="srt" value="0.02"/>
    <param name="str" value="0.01"/>
    <param name="stt" value="0.02"/>
    <param name="linearUpdate" value="0.5"/>
    <param name="angularUpdate" value="0.436"/>
    <param name="temporalUpdate" value="-1.0"/>
    <param name="resampleThreshold" value="0.5"/>
    <param name="particles" value="80"/>
    <param name="xmin" value="-1.0"/>
    <param name="ymin" value="-1.0"/>
    <param name="xmax" value="1.0"/>
    <param name="ymax" value="1.0"/>

    <param name="delta" value="0.05"/>
    <param name="llsamplerange" value="0.01"/>
    <param name="llsamplestep" value="0.01"/>
    <param name="lasamplerange" value="0.005"/>
    <param name="lasamplestep" value="0.005"/>
  </node>

</launch>

The above code needs to be copied on a file with .launch extension and launched using roslaunch.

roslaunch your_file.launch

or

roslaunch pkg_name launch_file_name (if your launch file is located inside an existing ros pkg inside your catkin workspace)

Configure rviz for map visualization: add laser scanners, set fixed frame as map and add a map topic, make all topics listen to the ones matching your robot.

After you run the gmapping, teleoperate the robot around and watch the map being updated with the laser scanner readings.

[oscar-lima]

for AMCL:

You can use this sample launch-file code:

<?xml version="1.0"?>
<launch>

  <arg name="use_map_topic"   default="true"/><!--default: false-->
  <arg name="scan_topic"      default="/robot_0/base_scan_1"/> 
  <arg name="initial_pose_x"  default="-2.2"/>
  <arg name="initial_pose_y"  default="-5.1"/>
  <arg name="initial_pose_a"  default="-160"/>
  <arg name="odom_frame_id"   default="robot_0/odom"/>
  <arg name="base_frame_id"   default="robot_0/base_link"/>
  <arg name="global_frame_id" default="map"/>

  <node pkg="amcl" type="amcl" name="amcl">
    <param name="use_map_topic"             value="$(arg use_map_topic)"/>
    <!-- Publish scans from best pose at a max of 10 Hz -->
    <param name="odom_model_type"           value="diff"/>
    <param name="odom_alpha5"               value="0.1"/>
    <param name="gui_publish_rate"          value="10.0"/>
    <param name="laser_max_beams"             value="60"/>
    <param name="laser_max_range"           value="12.0"/>
    <param name="min_particles"             value="500"/>
    <param name="max_particles"             value="2000"/>
    <param name="kld_err"                   value="0.05"/>
    <param name="kld_z"                     value="0.99"/>
    <param name="odom_alpha1"               value="0.2"/>
    <param name="odom_alpha2"               value="0.2"/>
    <!-- translation std dev, m -->
    <param name="odom_alpha3"               value="0.2"/>
    <param name="odom_alpha4"               value="0.2"/>
    <param name="laser_z_hit"               value="0.5"/>
    <param name="laser_z_short"             value="0.05"/>
    <param name="laser_z_max"               value="0.05"/>
    <param name="laser_z_rand"              value="0.5"/>
    <param name="laser_sigma_hit"           value="0.2"/>
    <param name="laser_lambda_short"        value="0.1"/>
    <param name="laser_model_type"          value="likelihood_field"/>
    <!-- <param name="laser_model_type" value="beam"/> -->
    <param name="laser_likelihood_max_dist" value="2.0"/>
    <param name="update_min_d"              value="0.25"/>
    <param name="update_min_a"              value="0.2"/>
    <param name="odom_frame_id"             value="$(arg odom_frame_id)"/> 
    <param name="base_frame_id"             value="$(arg base_frame_id)"/> 
    <param name="global_frame_id"           value="$(arg global_frame_id)"/>
    <param name="resample_interval"         value="1"/>
    <!-- Increase tolerance because the computer can get quite busy -->
    <param name="transform_tolerance"       value="1.0"/>
    <param name="recovery_alpha_slow"       value="0.0"/>
    <param name="recovery_alpha_fast"       value="0.0"/>
    <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
    <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
    <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
    <remap from="scan"                      to="$(arg scan_topic)"/>
  </node>

</launch>

configure rviz: set fixed frame as map, add laser scanner, map, pose array topic and make them listen to the appropriate topics.

pose estimate: AMCL can subscribe to a pose estimate topic to give him a hint of were the robot is, this helps for the particle filter to converge to a solution. You can give the pose estimate by publishing to the topic: /initialpose

rviz can publish this pose for you, make sure that the fixed frame is map and then click on 2D Pose Estimate button and click on the screen grid to provide with an estimation of where the robot is

[h-s1996]

Thank you very much! It works now but the amcl_pose doesn't publish the position very accurately. The laser pose published by map.launch shows much more better values.

[oscar-lima]

If you want perfect localization just use odom topic. In stage it comes with no error