I've been trying to fuse gps data with a odometry and an imu for a few days now and for some reason cannot get the navsat_transform_node to work. I plot the odometry/gps topic right next to the raw converted gps data from the receiver and the odometry/gps will randomly start slowly shooting off to hundreds of meters while the raw data stays in a nice small area.

Am I doing anything obviously wrong? I just can't seem to figure it out.

EDIT: i forced the imu and odometry to do nothing and only looked at the gps data, and here is what happens. The raw gps data is on the bottom and stays in a nice area, and the odometry/gps topic is on top, and randomly drifts off into the distance.

EDIT 2: Still not entirely sure if I've solved the issue, but one problem I am having is that if I use the example typical navsat transform and two ekf nodes approach I get the random drift off into the distance. I then change the world frame of the second ekf that combines (odometry/gps,odom,and imu/data) to odom and it starts to work... I still have plenty of issues, but the random drift is gone. I'm not sure I entirely understand why the second ekf has a different world frame then the first anyway...

My launch file is here:

<launch>

<node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 base_link imu" />
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_gps" args="0 0 0 0 0 0 base_link gps" />

<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
<param name="zero_altitude" value="true"/>
<param name="publish_filtered_gps" value="true"/>
<param name="wait_for_datum" value="false"/>
<remap from="/odometry/filtered" to="/odometry/filtered_local"/>
</node>

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_local" clear_params="true">

<param name="two_d_mode" value="true"/>

<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="world_frame" value="odom"/>

<param name="odom0" value="odom"/>
<param name="imu0" value="/imu/data"/>

<rosparam param="odom0_config">[false,false,false,
false,false,false,
true,false,false,
false,false,true,
false,false,false]</rosparam>

<rosparam param="imu0_config">[false,false,false,
false,false,true,
false,false,false,
false,false,true,
false,false,false]</rosparam>

<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>

<param name="imu0_relative" value="true"/>
<param name="odom0_relative" value="false"/>

<param name="imu0_remove_gravitational_acceleration" value="false"/>
<remap from="/odometry/filtered" to="/odometry/filtered_local"/>

</node>


</launch>

This is my gps/fix topic:

header:
seq: 1018
stamp:
secs: 1492713144
nsecs: 431811094
frame_id: gps
status:
status: 0
service: 1
latitude: 40.7624
longitude: 73.9738
altitude: 0.0
position_covariance: [2.1315999999999997, 0.0, 0.0, 0.0, 2.1315999999999997, 0.0, 0.0, 0.0, 8.526399999999999]
position_covariance_type: 1


My imu/data topic:

header:
seq: 134445
stamp:
secs: 1492713512
nsecs: 637841939
frame_id: imu
orientation ...
edit retag close merge delete