AMCL Scans Drifting / Not Correctly Registering

edit

Hi,

I've got an issue similar to this post, but not the exact error: https://answers.ros.org/question/2624...

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: https://youtu.be/H9JUsIWhJbE

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:

amcl.launch

<launch>
  <!-- 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"/>

  </node>
</launch>

I'd appreciate any help with this!

Many thanks in advance!! :)