Robot position moving on rviz after goal reached with dual ekf
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 ...