AMCL Scans Drifting / Not Correctly Registering

asked 2021-05-18 21:12:58 -0500

torlog gravatar image


I've got an issue similar to this post, but not the exact error:

I'm trying to localize a simulated Clearpath Warthog and autonomously navigate within one of the tests worlds. I successfully mapped the area using gmapping and I'm using a simulared SICK lidar as the perception input.

I am able to initially localize the robot when using the 2D Pose Estimate, but as soon as I start moving about the world, it loses localization and the scans no longer match the world (I can't post an screenshot as I'm new to this forum). Furthermore, even when the Warthog receives no /cmd_vel from either teleop_joy or 2D Nav Goal, the robot continues to output drifted scans. I've made a video illustrating the issue here:

Most of the code has been adapted from turtlebot3, and I've adjusted it to suit the Warthog model that I've made up. When comparing my system against the turtlebot's, I noticed my odom is published by /ekf_localisation, while turtlebot3's is published by /gazebo - would this result in the error I'm having?

Below is the amcl.launch file that I think might be the culprits:


  <!-- Arguments -->
  <arg name="scan_topic"     default="scan"/>
  <arg name="initial_pose_x" default="0.0"/>
  <arg name="initial_pose_y" default="0.0"/>
  <arg name="initial_pose_a" default="0.0"/>

  <!-- AMCL -->
  <node pkg="amcl" type="amcl" name="amcl">

    <param name="min_particles"             value="500"/>
    <param name="max_particles"             value="3000"/>
    <param name="kld_err"                   value="0.02"/>
    <param name="update_min_d"              value="0.20"/>
    <param name="update_min_a"              value="0.20"/>
    <param name="resample_interval"         value="1"/>
    <param name="transform_tolerance"       value="0.5"/>
    <param name="recovery_alpha_slow"       value="0.00"/>
    <param name="recovery_alpha_fast"       value="0.00"/>
    <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)"/>
    <param name="gui_publish_rate"          value="50.0"/>

    <remap from="scan"                      to="$(arg scan_topic)"/>
    <param name="laser_max_range"           value="3.5"/>
    <param name="laser_max_beams"           value="180"/>
    <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_likelihood_max_dist" value="2.0"/>
    <param name="laser_model_type"          value="likelihood_field"/>

    <param name="odom_model_type"           value="diff"/>
    <param name="odom_alpha1"               value="0.1"/>
    <param name="odom_alpha2"               value="0.1"/>
    <param name="odom_alpha3"               value="0.1"/>
    <param name="odom_alpha4"               value="0.1"/>
    <param name="odom_frame_id"             value="odom"/>
    <param name="base_frame_id"             value="base_link"/>


I'd appreciate any help with this!

Many thanks in advance!! :)

edit retag flag offensive close merge delete



Can you please run the odometry tests from

Humpelstilzchen gravatar image Humpelstilzchen  ( 2021-05-18 23:12:11 -0500 )edit

Followed the instructions and I uploaded the results here:

Looks like the odometry is a bit off when rotating in place, but fairly good when driving in a straight line.

torlog gravatar image torlog  ( 2021-05-18 23:31:39 -0500 )edit

ok, anything in the amcl output? When you enable the visualization of the amcl pointcloud in rviz, anything special?

Humpelstilzchen gravatar image Humpelstilzchen  ( 2021-05-19 00:38:09 -0500 )edit

Here are the results here:

Very interesting results - the performance seems to fluctuate widely, but never has it correctly registered and stayed registered.

Let me know what you think and if you'd like more examples

torlog gravatar image torlog  ( 2021-05-20 22:43:38 -0500 )edit

ok, could you please add a tf tree and node graph? (rqt_graph)

Humpelstilzchen gravatar image Humpelstilzchen  ( 2021-05-21 12:53:38 -0500 )edit
torlog gravatar image torlog  ( 2021-05-23 16:30:36 -0500 )edit

Both ok, the URDF please

Humpelstilzchen gravatar image Humpelstilzchen  ( 2021-05-24 01:04:16 -0500 )edit

Popped them in a Google drive folder here - both the urdf.xacro and .gazebo files:

torlog gravatar image torlog  ( 2021-05-24 16:10:26 -0500 )edit