Fixed Odometry Publisher Angular Velocity Singularity in 3D

src/systems/odometry_publisher/OdometryPublisher.cc

@@ -55,6 +55,15 @@ class gz::sim::systems::OdometryPublisherPrivate
   public: void UpdateOdometry(const gz::sim::UpdateInfo &_info,
     const gz::sim::EntityComponentManager &_ecm);
 
+  /// \brief Calculates angular velocity in body frame from world frame poses.
+  /// \param[in] _lastPose Pose at last timestep in world frame.
+  /// \param[in] _currentPose Pose at current timestep in world frame.
+  /// \param[in] _dt Time elapsed from last to current timestep.
+  /// \return Angular velocity computed in body frame at current timestep.
+  public: static math::Vector3d CalculateAngularVelocity(
+    const math::Pose3d &_lastPose, const math::Pose3d &_currentPose,
+    std::chrono::duration<double> _dt);
+
   /// \brief Gazebo communication node.
   public: transport::Node node;
 
@@ -119,18 +128,12 @@ OdometryPublisher::OdometryPublisher()
   std::get<0>(this->dataPtr->linearMean).SetWindowSize(10);
   std::get<1>(this->dataPtr->linearMean).SetWindowSize(10);
   std::get<2>(this->dataPtr->angularMean).SetWindowSize(10);
-  std::get<0>(this->dataPtr->linearMean).Clear();
-  std::get<1>(this->dataPtr->linearMean).Clear();
-  std::get<2>(this->dataPtr->angularMean).Clear();
 
   if (this->dataPtr->dimensions == 3)
   {
     std::get<2>(this->dataPtr->linearMean).SetWindowSize(10);
     std::get<0>(this->dataPtr->angularMean).SetWindowSize(10);
     std::get<1>(this->dataPtr->angularMean).SetWindowSize(10);
-    std::get<2>(this->dataPtr->linearMean).Clear();
-    std::get<0>(this->dataPtr->angularMean).Clear();
-    std::get<1>(this->dataPtr->angularMean).Clear();
   }
 }
 
@@ -329,6 +332,26 @@ void OdometryPublisher::PostUpdate(const UpdateInfo &_info,
   this->dataPtr->UpdateOdometry(_info, _ecm);
 }
 
