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

edit

Hi,

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) .

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

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.

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.

Configuration

Some info about my amcl node:

Node [/amcl]
Publications: 
 * /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]

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

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


contacting node http://192.168.0.121:38081/ ...
Pid ...