AMCL issue: laser scan not matching the map after moving the robot

asked 2021-06-15 06:27:21 -0600

balint.tahi gravatar image

updated 2021-06-15 06:28:18 -0600


I have a problem with amcl and I am unable to locate the root of it. When I am starting my problem and set the initial pose (2D Pose Estimate in rviz) the map matches the laser scan, so I guess the map->odom transformation is correct (odom -> base_footprint is 0 at this time, because I am not moving the robot) picture.

When I start moving my robot, the laser scan is not matching the map anymore: picture

When I am moving my robot, I am providing encoder ticks to my hardware interface, and my read() function looks the following:

void read(const ros::Duration &period) {
    double distance_left = (_wheel_ticks[0] * ((_wheel_diameter * M_PI) / _wheel_encoder_ticks));
    double distance_right = (_wheel_ticks[1] * ((_wheel_diameter * M_PI) / _wheel_encoder_ticks));

    pos[0] += linearToAngular(distance_left - last_dist_left);
    vel[0] += linearToAngular((distance_left - last_dist_left)) / period.toSec();
    pos[1] += linearToAngular(distance_right - last_dist_right);
    vel[1] += linearToAngular((distance_right - last_dist_right)) / period.toSec();

    last_dist_left = distance_left;
    last_dist_right = distance_right;

I am publishing absolute ticks (FC-03 motor wheel encoder on GPIO for the left and right wheels), based on the last command, I am increasing or decreasing the ticks, since the hw can not tell the direction of the movement itself.

Is my read() function looks okay?

  • _wheel_ticks is an array for the left and right wheels
  • _wheel_encoder_ticks is 20 (20 ticks / rotation)
  • _wheel_diameter is 0.06

I am not saying it is superprecise (hw limits ...), but it should be ok for experimental purposes. I am also using robot_localization (currently with only the encoder values), but with or without the robot_localization it is totally the same.

The TF looks okay (AMCL is responsible for map->odom, robot_localization is responsible for : odom->base_footprint.

image description

Expected outcome

I would expect, that AMCL is correcting the map->odom transformation to match the laser scan with the map, because the odom->base_footprint drift errors and "not very precise" sensor reading. But it is not doing a very good job. If I am moving very slow I can see, that sometimes the map->odom is updating, but the laser never matches the map again. Which paramters should I adjust or what am I doing wrong?

Plus one problem

The other problem I am facing is the path planning, and actually the execution of the plan. If I am controlling my robot with a gamepad, it is fully okay. If I am sending it to somewhere with 2D Nav Goal, it starts very crazy movements, like spinning when it stucks (rotational recovery), even if I am sending it on a straight line. Maybe the problems are connected, but since I can reproduce the unmatching laser-map (map->odom problem?!) with the gamepad, maybe this is the 1st thing I should fix.


Some info about my amcl node:

Node [/amcl]
 * /amcl/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /amcl/parameter_updates [dynamic_reconfigure/Config]
 * /amcl_pose [geometry_msgs/PoseWithCovarianceStamped]
 * /diagnostics [diagnostic_msgs/DiagnosticArray]
 * /particlecloud [geometry_msgs/PoseArray]
 * /rosout [rosgraph_msgs/Log]
 * /tf [tf/tfMessage]

 * /initialpose [geometry_msgs/PoseWithCovarianceStamped]
 * /jetbot/scan [sensor_msgs/LaserScan]
 * /tf [tf/tfMessage]
 * /tf_static [tf2_msgs/TFMessage]

 * /amcl/get_loggers
 * /amcl/set_logger_level
 * /amcl/set_parameters
 * /global_localization
 * /request_nomotion_update
 * /set_map

contacting node ...
Pid ...
edit retag flag offensive close merge delete


Have you checked the mounting position of the LiDAR? If the orientation is not correct, you may get a scan that is inconsistent with the robot's movement, and amcl may not work properly.

miura gravatar image miura  ( 2021-06-15 19:25:50 -0600 )edit

Thanks for the input, I replaced the model in the jetbot_description from rplidar to ydlidar, setted it up, it seems to be a bit more stable, but still not perfect. But for sure the odometry is not right ... if I am pushing the robot by hand forward (both wheels 1 rotation) when it should be 20cm travel, the odom/pose/pose/position X direction shows 10 cm.

balint.tahi gravatar image balint.tahi  ( 2021-06-16 06:09:11 -0600 )edit

I don't think I'm doing anything wrong with the read function. Is it possible that the tire is slipping? Also, increasing odom_alpha1 to odom_alpha4 might help. This will make the scan more reflective.

miura gravatar image miura  ( 2021-06-16 09:12:05 -0600 )edit

Did you ment I am not doing anything wrong with the read calculation? :) I am sure, that sometimes the wheel is slipping (slippery floor...), but when I am pushing it by hand, I am watching for the wheel to have a full rotation. Diff Drive Controller says (in the odom topic), that the X coordinate of the Position is 10cm, instead of 20cm ... Logging out the calculation for the left and right wheels, those moved 20 cm (linear). But when it goes through the diff_drive_controller's pipeline, somehow it shows 10 cm. And again: I am pushing it by hand, only forward, watching for the wheel and not the distance itself. So 1 wheel rotation = 10cm, (<---- this is the error) while the wheels were moving 20cm (linear).

balint.tahi gravatar image balint.tahi  ( 2021-06-16 09:50:13 -0600 )edit

and btw ... The amcl laser matching seems to be better

balint.tahi gravatar image balint.tahi  ( 2021-06-16 09:51:42 -0600 )edit

The diff_drive_controller may be set incorrectly. For example, wheel_radius is not set to 0.03.


miura gravatar image miura  ( 2021-06-16 19:19:11 -0600 )edit

I thought the read function was correct, but it seems to have a bug. I answered it here.

miura gravatar image miura  ( 2021-06-19 22:06:47 -0600 )edit