navsat_transform_node bad data

asked 2017-04-20 13:52:33 -0500

lasersocks gravatar image

updated 2017-04-25 13:46:36 -0500

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...

gps data

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="magnetic_declination_radians" value=".138172"/>
  <param name="zero_altitude" value="true"/>
  <param name="publish_filtered_gps" value="true"/>
  <param name="broadcast_utm_transform" 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="base_link_frame" value="base_link"/>
  <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 ...
(more)
edit retag flag offensive close merge delete

Comments

I'm having the your same problem and I'm not able to solve it. Our configuration is the same, too. Hope someone will help!

Marcus Barnet gravatar imageMarcus Barnet ( 2017-04-23 14:48:22 -0500 )edit

Can you include the configuration for the second EKF, and maybe provide a bag file? Also, the two EKFs must have separate world frames, since tf does not allow two publishers of the same transform. Read up on tf and frame authorities for more information.

Tom Moore gravatar imageTom Moore ( 2017-07-07 07:20:17 -0500 )edit