Unexpected results from robot_localization
I'm playing with robot_localization
trying to get position estimate in a very simple scenario. My emulated vehicle is starting from position x=0, y=0, orientation=0
, and receives the following velocity command at /cmd_vel
for 10 seconds: linear = (1,0,0), angular = (0,0,0)
. I expect my base_link
to be at x=10, y=0
after 10 seconds. Instead it drifts off as shown on the screenshot attached:
Screenshot and bagfile on Google Drive
I've added my bagfile as well with /cmd_vel
, /gps/fix
, /imu/data
, /odom/twist
and this is my launch file:
<launch>
<node name="ekf_loc_odom" pkg="robot_localization" type="ekf_localization_node">
<param name="world_frame" value="odom"/>
<param name="two_d_mode" value="true"/>
<param name="frequency" value="10"/>
<param name="twist0" value="/odom/twist"/>
<rosparam param="twist0_config">[ false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]</rosparam>
</node>
<node name="ekf_loc_map" pkg="robot_localization" type="ekf_localization_node">
<param name="world_frame" value="map"/>
<param name="two_d_mode" value="true"/>
<param name="frequency" value="10"/>
<param name="odom0" value="/odometry/gps"/>
<rosparam param="odom0_config">[ true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0" value="/imu/data"/>
<rosparam param="imu0_config">[false, false, false,
false, false, true,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<param name="imu0_differential" value="false"/>
</node>
<node name="navsat_transform" pkg="robot_localization" type="navsat_transform_node">
<param name="magnetic_declination_radians" value="0"/>
<param name="zero_altitude" value="true"/>
</node>
<node name="vehicle_emu" pkg="mario_control" type="vehicle_emu.py">
<remap from="twist" to="odom/twist"/>
<remap from="fix" to="gps/fix"/>
<remap from="imu" to="imu/data"/>
</node>
</launch>
I'm not sure if it's related but robot_localization
throws these warnings:
[ WARN] [1595237876.432141012]: Transform from base_link to odom was unavailable for the time requested. Using latest instead.
[ WARN] [1595237876.432755891]: Transform from odom to map was unavailable for the time requested. Using latest instead.
[ WARN] [1595237879.429787646]: Transform from base_link to odom was unavailable for the time requested. Using latest instead.
[ WARN] [1595237879.430319590]: Transform from odom to map was unavailable for the time requested. Using latest instead.
Could someone help me with pointing out what am I doing wrong? Thank you!
EDIT-1: Just in case it helps, here's an example of the sensor_msgs/Imu
messages published on /imu/data
:
header:
seq: 672
stamp:
secs: 1595237860
nsecs: 798180103
frame_id: "base_link"
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
x: 0.0
y: 0.0
z: 0.0
angular_velocity_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
x: 0.0
y: 0.0
z ...
What kind of IMU are you using?
The real hardware doesn't have a standard IMU but a compass. In my current test it's all emulated anyway. In
/imu/data
I publish orientation data only. I'm updating my original post with an example of the IMU message, just in case it helps.