diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 6d12c5fc04..65ddc67689 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -97,6 +97,7 @@ add_subdirectory(logical_camera) add_subdirectory(magnetometer) add_subdirectory(multicopter_motor_model) add_subdirectory(multicopter_control) +add_subdirectory(odometry_publisher) add_subdirectory(optical_tactile_plugin) add_subdirectory(particle_emitter) add_subdirectory(particle_emitter2) diff --git a/src/systems/odometry_publisher/CMakeLists.txt b/src/systems/odometry_publisher/CMakeLists.txt new file mode 100644 index 0000000000..1b12fd3fde --- /dev/null +++ b/src/systems/odometry_publisher/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(odometry-publisher + SOURCES + OdometryPublisher.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc new file mode 100644 index 0000000000..ac452aae68 --- /dev/null +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -0,0 +1,285 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "OdometryPublisher.hh" + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::OdometryPublisherPrivate +{ + /// \brief Calculates odometry and publishes an odometry message. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Ignition communication node. + public: transport::Node node; + + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief Name of the world-fixed coordinate frame for the odometry message. + public: std::string odomFrame; + + /// \brief Name of the coordinate frame rigidly attached to the mobile + /// robot base. + public: std::string robotBaseFrame; + + /// \brief Update period calculated from . + public: std::chrono::steady_clock::duration odomPubPeriod{0}; + + /// \brief Last sim time odom was published. + public: std::chrono::steady_clock::duration lastOdomPubTime{0}; + + /// \brief Diff drive odometry message publisher. + public: transport::Node::Publisher odomPub; + + /// \brief Rolling mean accumulators for the linear velocity + public: std::pair linearMean; + + /// \brief Rolling mean accumulators for the angular velocity + public: math::RollingMean angularMean; + + /// \brief Initialized flag. + public: bool initialized{false}; + + /// \brief Current pose of the model in the odom frame. + public: math::Pose3d lastUpdatePose{0, 0, 0, 0, 0, 0}; + + /// \brief Current timestamp. + public: math::clock::time_point lastUpdateTime; +}; + +////////////////////////////////////////////////// +OdometryPublisher::OdometryPublisher() + : dataPtr(std::make_unique()) +{ + this->dataPtr->linearMean.first.SetWindowSize(10); + this->dataPtr->linearMean.second.SetWindowSize(10); + this->dataPtr->angularMean.SetWindowSize(10); + this->dataPtr->linearMean.first.Clear(); + this->dataPtr->linearMean.second.Clear(); + this->dataPtr->angularMean.Clear(); +} + +////////////////////////////////////////////////// +void OdometryPublisher::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->model = Model(_entity); + + if (!this->dataPtr->model.Valid(_ecm)) + { + ignerr << "DiffDrive plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + + this->dataPtr->odomFrame = this->dataPtr->model.Name(_ecm) + "/" + "odom"; + if (!_sdf->HasElement("odom_frame")) + { + ignwarn << "OdometryPublisher system plugin missing , " + << "defaults to \"" << this->dataPtr->odomFrame << "\"" << std::endl; + } + else + { + this->dataPtr->odomFrame = _sdf->Get("odom_frame"); + } + + this->dataPtr->robotBaseFrame = this->dataPtr->model.Name(_ecm) + + "/" + "base_footprint"; + if (!_sdf->HasElement("robot_base_frame")) + { + ignwarn << "OdometryPublisher system plugin missing , " + << "defaults to \"" << this->dataPtr->robotBaseFrame << "\"" << std::endl; + } + else + { + this->dataPtr->robotBaseFrame = _sdf->Get("robot_base_frame"); + } + + double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; + if (odomFreq > 0) + { + std::chrono::duration period{1 / odomFreq}; + this->dataPtr->odomPubPeriod = + std::chrono::duration_cast(period); + } + + // Setup odometry + std::string odomTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/odometry"}; + if (_sdf->HasElement("odom_topic")) + odomTopic = _sdf->Get("odom_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); +} + +////////////////////////////////////////////////// +void OdometryPublisher::PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("OdometryPublisher::PreUpdate"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + // Create the pose component if it does not exist. + auto pos = _ecm.Component( + this->dataPtr->model.Entity()); + if (!pos) + { + _ecm.CreateComponent(this->dataPtr->model.Entity(), + components::Pose()); + } +} + +////////////////////////////////////////////////// +void OdometryPublisher::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + IGN_PROFILE("OdometryPublisher::PostUpdate"); + // Nothing left to do if paused. + if (_info.paused) + return; + + this->dataPtr->UpdateOdometry(_info, _ecm); +} + +////////////////////////////////////////////////// +void OdometryPublisherPrivate::UpdateOdometry( + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("DiffDrive::UpdateOdometry"); + // Record start time. + if (!this->initialized) + { + this->lastUpdateTime = std::chrono::steady_clock::time_point(_info.simTime); + this->initialized = true; + return; + } + + // Construct the odometry message and publish it. + msgs::Odometry msg; + + const std::chrono::duration dt = + std::chrono::steady_clock::time_point(_info.simTime) - lastUpdateTime; + // We cannot estimate the speed if the time interval is zero (or near + // zero). + if (math::equal(0.0, dt.count())) + return; + + // Get and set robotBaseFrame to odom transformation. + const math::Pose3d pose = worldPose(this->model.Entity(), _ecm); + 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()); + + // Get linear and angular displacements from last updated pose. + double linearDisplacementX = pose.Pos().X() - this->lastUpdatePose.Pos().X(); + double linearDisplacementY = pose.Pos().Y() - this->lastUpdatePose.Pos().Y(); + + double currentYaw = pose.Rot().Yaw(); + const double lastYaw = this->lastUpdatePose.Rot().Yaw(); + while (currentYaw < lastYaw - M_PI) currentYaw += 2 * M_PI; + while (currentYaw > lastYaw + M_PI) currentYaw -= 2 * M_PI; + const float angularDiff = currentYaw - lastYaw; + + // Get velocities in robotBaseFrame and add to message. + double linearVelocityX = (cosf(currentYaw) * linearDisplacementX + + sinf(currentYaw) * linearDisplacementY) / dt.count(); + double linearVelocityY = (cosf(currentYaw) * linearDisplacementY + - sinf(currentYaw) * linearDisplacementX) / dt.count(); + this->linearMean.first.Push(linearVelocityX); + this->linearMean.second.Push(linearVelocityY); + this->angularMean.Push(angularDiff / dt.count()); + msg.mutable_twist()->mutable_linear()->set_x(this->linearMean.first.Mean()); + msg.mutable_twist()->mutable_linear()->set_y(this->linearMean.second.Mean()); + msg.mutable_twist()->mutable_angular()->set_z(this->angularMean.Mean()); + + // Set the time stamp in the header. + msg.mutable_header()->mutable_stamp()->CopyFrom( + convert(_info.simTime)); + + // Set the frame ids. + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(odomFrame); + auto childFrame = msg.mutable_header()->add_data(); + childFrame->set_key("child_frame_id"); + childFrame->add_value(robotBaseFrame); + + this->lastUpdatePose = pose; + this->lastUpdateTime = std::chrono::steady_clock::time_point(_info.simTime); + + // Throttle publishing. + auto diff = _info.simTime - this->lastOdomPubTime; + if (diff > std::chrono::steady_clock::duration::zero() && + diff < this->odomPubPeriod) + { + return; + } + this->lastOdomPubTime = _info.simTime; + this->odomPub.Publish(msg); +} + +IGNITION_ADD_PLUGIN(OdometryPublisher, + ignition::gazebo::System, + OdometryPublisher::ISystemConfigure, + OdometryPublisher::ISystemPreUpdate, + OdometryPublisher::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(OdometryPublisher, + "ignition::gazebo::systems::OdometryPublisher") diff --git a/src/systems/odometry_publisher/OdometryPublisher.hh b/src/systems/odometry_publisher/OdometryPublisher.hh new file mode 100644 index 0000000000..1937dfd752 --- /dev/null +++ b/src/systems/odometry_publisher/OdometryPublisher.hh @@ -0,0 +1,91 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_ODOMETRYPUBLISHER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_ODOMETRYPUBLISHER_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class OdometryPublisherPrivate; + + /// \brief Odometry Publisher which can be attached to any entity in + /// order to periodically publish 2D odometry data in the form of + /// ignition::msgs::Odometry messages. + /// + /// # System Parameters + /// + /// ``: Name of the world-fixed coordinate frame for the + // odometry message. This element is optional, and the default value + /// is `{name_of_model}/odom`. + /// + /// ``: Name of the coordinate frame rigidly attached + /// to the mobile robot base. This element is optional, and the default + /// value is `{name_of_model}/base_footprint`. + /// + /// ``: Odometry publication frequency. This + /// element is optional, and the default value is 50Hz. + /// + /// ``: Custom topic on which this system will publish odometry + /// messages. This element is optional, and the default value is + /// `/model/{name_of_model}/odometry`. + class IGNITION_GAZEBO_VISIBLE OdometryPublisher + : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: OdometryPublisher(); + + /// \brief Destructor + public: ~OdometryPublisher() override = default; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + // Documentation inherited + public: void PostUpdate( + const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 59a3c003a9..c423de553c 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -36,6 +36,7 @@ set(tests multicopter.cc multiple_servers.cc network_handshake.cc + odometry_publisher.cc particle_emitter.cc particle_emitter2.cc performer_detector.cc diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc new file mode 100644 index 0000000000..248f33e18d --- /dev/null +++ b/test/integration/odometry_publisher.cc @@ -0,0 +1,313 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include +#include + +#include "ignition/gazebo/components/AngularVelocity.hh" +#include "ignition/gazebo/components/AngularVelocityCmd.hh" +#include "ignition/gazebo/components/LinearVelocity.hh" +#include "ignition/gazebo/components/LinearVelocityCmd.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/Relay.hh" + +#define tol 0.005 + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test OdometryPublisher system +class OdometryPublisherTest : public ::testing::TestWithParam +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); + } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _odomTopic Odometry topic. + protected: void TestMovement(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 vehicle poses and velocities + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle")); + EXPECT_NE(kNullEntity, id); + + auto poseComp = _ecm.Component(id); + ASSERT_NE(nullptr, poseComp); + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + // Run server while model is stationary + server.Run(true, 1000, false); + + EXPECT_EQ(1000u, poses.size()); + + for (const auto &pose : poses) + { + EXPECT_EQ(poses[0], pose); + } + + // 50 Hz is the default publishing freq + double period{1.0 / 50.0}; + double lastMsgTime{1.0}; + std::vector odomPoses; + std::vector odomLinVels; + std::vector odomAngVels; + // Create function to store data from odometry messages + std::function odomCb = + [&](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + double msgTime = + static_cast(_msg.header().stamp().sec()) + + static_cast(_msg.header().stamp().nsec()) * 1e-9; + + EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period); + lastMsgTime = msgTime; + + odomPoses.push_back(msgs::Convert(_msg.pose())); + odomLinVels.push_back(msgs::Convert(_msg.twist().linear())); + odomAngVels.push_back(msgs::Convert(_msg.twist().angular())); + }; + transport::Node node; + node.Subscribe(_odomTopic, odomCb); + + test::Relay velocityRamp; + math::Vector3d linVelCmd(1, 0.5, 0.0); + math::Vector3d angVelCmd(0.0, 0.0, 0.2); + velocityRamp.OnPreUpdate( + [&](const gazebo::UpdateInfo &/*_info*/, + gazebo::EntityComponentManager &_ecm) + { + auto en = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle")); + EXPECT_NE(kNullEntity, en); + + // Set the linear velocity of the model + auto linVelCmdComp = + _ecm.Component(en); + if (!linVelCmdComp) + { + _ecm.CreateComponent(en, + components::LinearVelocityCmd(linVelCmd)); + } + else + { + linVelCmdComp->Data() = linVelCmd; + } + + // Set the angular velocity of the model + auto angVelCmdComp = + _ecm.Component(en); + if (!angVelCmdComp) + { + _ecm.CreateComponent(en, + components::AngularVelocityCmd(angVelCmd)); + } + else + { + angVelCmdComp->Data() = angVelCmd; + } + }); + + server.AddSystem(velocityRamp.systemPtr); + + // Run server while the model moves with the velocities set earlier + server.Run(true, 3000, false); + + // Poses for 4s + ASSERT_EQ(4000u, poses.size()); + + 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); + + // Odom for 3s + ASSERT_FALSE(odomPoses.empty()); + EXPECT_EQ(150u, odomPoses.size()); + EXPECT_EQ(150u, odomLinVels.size()); + EXPECT_EQ(150u, odomAngVels.size()); + + // Check accuracy of poses published in the odometry message + auto finalModelFramePose = odomPoses.back(); + EXPECT_NEAR(poses[1020].Pos().X(), odomPoses[0].Pos().X(), 1e-2); + EXPECT_NEAR(poses[1020].Pos().Y(), odomPoses[0].Pos().Y(), 1e-2); + EXPECT_NEAR(poses[1020].Pos().Z(), odomPoses[0].Pos().Z(), 1e-2); + EXPECT_NEAR(poses[1020].Rot().X(), odomPoses[0].Rot().X(), 1e-2); + EXPECT_NEAR(poses[1020].Rot().Y(), odomPoses[0].Rot().Y(), 1e-2); + EXPECT_NEAR(poses[1020].Rot().Z(), odomPoses[0].Rot().Z(), 1e-2); + EXPECT_NEAR(poses.back().Pos().X(), finalModelFramePose.Pos().X(), 1e-2); + EXPECT_NEAR(poses.back().Pos().Y(), finalModelFramePose.Pos().Y(), 1e-2); + EXPECT_NEAR(poses.back().Pos().Z(), finalModelFramePose.Pos().Z(), 1e-2); + EXPECT_NEAR(poses.back().Rot().X(), finalModelFramePose.Rot().X(), 1e-2); + EXPECT_NEAR(poses.back().Rot().Y(), finalModelFramePose.Rot().Y(), 1e-2); + EXPECT_NEAR(poses.back().Rot().Z(), finalModelFramePose.Rot().Z(), 1e-2); + + // Check accuracy of velocities published in the odometry message + EXPECT_NEAR(odomLinVels[5].X(), linVelCmd[0], 1e-1); + EXPECT_NEAR(odomLinVels[5].Y(), linVelCmd[1], 1e-1); + EXPECT_NEAR(odomLinVels[5].Z(), 0.0, 1e-1); + EXPECT_NEAR(odomAngVels[5].X(), 0.0, 1e-1); + EXPECT_NEAR(odomAngVels[5].Y(), 0.0, 1e-1); + EXPECT_NEAR(odomAngVels[5].Z(), angVelCmd[2], 1e-1); + + EXPECT_NEAR(odomLinVels.back().X(), linVelCmd[0], 1e-1); + EXPECT_NEAR(odomLinVels.back().Y(), linVelCmd[1], 1e-1); + EXPECT_NEAR(odomLinVels.back().Z(), 0.0, 1e-1); + EXPECT_NEAR(odomAngVels.back().X(), 0.0, 1e-1); + EXPECT_NEAR(odomAngVels.back().Y(), 0.0, 1e-1); + EXPECT_NEAR(odomAngVels.back().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 + /// for the odometry message. + /// \param[in] _childFrameId Name of the coordinate frame rigidly + /// attached to the mobile robot base. + protected: void TestPublishCmd(const std::string &_sdfFile, + const std::string &_odomTopic, + const std::string &_frameId, + const std::string &_childFrameId) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + unsigned int odomPosesCount = 0; + std::function odomCb = + [&odomPosesCount, &_frameId, &_childFrameId](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + ASSERT_GT(_msg.header().data_size(), 1); + + EXPECT_STREQ(_msg.header().data(0).key().c_str(), "frame_id"); + EXPECT_STREQ( + _msg.header().data(0).value().Get(0).c_str(), + _frameId.c_str()); + + EXPECT_STREQ(_msg.header().data(1).key().c_str(), "child_frame_id"); + EXPECT_STREQ( + _msg.header().data(1).value().Get(0).c_str(), + _childFrameId.c_str()); + + odomPosesCount++; + }; + + transport::Node node; + node.Subscribe(_odomTopic, odomCb); + + server.Run(true, 100, false); + + int sleep = 0; + int maxSleep = 30; + for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); + + EXPECT_EQ(5u, odomPosesCount); + } +}; + +///////////////////////////////////////////////// +TEST_P(OdometryPublisherTest, Movement) +{ + TestMovement( + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/odometry_publisher.sdf", + "/model/vehicle/odometry"); +} + +///////////////////////////////////////////////// +TEST_P(OdometryPublisherTest, MovementCustomTopic) +{ + TestMovement( + std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/odometry_publisher_custom.sdf", + "/model/bar/odom"); +} + +///////////////////////////////////////////////// +TEST_P(OdometryPublisherTest, OdomFrameId) +{ + TestPublishCmd( + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/odometry_publisher.sdf", + "/model/vehicle/odometry", + "vehicle/odom", + "vehicle/base_footprint"); +} + +///////////////////////////////////////////////// +TEST_P(OdometryPublisherTest, OdomCustomFrameId) +{ + TestPublishCmd( + std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/odometry_publisher_custom.sdf", + "/model/bar/odom", + "odomCustom", + "baseCustom"); +} + +// Run multiple times +INSTANTIATE_TEST_SUITE_P(ServerRepeat, OdometryPublisherTest, + ::testing::Range(1, 2)); diff --git a/test/worlds/odometry_publisher.sdf b/test/worlds/odometry_publisher.sdf new file mode 100644 index 0000000000..e572a8cc88 --- /dev/null +++ b/test/worlds/odometry_publisher.sdf @@ -0,0 +1,230 @@ + + + + + + 0.001 + 1.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 + + + + + + + + + diff --git a/test/worlds/odometry_publisher_custom.sdf b/test/worlds/odometry_publisher_custom.sdf new file mode 100644 index 0000000000..7ef913aea1 --- /dev/null +++ b/test/worlds/odometry_publisher_custom.sdf @@ -0,0 +1,234 @@ + + + + + + 0.001 + 1.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 + + + + 50 + /model/bar/odom + odomCustom + baseCustom + + + + + +