Ask Your Question
0

no output from navsat_transform_node when fusing GPS and imu data

asked 2019-04-01 12:50:16 -0500

Manas Rawat gravatar image

updated 2019-04-06 18:25:33 -0500

jayess gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Can you add the messages you get in terminal as the node starts?

billy gravatar imagebilly ( 2019-04-01 14:19:07 -0500 )edit

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.

Manas Rawat gravatar imageManas Rawat ( 2019-04-01 14:38:48 -0500 )edit

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:

[ INFO] [1532908652.683481047]: Datum (latitude, longitude, altitude) is (37.272746, -121.818616, 46.585000)
[ INFO] [1532908652.683769224]: Datum UTM coordinate is (604740.259728, 4125783.673495)
[ INFO] [1532908652.783517516]: Initial odometry pose is Origin: (0 0 0)
Rotation (RPY): (0, -0, 0)

[ INFO] [1532908652.783951393]: Corrected for magnetic ...
(more)
billy gravatar imagebilly ( 2019-04-01 15:56:23 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-04-07 15:42:37 -0500

Manas Rawat gravatar image

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!

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-04-01 12:50:16 -0500

Seen: 69 times

Last updated: Apr 07