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 -> baselink 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/8ZBq7ZV2e0 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 ###
ekffilternodeodom: 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.5]
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
process_noise_covariance: [1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3]
initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9]
ekffilternodemap: ros_parameters:
use_sim_time: true
frequency: 30.0 # 30.0
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: true # true
# If the filter sees a jump back in time, the filter is reset (convenient for testing with rosbags!)
reset_on_time_jump: true # true
map_frame: map
odom_frame: prius_hybrid/odom
base_link_frame: prius_hybrid/base_link
world_frame: map
initial_state: [0.040, 0.638, 0.0,
0.0, 0.0, 0.060, # yaw: 0.060
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0]
odom1: odometry/gps
odom1_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_queue_size: 10
odom1_nodelay: false
odom1_differential: false
odom1_relative: false
#odom1_rejection_threshold: 1.0
imu0: imu/data
imu0_config: [false, false, false,
true, true, true,
false, false, false,
true, true, true,
false, false, false]
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
pose0: amcl_pose # AMCL!
pose0_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
pose0_queue_size: 10
pose0_nodelay: false
pose0_differential: false
pose0_relative: false
#pose0_rejection_threshold: 1.0
use_control: false
# yaw : 0.01
process_noise_covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]
initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9]
navsat_transform:
ros__parameters:
use_sim_time: true
frequency: 10.0
delay: 3.0
magnetic_declination_radians: 0.0 #0.3089233 # For lat/long of CTR - Caxias do sul
yaw_offset: 0.0 #1.570796327 # if IMU reads 0 when facing magnetic north instead of east then set param to pi/2
zero_altitude: true
broadcast_cartesian_transform: true
broadcast_cartesian_transform_as_parent_frame: true
publish_filtered_gps: true
use_odometry_yaw: false
wait_for_datum: true
I already tweaked with the params on ekf a little bit, with covariances and take off some of the sensors, utilizing different combinations but nothing works. I already tried to set a static tf from map to odom and the drift still happens. I tried different frames combinations in my nav2_params file but i dont think it is the problem. So if anyone has any insight i would appreciate a lot! Thank you for your time!
Asked by marcelomm103 on 2022-07-26 16:43:44 UTC
Answers
Can anybody help me ? i'm still with this problem :/
Asked by marcelomm103 on 2022-08-02 10:07:38 UTC
Comments