Child Frame of Odometry Messages is ignored

[frygge]

Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04
• Installation type:
  • binary and source
• Version or commit hash:
  • git rev-parse HEAD b534574

Steps to reproduce issue

• measure an odometry in a sensor frame that is rotated by (for example) 90 degree
• process this odometry as odom0 (and as single measurement source) in robot_localization as odom0

Expected behavior

• the robot should run forward

Actual behavior

• the robot is running sideways

Additional information

• I could fix it already and would like to open a PR

[ayrton04]

Please ask this as a question in answers.ros.org or robotics.stackexchange.com. When you do, please include your full configuration and a sample input message from every sensor input. I'm sceptical that there is a bug here, but regardless, this process should start with a question on one of those forums. Thanks.

[frygge]

Hmm, I could do that. Still, please give it a thought since I read the code and can see the error there:

• odometryCallback(..) calls poseCallback and misses to send the childFrameID (~line 1744)
• poseCallback calls preparePose but the child Frame is gone already (~line 1810)
• in differential mode, preparePose calls prepareTwist but the message gets the baseLinkFrame (since it doesn't know the right one) (~line 2868 and 2894)
• Hence, the correction of the child frame is just wrong (the identity) (~line 3041)

The line numbers might be off by a little since I fixed it already (my fix is basically just passing the childFrame)

Do you still think, I should open a question somewhere? Then I would do it...

[ayrton04]

I will need to read the code, but aren't we transforming the pose data into the target frame (e.g., odom) before we differentiate it? If so, the correct child frame would be base_link.

[ayrton04]

Oh, you're measuring pose data from a sensor that is on the robot and has a non-zero transform from the robot origin. This is not a use case that is well supported by the package. Pose data needs to have a direct transform from the world frame of that pose data to your world frame (so odom->your_world_frame). If you are measuring pose data from a sensor with a transform relative to base_link, it doesn't behave well. You'd need to have something that is transforming your base_link->sensor transform into the world frame, then applying that transform to the pose measurement.

You can hold off on asking the question. I'll reopen this and look into what you're saying, but I am concerned that we'll break other use cases if we change that.

[frygge]

I see. Thanks for having a look.
The situation is the following: the sensor provides the transformation between the Odom frame and a sensor frame. The sensor frame is a child of base_link. The transformation between base_link and the sensor frame is provided via tf_static.

I checked my fix against various types (we got an autonomous driving car and have a complex sensor setup). The fix should only change the behaviour in the desired situation. I tried to build it such that everything else should compute as before

[ayrton04]

It would still be helpful to see the configs, sensor data, and static transform, if that's possible.

[frygge]

Sure, I'll prepare a minimal example that shows the effect

[frygge]

So, I managed to create a minimal example (stripped down our car setup). Here it is:

The launch files load a urdf that shows (part of) our vehicle frames. Then, it starts the robot_localization. The localization comes with a yaml file containing the config. Here I stripped it down to odometry that contains both, a pose (from a slam system) and a twist (from an IMU). I created two versions: _relative and _diff that differ by running the robot_localization in relative mode and differential mode, respectively. In both cases, the error is of the same nature: forgetting the child frame of the odometry message.

localization_relative.launch

<launch>
    <param
            name="robot_description"
            command="$(find xacro)/xacro 'vehicle.urdf.xacro'"
    />
    <node
            name="robot_state_publisher"
            pkg="robot_state_publisher"
            type="robot_state_publisher"
            output="screen"
    />

    <rosparam command="load" file="localization_relative.yaml"/>
    <node pkg="robot_localization" type="ukf_localization_node" name="robot_odometry"/>
</launch>

localization_relative.yaml

robot_odometry:
    frequency: 85
    publish_tf: true

    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    # Corrected IMU
    imu0: gps/imu
    imu0_config: [
        false, false, false, # X, Y, Z
        false, false, false, # roll, pitch, yaw
        false, false, false, # X', Y', Z'
        true,  true,  true,  # roll', pitch', yaw'
        true,  true,  true   # X'', Y'', Z''
    ]
    imu0_differential: false
    imu0_relative: false

    odom0: /alb/odometry
    odom0_config: [
        true,  true,  true,  # X, Y, Z
        true,  true,  true,  # roll, pitch, yaw
        false, false, false, # X', Y', Z'
        false, false, false, # roll', pitch', yaw'
        false, false, false  # X'', Y'', Z''
    ]
    odom0_differential: false
    odom0_relative: true

vehicle.urdf.xacro

<robot name="vehicle">
    <!-- At center of rear axle. -->
    <link name="base_link"/>

    <!-- for gps/imu topic -->
    <link name="imu_link"/>
    <joint name="imu_link_joint" type="fixed">
        <parent link="base_link"/>
        <child link="imu_link"/>
        <origin rpy="0.0 0.0 0.0" xyz="-0.350 -0.010 0.575"/>
    </joint>

    <!-- for /alb/odometry topic -->
    <link name="lidar_base_link"/>
    <joint name="lidar_base_link_joint" type="fixed">
        <parent link="base_link" />
        <child link="lidar_base_link" />
        <origin rpy="0 0 0" xyz="1.31 0.0 1.27" />
    </joint>
    <link name="velodyne"/>
    <joint name="velodyne_joint" type="fixed">
        <parent link="lidar_base_link" />
        <child link="velodyne" />
        <origin rpy="0 0 -0.78539816" xyz="0.0 0.0 0.038" />
    </joint>
    <link name="alb_sensor_frame"/>
    <joint name="alb_sensor_frame_joint" type="fixed">
        <parent link="velodyne" />
        <child link="alb_sensor_frame" />
        <origin rpy="0 0 -1.570796327" xyz="0 0 0" />
    </joint>

</robot>

The bug yields the following behavior:

The fix leads to:

Now, a similar thing happens with the differential setting.

localization_diff.launch

<launch>
    <param
            name="robot_description"
            command="$(find xacro)/xacro 'vehicle.urdf.xacro'"
    />
    <node
            name="robot_state_publisher"
            pkg="robot_state_publisher"
            type="robot_state_publisher"
            output="screen"
    />

    <rosparam command="load" file="localization_diff.yaml"/>
    <node pkg="robot_localization" type="ukf_localization_node" name="robot_odometry"/>
</launch>

localization_relative.yaml

robot_odometry:
    frequency: 85
    publish_tf: true

    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    # Corrected IMU
    imu0: gps/imu
    imu0_config: [
        false, false, false, # X, Y, Z
        false, false, false, # roll, pitch, yaw
        false, false, false, # X', Y', Z'
        true,  true,  true,  # roll', pitch', yaw'
        true,  true,  true   # X'', Y'', Z''
    ]
    imu0_differential: false
    imu0_relative: false

    odom0: /alb/odometry
    odom0_config: [
        true,  true,  true,  # X, Y, Z
        true,  true,  true,  # roll, pitch, yaw
        false, false, false, # X', Y', Z'
        false, false, false, # roll', pitch', yaw'
        false, false, false  # X'', Y'', Z''
    ]
    odom0_differential: true
    odom0_relative: false

With the bug:

and fixed:

In the pictures, I also included the car, so it's easier to see it going sideways.

You can find a stripped down bag file that contains the sensor data to these pictures here:
https://file.io/hq2p6VA6Vut1

[frygge]

I just opened a PR to see if your CI is running.

[Timple]

I've been subscribed to this repository for a while. I think I've seen this (kind of) request every now and then.

I think the reason it is not implemented is that it creates a loop? To do a TF lookup of the child frame, one needs the orientation of the vehicle. But this data is also used to determine that orientation.

Only exception is the imu, as that sensor provides it's own orientation and can be calculated back to base_link using only this sensor.

Note: I'm not a maintainer here, just stating my thoughts, if it can be fixed/added anyhow I'm happy!

[frygge]

@Timple Actually it doesn't. You can remove the IMU from the example I provided and it still works. The lookup of the child frame only concerns the orientation relative to the base_link, which shall be static. Hence, the orientation of the vehicle is not necessary in this case.

[ayrton04]

@frygge can you show me a sample input message from the odometry data? I don't have the cycles to pull the bag right now.

The thing that concerns me is that in ROS, the nav_msgs/Odometry message assumes that the frame_id is the frame of the pose data in the message, and the child_frame_id is the frame of the twist data. In this instance, it sounds like you are not generating velocity data in that message at all, but are just filling out the pose, and then wanting the EKF to do differentiation and assign the child_frame_id of your message to the differentiated data.

In any case, this is why I wanted to see a sample message. If it's not too much trouble, that would be helpful, thanks.

[ayrton04]

Oh, also, can I see your tf tree? Thanks.

[frygge]

Sure, here's an example of our /gps/imu topic:

header: 
  seq: 1012
  stamp: 
    secs: 1645024083
    nsecs:  69816822
  frame_id: "imu_link"
orientation: 
  x: 0.015425120111168108
  y: -0.014461853304478484
  z: -0.04269186517284182
  w: 0.9988645178985084
orientation_covariance: [0.0002407410787039219, 0.0, 0.0, 0.0, 0.00030136817514864536, 0.0, 0.0, 0.0, 0.0002778071131437389]
angular_velocity: 
  x: 0.003837878632190295
  y: 0.0038239841972942117
  z: 0.01944696758422607
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.032451134997749494
  y: -0.022353834542992515
  z: -0.06860744721792925
linear_acceleration_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

An example of our /alb/odometry message:

header: 
  seq: 295
  stamp: 
    secs: 1645024083
    nsecs: 102326006
  frame_id: "odom"
child_frame_id: "alb_sensor_frame"
pose: 
  pose: 
    position: 
      x: 9.621140480041504
      y: 152.453125
      z: 0.2741394639015198
    orientation: 
      x: -0.00733062604082584
      y: 0.004882273591035721
      z: -0.19940068750709866
      w: 0.9798784573344431
  covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

The odometry message does (in this case) contain no twist. However, I assume that the pose shall be interpreted as pose between the header frame and the child frame (odom -> alb_sensor_frame in this case). Others of our odometry messages do indeed contain a twist (and meaningful cov) as well.

Here is the tf tree:

[ayrton04]

I see what you're doing in the PR (thanks for that), and I don't know that I'd call this a bug as much as a new feature. Re-labeling.

[frygge]

Ahh, OK. Then I think misunderstood the documentation: http://docs.ros.org/en/melodic/api/robot_localization/html/preparing_sensor_data.html, 2nd bullet point under "Common errors" says that velocities get transformed to the base_link. I thought this is true even if they come from the odometry message with differential enabled.