Ask Your Question
0

move_base strange behaviour when using earth referenced heading

asked 2019-04-16 01:47:43 -0600

agurman gravatar image

updated 2019-04-16 04:10:56 -0600

Hey guys,

I am performing gps waypoint navigation using move_base for the navigation side of things. My localization setup uses robot_localization i run two instances of this in order to fuse the gps, so first instance has wheel odom + IMU and second instance has wheel odom + IMU + GPS.

My IMU does not provide an earth referenced heading so i have a gnss heading receiver setup and i pipe the heading into a Pose message of the following format:

image description

I have two pose messages setup, one for the odom frame and one for the map frame both are the exact same but with different frame_id. I fuse the earth reference heading into both robot_localization nodes in the following way:

ekf_se_odom:
  frequency: 20
  two_d_mode: true
  sensor_timeout: 0.15
  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

  # GNSS HEADING POSE MESSAGE (ODOM)

  pose0: /gps/odometry_odom
  pose0_config: [false, false, false,
                 false, false, true,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  pose0_queue_size: 10
  pose0_nodelay: true
  pose0_differential: false
  pose0_relative: false

  imu0: /mcu_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

  use_control: false

ekf_se_map:
  frequency: 20
  sensor_timeout: 0.15
  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

  # GNSS HEADING POSE MESSAGE (MAP)

  pose0: /gps/odometry_map
  pose0_config: [false, false, false,
                 false, false, true,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  pose0_queue_size: 10
  pose0_nodelay: true
  pose0_differential: false
  pose0_relative: false

  odom2: /bunkbot_localization/odometry/gps
  odom2_config: [true,  true,  false,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom2_queue_size: 10
  odom2_nodelay: true
  odom2_differential: false
  odom2_relative: false

  imu0: /mcu_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

  use_control: false

The Problem Whenever i fuse in the earth referenced heading, record some GPS waypoints and attempt to navigate i get a very strange behaviour! see below:

image description

BUT: When i do not fuse the earth reference heading. the navigation works fine????? see below:

image description

Does anyone know why this would be happening? I remap the move_base odom topic to my ekf filtered output:

<remap from="odom" to="/bunkbot_localization/odometry/filtered_map" />

And i have checked this topic's output with both fused gnss heading and without and they seem to be fine?

UPDATE 1:

I have run the outputs of some bag files i recorded of a figure 8 movement of both robot_localization nodes and the gps odometry output of navsat_transform_node through matlab to ... (more)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-04-16 21:38:58 -0600

agurman gravatar image

I have solved this issue, My GNSS Heading is provided in terms of an angle between 0 and 360 degrees True North, i then cast this to a domain between pi and -pi for the Pose message. The way i was casting this angle to the new domain was incorrect and hence the gps odometry was showing an incorrect orientation which when fused with the ekf was applying that orientation to the ouput meaning the robot was getting lost.

edit flag offensive delete link more

Comments

hi,friends! what's the data of /gps/odometry_odom? i have the same problem.i use 9 aixs imu + GPS + odom,but i can not get the origin pose when the power on,Could you give me some suggestions?Thank you

cyysu gravatar imagecyysu ( 2019-11-25 01:02:03 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-04-16 01:47:43 -0600

Seen: 75 times

Last updated: Nov 25