robot_localization producing 180° rotation jumps only when static publishing map->odom
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 ...
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.