From d01652c6fece52a178a670b72f2a4b54359085f4 Mon Sep 17 00:00:00 2001 From: Aditya Pande Date: Mon, 4 Apr 2022 18:45:10 -0700 Subject: [PATCH] Add Gaussian noise to Odometry Publisher (#1393) * Added gaussian noise and odometry with covariance publisher Signed-off-by: Aditya Co-authored-by: Louise Poubel --- CMakeLists.txt | 2 +- .../odometry_publisher/OdometryPublisher.cc | 138 ++++++++++- .../odometry_publisher/OdometryPublisher.hh | 10 +- test/integration/odometry_publisher.cc | 115 +++++++++ test/worlds/odometry_noise.sdf | 231 ++++++++++++++++++ 5 files changed, 482 insertions(+), 14 deletions(-) create mode 100644 test/worlds/odometry_noise.sdf diff --git a/CMakeLists.txt b/CMakeLists.txt index ca8ffacada..13443e62e7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -74,7 +74,7 @@ set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) #-------------------------------------- # Find ignition-msgs -ign_find_package(ignition-msgs8 REQUIRED VERSION 8.2) +ign_find_package(ignition-msgs8 REQUIRED VERSION 8.3) set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) #-------------------------------------- diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index 0a8a588b35..8bdd6f215b 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -18,6 +18,7 @@ #include "OdometryPublisher.hh" #include +#include #include #include @@ -76,6 +77,9 @@ class ignition::gazebo::systems::OdometryPublisherPrivate /// \brief Diff drive odometry message publisher. public: transport::Node::Publisher odomPub; + /// \brief Diff drive odometry with covariance message publisher. + public: transport::Node::Publisher odomCovPub; + /// \brief Rolling mean accumulators for the linear velocity public: std::tuple linearMean; @@ -95,6 +99,9 @@ class ignition::gazebo::systems::OdometryPublisherPrivate /// \brief Allow specifying constant xyz and rpy offsets public: ignition::math::Pose3d offset = {0, 0, 0, 0, 0, 0}; + + /// \brief Gaussian noise + public: double gaussianNoise = 0.0; }; ////////////////////////////////////////////////// @@ -158,6 +165,11 @@ void OdometryPublisher::Configure(const Entity &_entity, "rpy_offset")); } + if (_sdf->HasElement("gaussian_noise")) + { + this->dataPtr->gaussianNoise = _sdf->Get("gaussian_noise"); + } + this->dataPtr->robotBaseFrame = this->dataPtr->model.Name(_ecm) + "/" + "base_footprint"; if (!_sdf->HasElement("robot_base_frame")) @@ -199,17 +211,38 @@ void OdometryPublisher::Configure(const Entity &_entity, // Setup odometry std::string odomTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/odometry"}; + std::string odomCovTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/odometry_with_covariance"}; + if (_sdf->HasElement("odom_topic")) odomTopic = _sdf->Get("odom_topic"); + if (_sdf->HasElement("odom_covariance_topic")) + odomCovTopic = _sdf->Get("odom_covariance_topic"); + std::string odomTopicValid {transport::TopicUtils::AsValidTopic(odomTopic)}; if (odomTopicValid.empty()) { ignerr << "Failed to generate odom topic [" << odomTopic << "]" << std::endl; - return; } - this->dataPtr->odomPub = this->dataPtr->node.Advertise( - odomTopicValid); + else + { + this->dataPtr->odomPub = this->dataPtr->node.Advertise( + odomTopicValid); + } + + std::string odomCovTopicValid { + transport::TopicUtils::AsValidTopic(odomCovTopic)}; + if (odomCovTopicValid.empty()) + { + ignerr << "Failed to generate odom topic [" + << odomCovTopic << "]" << std::endl; + } + else + { + this->dataPtr->odomCovPub = this->dataPtr->node.Advertise< + msgs::OdometryWithCovariance>(odomCovTopicValid); + } } ////////////////////////////////////////////////// @@ -303,9 +336,18 @@ void OdometryPublisherPrivate::UpdateOdometry( std::get<0>(this->linearMean).Push(linearVelocityX); std::get<1>(this->linearMean).Push(linearVelocityY); msg.mutable_twist()->mutable_linear()->set_x( - std::get<0>(this->linearMean).Mean()); + std::get<0>(this->linearMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); msg.mutable_twist()->mutable_linear()->set_y( - std::get<1>(this->linearMean).Mean()); + std::get<1>(this->linearMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); + msg.mutable_twist()->mutable_linear()->set_z( + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); + + msg.mutable_twist()->mutable_angular()->set_x( + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); + msg.mutable_twist()->mutable_angular()->set_y( + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); } // Get velocities and roll/pitch rates assuming 3D else if (this->dimensions == 3) @@ -334,21 +376,27 @@ void OdometryPublisherPrivate::UpdateOdometry( std::get<0>(this->angularMean).Push(rollDiff / dt.count()); std::get<1>(this->angularMean).Push(pitchDiff / dt.count()); msg.mutable_twist()->mutable_linear()->set_x( - std::get<0>(this->linearMean).Mean()); + std::get<0>(this->linearMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); msg.mutable_twist()->mutable_linear()->set_y( - std::get<1>(this->linearMean).Mean()); + std::get<1>(this->linearMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); msg.mutable_twist()->mutable_linear()->set_z( - std::get<2>(this->linearMean).Mean()); + std::get<2>(this->linearMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); msg.mutable_twist()->mutable_angular()->set_x( - std::get<0>(this->angularMean).Mean()); + std::get<0>(this->angularMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); msg.mutable_twist()->mutable_angular()->set_y( - std::get<1>(this->angularMean).Mean()); + std::get<1>(this->angularMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); } // Set yaw rate std::get<2>(this->angularMean).Push(yawDiff / dt.count()); msg.mutable_twist()->mutable_angular()->set_z( - std::get<2>(this->angularMean).Mean()); + std::get<2>(this->angularMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); // Set the time stamp in the header. msg.mutable_header()->mutable_stamp()->CopyFrom( @@ -373,7 +421,73 @@ void OdometryPublisherPrivate::UpdateOdometry( return; } this->lastOdomPubTime = _info.simTime; - this->odomPub.Publish(msg); + if (this->odomPub.Valid()) + { + this->odomPub.Publish(msg); + } + + // Generate odometry with covariance message and publish it. + msgs::OdometryWithCovariance msgCovariance; + + // Set the time stamp in the header. + msgCovariance.mutable_header()->mutable_stamp()->CopyFrom( + convert(_info.simTime)); + + // Set the frame ids. + frame = msgCovariance.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(odomFrame); + childFrame = msg.mutable_header()->add_data(); + childFrame->set_key("child_frame_id"); + childFrame->add_value(robotBaseFrame); + + // Copy position from odometry msg. + msgCovariance.mutable_pose_with_covariance()-> + mutable_pose()->mutable_position()->set_x(msg.pose().position().x()); + msgCovariance.mutable_pose_with_covariance()-> + mutable_pose()->mutable_position()->set_y(msg.pose().position().y()); + msgCovariance.mutable_pose_with_covariance()-> + mutable_pose()->mutable_position()->set_z(msg.pose().position().z()); + + // Copy twist from odometry msg. + msgCovariance.mutable_twist_with_covariance()-> + mutable_twist()->mutable_angular()->set_x(msg.twist().angular().x()); + msgCovariance.mutable_twist_with_covariance()-> + mutable_twist()->mutable_angular()->set_y(msg.twist().angular().y()); + msgCovariance.mutable_twist_with_covariance()-> + mutable_twist()->mutable_angular()->set_z(msg.twist().angular().z()); + + msgCovariance.mutable_twist_with_covariance()-> + mutable_twist()->mutable_linear()->set_x(msg.twist().linear().x()); + msgCovariance.mutable_twist_with_covariance()-> + mutable_twist()->mutable_linear()->set_y(msg.twist().linear().y()); + msgCovariance.mutable_twist_with_covariance()-> + mutable_twist()->mutable_linear()->set_z(msg.twist().linear().z()); + + // Populate the covariance matrix. + // Should the matrix me populated for pose as well ? + auto gn2 = this->gaussianNoise * this->gaussianNoise; + for (int i = 0; i < 36; i++) + { + if (i % 7 == 0) + { + msgCovariance.mutable_pose_with_covariance()-> + mutable_covariance()->add_data(gn2); + msgCovariance.mutable_twist_with_covariance()-> + mutable_covariance()->add_data(gn2); + } + else + { + msgCovariance.mutable_pose_with_covariance()-> + mutable_covariance()->add_data(0); + msgCovariance.mutable_twist_with_covariance()-> + mutable_covariance()->add_data(0); + } + } + if (this->odomCovPub.Valid()) + { + this->odomCovPub.Publish(msgCovariance); + } } IGNITION_ADD_PLUGIN(OdometryPublisher, diff --git a/src/systems/odometry_publisher/OdometryPublisher.hh b/src/systems/odometry_publisher/OdometryPublisher.hh index 73c328610d..7fdab8253b 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.hh +++ b/src/systems/odometry_publisher/OdometryPublisher.hh @@ -53,6 +53,10 @@ namespace systems /// messages. This element is optional, and the default value is /// `/model/{name_of_model}/odometry`. /// + /// ``: Custom topic on which this system will publish + /// odometry with covariance messages. This element is optional, and the + /// default value is `/model/{name_of_model}/odometry_with_covariance`. + /// /// ``: Number of dimensions to represent odometry. Only 2 and 3 /// dimensional spaces are supported. This element is optional, and the /// default value is 2. @@ -63,7 +67,11 @@ namespace systems /// /// ``: Rotation offset relative to the body fixed frame, the /// default value is 0 0 0. This offset will be added to the odometry - // message. + /// message. + /// + /// ``: Standard deviation of the Gaussian noise to be added + /// to pose and twist messages. This element is optional, and the default + /// value is 0. class OdometryPublisher : public System, public ISystemConfigure, diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc index 7912cb7986..d5994ac36c 100644 --- a/test/integration/odometry_publisher.cc +++ b/test/integration/odometry_publisher.cc @@ -440,6 +440,111 @@ class OdometryPublisherTest EXPECT_NEAR(lastPose.Rot().Pitch(), 0, 1e-2); EXPECT_NEAR(lastPose.Rot().Yaw(), 0, 1e-2); } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _odomTopic Odometry topic. + protected: void TestGaussianNoise(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)); + + std::vector odomLinVels; + std::vector odomAngVels; + google::protobuf::RepeatedField odomTwistCovariance; + // Create function to store data from odometry messages + std::function odomCb = + [&](const msgs::OdometryWithCovariance &_msg) + { + odomLinVels.push_back(msgs::Convert(_msg.twist_with_covariance(). + twist().linear())); + odomAngVels.push_back(msgs::Convert(_msg.twist_with_covariance(). + twist().angular())); + odomTwistCovariance = _msg.twist_with_covariance().covariance().data(); + }; + transport::Node node; + node.Subscribe(_odomTopic, odomCb); + + // Run server while the model moves with the velocities set earlier + server.Run(true, 3000, false); + + int sleep = 0; + int maxSleep = 30; + for (; odomLinVels.size() < 500 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + // Verify the Gaussian noise. + ASSERT_FALSE(odomLinVels.empty()); + ASSERT_FALSE(odomAngVels.empty()); + int n = odomLinVels.size(); + + // Calculate the means. + double linVelSumX = 0, linVelSumY = 0, linVelSumZ = 0; + double angVelSumX = 0, angVelSumY = 0, angVelSumZ = 0; + for (int i = 0; i < n; i++) + { + linVelSumX += odomLinVels[i].X(); + linVelSumY += odomLinVels[i].Y(); + linVelSumZ += odomLinVels[i].Z(); + + angVelSumX += odomAngVels[i].X(); + angVelSumY += odomAngVels[i].Y(); + angVelSumZ += odomAngVels[i].Z(); + } + + // Check that the mean values are close to zero. + EXPECT_NEAR(linVelSumX/n, 0, 0.3); + EXPECT_NEAR(linVelSumY/n, 0, 0.3); + EXPECT_NEAR(linVelSumZ/n, 0, 0.3); + + EXPECT_NEAR(angVelSumX/n, 0, 0.3); + EXPECT_NEAR(angVelSumY/n, 0, 0.3); + EXPECT_NEAR(angVelSumZ/n, 0, 0.3); + + // Calculate the variation (sigma^2). + double linVelSqSumX = 0, linVelSqSumY = 0, linVelSqSumZ = 0; + double angVelSqSumX = 0, angVelSqSumY = 0, angVelSqSumZ = 0; + for (int i = 0; i < n; i++) + { + linVelSqSumX += std::pow(odomLinVels[i].X() - linVelSumX/n, 2); + linVelSqSumY += std::pow(odomLinVels[i].Y() - linVelSumY/n, 2); + linVelSqSumZ += std::pow(odomLinVels[i].Z() - linVelSumZ/n, 2); + + angVelSqSumX += std::pow(odomAngVels[i].X() - angVelSumX/n, 2); + angVelSqSumY += std::pow(odomAngVels[i].Y() - angVelSumY/n, 2); + angVelSqSumZ += std::pow(odomAngVels[i].Z() - angVelSumZ/n, 2); + } + + // Verify the variance values. + EXPECT_NEAR(linVelSqSumX/n, 1, 0.3); + EXPECT_NEAR(linVelSqSumY/n, 1, 0.3); + EXPECT_NEAR(linVelSqSumZ/n, 1, 0.3); + + EXPECT_NEAR(angVelSqSumX/n, 1, 0.3); + EXPECT_NEAR(angVelSqSumY/n, 1, 0.3); + EXPECT_NEAR(angVelSqSumZ/n, 1, 0.3); + + // Check the covariance matrix. + EXPECT_EQ(odomTwistCovariance.size(), 36); + for (int i = 0; i < 36; i++) + { + if (i % 7 == 0) + { + EXPECT_NEAR(odomTwistCovariance.Get(i), 1, 1e-2); + } + else + { + EXPECT_NEAR(odomTwistCovariance.Get(i), 0, 1e-2); + } + } + } }; ///////////////////////////////////////////////// @@ -502,6 +607,16 @@ TEST_P(OdometryPublisherTest, "/model/vehicle/odometry"); } +///////////////////////////////////////////////// +TEST_P(OdometryPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(GaussianNoiseTest)) +{ + TestGaussianNoise( + std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/odometry_noise.sdf", + "/model/vehicle/odometry_with_covariance"); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, OdometryPublisherTest, ::testing::Range(1, 2)); diff --git a/test/worlds/odometry_noise.sdf b/test/worlds/odometry_noise.sdf new file mode 100644 index 0000000000..e5d158f5a5 --- /dev/null +++ b/test/worlds/odometry_noise.sdf @@ -0,0 +1,231 @@ + + + + + + 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 + + + + + + + 10 -10 0 0 0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + 1 + + + + + +