From c24a4e9ba0ce25b98e62260f8e8d974276660fc6 Mon Sep 17 00:00:00 2001 From: Aditya Pande Date: Fri, 11 Mar 2022 15:53:54 -0800 Subject: [PATCH] Add xyz and rpy offset to published odometry pose (#1341) * Added xyz and rpy offset to published pose Signed-off-by: Aditya --- .../odometry_publisher/OdometryPublisher.cc | 19 +- .../odometry_publisher/OdometryPublisher.hh | 8 + test/integration/odometry_publisher.cc | 56 +++++ test/worlds/odometry_offset.sdf | 232 ++++++++++++++++++ 4 files changed, 314 insertions(+), 1 deletion(-) create mode 100644 test/worlds/odometry_offset.sdf diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index 3a591d75f5..0a8a588b35 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 = {0, 0, 0, 0, 0, 0}; }; ////////////////////////////////////////////////// @@ -142,6 +145,19 @@ void OdometryPublisher::Configure(const Entity &_entity, this->dataPtr->odomFrame = _sdf->Get("odom_frame"); } + 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(_sdf->Get( + "rpy_offset")); + } + this->dataPtr->robotBaseFrame = this->dataPtr->model.Name(_ecm) + "/" + "base_footprint"; if (!_sdf->HasElement("robot_base_frame")) @@ -257,7 +273,8 @@ 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 = 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, diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc index c60e19588e..7912cb7986 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(), 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); + 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..ec5468ff63 --- /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 + + + + + + + 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 -1 0 + 1.5708 0 0 + + + + + +