search

cra-ros-pkg / robot_localization

robot_localization is a package of nonlinear state estimation nodes. The package was developed by Charles River Analytics, Inc. Please ask questions on answers.ros.org.
http://www.cra.com
Other
1.38k stars 888 forks source link

Child Frame of Odometry Messages is ignored #726

Open frygge opened 2 years ago

frygge commented 2 years ago

Bug report

Required Info:

Steps to reproduce issue

Expected behavior

Actual behavior

Additional information

ayrton04 commented 2 years ago

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 commented 2 years ago

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

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 commented 2 years ago

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 commented 2 years ago

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 commented 2 years ago

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 commented 2 years ago

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

frygge commented 2 years ago

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

frygge commented 2 years ago

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: robot_localization_relative_bug

The fix leads to: robot_localization_relative_fixed

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: robot_localization_diff_bug

and fixed: robot_localization_diff_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 commented 2 years ago

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

Timple commented 2 years ago

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 commented 2 years ago

@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 commented 2 years ago

@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 commented 2 years ago

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

frygge commented 2 years ago

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: tf_tree

ayrton04 commented 2 years ago

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 commented 2 years ago

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.