0
$\begingroup$

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

https://drive.google.com/file/d/0B6qINUvvDwDfOGxYdVFvUlFMNzg/view?usp=sharing

My ekf_localization_node.launch

<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>
  </node>
</launch>

My rtabmap rgbd_odometry.launch

<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"/>
            </node>
    </group>
</launch>

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.

$\endgroup$

1 Answer 1

0
$\begingroup$

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

$\endgroup$

Your Answer

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