Navsat_transform ekf Odometry/gps jumping to random values
Hey you all!
I'm trying to simulate a navsat (gps) on a ignition gazebo simulation but i'm having some troubles with different results in the odometry/global and odometry/local topics using the navsat transform node, it is supposed to be that different? I tried to do the EKF with the same sensor inputs and still have an offset which is pretty weird. The topic output of odometry/global and odometry/local after i move the robot a little in the simulation are as follows:
EDIT1: I think it might have to do with the values of my Odometry/gps being too far from each other and changing too much at every step. I also noticed that my gps/fix and gps/filtered topics stays the same. It doesnt update, is that suposed to be like that? It stays in the spherical coordinates in origin even if my robot moves. As follows my odometry/gps topic echos:
header:
stamp:
sec: 9
nanosec: 500000000
frame_id: map
child_frame_id: ''
pose:
pose:
position:
x: -14284.242740421556
y: 40063.96676235087
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
header:
stamp:
sec: 9
nanosec: 400000000
frame_id: map
child_frame_id: ''
pose:
pose:
position:
x: 51796.78036309546
y: -73793.62852960825
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
header:
stamp:
sec: 9
nanosec: 300000000
frame_id: map
child_frame_id: ''
pose:
pose:
position:
x: -109302.91985165933
y: -54294.5602690829
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
header:
stamp:
sec: 9
nanosec: 200000000
frame_id: map
child_frame_id: ''
pose:
pose:
position:
x: 55014.76906251442
y: 13793.76866940409
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
Does anybody knows why the values are variating that much ?
Odometry/global:
header:
stamp:
sec: 128
nanosec: 400000000
frame_id: map
child_frame_id: prius_hybrid/base_link
pose:
pose:
position:
x: 30.22977389559373
y: -1.681874941073202
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.6969526635902402
w: 0.7171171345843506
Odometry/local:
header:
stamp:
sec: 132
nanosec: 900000000
frame_id: prius_hybrid/odom
child_frame_id: prius_hybrid/base_link
pose:
pose:
position:
x: 29.080819302609783
y: -5.793992834450633
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.06309003914389398
w: 0.9980078391279409
My Odom topic:
header:
stamp:
sec: 168
nanosec: 380000000
frame_id: prius_hybrid/odom
child_frame_id: prius_hybrid/base_link
pose:
pose:
position:
x: 29.080834914179928
y: -5.794012295793156
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.02474994638018044
w: 0.9996936731590224
My Odometry/gps topic:
header:
stamp:
sec: 181
nanosec: 300000000
frame_id: map
child_frame_id: ''
pose:
pose:
position:
x: 6355519.595500257
y: -2377438.96173511
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
My Gps/fix topic:
header:
stamp:
sec: 200
nanosec: 200000000
frame_id: prius_hybrid/base_link/gps_sensor
status:
status: 0
service: 0
latitude: -29.06480303731489
longitude: -51.81648093165863
altitude: 718.3080508620511
My Gps/filtered topic:
header:
stamp:
sec: 209
nanosec: 334000000
frame_id: prius_hybrid/base_link
status:
status: 2
service: 0
latitude: 0.0002512632909927111
longitude: -6.3507150614317e-05
altitude: 0.0
My launch file with the relevant parts:
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'),
('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')])
])
Run initial GPS pose service
initial_pose_gps = ExecuteProcess(
cmd=[[
'ros2 service call ',
'/datum robot_localization/srv/SetDatum ',
'"{geo_pose: {position: {latitude: -29.1699, longitude: -51.2838, altitude: 716}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}"'
]],
shell=True
)
My EKF file as it is:
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,
true, true, true]
# [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]
My world.sdf file with the spherical coordinates and my gps sensor sdf which is in my baselink frame. I also have a static tf being published from baselink to baselink/gpssensor.
<sensor name="gps_sensor" type="navsat">
<navsat>
<position_sensing>
<horizontal>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>0.5</stddev>
</noise>
</horizontal>
<vertical>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>0.5</stddev>
</noise>
</vertical>
</position_sensing>
<velocity_sensing>
<horizontal>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>0.5</stddev>
</noise>
</horizontal>
<vertical>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>0.5</stddev>
</noise>
</vertical>
</velocity_sensing>
</navsat>
<always_on>1</always_on>
<update_rate>10</update_rate>
<topic>/model/prius_hybrid/gps</topic>
</sensor>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<world_frame_orientation>ENU</world_frame_orientation>
<latitude_deg>-29.169906895887486</latitude_deg>
<longitude_deg>-51.28381966714432</longitude_deg>
<elevation>716</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
ekffilternodemap: 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
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: false # 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
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
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: true
odom1_relative: true
imu0: imu/data
imu0_config: [false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: false
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, true,
false, false, false,
false, false, false,
false, false, false]
#pose0_rejection_threshold: 1.0
use_control: false
process_noise_covariance: [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, 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]
navsattransform: rosparameters: usesimtime: true frequency: 10.0 delay: 3.0 magneticdeclinationradians: 0.3089233 # For lat/long of CTR - Caxias do sul yawoffset: 1.570796327 # if IMU reads 0 when facing magnetic north instead of east then set parameter to pi/2 zeroaltitude: true broadcastcartesiantransform: true broadcastcartesiantransformasparentframe: true publishfilteredgps: true useodometryyaw: false waitfordatum: true
My TF tree as follows:
I tried with and without setting the Datum, with and without yaw offset and magnetic declination. If someone has some experience using simulated navsat plz a little help!
Asked by marcelomm103 on 2022-07-04 18:23:07 UTC
Comments