Fusing GPS, IMU and odom data

edit

Hi, I am trying to fuse the GPS and IMU data using robot_localization package. I have configured the nodes as mentioned here.

I am running

1. one instance of ekf_localization_node which provides output on topic /odometry/filtered.
2. one instance for navsat_transform_node which takes input from these 3 topics - /odometry/filtered, /imu/data and /fix.

Now at this point I should get the output on /odometry/gps but there is nothing published on this topic. I know that in order to fuse the GPS, IMU and odom data together I need to run one more instance of ekf_localization_node but since I am not getting any output from navsat_transform_node I haven't proceeded further.

For testing this out I am using the test1.bag provided with robot_localization package inside the test folder.

This is my launch file -

 <launch> 
        <node pkg="robot_localization" type="ekf_localization_node" name="ekf_1" clear_params="true">
        <rosparam command="load" file="$(find robot_localization)/params/GPS_IMU.yaml" />
         <!--  Placeholder for output topic remapping
        <remap from="odometry/filtered" to=""/>
        <remap from="accel/filtered" to=""/>
        -->
      </node>

      <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true">
        <rosparam command="load" file="$(find robot_localization)/params/navsat_transform_GPS_IMU.yaml" />
        <!-- Placeholders for input remapping. Set your topic names as the "to" values.
        <remap from="odometry/filtered" to="odometry/filtered_map"/> -->
        <remap from="imu/data" to="/imu/data"/>  
        <remap from="gps/fix" to="/fix"/>              
      </node>

 </launch>

Here is the YAML file -

frequency: 30
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
debug_out_file: /path/to/debug/file.txt
publish_tf: true
publish_acceleration: false

map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified


odom0: /husky_velocity_controller/odom


odom0_config: [true,  true,  false,
               false, false, false,
               false, false, false,
               false, false, true,
               false, false, false]

odom0_queue_size: 2
odom0_nodelay: false
odom0_differential: false
odom0_relative: false
odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1

imu0: /imu/data
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8                #
imu0_linear_acceleration_rejection_threshold: 0.8  #

imu0_remove_gravitational_acceleration: true

use_control: true
stamped_control: false
control_timeout: 0.2
control_config: [true, false, false, false, false, true]
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]


process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0,  0, 0, 0, 0, 0, 0, 0,
                           0, 0.05, 0, 0, 0, 0, 0,  0,  0,  0, 0, 0, 0, 0, 0,
                           0, 0, 0.06, 0, 0, 0, 0,  0,  0,  0, 0, 0, 0, 0, 0,
                           0, 0, 0, 0.03, 0, 0, 0,  0,  0,  0, 0, 0, 0, 0, 0,
                           0, 0, 0, 0, 0.03, 0, 0,  0,  0 ...