Rosanswers logo

I have configured world_frame=odom. When I statically publish TF map->odom my output of ekf_localization_node is jumping a lot. I've looked all over for the problem but I am stuck!

Here's an 8 second streamable video of the problem seen in rviz: https://drive.google.com/file/d/0B6qINUvvDwDfMzdWUG1CWWNlMm8/view?usp=sharing

When I comment out...

<node pkg="tf2_ros" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 1 map odom" />

...the problem goes away and I see good, stable, filtered odom.

My data bag of the problem


My ekf_localization_node.launch

  <!-- Problem is introduced when this is uncommented 
  <node pkg="tf2_ros" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 1 map odom" />

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_local" clear_params="true">
    <remap from="/odometry/filtered" to="/odometry/filtered_local"/>
    <param name="frequency" value="10"/>

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

    <!-- IMU -->
    <param name="imu0" value="/imu/data_fixed"/>
    <rosparam param="imu0_config">[false, false, false,
                                   true,  true,  true,
                                   false, false, false,
                                   false, false, false,
                                   false,  false,  false]</rosparam>

    <!-- VISUAL ODOMETRY -->
    <param name="odom0" value="/rgbd_odometry/odom"/>
    <rosparam param="odom0_config">[true, true, false,
                                    false, false, false,
                                    false, false, false,
                                    false, false, true,
                                    false, false, false]</rosparam>

My rtabmap rgbd_odometry.launch

    <node pkg="tf2_ros" type="static_transform_publisher" name="robot_base_to_camera_link" args="0.7 0 0.6 0 0 0 robot_base camera_link"/>
    <node pkg="tf2_ros" type="static_transform_publisher" name="camera_link_to_zed_actual_frame" args="0 0 0 0 0 0 camera_link zed_actual_frame"/>

    <!-- RGBD_ODOMETRY -->
    <group ns="rgbd_odometry">
            <node output="screen" type="rgbd_odometry" name="zed_odom" pkg="rtabmap_ros">
                <param name="frame_id" value="robot_base"/>
                <param name="publish_tf" value="false"/>
                <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
                <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
                <remap from="depth/image" to="/camera/depth/depth_registered"/>
                <param name="Odom/ResetCountdown" value="1"/>

My roswtf

dan@ubuntu:~/URC$ roswtf
Loaded plugin tf.tfwtf
No package or stack in context
Static checks summary:

No errors or warnings
Beginning tests of your ROS graph. These may take awhile...
analyzing graph...
... done analyzing graph
running graph rules...
... done running graph rules
running tf checks, this will take a second...
... tf checks complete

Online checks summary:

Found 2 warning(s).
Warnings are things that may be just fine, but are sometimes at fault

WARNING The following node subscriptions are unconnected:
 * /ekf_localization_local:
   * /set_pose
 * /rqt_gui_py_node_8452:
   * /statistics

WARNING The following nodes are unexpectedly connected:
 * /rgbd_odometry/zed_odom->/rviz_1492655609198267406 (/tf)
 * /rgbd_odometry/zed_odom->/roswtf_9931_1492660614427 (/tf)
 * /robot_base_to_camera_link->/rviz_1492655609198267406 (/tf_static)
 * /map_to_odom->/rviz_1492655609198267406 (/tf_static)
 * /camera_link_to_zed_actual_frame->/rviz_1492655609198267406 (/tf_static)

My bad TF (when publishing static map->odom transform)

$ rosrun tf tf_echo robot_base odom
At time 1492662864.848                                  
- Translation: [0.001, -0.017, -0.038]                  
- Rotation: in Quaternion [0.639, -0.283, -0.638, 0.324]
            in RPY (radian) [1.541, 0.684, -1.535]      
            in RPY (degree) [88.289, 39.178, -87.969]   

My good TF (when NOT publishing static map->odom transform)

$ rosrun tf tf_echo robot_base odom
At time 1492661532.036                                   
- Translation: [0.016, -0.004, 0.000]                    
- Rotation: in Quaternion [-0.000, -0.000, 0.001, 1.000] 
            in RPY (radian) [-0.000, -0.000, 0.003]      
            in RPY (degree) [-0.000, -0.000, 0.154]      

I'm using Ubuntu 16.04 on a TX1.

Here's my version of robot_localization

commit 3c7521fb109fcd0fe6090ae8ae407b031838e80d
Merge: fe2f10e cd86690
Author: Tom Moore <[email protected]>
Date:   Mon Mar 27 19:45:55 2017 +0100

    Merge pull request #355 from clearpathrobotics/fix_gains_defaults
    Fix acceleration and deceleration gains default logic

I appreciate any help! Thanks!

Fixed typo: my problem transform IS map -> odom

Originally posted by danielsnider on ROS Answers with karma: 203 on 2017-04-19

Post score: 0

Original comments

Comment by danielsnider on 2017-04-20:
UPDATE: When I remove the IMU section from robot_localization the problem goes away. The IMU is not moving though, you can see when I visualize the IMU in the video link I posted, it's the yellow box.


1 Answer 1


Rosanswers logo

Success! The problem goes away when I change the frame ID of my IMU message (published by https://github.com/romainreignier/rtimulib_ros) from map to something not contained in my TF tree.

Why tho?

Maybe someone can teach me why a frame of map on my IMU message caused my odom to jump around.

It is too bad rqt_tf_tree doesn't visualize this sort of problem. Maybe I'll think about how to improve on rqt_tf_tree.

Originally posted by danielsnider with karma: 203 on 2017-04-20

This answer was ACCEPTED on the original site

Post score: 0


Your Answer

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