Odometry velocities reported in wrong frame

[Jannkar]

According to msg/Odometry documentation, linear and angular velocities in Odometry message should be reported relative to child_frame_id.

Currently the velocities are reported in odom frame, which is causing issues when using this data with other existing ROS packages.

This is how the velocities are currently set in ros_helpers.py:

def GetOdomTwistFromState(state, spot_wrapper):
    """Maps odometry data from robot state proto to ROS TwistWithCovarianceStamped message
    Args:
        data: Robot State proto
        spot_wrapper: A SpotWrapper object
    Returns:
        TwistWithCovarianceStamped message
    """
    twist_odom_msg = TwistWithCovarianceStamped()
    local_time = spot_wrapper.robotToLocalTime(state.kinematic_state.acquisition_timestamp)
    twist_odom_msg.header.stamp = Time(sec=local_time.seconds, nanosec=local_time.nanos)
    twist_odom_msg.twist.twist.linear.x = state.kinematic_state.velocity_of_body_in_odom.linear.x
    twist_odom_msg.twist.twist.linear.y = state.kinematic_state.velocity_of_body_in_odom.linear.y
    twist_odom_msg.twist.twist.linear.z = state.kinematic_state.velocity_of_body_in_odom.linear.z
    twist_odom_msg.twist.twist.angular.x = state.kinematic_state.velocity_of_body_in_odom.angular.x
    twist_odom_msg.twist.twist.angular.y = state.kinematic_state.velocity_of_body_in_odom.angular.y
    twist_odom_msg.twist.twist.angular.z = state.kinematic_state.velocity_of_body_in_odom.angular.z
    return twist_odom_msg

[heuristicus]

This has also been reported at heuristicus/spot_ros#95

[kjvanteeffelen]

Hello,

Are there any plans to add the fix to the current code? Indeed, the Twist definition in odometry is 'wrong' according to the ROS standards. This will lead to problems when for instance using the robot_localization package with Spot.

Until it's fixed, I'm using an additional node which subscribes to the current odometry, applies the fix and publishes it to the '/odometry_corrected' topic. This could easily be integrated into the code directly I believe. It was largely copied from heuristicus/spot_ros#95.

import rclpy
import copy
import transforms3d
import numpy as np

from rclpy.node import Node
from nav_msgs.msg import Odometry

"""
The Spot ROS 2 driver reports the Twist part of the odometry in odom frame.
According to ROS standards this should be reported in body frame.
This script subscribes to the odometry, transforms it to body frame and publishes it 
to /odometry_corrected.
"""

class OdometryTransformer(Node):

    def __init__(self):
        super().__init__('odometry_transformer')

        self.odom_pub = self.create_publisher(
            Odometry,
            '/odometry_corrected',
            10)
        
        self.odom_sub = self.create_subscription(
            Odometry,
            '/odometry',
            self.odometry_callback,
            10)
        self.odom_sub  # prevent unused variable warning

    def odometry_callback(self, msg):

        transformed_odom = self.transform_odom(msg)

        self.odom_pub.publish(transformed_odom)
        
    def transform_odom(self, base_odometry: Odometry):
        """
        Copied from the ROS 1 fix: 
        https://github.com/heuristicus/spot_ros/blob/2a1d26da976a6376b21b5af9ce5328899fb7a8c4/spot_driver/src/spot_driver/ros_helpers.py#L402
        """

        # Current inverse rotation of body frame w.r.t. odom frame
        inverse_rotation = transforms3d.quaternions.quat2mat(
            transforms3d.quaternions.qinverse(
                [
                    base_odometry.pose.pose.orientation.w,
                    base_odometry.pose.pose.orientation.x,
                    base_odometry.pose.pose.orientation.y,
                    base_odometry.pose.pose.orientation.z,
                ]
            )
        )

        # Transform the linear twist by rotating the vector according to the rotation from body to odom
        linear_twist = np.array(
            [
                [base_odometry.twist.twist.linear.x],
                [base_odometry.twist.twist.linear.y],
                [base_odometry.twist.twist.linear.z],
            ]
        )

        corrected_linear = inverse_rotation.dot(linear_twist)

        # Do the same for the angular twist
        angular_twist = np.array(
            [
                [base_odometry.twist.twist.angular.x],
                [base_odometry.twist.twist.angular.y],
                [base_odometry.twist.twist.angular.z],
            ]
        )

        corrected_angular = inverse_rotation.dot(angular_twist)

        corrected_odometry = copy.deepcopy(base_odometry)
        corrected_odometry.twist.twist.linear.x = corrected_linear[0][0]
        corrected_odometry.twist.twist.linear.y = corrected_linear[1][0]
        corrected_odometry.twist.twist.linear.z = corrected_linear[2][0]
        corrected_odometry.twist.twist.angular.x = corrected_angular[0][0]
        corrected_odometry.twist.twist.angular.y = corrected_angular[1][0]
        corrected_odometry.twist.twist.angular.z = corrected_angular[2][0]

        return corrected_odometry


