Robotics StackExchange | Archived questions

AMCL Scans Drifting / Not Correctly Registering

Hi,

I've got an issue similar to this post, but not the exact error: https://answers.ros.org/question/262417/amcl-not-localizing-in-pre-built-map/

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 /cmdvel from either teleopjoy 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!! :)

Asked by torlog on 2021-05-18 21:12:58 UTC

Comments

Can you please run the odometry tests from http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide#Odometry

Asked by Humpelstilzchen on 2021-05-18 23:12:11 UTC

Followed the instructions and I uploaded the results here: https://imgur.com/a/0H9quU4

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

Asked by torlog on 2021-05-18 23:31:39 UTC

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

Asked by Humpelstilzchen on 2021-05-19 00:38:09 UTC

Here are the results here: https://imgur.com/a/0uIOvVZ

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

Asked by torlog on 2021-05-20 22:43:38 UTC

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

Asked by Humpelstilzchen on 2021-05-21 12:53:38 UTC

Uploaded here: https://imgur.com/a/4HsAPoE

Asked by torlog on 2021-05-23 16:30:36 UTC

Both ok, the URDF please

Asked by Humpelstilzchen on 2021-05-24 01:04:16 UTC

Popped them in a Google drive folder here - both the urdf.xacro and .gazebo files: https://drive.google.com/drive/folders/1QVBIRAjmA3ZcfXdyP5BmX735dTKtFrmy?usp=sharing

Asked by torlog on 2021-05-24 16:10:26 UTC

Sorry no idea. But one thing to note: Compared to Turtlebot everything seems to be bigger by a factor of 10 in the warthog world and you have configured amcl with a max laser range of 3.5m while the laser can certainly do much more and there are not many features in the 3.5m radius. => Try with higher laser_max_range=30 according to the urdf.

Asked by Humpelstilzchen on 2021-05-26 14:57:19 UTC

Cheers for that - will give it a shot!

Asked by torlog on 2021-05-26 17:30:00 UTC

Answers