Environment

• OS Version: Ubuntu 24.04
• Source or binary build? Binary, Gazebo Sim version 9.5.0

Description

• Expected behavior:

1. The pose orientation published by the OdometryPublisher in subsequent messages should have a very small difference to each other with a high enough publishing rate.
2. For an object turning at a constant rate, the published angular twist should be a constant as well

• Actual behavior:
  For a 2D constant rate rotation, the published orientation quaternion q is flipped at the point, where q.w = +0.5 or -0.5 and q.z = sqrt(0.75) during the rotation. This is not a problem for pose orientation, as q and -q represent the same rotation.
  But as the computation of compute the angular velocity uses the difference between subsequent q, this leads to very large peaks in the angular velocity, similar to singularity behavior of Euler angle based computations.

Opinion on fix

The simplest option to fix the angular velocity calculation is by making the orientation quaternion values continous. This can be accomplished by either a check of the q.w values sign compared to the last poses q.w (after the pose calculation in line

) or another check whether q or -q is closer to the last pose q.

Steps to reproduce

1. Add the basic OdometryPublisher plugin to any robot URDF in a ROS2 (Kilted) setup with Gazebo (Ionic, 9.5.0).
2. For ROS2 handling and visualization, conversion is required. Use the ROS2 to Gazebo Bridge to republish the Gazebo topic as ROS2 topic:
   ros2 run ros_gz_bridge parameter_bridge --ros-args -p config_file:={bridge_params.yaml}
   where bridge_params.yaml includes

' - ros_topic_name: "/odometry/ground_truth"
gz_topic_name: "/model/your robot/odometry_with_covariance"
ros_type_name: "nav_msgs/msg/Odometry"
gz_type_name: "gz.msgs.OdometryWithCovariance"
direction: GZ_TO_ROS

4. Start ros2 bag recording
5. Make the robot turn in circles (for example via teleop )
6. Stop the recording and use ROS2 plotjuggler to plot the recorded odometry.

Output

Opinion on fix

The simplest option to fix the angular velocity calculation is by making the orientation quaternion values continous. This can be accomplished by either a check of the q.w values sign compared to the last poses q.w (after the pose calculation in line

) or another check whether q or -q is closer to the last pose q.