0
$\begingroup$

Rosanswers logo

Hello Tom/All,

I am currently using robot_pose_ekf to fuse wheel odometry and imu data on a custom robot with two tracks. This is wokring pretty well (works perfectly with gmapping and the navigation stack). I am now in the process of adding GPS into the mix, which would allow me to do some outdoor navigation experiments.

As adding GPS as sensor source is not officially supported by robot_pose_ekf, so I was on the lookout for an alternative solution. On paper, I found that robot_localization is exactly what is was looking for. Unfortunately, I am facing issues getting it to work.

My Setup

  1. odom0 is coming from by custom base_controller on topic /wheel_odom as nav_msgs/Odometry. It provides the following data: x, y, yaw, x velocity, yaw velocity. All remaining fields included in nav_msgs/Odometry are 0's.
  2. odom1 is coming from a gps_common/utm_dodmetry_node as nav_msgs/Odometry. It provides the following data: x, y and z. All remaining fields included in nav_msgs/Odometry are 0's.
  3. imu0 is coming from a XSens IMU (lse_xses_mti driver) on topic /imu_data as sensor_msgs/Imu. It provides the following data: roll, pitch and yaw angles. All remaining fields of sensor_msgs/Imu are 0's.

For now, the robot is meant to operate on the plane only. Hence, we can deduce that z is always 0, and so are the roll and pitch angles. My launch file below takes that into account.

My robot_localization launch file

<launch>  
  <!-- Launch robot_localization node -->
  <node pkg="robot_localization" type="ekf_localization_node" name="robot_localization" >
    <remap from="set_pose" to="/robot_localization/set_pose"/>           
    <remap from="odometry/filtered" to="/robot_localization/odom_combined"/>
    
    <param name="odom_frame" value="odom_combined"/>
    <param name="base_link_frame" value="base_footprint"/>    
    
    <!-- =============================================================================== -->    
    <!-- Configure odom0 (WHEEL_ODOM) -->
    <param name="odom0" value="/wheel_odom" />
    <rosparam param="odom0_config">[true,  true,  false,    <!-- x, y, z position -->
                                    false, false, true,    <!-- roll, pitch, yaw angles-->
                                    true,  true,  false,   <!-- x/y/z velocity -->
                                    false, false, true]    <!-- roll/pitch/yaw velocity -->
    </rosparam>
    <rosparam param="odom0_differential">[false,  false,  false,   <!-- x, y, z position -->
                                          false,  false,  false]   <!-- roll, pitch, yaw angles-->
    </rosparam>    
    <!-- =============================================================================== -->    
    <!-- Configure odom1 (GPS_ODOM) -->
    <param name="odom1" value="/utm_odometry_node/gps_odom"/>
    <rosparam param="odom1_config">[true,  true,  false,      <!-- x, y, z position -->
                                    false, false, false,   <!-- roll, pitch, yaw angles-->
                                    false, false, false,   <!-- x/y/z velocity -->
                                    false, false, false]   <!-- roll/pitch/yaw velocity -->
    </rosparam>
    <rosparam param="odom1_differential">[false, false, false,  <!-- x, y, z position -->
                                          false, false, false]  <!-- roll, pitch, yaw angles-->
    </rosparam>
    <!-- =============================================================================== -->    
    <!-- Configure imu0 (XSENS)-->
    <param name="imu0" value="/imu_data"/>               
    <rosparam param="imu0_config">[false, false, false,  <!-- x, y, z position -->
                                   false, false, true,   <!-- roll, pitch, yaw angles-->
                                   false, false, false,  <!-- x/y/z velocity -->
                                   false, false, false]  <!-- roll/pitch/yaw velocity -->
    </rosparam>
    <rosparam param="imu0_differential">[false, false, false,   <!-- x, y, z position -->
                                         false, false, false]   <!-- roll, pitch, yaw angles -->
    </rosparam>
    <!-- =============================================================================== -->    
  </node>

</launch>

The Issue

The problem is that the yaw angle of /robot_localization/odom_combined is jumping erratically as long as data is coming in on the /imu_data topic. (yaw on /imu_data is rock solid however).

