no output from navsat_transform_node when fusing GPS and imu data
I have been trying to integrate data from my GPS (/ublox_gps/fix
) and imu (/imu/data
) using robot_localization
package.
My setup looks as:
navsat_transform_node
Subscribed topics: /imu/data
, /odometry/filtered /ublox_gps/fix
Published topics: /odometry/gps
ekf_localization_node
Subscribed topics: /imu/data
, /odometry/gps
Published topics: /odometry/filtered
But no messages are published on /odometry/gps
and so pose and twist values on /odometry/filtered
are just drifting away.
Below is the launch file state_est.launch
<launch>
<node name="bl_gps" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0 0 0 0 1 base_link gps" />
<node name="bl_imu" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0 0 0 0 1 base_link imu_link" />
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
<param name="magnetic_declination_radians" value="0.36878808"/>
<param name="zero_altitude" value="false"/>
<param name="publish_filtered_gps" value="false"/>
<param name="broadcast_utm_transform" value="true"/>
<param name="wait_for_datum" value="false"/>
<remap from="/gps/fix" to="/ublox_gps/fix"/>
<remap from="/odometry/filtered" to="/odometry/filtered"/>
<remap from="/imu/data" to="/imu/data"/>
</node>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_odom" clear_params="true">
<param name="odom0" value="/odometry/gps"/>
<param name="imu0" value="/imu/data"/>
<param name="frequency" value="10"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="false"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]
</rosparam>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="false"/>
<rosparam param="odom0_config">[true, true, true,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
</rosparam>
<param name="odom0_differential" value="false"/>
<param name="print_diagnostics" value="true"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="$(env HOME)/debug_ekf_localization.txt"/>
</node>
</launch>
Sample messages: IMU
---
header:
seq: 10203
stamp:
secs: 1554055128
nsecs: 142280585
frame_id: "imu_link"
orientation:
x: -0.0357513045
y: -0.9831105198
z: -0.1790586462
w: -0.0097686663
orientation_covariance: [0.002741552146694444, 0.0, 0.0, 0.0, 0.002741552146694444, 0.0, 0.0, 0.0, 0.007615422629706791]
angular_velocity:
x: 0.00155473888023
y: -0.00131564657136
z: 0.0103541060827
angular_velocity_covariance: [1.0966208586777776e-06, 0.0, 0.0, 0.0, 1.0966208586777776e-06, 0.0, 0.0, 0.0, 1.0966208586777776e-06]
linear_acceleration:
x: 0.782283911277
y: -0.0314218966287
z: 10.4502588694
linear_acceleration_covariance: [0.0015387262937311438, 0.0, 0.0, 0.0, 0.0015387262937311438, 0.0, 0.0, 0.0, 0.0015387262937311438]
GPS
---
header:
seq: 632
stamp:
secs: 1554055120
nsecs: 43773
frame_id: "gps"
status:
status: -1
service: 3
latitude: 26.5106231
longitude: 80.2273833
altitude: 79.619
position_covariance: [58327.0801, 0.0, 0.0, 0.0, 58327.0801, 0.0, 0.0, 0.0, 88508.03500899998]
position_covariance_type: 2
Edit: following statements are displayed as the file is launched:
process[bl_gps-1]: started with pid [3411]
process[bl_imu-2]: started with pid [3412]
process[navsat_transform_node-3]: started with pid [3413]
process[ekf_odom-4]: started with pid [3414]
[ INFO] [1554146782.295926298]: Waiting for valid clock time...
[ INFO] [1554146782.295971492]: Valid clock time received. Starting node.
[ INFO] [1554146783.026898184]: Initial odometry pose ...
Can you add the messages you get in terminal as the node starts?
I have added the messages in the question. Basically, the INFO messages show the position drifting away very fast, even though robot was not moving.
I'd have to test at home to be sure but I think NAV-SAT will output the start of data when it gets it's first IMU message and initializes the orientation and I don't see that happen in the output above. Make sure sure that the remapping for IMU into NAV-SAT is correct as it doesn't look like NAV_SAT is recognizing IMU.
EDIT: I looked again at your output. It is getting IMU data. But I don't see it getting GPS data. Double check your mapping of GPS data. When GPS comes in you should see output like this:
(more)