Robot loses proper localization

Question

Having trouble getting robot to stay localized in map. The lidar maps the correct position of robot but not the odometry or ekf tf. One can see the two positions on graphic below.

The robot base is Linorobot2 with differential drive and I'm using Nav2 and Humble. I'm running Rtabmap on the robot without the rtabmapviz node and it publishes the Velodyne puck ICP odometry. The host runs the complete Nav2 stack and SLAM Toolbox with Rviz2.

The Gazebo sim works well for mapping and navigation but the real robot has this trouble, as shown in graphic.

I tried feeding the ICP odom into the EKF node also but it just made things worse, I had to change it back to just the IMU and wheel odometry going into the EKF node.

What is the correct way to troubleshoot this setup? Should I adjust the encoder ticks per meter? I have never seen the amcl arrows, though it seems to be running, as it sometimes posts warnings.

From launch file:

    Node(
        package='rtabmap_ros', executable='icp_odometry', output='screen',
        parameters=[{
          'frame_id':'velodyne',
          'odom_frame_id':'odom',
          'wait_for_transform':0.2,
          'expected_update_rate':15.0,
          'deskewing':deskewing,
          'use_sim_time':use_sim_time,
        }],
        remappings=[
          ('scan_cloud', '/velodyne_points'),
          ('odom', 'odom_icp')
        ],
        arguments=[
          'Icp/PointToPlane', 'true',
          'Icp/Iterations', '10',
          'Icp/VoxelSize', '0.1',
          'Icp/Epsilon', '0.001',
          'Icp/PointToPlaneK', '20',
          'Icp/PointToPlaneRadius', '0',
          'Icp/MaxTranslation', '2',
          'Icp/MaxCorrespondenceDistance', '1',
          'Icp/Strategy', '1',
          'Icp/OutlierRatio', '0.7',
          'Icp/CorrespondenceRatio', '0.01',
          'Odom/ScanKeyFrameThr', '0.6',
          'OdomF2M/ScanSubtractRadius', '0.1',
          'OdomF2M/ScanMaxSize', '15000',
          'OdomF2M/BundleAdjustment', 'false',
        ]),

ekf_filter_node: ros__parameters:

    frequency: 50.0

    two_d_mode: true

    publish_tf: true

    use_control: false

    map_frame: map             
    odom_frame: odom            
    base_link_frame: base_footprint 
    world_frame: odom 


    #x     , y     , z,
    #roll  , pitch , yaw,
    #vx    , vy    , vz,
    #vroll , vpitch, vyaw,
    #ax    , ay    , az
    
    # from Robbie's encoders
    odom0: odom/unfiltered
    odom0_config: [false, false, false,
                   false, false, false,
                   true, true, false,
                   false, false, true,
                   false, false, false]
                   
    # from ICP module of Rtabmap
    odom1: odom_icp
    odom0_config: [true, true, false,
                   false, false, true,
                   false, false, false,
                   false, false, false,
                   false, false, false]

    # from MPU 9250
    imu0: imu/data
    imu0_config: [false, false, false,
                  false, false, false,
                  false, false, false,
                  false, false, true,
                  true, false, false]

[component_container_isolated-1] [INFO] [1677951250.137225667] [amcl]: Message Filter dropping message: frame 'velodyne' at time 1677951249.140 for reason 'the timestamp on the message is earlier than all the data in the transform cache' [component_container_isolated-1] [INFO] [1677951250.208334876] [local_costmap.local_costmap]: Timed out waiting for transform from base_link to odom to become available, tf error: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist [component_container_isolated-1] [INFO] [1677951250.237334995] [amcl]: Message Filter dropping message: frame 'velodyne' at time 1677951249.240 for reason 'the timestamp on the message is earlier than all the data in the transform cache' [component_container_isolated-1] [INFO] [1677951250.366020161] [amcl]: Message Filter dropping message: frame 'velodyne' at time 1677951249.340 for reason 'discarding message because the queue is full' [component_container_isolated-1] [INFO] [1677951250.456398646] [amcl]: Message Filter dropping message: frame 'velodyne' at time 1677951249.441 for reason 'discarding message because the queue is full' [component_container_isolated-1] [INFO] [1677951250.541983751] [amcl]: Message Filter dropping message: frame 'velodyne' at time 1677951249.540 for reason 'the timestamp on the message is earlier than all the data in the transform cache' [component_container_isolated-1] [INFO] [1677951250.666734227] [amcl]: Message Filter dropping message: frame 'velodyne' at time 1677951249.643 for reason 'discarding message because the queue is full' [component_container_isolated-1] [INFO] [1677951250.708331804] [local_costmap.local_costmap]: Timed out waiting for transform from base_link to odom to become available, tf error: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist [component_container_isolated-1] [INFO] [1677951250.744652632] [amcl]: Message Filter dropping message: frame 'velodyne' at time 1677951249.739 for reason 'discarding message because the queue is full'

Answer 1

One number is wrong in the EKF config. Odom1 is configured as another odom0! This won't work right! Got it fixed and things are moving forward.

Answer 2

I don't see green points from AMCl, are you sure it is running correctly? What type of warning do you see from it? Can you add them to the post?

What are your parameters for ICP and the rest in the EKF? Can you show them? Maybe something there could be tinkered with to help.