When I disable the yaw-angle-component of odom0 (/wheel_odom), the problem disappears. I have also been experimenting with different parameter combinations. No matter what I try, if odom0/yaw and imu0/yaw are enabled at the same time, the jumping occurs on /odom_combined.

"@Tom Moore": Any advice? What I am doing wrong?

Cheers


Update

Looking at the debug output file of robot_localization, I found a few errors:

...
------ EkfNavigation::preparePose (imu0_pose) ------
Transform from /imu->base_footprint failed for topic imu0_pose. Ignoring pose measurement.
Last message time for imu0 is now 1400253327.488702916
---
------ EkfNavigation::prepareTwist (odom0_twist) ------
Transform from /base_frame->base_footprint failed. Ignoring twist measurement.Last message time for odom0 is now 1400253328.236508206
...

Not sure why there transforms fail - the tf tree is perfect according to tf_monitor:

ros@base:~$ rosrun tf tf_monitor
 
RESULTS: for all Frames

Frames:
Frame: /base_frame published by /tf_base_footprint_TO_base_frame Average Delay: -0.012254 Max Delay: 0
Frame: /imu published by /tf_base_frame_TO_imu Average Delay: -0.0121784 Max Delay: 0
Frame: /laser_scanner published by /base_laser_tilt_TO_laser_scanner Average Delay: -0.0123668 Max Delay: 0
Frame: /laser_scanner_2 published by /tf_base_laser_2_TO_laser_scanner_2 Average Delay: -0.0121929 Max Delay: 0
Frame: base_footprint published by /robot_localization Average Delay: 0.00781267 Max Delay: 0.00946112
Frame: base_laser published by /robot_state_publisher Average Delay: -0.486066 Max Delay: 0
Frame: base_laser_2 published by /robot_state_publisher Average Delay: -0.486063 Max Delay: 0
Frame: base_laser_support published by /robot_state_publisher Average Delay: -0.48609 Max Delay: 0
Frame: base_laser_tilt published by /tilt_laser Average Delay: 0.0485597 Max Delay: 0.0646518
Frame: base_turret published by /robot_state_publisher Average Delay: -0.486084 Max Delay: 0
Frame: box_battery published by /robot_state_publisher Average Delay: -0.486082 Max Delay: 0
Frame: box_slam published by /robot_state_publisher Average Delay: -0.486079 Max Delay: 0
Frame: flipper_left_front published by /robot_state_publisher Average Delay: -0.486077 Max Delay: 0
Frame: flipper_left_rear published by /robot_state_publisher Average Delay: -0.486074 Max Delay: 0
Frame: flipper_right_front published by /robot_state_publisher Average Delay: -0.486071 Max Delay: 0
Frame: flipper_right_rear published by /robot_state_publisher Average Delay: -0.486069 Max Delay: 0
Frame: local_map published by /local_map_tf Average Delay: -0.0923122 Max Delay: 0
Frame: odom published by /slam_gmapping Average Delay: -0.0422509 Max Delay: 0

All Broadcasters:
Node: /base_laser_tilt_TO_laser_scanner 49.94 Hz, Average Delay: -0.0123668 Max Delay: 0
Node: /local_map_tf 10.1656 Hz, Average Delay: -0.0923122 Max Delay: 0
Node: /robot_localization 30.1847 Hz, Average Delay: 0.00781267 Max Delay: 0.00946112
Node: /robot_state_publisher 50.2335 Hz, Average Delay: -0.486075 Max Delay: 0
Node: /slam_gmapping 20.1886 Hz, Average Delay: -0.0422509 Max Delay: 0
Node: /tf_base_footprint_TO_base_frame 49.9392 Hz, Average Delay: -0.012254 Max Delay: 0
Node: /tf_base_frame_TO_imu 49.9509 Hz, Average Delay: -0.0121784 Max Delay: 0
Node: /tf_base_laser_2_TO_laser_scanner_2 49.9511 Hz, Average Delay: -0.0121929 Max Delay: 0
Node: /tilt_laser 40.1991 Hz, Average Delay: 0.0485597 Max Delay: 0.0646518

Originally posted by Huibuh on ROS Answers with karma: 399 on 2014-05-16

