First time here? Check out the FAQ!


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

How to use robot_localization /odometry/filtered

asked Mar 18 '16

baronep gravatar image

updated Mar 18 '16

So I have two robot_localization nodes setup. The first fuses the vehicle odometry and the imu data. The second will add in GPS and hector_slam pose estimates.

My question is, should I fuse the output of the first node (/odometry/filtered) into the second node or should I re-fuse the vehicle odometry and imu data? If the former, the second (map) robot_localization node never publishes anything if I fuse the output of the first directly. (Also, the navsat node doesn't work with the /odometry/filtered data either).

Here is a sample /odometry/filtered message (output from the first node).

header: 
  seq: 235
  stamp: 
    secs: 1455846048
    nsecs: 782704923
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: 0.060692385786
      y: 8.51043448483e-06
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 7.45214107357e-05
      w: 0.999999997223
  covariance: [0.39148006559512866, -1.0221015344746652e-08, 0.0, 0.0, 0.0, 7.160358808761269e-08, -1.0221015344746746e-08, 0.39152977031124553, 0.0, 0.0, 0.0, 6.453229401104118e-05, 0.0, 0.0, 3.332912064876806e-07, 1.341899911334871e-24, 5.233156817556127e-19, 0.0, 0.0, 0.0, 1.3418999113348712e-24, 3.3324912207294784e-07, -1.6669589116712293e-32, 0.0, 0.0, 0.0, 5.233156817556124e-19, -1.6669590914679116e-32, 3.3324912207294784e-07, 0.0, 7.160358808761269e-08, 6.453229401104114e-05, 0.0, 0.0, 0.0, 0.0773158946732772]
twist: 
  twist: 
    linear: 
      x: -0.000285365096838
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 5.54621444365e-05
  covariance: [0.0019138416154638686, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0019138416154638686, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.332701589929017e-07, 1.9950504214512513e-31, 1.2764323815697058e-27, 0.0, 0.0, 0.0, 1.9950504214512495e-31, 3.330812024673111e-07, -1.34664748772665e-39, 0.0, 0.0, 0.0, 1.276432381569705e-27, -1.5229681057398406e-38, 3.330812024673111e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04978277263316132]

And here is my localization launch file ...

<!-- Launch file for ekf_localization_node -->

<!-- Layer 1 Localization: Odometry Frame -->
<launch>
    <node pkg="tf" type="static_transform_publisher" name="tach_odom123" args="0 0 0 0 0 0 1 odom tach_odom 100" />
    <node pkg="rsl_rover" type="imu_overrid_covariance.py" name="IOC" />
    <!--    <node pkg="rsl_rover" type="virt_yaw_sensor.py" name="VirtYaw" output="screen"/> -->
    <node pkg="imu_complementary_filter" type="complementary_filter_node" name="complementary_filter_node" >
    <remap from="imu/data_raw" to="imu/data_cov" />
    <remap from="imu/mag" to="imu/mag" />
    <remap from="imu/data" to="imu/data_filtered" />
    <param name="do_bias_estimation" value="true"/>
    <param name="do_adaptive_gain" value="true"/>
    <param name="use_mag" value="false"/>
    <param name="gain_acc" value="0.01"/>
    <param name="gain_mag" value="0.01"/>
    </node>

    <node pkg="robot_localization" type="ekf_localization_node" name="odom_localization" clear_params="true" output="screen">

      <param name="frequency" value="30"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="true"/>

      <param name="map_frame" value="map"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_link"/>
      <param name="world_frame" value="odom"/>

      <param name="transform_time_offset" value="0.0"/>

      <param name="odom0" value="/VehicleTach"/>
      <param name="imu0" value="imu/data_filtered"/>

      <rosparam param="odom0_config">[false, false, false,
                                      false, false, false,
                                      true,  true, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

                  <!--      <rosparam param="odom1_config ...
(more)
Preview: (hide)

1 Answer

Sort by » oldest newest most voted
1

answered May 10 '16

Tom Moore gravatar image

I don't recommend fusing the output of the odom->base_link instance into the map->odom instance. The main reason is that the output of the first EKF will be in the odom frame. To get it into the map from of the second EKF, it needs a map->odom transform, which is exactly what the second EKF is meant to be producing. You get into a chicken-or-egg problem. I usually just fuse the raw inputs (better to use velocities int his case).

I'd be careful with the threshold params. You can end up rejecting valid measurements if they're not tuned well.

Preview: (hide)

Question Tools

3 followers

Stats

Asked: Mar 18 '16

Seen: 5,134 times

Last updated: May 09 '16