robot_localization doesn't publish odom when only using gps and imu-data.
I'm trying to use robot_localization in order to fuse IMU and GPS data to obtain odometry information (I'm aware of GPS may not provide discrete data which may lead to jumps). My system has the following setup:
- ROS Melodic on Ubuntu 18.04
- robot_localization
- GNSS-sensor (sensor_msgs/NavSatFix) -> refers to "gps"-frame
- Magnetometer-IMU (sensormsgs/Imu) with NED-convention (i guess), meaning 0° is north and it increases counter-clockwise -> refers to "imulink"-frame
I'm trying to use a combination of ekflocalizationnode and navsattransformnode with the configuration below. Since I want to use a 2D-localization, I only need the x- and y-localization based on the gps-data fused with the yaw-angle based on the imu-data.
ekf_odom.launch:
<?xml version="1.0"?> <launch>
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_to_imu"
args="0 0 0 0 0 0 1 base_link imu_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_to_gps"
args="0 0 0 0 0 0 1 base_link gps" />
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization"
clear_params="true">
<param name="frequency" value="20"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="true"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.0"/>
<param name="odom0" value="/odometry/gps"/>
<param name="imu0" value="/imu/data"/>
<rosparam param="odom0_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="true"/>
<param name="odom0_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="print_diagnostics" value="true"/>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
</node>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" respawn="true" output="screen">
<param name="frequency" value="20"/>
<param name="delay" value="3"/>
<param name="magnetic_declination_radians" value="0"/>
<param name="yaw_offset" value="1.570796327"/>
<param name="zero_altitude" value="true"/>
<param name="broadcast_cartesian_transform" value="true"/>
<param name="publish_filtered_gps" value="false"/>
<param name="use_odometry_yaw" value="false"/>
<remap from="imu/data" to="/imu/data" />
<remap from="gps/fix" to="/ublox_node/fix" />
<remap from="/odometry/filtered" to="/odometry/filtered" />
<remap from="/odometry/gps" to="/odometry/gps"/>
</node>
</launch>
The code is based on this tutorial.
When starting the system, navsattransformnode appears to receive the gnss-data (see terminal-output below):
Since it appeared to be a common issue in related posts that several frames weren't defined, here's my tf-tree when running the system:
As you can see, ekflocalizationnode doesn't generate the transform between odom and baselink. Furthermore, neither odom/filtered nor odom/gps publish any data. I've had a look at several related issues and therefore made sure that all relevant frames exist and are connected and that I add the IMU both in navsattransformnode and ekflocalization_node.
My questions are:
- Where's the issue within my code?
- Will it cause issues that navsattransformnode has the option yaw_offset (which I need since my IMU isn't in ENU-convention) activated while ekflocalizationnode doesn't provide such a offset-variable? (why do I have to add the IMU to both ekflocalizationnode and navsattransformnode anyway instead of just adding it to last one?)
Edit: Activating the print_diagnostics-option shows following output:
is it normal that frame_id is empty?
Asked by Zimba96 on 2020-11-29 17:28:15 UTC
Answers
Figured out that the issue was with the imu-data which for some reason has a very weird covariance
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
which prevents the filter from considering its measurements and therefore makes it stuck waiting for proper data. Using a modified data-input with
orientation_covariance: [0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]
makes it work.
Asked by Zimba96 on 2020-12-01 17:12:51 UTC
Comments