Robotics StackExchange | Archived questions

navsat_transform_node map frame wrong orientation unless facing east

Hey everyone,

I am currently running a differential drive setup with an RTK gps, IMU and wheel odometry. I am using the robot_localization ROS as a dual node setup as advised here for fusing GPS data.

As i understand the map and odom frame's are world fixed and start and the robot's initial pose. My problem is, the map frame only orientates correctly if the robot starts off facing EAST (with a yaw_offset of 0.0). If i start in any other orientation upon driving around a little bit, the map frame rotates on the spot and there is an offset between the odom frame and map frame. This is an issue as i am performing GPS waypoint navigaiton however if the map frame is in the wrong orientation, the transformed pose from UTM coords to the map frame also has the wrong orientation so the robot end's up going in the complete wrong direction!

Below is a screenshot of the map pose with the robot starting in an orientation that was not east:

image description

EKF PARAMS

ekf_se_odom:
  frequency: 50
  two_d_mode: true
  sensor_timeout: 0.1
  transform_time_offset: 0.0
  transform_timeout: 0.0
  print_diagnostics: true
  debug: false

  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

  odom0: /warthog_velocity_controller/odom
  odom0_config: [false, false, false,
                 false, false, false,
                 true, true, false,
                 false, false, true,
                 false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false

  # imu fused using madgwick imu filter

  imu0: /imu/data
  imu0_config: [false, false, false,
                false, false, false,
                false, false, false,
                false, false, true,
                true, true, false]
  imu0_differential: false
  imu0_nodelay: false
  imu0_relative: false
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: false

  use_control: false

  # process_noise_covariance: [1e-3, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  #                            0,    1e-3, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  #                            0,    0,    1e-3, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  #                            0,    0,    0,    0.3,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  #                            0,    0,    0,    0,    0.3,  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.5,   0,     0,    0,    0,    0,    0,    0,    0,
  #                            0,    0,    0,    0,    0,    0,    0,     0.5,   0,    0,    0,    0,    0,    0,    0,
  #                            0,    0,    0,    0,    0,    0,    0,     0,     0.1,  0,    0,    0,    0,    0,    0,
  #                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0.3,  0,    0,    0,    0,    0,
  #                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.3,  0,    0,    0,    0,
  #                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.3,  0,    0,    0,
  #                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.3,  0,    0,
  #                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.3,  0,
  #                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.3]
  #
  # 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,    1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
  #                               0,    0,    0,    0,    1.0,  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,    1.0,  0,    0,    0,     0,     0,     0,    0,    0,
  #                               0,    0,    0,    0,    0,    0,    0,    1.0,  0,    0,     0,     0,     0,    0,    0,
  #                               0,    0,    0,    0,    0,    0,    0,    0,    1.0,  0,     0,     0,     0,    0,    0,
  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    1.0,   0,     0,     0,    0,    0,
  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1.0,   0,     0,    0,    0,
  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1.0,  0,    0,
  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1.0,  0,
  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1.0]

ekf_se_map:
  frequency: 50
  sensor_timeout: 0.1
  two_d_mode: true
  transform_time_offset: 0.0
  transform_timeout: 0.0
  print_diagnostics: true
  debug: false
  debug_out_file: "/home/alec/debug_ekf.txt"

  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: map

  odom0: /warthog_velocity_controller/odom
  odom0_config: [false, false, false,
                 false, false, false,
                 true, true, false,
                 false, false, true,
                 false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false

  odom1: /bunkbot_localization/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: /imu/data
  imu0_config: [false, false, false,
                false, false, false,
                false, false, false,
                false, false, true,
                true, true, false]
  imu0_differential: false
  imu0_nodelay: true
  imu0_relative: false
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: false

  use_control: false

  # process_noise_covariance: [1.0,  0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  #                            0,    1.0,  0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  #                            0,    0,    1e-3, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  #                            0,    0,    0,    0.3,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  #                            0,    0,    0,    0,    0.3,  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.5,   0,     0,    0,    0,    0,    0,    0,    0,
  #                            0,    0,    0,    0,    0,    0,    0,     0.5,   0,    0,    0,    0,    0,    0,    0,
  #                            0,    0,    0,    0,    0,    0,    0,     0,     0.1,  0,    0,    0,    0,    0,    0,
  #                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0.3,  0,    0,    0,    0,    0,
  #                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.3,  0,    0,    0,    0,
  #                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.3,  0,    0,    0,
  #                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.3,  0,    0,
  #                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.3,  0,
  #                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.3]
  #
  # initial_estimate_covariance: [1.0,  0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
  #                               0,    1.0,  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,    1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
  #                               0,    0,    0,    0,    1.0,  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,    1.0,  0,    0,    0,     0,     0,     0,    0,    0,
  #                               0,    0,    0,    0,    0,    0,    0,    1.0,  0,    0,     0,     0,     0,    0,    0,
  #                               0,    0,    0,    0,    0,    0,    0,    0,    1.0,  0,     0,     0,     0,    0,    0,
  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    1.0,   0,     0,     0,    0,    0,
  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1.0,   0,     0,    0,    0,
  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1.0,  0,    0,
  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1.0,  0,
  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1.0]