Post score: 1


Original comments

Comment by ahendrix on 2014-05-16:
Can you run tf view_frames and include that in your question as well?

Comment by Tom Moore on 2014-05-16:
Also, can you verify that you get the transform failure for all measurements, or is it just the first one or two?

Comment by Huibuh on 2014-05-16:
When looking at the log file of 10 seconds of operation, I maybe get 3-5 transform failures. So I suppose only some tf's fail.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

So your odom0 topic is providing yaw and yaw velocity? My guess is that whatever is producing that message is integrating the yaw velocities to produce a yaw estimate. robot_localization is already handling that integration, so you should remove yaw from your odom0 measurement. My guess is that what's happening is your odometry's yaw measurement starts to disagree strongly with your IMU's yaw measurement after a few turns, and so you'll see oscillations back and forth as the measurements come in.

EDIT: I see you've already tried excluding yaw from odom0 and it worked. This leads me to believe what I said is true.

A second possibility (and one that I don't really recommend) would be for you to make sure that your yaw variance value for odom0 is growing as the robot makes more turns. Unlike the IMU, the error on your odometry's yaw estimate should grow every time you get nonzero yaw velocity. The IMU, on the other hand, will have a more or less fixed error on its yaw measurements. Eventually, odom0's yaw variance will be so poor that it will be effectively ignored by the filter (though this means there isn't much value in including it anyway).

SECOND EDIT: @Huibuh was kind enough to send me a bag file, and I have two things to add:

To stop the oscillation when the robot first starts, you should change the launch by making the yaw measurements differentially integrated:

<!-- Configure imu0 (XSENS)-->
<param name="imu0" value="/imu_data"/>               
<rosparam param="imu0_config">[false, false, false,  <!-- x, y, z position -->
                               false, false, true,   <!-- roll, pitch, yaw angles-->
                               false, false, false,  <!-- x/y/z velocity -->
                               false, false, false]  <!-- roll/pitch/yaw velocity -->
</rosparam>
<rosparam param="imu0_differential">[false, false, false,   <!-- x, y, z position -->
                                     false, false, true]   <!-- THIS CHANGED! Yaw is now true -->
</rosparam>

Since the robot's yaw measurement from odometry starts at 0, we don't have to set it to true there (though it wouldn't hurt). However, the IMU will start at a non-zero heading. robot_localization allows you to integrate measurements differentially, so the first measurement becomes a "zero point" and subsequent measurements are made relative to that first measurement. As I recall, this is robot_pose_ekf's default behavior (someone correct me if I'm wrong), which is why you didn't see the issue when you used it.

However, my earlier point still stands: as the robot moves, the yaw odometry will start to disagree with the IMU, even if they both start out at 0. For that reason, I'd stick with absolute yaw from the IMU only, and yaw velocity from both the IMU (if you have it) and odometry.

There are other suggestions I'd make for your launch file, but the gist of them are covered in robot_localization's best practices tutorial.


Originally posted by Tom Moore with karma: 13689 on 2014-05-16

This answer was ACCEPTED on the original site

Post score: 3


Original comments

Comment by Huibuh on 2014-05-16:
It makes sense what you are saying. If I set the yaw parameter for odom0 to false, there are no oscillations any more. However, my IMU does not have an absolute reference for yaw (I cannot use the included mag sensors as they won't work next to the big electric drive motors of the robot/won't work reliably indoors), the IMU yaw value will drift over time. By also including yaw from odom0 as measurement source for robot_localization, I was hoping to counteract the IMU gyro drift at least while the robot is stationary.

Comment by Tom Moore on 2014-05-16:
Does your IMU produce yaw velocity? If it does, then in your case, I would integrate yaw velocity from both the IMU and odometry, and then exclude all yaw measurements from all sensors. If your absolute yaw measurement can't be trusted, then your best bet is to integrate velocities.

Comment by Tom Moore on 2014-05-16:
Also, if you have access to the node that generates the odometry, you could try to change the variance on your yaw velocity to a very (very) small number whenever the robot is stationary. This is clearly not ideal, but in this case, it could aid in the drift.

$\endgroup$

Your Answer

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