robot_localization 2nd instance not publishing map->odom transform
Hi all,
I'm trying to use robot_localization in Groovy to fuse information from several sensors (IMU, turn rate sensor and GPS).
Following the tutorials from robot_localization ( http://wiki.ros.org/robot_localization ) and GPS integration ( http://wiki.ros.org/robot_localizatio... ) I have setup two instances of robot_localization. One fuses IMU and turn rate sensor data, and the second one fuses IMU, turn rate and GPS. world_frame
of the 1st instance is set to odom, and world_frame
of 2nd one is set to map.
Both nodes seems to work fine and publish their respective odometries. However, according to the documentation, the 2nd instance should be publishing the map-->odom TF, but in my case, it publishes the TF odom-->base_link, just like the first instance.
There must be something I'm missing, but I'm unable to find it. Any insight in the posible cause will be appreciated.
Thank you and best regards.
Solution:
As pointed in the answers, I was using an outdated version of robot_localization
. Worked fine after getting the source from the hydro-devel branch and compiling it.
For compiling it in Groovy, I had also to uninstall gps_umd
meta-package and install it manually from source, since robot_localization gave a compilation error.
CMake Error at /opt/ros/groovy/share/catkin/cmake/catkinConfig.cmake:75 (find_package): Could not find a configuration file for package gps_common.
Link to rosrun tf view_frames output:
Launch file:
<launch>
<!-- IMU localization-->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_imu" respawn="true" output="screen">
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_footprint"/>
<param name="world_frame" value="odom"/>
<param name="imu0" value="imu/imu"/>
<rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, false, false, false]</rosparam>
<param name="imu0_differential" value="false"/>
<param name="imu1" value="imu/pseaa"/>
<rosparam param="imu1_config">[false, false, false, true, true, true, false, false, false, false, false, true]</rosparam>
<param name="imu1_differential" value="true"/>
<param name="twist0" value="imu/twist"/>
<rosparam param="twist0_config">[false, false, false, false, false, false, true, true, false, false, false, false]</rosparam>
<param name="twist0_differential" value="false"/>
<remap from="/odometry/filtered" to="/odometry/imu"/>
</node>
<!--robot_localization: utm_transform_node, ekf_localization_node -->
<node pkg="robot_localization" type="utm_transform_node" name="utm_transform_node" respawn="true" output="screen">
<!-- Compass reports magnetic north-->
<param name="yaw_offset" value="0"/>
<!-- Magnetic declination in Pasajes: -0º 49' W (http://magnetic-declination.com)-->
<param name="magnetic_declination_radians" value="-0.014253522"/>
<remap from="/imu/data" to="imu/imu" />
<remap from="/gps/gps_utm" to="gps/utm" />
<remap from="/gps/fix" to="gps/fix" />
<remap from="/odometry/filtered" to="/odometry/imu"/>
</node>
<!-- GPS integrated localization -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_gps" respawn="true" output="screen">
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_footprint"/>
<param name="world_frame" value="map"/>
<param name="imu0" value="imu/imu"/>
<rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, false, false, false]</rosparam>
<param name="imu0_differential" value="false"/>
<param name="imu1" value="imu/pseaa"/>
<rosparam param="imu1_config">[false, false, false, true ...
robot_localization has a dependency on gps_common, as I am using its lat/lon->UTM conversion logic. This allows me to eliminate the need for a GPS fix topic and a UTM topic in utm_transform_node. I take it rosdep didn't work?
No, it didn't work. Also, in my computer I had to delete also the package ' gpsd_client' from ' gps_umd' , as it was unable to find gps_common/GPSFix.h. This problem didn't occur when I did the same thing in the USV, so I haven't included it in the edition, as it seem specific of my ROS instalation