navsat_transform_node outputs zero pose on /odometry/gps
Hi all,
I'am trying to get robotlocalization to work with imu and GPS (no real odometry) using the navsattransform_node node with Gazebo.
I have my first ekf node, running with imu0 only as input, to obtain the odom frame. Then I try to fuse GPS information using the navsattransformnode, but this node only outputs zero odometry on /odometry/gps:
header:
seq: 194
stamp:
secs: 40
nsecs: 200000000
frame_id: "odom"
child_frame_id: ''
pose:
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
covariance: [1.0099999999999998, -8.788794008375919e-25, 0.0, 0.0, 0.0, 0.0, -8.271806125530277e-25, 1.0099999999999998, -1.6940658945086007e-21, 0.0, 0.0, 0.0, 6.776263578034403e-21, 0.0, 1.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.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist:
twist:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Is there something obvious that I am missing?
My launch file for the robot_localization components is the following
<launch>
<rosparam command="load" file="$(find symeter2)/config/ekf_config.yaml" />
<!-- Launch Symeter 2 gui-->
<node pkg="symeter2_gui" type="symeter2_gui" name="symeter2_gui" output="screen" />
<!-- Launch the robot_localization node -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true">
<rosparam param="initial_state">[0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0]</rosparam>
</node>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
</node>
<node pkg="robot_localization" type="navsat_transform_node"
name="navsat_transform" respawn="true" output="log">
<rosparam param="datum">[49.9, 8.9, 0.0, map, base_link]</rosparam>
<remap from="/imu/data" to="/symeter2/imu0" />
<remap from="/gps/fix" to="/symeter2/fix/" />
<remap from="/odometry/filtered" to="/odometry/filtered" />
</node>
</launch>
and the ekf_config.yaml file is the following:
ekf_se_odom:
frequency: 30
sensor_timeout: 1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
publish_tf: true
publish_acceleration: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
imu0: symeter2/imu0
imu0_config: [false, false, false, #x, y, z
true, true, true, #roll, pitch, yaw
false, false, false, #vx, vy, vz,
true, true, true, #vroll, vpitch, vyaw
true, true, true] #ax, ay, az
imu0_nodelay: false
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
use_control: false
process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, #x
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, #y
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, #z
0, 0, 0, 0.004, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, #roll
0, 0, 0, 0, 0.004, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, #pitch
0, 0, 0, 0, 0, 0.004, 0, 0, 0, 0, 0, 0, 0, 0, 0, #yaw
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, #vx
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, #vy
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, #vz
0, 0, 0, 0, 0, 0, 0, 0, 0, 4e-6, 0, 0, 0, 0, 0, #vroll
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4e-6, 0, 0, 0, 0, #vpitch
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4e-6, 0, 0, 0, #vyaw
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.004, 0, 0, #ax
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.004, 0, #ay
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.004] #az
initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, #x
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, #y
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, #z
0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, #roll
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, #pitch
0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, #yaw
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, #vx
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, #vy
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, #vz
0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, #vroll
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, #vpitch
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, #vyaw
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, #ax
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, #ay
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1] #az
ekf_se_map:
frequency: 30
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: map
odom1: odometry/gps
odom1_config: [true, true, true,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_queue_size: 10
odom1_nodelay: true
odom1_differential: false
odom1_relative: false
imu0: imu/data
imu0_config: [false, false, false,
true, true, false,
false, false, false,
true, true, true,
true, true, true]
imu0_nodelay: true
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
use_control: false
process_noise_covariance: [1.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, 1.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.3, 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.5, 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.1, 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.3, 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.3, 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.3]
initial_estimate_covariance: [1.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, 1.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, 1.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, 1.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, 1.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, 1.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, 1.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, 1.0]
navsat_transform:
frequency: 30
delay: 3.0
#magnetic_declination_radians: 0.0429351 # For lat/long 55.944831, -3.186998
yaw_offset: 0
zero_altitude: false
broadcast_utm_transform: true
publish_filtered_gps: true
use_odometry_yaw: false
wait_for_datum: false
Asked by sebh on 2018-06-12 08:58:47 UTC
Answers
It turns out all my problems came out of a faulty configuration of my gazebo hector gps plugin.
I had to specify in the <frameId>
tag and the <referenceHeading>
tag in order to get the navsat_transform_node working correctly.
navsat_transform_node needs the <frameId>
of the GPS link to perform the transform from the GPS link to the base_link, in my case:
<frameId>gps_link</frameId>
And by default the hector gazebo gps plugin mesures yaw from the NORTH. So when we move along the x-axis in gazebo, the navsat_transform_node returns a gps odometry moving along the y-axis. In order to compensate we need to configure the <referenceHeading>
to 90 in the hector gazebo gps plugin:
<referenceHeading>90</referenceHeading>
Now everything works!
Asked by sebh on 2018-06-13 06:13:56 UTC
Comments
After reading https://answers.ros.org/question/200071/how-to-fuse-imu-gps-using-robot_localization/#200087, it seems that I do not need to have two ekf nodes when I only have IMU to fuse with GPS.
But I would still have my problem with navsat_transform_node generating 0 pose in /odometry/gps.
Asked by sebh on 2018-06-12 09:55:36 UTC
I'am probably missing something quite simple, but it still eludes me...
Asked by sebh on 2018-06-12 09:56:31 UTC
It turns out that I have the exact same behavior as described in https://answers.ros.org/question/282246/no-pose-update-in-navsat_transform_node-topic/
I also have an empty child_frame_id
Asked by sebh on 2018-06-13 03:36:20 UTC