Ask Your Question
0

Unexpected results from robot_localization

asked 2020-07-20 05:35:00 -0600

tbondar gravatar image

updated 2020-08-07 04:06:19 -0600

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

Comments

What kind of IMU are you using?

Dragonslayer gravatar image Dragonslayer  ( 2020-07-20 08:57:15 -0600 )edit

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.

tbondar gravatar image tbondar  ( 2020-07-20 09:34:36 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-07-20 10:31:36 -0600

Dragonslayer gravatar image

updated 2020-07-20 10:33:58 -0600

I would suggest fusion of imu and odometry/twist and then see how to bring gps into play. Your ekf_loc_odom seem to do nothing, but maybe its in the yaml files? The covariances in the imu msg seem od, Zero and -1.0 link text but maybe thats not an issue, just something that made me wonder.

Regarding your picture: Odom frame and base_link frame are 10 cells linear_x appart. If the cell size is one meter thats where your "data" ended up. Once again I would suggest sensor fusion of odom and imu in an ekf_node (without gps at first) and see how that turns out. Your odom --> base_link transform seem to come from ekt_loc_odom and as mentioned seems to be the 10 meters expected. This would imply that the other ekf node generates the tf from map --> odom. Your odom start position x=0, y=0, orientation=0, would be transformed by the gps. Thus the curve that we see from map to odom and base_link is likely a gps transform.

edit flag offensive delete link more

Comments

@Dragonslayer Thank you for taking the time to answer. For the sake of reference, a few notes. It might be useful for other people with similar issues.

  • Your ekf_loc_odom seem to do nothing... You're right, it doesn't do any sensor fusion but it provides the odom -> base_link transform.
  • The covariances in the imu msg... I did it following the comments in sensor_msgs/Imu which say A covariance matrix of all zeros will be interpreted as covariance unknown and if you have no estimate for one of the data elements ... set element 0 of the associated covariance matrix to -1
  • I agree with sorting out odometry first and then adding GPS. I think, in my case, ekf_loc_odom does its simple job alright.

However, going through my launch file again, I spotted an issue. I didn't remap the output of ekf_loc_odom so it was publishing to its default ...(more)

tbondar gravatar image tbondar  ( 2020-07-21 11:31:18 -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: 2020-07-20 05:35:00 -0600

Seen: 100 times

Last updated: Aug 07 '20