NAVSAT PARAMS

navsat_transform:
  broadcast_utm_transform: true
  delay: 3.0
  frequency: 50
  magnetic_declination_radians: 0.19146262 # Set this depdending on origin location in the world
  publish_filtered_gps: true
  use_odometry_yaw: false
  wait_for_datum: false
  yaw_offset: 0.0
  zero_altitude: true

I will also note, i have tried many orientations of yawoffset, if i start the robot in some random orientation X that is not EAST, and then find the yawoffset value that the map frame aligns correctly and does not rotate once driving around, this will work however, as soon as i start the robot in any different orientation Y, the map orientation is completely off again.

Also in case this is an issue regarding the way i am transforming UTM to map pose, this is currently how i am doing it:

def convert_UTMPose_to_MapPose(UTMPoint):
    UTMPose = convert_UTMPoint_to_PoseStamped(UTMPoint)
    MapPose = PoseStamped()
    tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) #tf buffer length
    listener = tf2_ros.TransformListener(tf_buffer)
    notDone = True

    while notDone:
        try:
            transform = tf_buffer.lookup_transform("odom", "utm", rospy.Time(0), rospy.Duration(1.0))
            MapPose = tf2_geometry_msgs.do_transform_pose(UTMPose, transform)
            notDone = False
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
            print e
            rospy.sleep(1)
            continue

    #print MapPose

    return MapPose

Any help would be greatly appreciated!

UPDATE

I'm wondering if this issue is IMU related, i see strange behaviour out of my filtered imu data which uses the madgwick filter to fuse gryo, accel and mag data into an orientation, the bellow gif show's this behaviour, you can notice that the yaw (orientation z) does change when i rotate the robot, however when stationary it always "drifts" back to some random value?

image description image description

Asked by agurman on 2019-04-07 23:36:44 UTC

Comments

The IMU drifting to some random direction seems like it might be caused by using the magnetometer as part of the IMU orientation. Getting the magnetometer to output information that is usable would require quite a bit of work as there are changing magnetic fields on the robot you are using and in the environment most likely. So I would most likely start by not using the magnetometer as input at first.

The issue with the map orientation being correct only when starting your robot up facing east comes from the fact that you can't know the initial orientation of your robot from any one GPS reading. So it defaults to 0. If you want correct absolute heading you will need to get it from somewhere.

You say you are using RTK gps. It should be possible to get a somewhat accurate absolute heading estimation quite quickly if you drive straight for some time. Then you can apply the yaw offset you derived like that to the world transform.

Asked by Reamees on 2019-04-08 03:15:49 UTC

By getting the accurate heading estimation I mean filtering your gps readings in some node and calculating it based on the readings.

Asked by Reamees on 2019-04-08 03:33:59 UTC

Thanks for the help, after running some test's with another IMU placed in a different position i have been able to see that where the current IMU is placed, the magnetometer is getting significant interference. I am now setting up a gnss heading receiver to provide the accurate earth referenced heading, and not relying on the imu's magnetometer.

Asked by agurman on 2019-04-09 20:49:50 UTC

Can you also please include a sample message for each sensor input?

Asked by Tom Moore on 2019-05-15 04:06:29 UTC

Has this been solved? If so, can you please update or close the question?

Asked by Tom Moore on 2019-08-22 06:25:58 UTC

Answers