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 is Origin: (0 0 0)
Rotation (RPY): (2.9839166226664590731, 0.14130230546121383406, -2.9445824208027233659)
[ INFO] [1554146783.126890005]: Initial odometry pose is Origin: (8.5501746669549698821e-09 2.914996583406068901e-08 -6.0099531981956410063e-08)
Rotation (RPY): (2.8492830015343062477, 0.091088610014680021498, -2.9894939514537317571)
After this the [INFO] messages keep coming with increasing x,y,z values.
Frequency of /imu/data is 20Hz and /ublox_gps/fix is 1 Hz. Is there a problem due to frequency not matching?
Asked by Manas Rawat on 2019-04-01 12:50:16 UTC
Answers
I tried to run the node several times and finally found the trivial problem.
The quality of GPS data was a problem. The published NavSat messages show status:-1 which represents No Fix. I tuned the GPS node and waited for it to convert to status:0 and then executed my launch file and it worked then. Thanks for help!
Asked by Manas Rawat on 2019-04-07 15:42:37 UTC
Comments
Can you add the messages you get in terminal as the node starts?
Asked by billy on 2019-04-01 14:19:07 UTC
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.
Asked by Manas Rawat on 2019-04-01 14:38:48 UTC
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:
Asked by billy on 2019-04-01 15:56:23 UTC