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?

• wheelticks is an array for the left and right wheels
• wheelencoder_ticks is 20 (20 ticks / rotation)
• wheeldiameter is 0.06

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

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

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: 22164
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound (44399 - 192.168.0.121:48590) [16]
    * transport: TCPROS
 * topic: /tf
    * to: /amcl
    * direction: outbound
    * transport: INTRAPROCESS
 * topic: /tf
    * to: /robot_localization
    * direction: outbound (44399 - 192.168.0.121:48594) [10]
    * transport: TCPROS
 * topic: /tf
    * to: /move_base
    * direction: outbound (44399 - 192.168.0.121:48622) [11]
    * transport: TCPROS
 * topic: /tf
    * to: /rviz
    * direction: outbound (44399 - 192.168.0.121:48668) [19]
    * transport: TCPROS
 * topic: /tf
    * to: /amcl (http://192.168.0.121:38081/)
    * direction: inbound
    * transport: INTRAPROCESS
 * topic: /tf
    * to: /robot_state_publisher (http://192.168.0.121:38417/)
    * direction: inbound (56796 - 192.168.0.121:55443) [15]
    * transport: TCPROS
 * topic: /tf
    * to: /robot_localization (http://192.168.0.121:33583/)
    * direction: inbound (43412 - 192.168.0.121:56595) [17]
    * transport: TCPROS
 * topic: /tf
    * to: /jetbot_base_node (http://192.168.0.121:44967/)
    * direction: inbound (59342 - 192.168.0.121:57005) [22]
    * transport: TCPROS
 * topic: /tf_static
    * to: /robot_state_publisher (http://192.168.0.121:38417/)
    * direction: inbound (56800 - 192.168.0.121:55443) [18]
    * transport: TCPROS
 * topic: /jetbot/scan
    * to: /jetbot/ydlidar_node (http://192.168.0.137:34523/)
    * direction: inbound (34962 - 192.168.0.137:60007) [24]
    * transport: TCPROS
 * topic: /initialpose
    * to: /rviz (http://192.168.0.121:42711/)
    * direction: inbound (33284 - 192.168.0.121:36025) [23]
    * transport: TCPROS

My launch file is the following:

<launch>

    <!-- Arguments -->
    <arg name="rvizconfig"     default="$(find jetbot_description)/rviz/navigation.rviz"/>
    <arg name="map_file" default="$(find jetbot_navigation)/maps/small.yaml"/>  

    <!-- Map server -->
    <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>

    <include file="$(find jetbot_base)/launch/hw_control.launch" />

    <!-- Start robot localization node -->
    <node pkg="robot_localization" type="ekf_localization_node" name="robot_localization" clear_params="true">
        <rosparam command="load" file="$(find jetbot_navigation)/config/ekf.yaml" />
    </node>

    <!-- AMCL -->
    <node pkg="amcl" type="amcl" name="amcl">
        <remap from="scan"                      to="/jetbot/scan"/>
        <param name="odom_model_type"           value="diff-corrected"/>
        <param name="tf_broadcast"              value="true" />
        <param name="global_frame_id"           value="map" />

        <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="0.00"/>
        <param name="initial_pose_y"            value="0.00"/>
        <param name="initial_pose_a"            value="0.00"/>
        <param name="gui_publish_rate"          value="50.0"/>

        <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_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_footprint"/>
    </node>

    <node name="rviz" 
            pkg="rviz" 
            type="rviz" 
            args="-d $(arg rvizconfig)" 
            required="false" />

    <node pkg="move_base"
          type="move_base"
          name="move_base"
          respawn="false"
          output="screen">

          <rosparam file="$(find jetbot_navigation)/config/costmap_common_params.yaml"
                    command="load"
                    ns="global_costmap"/>

          <rosparam file="$(find jetbot_navigation)/config/costmap_common_params.yaml"
                    command="load"
                    ns="local_costmap"/>

          <rosparam file="$(find jetbot_navigation)/config/local_costmap_params.yaml"
                    command="load"/>

          <rosparam file="$(find jetbot_navigation)/config/global_costmap_params.yaml"
                    command="load"/>

          <rosparam file="$(find jetbot_navigation)/config/base_local_planner_params.yaml"
                    command="load"/>

          <remap from="cmd_vel" 
                 to="/mobile_base_controller/cmd_vel"/>

          <remap from="odom" 
                 to="/odometry/filtered"/>

    </node>

</launch>

I uploaded all of the config files I am using here. If you need any other information, just please let me know.