ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

robot_localization: visual odom + imu + gps

asked 2021-05-05 19:06:43 -0500

ClintQ gravatar image

I'm running into some challenges with my robot_localization setup. I have visual odometry from a ZED2, IMU and GPS from an xsens MTi-710.

Goal: Configure my system as outlined in the navsat_transform_workflow diagram.

image description

Problem: When just running the visual odometry and imu in a single ekf instance things work as expected. When adding in the second ekf instance and the navsat transform things go sideways. My map to odom transform goes haywire and I get warnings and errors. This is a benchtop implementation, so minimal movement from visual odom or imu. There is some noise on the GPS signal, but not as much as is showing up in the transform between odom and map.

Launch file:

  <arg name="svo_file"             default="" /> <!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
  <arg name="stream"               default="" /> <!-- <arg name="stream" default="<ip_address>:<port>"> -->

  <arg name="zed_node_name"        default="zed_node" />
  <arg name="camera_model"         default="zed2" />
  <arg name="publish_urdf"         default="true" />

  <arg name="camera_name"          default="zed" />

  <arg name="base_frame"           default="base_link" />
  <arg name="rviz_file"            default="zed-rtabmap.rviz" />

  <include file="$(find zed_wrapper)/launch/include/zed_no_tf.launch.xml">
    <arg name="camera_name"         value="$(arg camera_name)" />
    <arg name="svo_file"            value="$(arg svo_file)" />
    <arg name="stream"              value="$(arg stream)" />
    <arg name="node_name"           value="$(arg zed_node_name)" />
    <arg name="camera_model"        value="$(arg camera_model)" />
    <arg name="base_frame"          value="$(arg base_frame)" />
    <arg name="publish_urdf"        value="$(arg publish_urdf)" />

  <include file="$(find xsens_mti_driver)/launch/display.launch" />

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true"/>
    <rosparam command="load" file="$(find robot_localization)/params/dual_ekf_navsat_qurrent.yaml" />

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
    <remap from="odometry/filtered" to="odometry/filtered_map"/>

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



  frequency: 30
  silent_tf_failure: false
  sensor_timeout: 0.1
  two_d_mode: false
  transform_time_offset: 0.2
  transform_timeout: 0.2
  print_diagnostics: true
  debug: false

  publish_tf: true
  publish_acceleration: true
  permit_corrected_publichation: false
  predict_to_current_time: true

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

  odom0: /zed_node/odom
  odom0_config: [true, true, true,
                 true, true, true,
                 false, false, false,
                 false, false, false,
                 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

  pose0: /example/pose
  pose0_config: [true,  true,  true,
                true,  true,  true,
                false, false, false,
                false, false, false,
                false, false, false]
  pose0_differential: true
  pose0_relative: false
  pose0_queue_size: 5
  pose0_rejection_threshold: 2  # Note the difference in parameter name
  pose0_nodelay: false

  imu0: /imu/data
  # imu0: /example/imu_data

# The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az.
  imu0_config: [false, false, false,
                true,  true,  false,
                false, false, false,
                false, false, false,
                true,  true,  true]
  imu0_nodelay: false
  imu0_differential: false
  imu0_relative: false
  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

  process_noise_covariance: [Using default ...
edit retag flag offensive close merge delete


First thing to do would be to check if there are any other sources that are publishing the same transformations that you are publishing with the ekf.

My guess is that the NaNs are coming from the GPS and that is were the problem must be.

Can you check how consistently /odometry/gps is being published?

turtleMaster20 gravatar image turtleMaster20  ( 2021-05-06 02:15:38 -0500 )edit

/odometry/gps is being published at 4Hz pretty consistently until it crashes and then I get:

average rate: 1.587
    min: 0.200s max: 2.033s std dev: 0.72066s window: 55
no new messages

I also noticed that in my tf tree, I lost the transform from map to odom and only have map to utm after the failure occurs. Is this due to the warning that I'm seeing?

[ WARN] [1620322379.574716691]: Transform from imu_link to base_link was unavailable for the time requested. Using latest instead.
ClintQ gravatar image ClintQ  ( 2021-05-06 12:35:11 -0500 )edit

Transform form map to odom should be published by ekf_se_odom, try to see if this node is still running.

The error you see can be ignored as imu to base_link should remain constant throughout.

Are you using these parameters per chance?

Check this link for proper setup instructions if you have not already.

One quick check could also be to use the ukf in the package. I believe you can use similar params (if not identical) to use the nodes.

turtleMaster20 gravatar image turtleMaster20  ( 2021-05-12 04:37:09 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2021-06-22 03:52:26 -0500

Tom Moore gravatar image

Please include one sample message from each sensor input. Even without that, though, I can make some observations.

  1. For starters, please disable all rejection_threshold parameters. Those are advanced parameters that should only be used after you have your system working more or less as you want it, and are just looking to squash the odd outlier.
  2. Is the VO data in full 3D? I assume so. If I were you, I would not fuse two absolute pose sources unless you know they will always agree. In this case, your VO data will slowly diverge from the IMU orientation, so I'd just use roll and pitch from one of those sources, and then, if the sensor provides it, fuse velocity data from the other. This is just in reference to your odom-frame EKF.
  3. In your map frame EKF, you are fusing absolute pose data from your GPS and your odometry source. Assuming your VO data is reported in the odom frame, then every time the EKF receives a measurement, it has to transform that data to the map frame (using the very transform the EKF is creating), and then fuse it absolutely. And then that pose has to agree with your GPS. VO data will drift over time, so those are going to diverge. In this EKF, I would fuse only velocity data from the VO source, if possible.
  4. All that aside, I would expect this to not behave well, but not produce NaNs. NaNs very often indicate issues with sensor data, like ill-formed covariances. This is why you need to include sample sensor messages.
edit flag offensive delete link more

Question Tools



Asked: 2021-05-05 19:06:43 -0500

Seen: 990 times

Last updated: Jun 22 '21