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++).