From 62e29eae64cd5d2e5054e306eaabeb9101525d4b Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 15 Feb 2022 16:15:20 -0800 Subject: [PATCH 1/5] Added xyz and rpy offset to published pose Signed-off-by: Aditya --- .../odometry_publisher/OdometryPublisher.cc | 28 ++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index 3a591d75f5..5d6720b089 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -92,6 +92,9 @@ class ignition::gazebo::systems::OdometryPublisherPrivate /// \brief Current timestamp. public: math::clock::time_point lastUpdateTime; + + /// \brief Allow specifying constant xyz and rpy offsets + public: ignition::math::Pose3d offset; }; ////////////////////////////////////////////////// @@ -142,6 +145,26 @@ void OdometryPublisher::Configure(const Entity &_entity, this->dataPtr->odomFrame = _sdf->Get("odom_frame"); } + if (!_sdf->HasElement("xyz_offset")) + { + this->dataPtr->offset.Pos() = ignition::math::Vector3d(0, 0, 0); + } + else + { + this->dataPtr->offset.Pos() = _sdf->Get("xyz_offset"); + } + + if (!_sdf->HasElement("rpy_offset")) + { + this->dataPtr->offset.Rot() = + ignition::math::Quaterniond(ignition::math::Vector3d(0, 0, 0)); + } + else + { + this->dataPtr->offset.Rot() = + ignition::math::Quaterniond(_sdf->Get("rpy_offset")); + } + this->dataPtr->robotBaseFrame = this->dataPtr->model.Name(_ecm) + "/" + "base_footprint"; if (!_sdf->HasElement("robot_base_frame")) @@ -257,7 +280,10 @@ void OdometryPublisherPrivate::UpdateOdometry( return; // Get and set robotBaseFrame to odom transformation. - const math::Pose3d pose = worldPose(this->model.Entity(), _ecm); + const math::Pose3d rawPose = worldPose(this->model.Entity(), _ecm); + math::Pose3d pose; + pose.Pos() = rawPose.Pos() + this->offset.Pos(); + pose.Rot() = this->offset.Rot() * rawPose.Rot(); msg.mutable_pose()->mutable_position()->set_x(pose.Pos().X()); msg.mutable_pose()->mutable_position()->set_y(pose.Pos().Y()); msgs::Set(msg.mutable_pose()->mutable_orientation(), pose.Rot()); From f03559eab929ae380ddf80f03b24f3a60c3104fc Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 15 Feb 2022 19:35:17 -0800 Subject: [PATCH 2/5] Added test case Signed-off-by: Aditya --- test/integration/odometry_publisher.cc | 56 ++++++ test/worlds/odometry_offset.sdf | 232 +++++++++++++++++++++++++ 2 files changed, 288 insertions(+) create mode 100644 test/worlds/odometry_offset.sdf diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc index c60e19588e..098d966382 100644 --- a/test/integration/odometry_publisher.cc +++ b/test/integration/odometry_publisher.cc @@ -394,6 +394,52 @@ class OdometryPublisherTest EXPECT_EQ(5u, odomPosesCount); } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _odomTopic Odometry topic. + protected: void TestOffsetTags(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 odomPoses; + // Create function to store data from odometry messages + std::function odomCb = + [&](const msgs::Odometry &_msg) + { + odomPoses.push_back(msgs::Convert(_msg.pose())); + }; + 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 (; odomPoses.size() < 150 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); + + // Run for 3s and check the pose in the last message + ASSERT_FALSE(odomPoses.empty()); + auto lastPose = odomPoses[odomPoses.size() - 1]; + EXPECT_NEAR(lastPose.Pos().X(), 1, 1e-2); + EXPECT_NEAR(lastPose.Pos().Y(), -1, 1e-2); + EXPECT_NEAR(lastPose.Pos().Z(), 0, 1e-2); + + EXPECT_NEAR(lastPose.Rot().Roll(), 1.57, 1e-2); + EXPECT_NEAR(lastPose.Rot().Pitch(), 0, 1e-2); + EXPECT_NEAR(lastPose.Rot().Yaw(), 0, 1e-2); + } }; ///////////////////////////////////////////////// @@ -446,6 +492,16 @@ TEST_P(OdometryPublisherTest, "baseCustom"); } +///////////////////////////////////////////////// +TEST_P(OdometryPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(OffsetTagTest)) +{ + TestOffsetTags( + std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/odometry_offset.sdf", + "/model/vehicle/odometry"); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, OdometryPublisherTest, ::testing::Range(1, 2)); diff --git a/test/worlds/odometry_offset.sdf b/test/worlds/odometry_offset.sdf new file mode 100644 index 0000000000..2e49833a07 --- /dev/null +++ b/test/worlds/odometry_offset.sdf @@ -0,0 +1,232 @@ + + + + + + 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 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 -1 0 + 1.5708 0 0 + + + + + + From efabe271633bd42a77dc6112392cd117ba9df707 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 23 Feb 2022 13:57:07 -0800 Subject: [PATCH 3/5] codecheck fixes Signed-off-by: Aditya --- src/systems/odometry_publisher/OdometryPublisher.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index 5d6720b089..1831e48bcc 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -151,7 +151,8 @@ void OdometryPublisher::Configure(const Entity &_entity, } else { - this->dataPtr->offset.Pos() = _sdf->Get("xyz_offset"); + this->dataPtr->offset.Pos() = _sdf->Get( + "xyz_offset"); } if (!_sdf->HasElement("rpy_offset")) @@ -162,7 +163,8 @@ void OdometryPublisher::Configure(const Entity &_entity, else { this->dataPtr->offset.Rot() = - ignition::math::Quaterniond(_sdf->Get("rpy_offset")); + ignition::math::Quaterniond(_sdf->Get( + "rpy_offset")); } this->dataPtr->robotBaseFrame = this->dataPtr->model.Name(_ecm) From 7a9cea58c95b91d278819bc82a56ba1d354c2fcc Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 2 Mar 2022 12:35:43 -0800 Subject: [PATCH 4/5] offset is wrt body frame, updated doc in header file Signed-off-by: Aditya --- src/systems/odometry_publisher/OdometryPublisher.cc | 4 +--- src/systems/odometry_publisher/OdometryPublisher.hh | 8 ++++++++ 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index 1831e48bcc..7feeee7efc 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -283,9 +283,7 @@ void OdometryPublisherPrivate::UpdateOdometry( // Get and set robotBaseFrame to odom transformation. const math::Pose3d rawPose = worldPose(this->model.Entity(), _ecm); - math::Pose3d pose; - pose.Pos() = rawPose.Pos() + this->offset.Pos(); - pose.Rot() = this->offset.Rot() * rawPose.Rot(); + math::Pose3d pose = rawPose * this->offset; msg.mutable_pose()->mutable_position()->set_x(pose.Pos().X()); msg.mutable_pose()->mutable_position()->set_y(pose.Pos().Y()); msgs::Set(msg.mutable_pose()->mutable_orientation(), pose.Rot()); diff --git a/src/systems/odometry_publisher/OdometryPublisher.hh b/src/systems/odometry_publisher/OdometryPublisher.hh index 832d3ac301..73c328610d 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.hh +++ b/src/systems/odometry_publisher/OdometryPublisher.hh @@ -56,6 +56,14 @@ namespace systems /// ``: Number of dimensions to represent odometry. Only 2 and 3 /// dimensional spaces are supported. This element is optional, and the /// default value is 2. + /// + /// ``: Position offset relative to the body fixed frame, the + /// default value is 0 0 0. This offset will be added to the odometry + /// message. + /// + /// ``: Rotation offset relative to the body fixed frame, the + /// default value is 0 0 0. This offset will be added to the odometry + // message. class OdometryPublisher : public System, public ISystemConfigure, From d314d42789f1f2c6237be493a35d89423909062e Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 8 Mar 2022 12:47:11 -0800 Subject: [PATCH 5/5] Updated example, initialized offset Signed-off-by: Aditya --- .../odometry_publisher/OdometryPublisher.cc | 15 +++------------ test/integration/odometry_publisher.cc | 4 ++-- test/worlds/odometry_offset.sdf | 2 +- 3 files changed, 6 insertions(+), 15 deletions(-) diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index 7feeee7efc..0a8a588b35 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -94,7 +94,7 @@ class ignition::gazebo::systems::OdometryPublisherPrivate public: math::clock::time_point lastUpdateTime; /// \brief Allow specifying constant xyz and rpy offsets - public: ignition::math::Pose3d offset; + public: ignition::math::Pose3d offset = {0, 0, 0, 0, 0, 0}; }; ////////////////////////////////////////////////// @@ -145,22 +145,13 @@ void OdometryPublisher::Configure(const Entity &_entity, this->dataPtr->odomFrame = _sdf->Get("odom_frame"); } - if (!_sdf->HasElement("xyz_offset")) - { - this->dataPtr->offset.Pos() = ignition::math::Vector3d(0, 0, 0); - } - else + if (_sdf->HasElement("xyz_offset")) { this->dataPtr->offset.Pos() = _sdf->Get( "xyz_offset"); } - if (!_sdf->HasElement("rpy_offset")) - { - this->dataPtr->offset.Rot() = - ignition::math::Quaterniond(ignition::math::Vector3d(0, 0, 0)); - } - else + if (_sdf->HasElement("rpy_offset")) { this->dataPtr->offset.Rot() = ignition::math::Quaterniond(_sdf->Get( diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc index 098d966382..7912cb7986 100644 --- a/test/integration/odometry_publisher.cc +++ b/test/integration/odometry_publisher.cc @@ -432,8 +432,8 @@ class OdometryPublisherTest // Run for 3s and check the pose in the last message ASSERT_FALSE(odomPoses.empty()); auto lastPose = odomPoses[odomPoses.size() - 1]; - EXPECT_NEAR(lastPose.Pos().X(), 1, 1e-2); - EXPECT_NEAR(lastPose.Pos().Y(), -1, 1e-2); + EXPECT_NEAR(lastPose.Pos().X(), 11, 1e-2); + EXPECT_NEAR(lastPose.Pos().Y(), -11, 1e-2); EXPECT_NEAR(lastPose.Pos().Z(), 0, 1e-2); EXPECT_NEAR(lastPose.Rot().Roll(), 1.57, 1e-2); diff --git a/test/worlds/odometry_offset.sdf b/test/worlds/odometry_offset.sdf index 2e49833a07..ec5468ff63 100644 --- a/test/worlds/odometry_offset.sdf +++ b/test/worlds/odometry_offset.sdf @@ -52,7 +52,7 @@ - 0 0 0 0 0 0 + 10 -10 0 0 0 0 -0.151427 -0 0.175 0 -0 0