Robot position moving on rviz after goal reached with dual ekf

edit

Hi everyone!

I'm having some troubles using EKF dual navsat transform. After my robot reaches goal in rviz it continues to move. I can see the TF from map -> odom moving away with it. I dont know whats the issue is. I've looked up the tutorials about creating odom -> base_link and map -> odom with odometry, gps, imu and amcl and everything works until the robot reaches final goal, than even if it's still on ignition gazebo on rviz it drifts away. Does anyone ever saw that happen? Because i searched it and havent found anything related. Here goes a video of the bug/problem: https://youtu.be/8_ZBq7ZV2e0 You can see by the video that the TF is drifting away with the robot footprint, it has a certain inertia on the move before the goal was reached.

My launch file with the ekf's part:

# Start the navsat transform node which converts GPS data into the world coordinate frame
start_navsat_transform_cmd = Node(
  package='robot_localization',
  executable='navsat_transform_node',
  name='navsat_transform',
  output='screen',
  parameters=[ekf_params_file],
  remappings=[('gps/fix', 'gps/fix'), 
            ('imu/data', 'imu/data'),
            ('gps/filtered', 'gps/filtered'),
            ('odometry/gps', 'odometry/gps'),
            ('odometry/filtered', 'odometry/global')])

# Start robot localization using an Extended Kalman filter...map->odom transform
start_robot_localization_global_cmd = Node(
  package='robot_localization',
  executable='ekf_node',
  name='ekf_filter_node_map',
  output='screen',
  parameters=[ekf_params_file],
  remappings=[('odometry/filtered', 'odometry/global'),
              #('/set_pose', '/initialpose')])
              ])

# Start robot localization using an Extended Kalman filter...odom->base_footprint transform
start_robot_localization_local_cmd = Node(
  package='robot_localization',
  executable='ekf_node',
  name='ekf_filter_node_odom',
  output='screen',
  parameters=[ekf_params_file],
  remappings=[('odometry/filtered', 'odometry/local')]
              #('/set_pose', '/initialpose')])
              )

initial_pose_gps = ExecuteProcess(
   cmd=[[
       'ros2 service call ',
       '/datum robot_localization/srv/SetDatum ',
       '"{geo_pose: {position: {latitude: -0.002431010368916534, longitude: 0.004991542999585263, altitude: 2.86}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}"'
   ]],
   shell=True
)

My EKF Params file:

### ekf config file ###

ekf_filter_node_odom: ros__parameters:

    use_sim_time: true

    frequency: 30.0

    sensor_timeout: 0.1

    two_d_mode: true

    transform_time_offset: 0.0

    transform_timeout: 0.0

    print_diagnostics: true

    debug: false

    debug_out_file: /path/to/debug/file.txt

    publish_tf: true

    publish_acceleration: false

    reset_on_time_jump: false # true

    map_frame: map                   # Defaults to "map" if unspecified
    odom_frame: prius_hybrid/odom                 # Defaults to "odom" if unspecified
    base_link_frame: prius_hybrid/base_link  # Defaults to "base_link" if unspecified
    world_frame: prius_hybrid/odom                 # Defaults to the value of odom_frame if unspecified

    odom0: odom

    odom0_config: [true, true, true,
                   false, false, false,
                   true,  true,  true,
                   false, false, true,
                   false, false, false]

    odom0_queue_size: 2

    odom0_nodelay: false

    odom0_differential: false

    odom0_relative: false


    #odom0_pose_rejection_threshold: 5.0 # 5.0
    #odom0_twist_rejection_threshold: 1.0

    imu0: imu/data
    imu0_config: [false, false, false,
                   true,  true,  true,
                   false, false, false,
                   true,  true,  true,
                   false, false, false]

    #        [x_pos   , y_pos    , z_pos,
    #         roll    , pitch    , yaw,
    #         x_vel   , y_vel    , z_vel,
    #         roll_vel, pitch_vel, yaw_vel,
    #         x_accel , y_accel  , z_accel]

    imu0_nodelay: false
    imu0_differential: false
    imu0_relative: true
    imu0_queue_size: 7
    #imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
    #imu0_twist_rejection_threshold: 0.8                #
    #imu0_linear_acceleration_rejection_threshold: 0.8  #


    imu0_remove_gravitational_acceleration: true

    use_control: false

    stamped_control: false

    control_timeout: 0.2

    control_config: [true, false, false, false, false, true]

    acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]

    deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4 ...