+//////////////////////////////////////////////////
+math::Vector3d OdometryPublisherPrivate::CalculateAngularVelocity(
+    const math::Pose3d &_lastPose, const math::Pose3d &_currentPose,
+    std::chrono::duration<double> _dt)
+{
+  // Compute the first order finite difference between current and previous
+  // rotation as quaternion.
+  const math::Quaterniond rotationDiff =
+    _currentPose.Rot() * _lastPose.Rot().Inverse();
+
+  math::Vector3d rotationAxis;
+  double rotationAngle;
+  rotationDiff.AxisAngle(rotationAxis, rotationAngle);
+
+  const math::Vector3d angularVelocity =
+    (rotationAngle / _dt.count()) * rotationAxis;
+
+  return _currentPose.Rot().RotateVectorReverse(angularVelocity);
+}
+
 //////////////////////////////////////////////////
 void OdometryPublisherPrivate::UpdateOdometry(
     const gz::sim::UpdateInfo &_info,
@@ -375,10 +398,8 @@ void OdometryPublisherPrivate::UpdateOdometry(
   double linearDisplacementY = pose.Pos().Y() - this->lastUpdatePose.Pos().Y();
 
   double currentYaw = pose.Rot().Yaw();
-  const double lastYaw = this->lastUpdatePose.Rot().Yaw();
-  while (currentYaw < lastYaw - GZ_PI) currentYaw += 2 * GZ_PI;
-  while (currentYaw > lastYaw + GZ_PI) currentYaw -= 2 * GZ_PI;
-  const float yawDiff = currentYaw - lastYaw;
+  const math::Vector3d angularVelocityBody = CalculateAngularVelocity(
+    this->lastUpdatePose, pose, dt);
 
   // Get velocities assuming 2D
   if (this->dimensions == 2)
@@ -406,18 +427,6 @@ void OdometryPublisherPrivate::UpdateOdometry(
   // Get velocities and roll/pitch rates assuming 3D
   else if (this->dimensions == 3)
   {
-    double currentRoll = pose.Rot().Roll();
-    const double lastRoll = this->lastUpdatePose.Rot().Roll();
-    while (currentRoll < lastRoll - GZ_PI) currentRoll += 2 * GZ_PI;
-    while (currentRoll > lastRoll + GZ_PI) currentRoll -= 2 * GZ_PI;
-    const float rollDiff = currentRoll - lastRoll;
-
-    double currentPitch = pose.Rot().Pitch();
-    const double lastPitch = this->lastUpdatePose.Rot().Pitch();
-    while (currentPitch < lastPitch - GZ_PI) currentPitch += 2 * GZ_PI;
-    while (currentPitch > lastPitch + GZ_PI) currentPitch -= 2 * GZ_PI;
-    const float pitchDiff = currentPitch - lastPitch;
-
     double linearDisplacementZ =
       pose.Pos().Z() - this->lastUpdatePose.Pos().Z();
     math::Vector3 linearDisplacement(linearDisplacementX, linearDisplacementY,
@@ -427,8 +436,8 @@ void OdometryPublisherPrivate::UpdateOdometry(
     std::get<0>(this->linearMean).Push(linearVelocity.X());
     std::get<1>(this->linearMean).Push(linearVelocity.Y());
     std::get<2>(this->linearMean).Push(linearVelocity.Z());
-    std::get<0>(this->angularMean).Push(rollDiff / dt.count());
-    std::get<1>(this->angularMean).Push(pitchDiff / dt.count());
+    std::get<0>(this->angularMean).Push(angularVelocityBody.X());
+    std::get<1>(this->angularMean).Push(angularVelocityBody.Y());
     msg.mutable_twist()->mutable_linear()->set_x(
       std::get<0>(this->linearMean).Mean() +
       gz::math::Rand::DblNormal(0, this->gaussianNoise));
@@ -447,7 +456,7 @@ void OdometryPublisherPrivate::UpdateOdometry(
   }
 
   // Set yaw rate
-  std::get<2>(this->angularMean).Push(yawDiff / dt.count());
+  std::get<2>(this->angularMean).Push(angularVelocityBody.Z());
   msg.mutable_twist()->mutable_angular()->set_z(
     std::get<2>(this->angularMean).Mean() +
     gz::math::Rand::DblNormal(0, this->gaussianNoise));

test/integration/odometry_publisher.cc

@@ -24,6 +24,7 @@
 
 #include <gz/common/Console.hh>
 #include <gz/common/Util.hh>
+#include <gz/math/Helpers.hh>
 #include <gz/math/Pose3.hh>
 #include <gz/transport/Node.hh>
 #include <gz/utils/ExtraTestMacros.hh>
@@ -380,6 +381,86 @@ class OdometryPublisherTest
     EXPECT_NEAR(poses.back().Rot().Z(), tfPoses.back().Rot().Z(), 1e-2);
   }
 
+  /// \param[in] _sdfFile SDF file to load.
+  /// \param[in] _odomTopic Odometry topic.
+  protected: void TestMovement3dAtSingularity(const std::string &_sdfFile,
+                                              const std::string &_odomTopic)
+  {
+    // Start server
+    ServerConfig serverConfig;
+    serverConfig.SetSdfFile(_sdfFile);
+
+    Server server(serverConfig);
+    EXPECT_FALSE(server.Running());
+    EXPECT_FALSE(*server.Running(0));
+
+    // Create a system that records the body poses
+    test::Relay testSystem;
+
+    std::vector<math::Pose3d> poses;
+    testSystem.OnPostUpdate([&poses](const sim::UpdateInfo &,
+      const sim::EntityComponentManager &_ecm)
+      {
+        auto id = _ecm.EntityByComponents(
+          components::Model(),
+          components::Name("test_body"));
+        EXPECT_NE(kNullEntity, id);
+
+        auto poseComp = _ecm.Component<components::Pose>(id);
+        ASSERT_NE(nullptr, poseComp);
+        poses.push_back(poseComp->Data());
+      });
+    server.AddSystem(testSystem.systemPtr);
+
+    std::vector<math::Vector3d> odomAngVels;
+    // Create function to store data from odometry messages
+    std::function<void(const msgs::Odometry &)> odomCb =
+      [&](const msgs::Odometry &_msg)
+      {
+        odomAngVels.push_back(msgs::Convert(_msg.twist().angular()));
+      };
+
+    // Create node for publishing twist messages
+    transport::Node node;
+    auto cmdVel = node.Advertise<msgs::Twist>("/model/test_body/cmd_vel");
+    node.Subscribe(_odomTopic, odomCb);
+
+    // Set an angular velocity command that would cause pitch to update from 0
+    // to PI in 1 second, crossing the singularity when pitch is PI/2.
+    const math::Vector3d angVelCmd(0.0, GZ_PI, 0);
+    msgs::Twist msg;
+    msgs::Set(msg.mutable_linear(), math::Vector3d::Zero);
+    msgs::Set(msg.mutable_angular(), angVelCmd);
+    cmdVel.Publish(msg);
+
+    // Run server while the model moves with the velocities set earlier
+    server.Run(true, 1000, false);
+
+    // Poses for 1s
+    ASSERT_EQ(1000u, poses.size());
+
+    int sleep = 0;
+    int maxSleep = 30;
+    // Default publishing frequency for odometryPublisher is 50Hz.
+    for (; (odomAngVels.size() < 50) &&
+        sleep < maxSleep; ++sleep)
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+    EXPECT_NE(maxSleep, sleep);
+
+    // Odom for 1s
+    ASSERT_FALSE(odomAngVels.empty());
+    EXPECT_EQ(50u, odomAngVels.size());
+
+    // Check accuracy of velocities published in the odometry message
+    for (size_t i = 1; i < odomAngVels.size(); ++i) {
+      EXPECT_NEAR(odomAngVels[i].X(), angVelCmd[0], 1e-1);
+      EXPECT_NEAR(odomAngVels[i].Y(), angVelCmd[1], 1e-1);
+      EXPECT_NEAR(odomAngVels[i].Z(), angVelCmd[2], 1e-1);
+    }
+  }
+
   /// \param[in] _sdfFile SDF file to load.
   /// \param[in] _odomTopic Odometry topic.
   /// \param[in] _frameId Name of the world-fixed coordinate frame
@@ -624,6 +705,16 @@ TEST_P(OdometryPublisherTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Movement3d))
       "/model/X3/odometry", "/model/X3/pose", "X3/odom", "X3/base_footprint");
 }
 
+/////////////////////////////////////////////////
+TEST_P(OdometryPublisherTest,
+       GZ_UTILS_TEST_DISABLED_ON_WIN32(Movement3dAtSingularity))
+{
+  TestMovement3dAtSingularity(
+      gz::common::joinPaths(PROJECT_SOURCE_PATH,
+      "test", "worlds", "odometry_publisher_3d_singularity.sdf"),
+      "/model/test_body/odometry");
+}
+
 /////////////////////////////////////////////////
 TEST_P(OdometryPublisherTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(OdomFrameId))
 {

test/worlds/odometry_publisher_3d_singularity.sdf

@@ -0,0 +1,97 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="default">
+
+    <physics name="1ms" type="ode">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>0</real_time_factor>
+    </physics>
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>1 1 1 1</diffuse>
+      <specular>0.5 0.5 0.5 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name="test_body">
+      <pose>0 0 1 0 0 0</pose>
+      <link name="base_link">
+        <inertial>
+          <mass>1.5</mass>
+          <inertia>
+            <ixx>0.0347563</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.07</iyy>
+            <iyz>0</iyz>
+            <izz>0.0977</izz>
+          </inertia>
+        </inertial>
+        <collision name="base_link_inertia_collision">
+          <geometry>
+            <box>
+              <size>0.30 0.42 0.11</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="base_link_inertia_visual">
+          <geometry>
+            <box>
+              <size>0.30 0.42 0.11</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+      <plugin
+        filename="gz-sim-velocity-control-system"
+        name="gz::sim::systems::VelocityControl">
+        <initial_linear>0 0 0</initial_linear>
+        <initial_angular>0 3.1415 0</initial_angular>
+      </plugin>
+      <plugin
+        filename="gz-sim-odometry-publisher-system"
+        name="gz::sim::systems::OdometryPublisher">
+        <dimensions>3</dimensions>
+      </plugin>
+    </model>
+
+  </world>
+</sdf>