TF issue with robot_localization

Question

Hi, I'm fairly new to ROS. I'm trying to fuse wheel-odometry, GPS and IMU using robot_localization node. I'm attaching a rosbag which contains data published by the sensors. The car is essentially going in a circle. I've done sensor data publication exactly as specified by the robot_localization wiki. Yet the output of the robot_localization is incorrect and weird, it gives wrong Twist output data. I've already spent a week trying to solve this problem any help will be appreciated PS: data from all the sensors are pretty accurate, I've not added any noise to the sensors. The bagfile : link text TF tree :link text

Wheel-Odometry : child_frame_id = 'fusion/base_link' header.frame_id = 'fusion/base_link'

IMU: header.frame_id = 'base_footprint'

GPS: header.frame_id = 'fusion/base_link'

ekf_se_odom:
  frequency: 30
  sensor_timeout: 2
  two_d_mode: true
  transform_time_offset: 0.0
  transform_timeout: 0.0
  print_diagnostics: true
  debug: false

  map_frame: map
  odom_frame: odom
  base_link_frame: fusion/base_footprint
  world_frame: odom
  
  odom0: /fusion/odom
  odom0_config: [false, false, false,
                 true,  true,  true,
                 true,  false, false,
                 false, false, false,
                 false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false
 
  odom1: odometry/gps
  odom1_config: [true,  true,  false,
                 false, false, false,
                false, false, false,
                 false, false, false,
                 false, false, false]
  odom1_queue_size: 10
  odom1_nodelay: true
  odom1_differential: false
  odom1_relative: false

  imu0: fusion/imu/data_raw
  imu0_config: [false, false, false,
                false, false, false,
                false, false, false,
                true , true,  true,
                true,  true,  true]
  imu0_nodelay: true
  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: false
  stamped_control: false
  control_timeout: 0.2
  control_config: [true, false, false, false, false, true]
  acceleration_limits: [2, 0.0, 0.0, 0.0, 0.0, 3.4]
  deceleration_limits: [2, 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,    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.025, 0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]

  initial_estimate_covariance: [1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]
navsat_transform:
  frequency: 30
  delay: 3.0
  magnetic_declination_radians: 0.0429351  # For lat/long 55.944831, -3.186998
# yaw_offset: 1.570796327  # IMU reads 0 facing magnetic north, not east
  zero_altitude: false
  broadcast_utm_transform: true
  publish_filtered_gps: true
  use_odometry_yaw: true
  wait_for_datum: true
  datum: [45.0, -81.0, 0.0]


 <launch>
  
<node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/salahuddin/bagfiles/ekfdata.bag"/>
  
 <rosparam command="load" file="$(find dbw_mkz_gazebo)/params/ekf_localization.yaml" />

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true"/>
  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true">
    <remap from="odometry/filtered" to="odometry/filtered"/>
    <remap from="gps/fix" to="gps/data"/>
      
  </node>


</launch>

---

Originally posted by Salahuddin_Khan on ROS Answers with karma: 50 on 2017-12-11

Post score: 0

Answer 1

Please post full sample messages for every sensor input.

Just looking at your config and the frame_ids you posted, I can make the following recommendations:

1. Your odometry frame_id should not be in the base_link frame. The pose data should be in a world-fixed frame (e.g., odom). The child_frame_id is fine.
2. Get rid of the rejection thresholds. Those are advanced parameters that should be avoided until the system is behaving the way you want.

But I can't say much more without sample messages. Downloading and reviewing bags is time-consuming, so it would help me a great deal if you could post some sample messages.

---

Originally posted by Tom Moore with karma: 13689 on 2018-01-26

This answer was NOT ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by Salahuddin_Khan on 2018-03-14:
I apologise for posting a reply so late, I solved it after a few days of me posting question. Here's what I did.

1. I did change the frame_id of wheel odometry at later stage but since I was only fusing Longitudinal Velocity it worked then.2. Removed all the forward slashes from all the frame_id

Answer 2

It worked after I removed all the forward slashes from the frame_ids

---

Originally posted by Salahuddin_Khan with karma: 50 on 2018-03-14

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by Tom Moore on 2018-04-09:
Great! Glad you found a fix.