Ask Your Question
0

robot_localization producing 180° rotation jumps only when static publishing map->odom

asked 2017-04-19 23:42:32 -0600

danielsnider gravatar image

updated 2017-04-20 09:12:18 -0600

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/0B6qI...

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/0B6qI...

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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.

danielsnider gravatar imagedanielsnider ( 2017-04-20 13:54:48 -0600 )edit

2 Answers

Sort by » oldest newest most voted
0

answered 2017-04-20 14:38:28 -0600

danielsnider gravatar image

updated 2017-04-20 14:48:43 -0600

Success! The problem goes away when I change the frame ID of my IMU message (published by https://github.com/romainreignier/rti... ) 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.

edit flag offensive delete link more
0

answered 2017-04-20 07:51:32 -0600

matlabbe gravatar image

Hi,

your static map transform should be /map->/odom, not /odom->/robot_base. If you don't have a node publishing /map->/odom (amcl or other localization/mapping node), you may just ignore /map->/odom and don't set map_frame parameter in robot_localization, from the doc:

Note If your system does not have a map_frame, just remove it, and make sure world_frame is set to the value of odom_frame.

See also note 2 about using robot_localization for odometry fusion:

If you are only fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set world_frame to your odom_frame value. This is the default behavior for the state estimation nodes in robot_localization, and the most common use for it.

cheers

edit flag offensive delete link more

Comments

Thanks. Sorry for the confusing typo... I pasted the wrong snippet. I accidentally wrote /odom->robot_base but I am really publishing /map->odom and the problem happens. I know that would have been a perfect answer, thank you, but can you give it a second thought with this cleared up please?

danielsnider gravatar imagedanielsnider ( 2017-04-20 09:24:03 -0600 )edit

Can you try commenting <param name="map_frame" value="map"/>?

matlabbe gravatar imagematlabbe ( 2017-04-20 11:52:34 -0600 )edit

No luck :-(. Btw map is the default value for map_frame param. Thanks for helping. Can I print any more info to debug?

danielsnider gravatar imagedanielsnider ( 2017-04-20 13:51:19 -0600 )edit

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.

danielsnider gravatar imagedanielsnider ( 2017-04-20 13:54:53 -0600 )edit

Let me investigate further. I'll post an update in an hour.

danielsnider gravatar imagedanielsnider ( 2017-04-20 14:27:50 -0600 )edit

Fixed it! See my posted answer.

Maybe you 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.

danielsnider gravatar imagedanielsnider ( 2017-04-20 14:48:20 -0600 )edit

Indeed, frame_id in IMU msg should not be map or odom, but something like imu_link, otherwise robot_localization will estimate the wrong TF. In general, when there is a frame flickering between two poses, it is because two nodes are publishing on the same TF. You may try tf_monitor to debug.

matlabbe gravatar imagematlabbe ( 2017-04-22 16:48:54 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-04-19 23:42:32 -0600

Seen: 386 times

Last updated: Apr 20 '17