0
$\begingroup$

Rosanswers logo

Hi, I have a bag file which has IMU, GPS and wheel odometry and I'm trying to fuse it through robot_localization. I have followed everything as per the document. the ekf node is working fine but there is no output on the topic odometry/gps. I am manually tuning the covariance values for all the three sensors. For this I have a small code which runs above the bag file and just adds up the values in covariance matrix.

Please find the attached launch file and config file . Also, I am pasting few messages for IMU, GOS and odometry

IMU:

 header: 
   seq: 122
   stamp: 
     secs: 1531127130
     nsecs: 412077068
   frame_id: "imu_link"
  orientation: 
    x: 0.01904296875
    y: -0.0274047851562
    z: -0.852172851562
    w: 0.522216796875
  orientation_covariance: [24.0, 0.0, 0.0, 0.0, 24.0, 0.0, 0.0, 0.0, 24.0]
  angular_velocity: 
    x: 2.375
    y: -0.875
    z: 2.75
  angular_velocity_covariance: [24.0, 0.0, 0.0, 0.0, 24.0, 0.0, 0.0, 0.0, 24.0]
  linear_acceleration: 
    x: -0.270000010729
    y: 1.48000001907
    z: -1.27999997139
  linear_acceleration_covariance: [24.0, 0.0, 0.0, 0.0, 24.0, 0.0, 0.0, 0.0, 24.0]

GPS:

header: 
  seq: 8
  stamp: 
    secs: 1531127123
    nsecs:  87058067
  frame_id: "gps_link"
status: 
  status: 0
  service: 1
latitude: 50.72745
longitude: 7.087071
altitude: 110.3
position_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
position_covariance_type: 1

Odometry:

header: 
  seq: 633
  stamp:
    secs: 1531127147
    nsecs: 190536275
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 8.16897660416
      y: -7.75282666754
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.6552952454
      w: 0.755372849231
  covariance: [0.375, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.375, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.375, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.375, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.375, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.375]
twist: 
  twist: 
    linear: 
      x: 0.866949711312
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0533507514654
  covariance: [0.01, 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.01, 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.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]

Config file:

# https://github.com/cra-ros-pkg/robot_localization/blob/kinetic-devel/params/dual_ekf_navsat_example.yaml

ekf_se_odom:
    frequency: 30
    sensor_timeout: 0.1
    two_d_mode: true
    
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom
    
    odom0: odometry/cov
    odom0_config: [false, false, false,    #X,Y,Z
                 false, false, true,       #roll,pitch,yaw
                 true,  true,  false,      #X˙,Y˙,Z
                 false, false, true,       #roll˙,pitch˙,yaw 
                 false, false, false]      #X¨,Y¨,Z
    odom0_differential: false
    odom0_queue_size: 10
                    
    imu0: imu/cov
    imu0_config: [false, false, false,
                false,  false,  true,
                false, false, false,
                false,  false,  true,
                true,  false,  false]
    imu0_differential: false
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: false

    use_control: false

ekf_se_map:
    frequency: 30
    two_d_mode: true
    
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: map
    
    odom0: odometry/cov
    odom0_config: [false, false, false,    #X,Y,Z
                 false, false, false,       #roll,pitch,yaw
                 true,  true,  false,      #X˙,Y˙,Z
                 false, false, true,       #roll˙,pitch˙,yaw 
                 false, false, false]      #X¨,Y¨,Z
    odom0_differential: false
    odom0_queue_size: 10
    
    odom1: odometry/gps
    odom1_config: [true,  true,  false,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
    odom1_differential: false
    odom1_queue_size: 10
                    
    imu0: imu/cov
    imu0_config: [false, false, false,
                false,  false,  false,
                false, false, false,
                false,  false,  false,
                false,  false,  false]
    imu0_differential: false
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: false

    use_control: false

navsat_transform:
    frequency: 30
    delay: 3.0
    magnetic_declination_radians: 0.034732052 # For lat/long 50.727246, 7.086934
    yaw_offset: 2.35619449019 # IMU reads 0 facing magnetic north, not east
    broadcast_utm_transform: flase
    zero_altitude: true
    publish_filtered_gps: false
    use_odometry_yaw: false
    wait_for_datum: false

LAUNCH FILE


Originally posted by pk11 on ROS Answers with karma: 13 on 2018-08-15

Post score: 1

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Two main comments:

  1. Don't fuse yaw from your wheel odometry and your IMU in the odom EKF. Just use the data from the IMU. As you drive around, those values will diverge, and the filter will jump between them.
  2. Your GPS data is in the gps_link frame, and your IMU data is in the imu_link frame, but your launch file doesn't seem to provide any transforms from base_link->imu_link or base_link->gps_link. See the "Common errors" section of the wiki:

Velocity data should be reported in the frame given by the base_link_frame parameter, or a transform should exist between the frame_id of the velocity data and the base_link_frame.

In this case, even though the GPS and IMU both provide some pose data, you still need to provide transforms from base_link to the frame_ids in the messages. Your odom data is already in the odom and base_link frames, so that's fine.

But my general advice is to back up and do one thing at a time. Start with just a single odom EKF, and only fuse the wheel encoder data. When you're satisfied that it's working, add the IMU data. Then move on to the next EKF, and repeat. Then add navsat_transform_node, and repeat.


Originally posted by Tom Moore with karma: 13689 on 2018-08-27

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by pk11 on 2018-10-31:
Thanks..! that helped, there was no Tf, I just had to launch the robot description. Basic mistake..!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.