diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index fe5270ebe2..bc1e234d3e 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/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 _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 _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)); diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc index bf9cd2a66e..67e80d65f7 100644 --- a/test/integration/odometry_publisher.cc +++ b/test/integration/odometry_publisher.cc @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -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 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(id); + ASSERT_NE(nullptr, poseComp); + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + std::vector odomAngVels; + // Create function to store data from odometry messages + std::function 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("/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)) { diff --git a/test/worlds/odometry_publisher_3d_singularity.sdf b/test/worlds/odometry_publisher_3d_singularity.sdf new file mode 100644 index 0000000000..d74288d69f --- /dev/null +++ b/test/worlds/odometry_publisher_3d_singularity.sdf @@ -0,0 +1,97 @@ + + + + + + 0.001 + 0 + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 1 0 0 0 + + + 1.5 + + 0.0347563 + 0 + 0 + 0.07 + 0 + 0.0977 + + + + + + 0.30 0.42 0.11 + + + + + + + 0.30 0.42 0.11 + + + + + + 0 0 0 + 0 3.1415 0 + + + 3 + + + + +