def main(args=None):
    rclpy.init(args=args)

    odometry_transformer = OdometryTransformer()

    rclpy.spin(odometry_transformer)

    odometry_transformer.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

[kjvanteeffelen]

I am willing to contribute to the driver by solving this issue. However, an approach needs to be decided on. We could:

1. Integrate the publishing of the /odometry_corrected topic to the driver. This will not break any current implementations that rely on the odometry definition that is currently in place, however, it might spark confusion on which definition/topic is to be used when.
2. Change the odometry publishing to the ROS standards. That way there is no confusing between the two topics (when to use which), only the one that adheres to ROS standards is available. However, this could break current implementations.

Implementation wise they are both pretty straightforward, any thoughts on the best approach? I myself am a fan of option 2 for the sake of cleanliness and standards. However, I do not have a good idea of how many people are relying on the current definition of odometry.

[khughes-bdai]

Thanks for pointing out this issue, and thank you @kjvanteeffelen for providing some possible solutions. I think the correct approach here would be to modify the odometry publishing to fit the ROS standards. We will look into this to see how easy it would be to add and if there would be any significant downstream consequences (just FYI, the part of the driver that publishes this topic has since changed from Python to C++).

[khughes-bdai]

minor update on this, I made an attempt to get a fix for this a while back with #416 but it didn't seem to be giving me correct behavior when running on robot

[alisys-robotics]

Thanks @kjvanteeffelen, I tried your script and it works fine.
I would prefer changing the code, though. @khughes-bdai, any progress with the PR?

[khughes-bdai]

@alisys-robotics It's been a while since I have looked into this, let me try to get the branch from that PR out of the mess of rebase conflicts it's currently in and I can re-test it on robot sometime next week

[khughes-bdai]

Circling back to this!

II updated my draft PR, and I think this method (using BD's expressVelocityInNewFrame function) is giving the right results on robot. Specifically, I ran a test where I sent cmd_vel commands to the robot via teleop twist keyboard, and recorded both the cmd_vel and odometry topic in a rosbag. Looking at this data afterwards, it seems like the velocity component of the odometry topic roughly matches up with what was sent in cmd_vel. This is I believe a good indication that this conversion is doing the right thing.

I do want to note that the BD expressVelocityInNewFrame function gives different results than the code pasted above. BD's method uses the adjoint matrix of the transformation to convert the velocity (here is their python implementation) where the conversion node above in this thread does not. With this non-adjoint method, if the orientation between the body frame and the odom frame is identity, then the velocity is not transformed at all. This is explicitly different than the BD method that requires the full SE3 position+orientation to compute the adjoint. If folks are already successfully using this non-adjoint method in SLAM pipelines I would be curious to know how the version in my draft compares as it will give a different output but seems like the "correct" way to transform a twist. Please let me know!