diff --git a/examples/worlds/conveyor.sdf b/examples/worlds/conveyor.sdf index 940dd4af3f..61ba5eb159 100644 --- a/examples/worlds/conveyor.sdf +++ b/examples/worlds/conveyor.sdf @@ -121,6 +121,11 @@ 5 0.2 0.1 + + 0.05 0.05 0.70 1 + 0.05 0.05 0.70 1 + 0.8 0.8 0.8 1 + 2.5 0 0 -1.570796327 0 0 @@ -130,6 +135,11 @@ 0.05 + + 0.05 0.05 0.70 1 + 0.05 0.05 0.70 1 + 0.8 0.8 0.8 1 + -2.5 0 0 -1.570796327 0 0 @@ -139,6 +149,11 @@ 0.05 + + 0.05 0.05 0.70 1 + 0.05 0.05 0.70 1 + 0.8 0.8 0.8 1 + 1 0 @@ -147,6 +162,7 @@ base_link + 1 @@ -157,7 +173,7 @@ 87 - data: 10.0 + data: 1.0 @@ -185,7 +201,7 @@ - 0 0 1 0 0 0 + 0 0 1 0 0 0 1.06 @@ -206,7 +222,9 @@ - 1 1 1 1 + 0.60 0.0 0.0 1 + 0.60 0.0 0.0 1 + 0.8 0.8 0.8 1 @@ -273,6 +291,25 @@ + + + World control + 0 + 0 + 72 + 150 + 1 + floating + + + + + + 1 + 1 + 0 + + diff --git a/src/systems/track_controller/TrackController.cc b/src/systems/track_controller/TrackController.cc index 4ca4041ddf..a77df733e9 100644 --- a/src/systems/track_controller/TrackController.cc +++ b/src/systems/track_controller/TrackController.cc @@ -25,10 +25,12 @@ #include #include +#include #include #include #include +#include #include #include @@ -140,6 +142,8 @@ class gz::sim::systems::TrackControllerPrivate /// \brief World poses of all collision elements of the track's link. public: std::unordered_map collisionsWorldPose; + /// \brief Track position + public: double position {0}; /// \brief The last commanded velocity. public: double velocity {0}; /// \brief Commanded velocity clipped to allowable range. @@ -148,8 +152,9 @@ class gz::sim::systems::TrackControllerPrivate public: double prevVelocity {0}; /// \brief Second previous clipped commanded velocity. public: double prevPrevVelocity {0}; + /// \brief The point around which the track circles (in world coords). Should - /// be set to +Inf if the track is going straight. + /// be set to Inf or NaN if the track is going straight. public: math::Vector3d centerOfRotation {math::Vector3d::Zero * math::INF_D}; /// \brief protects velocity and centerOfRotation public: std::mutex cmdMutex; @@ -169,6 +174,13 @@ class gz::sim::systems::TrackControllerPrivate /// \brief Limiter of the commanded velocity. public: math::SpeedLimiter limiter; + + /// \brief Odometry message publisher. + public: transport::Node::Publisher odometryPub; + /// \brief Update period calculated from . + public: std::chrono::steady_clock::duration odometryPubPeriod{0}; + /// \brief Last sim time the odometry was published. + public: std::chrono::steady_clock::duration lastOdometryPubTime{0}; }; ////////////////////////////////////////////////// @@ -268,6 +280,26 @@ void TrackController::Configure(const Entity &_entity, gzdbg << "Subscribed to " << corTopic << " for receiving track center " << "of rotation commands." << std::endl; + // Publish track odometry + const auto kDefaultOdometryTopic = topicPrefix + "/odometry"; + const auto odometryTopic = validTopic({_sdf->Get( + "odometry_topic", kDefaultOdometryTopic).first, kDefaultOdometryTopic}); + this->dataPtr->odometryPub = + this->dataPtr->node.Advertise(odometryTopic); + + double odometryFreq = _sdf->Get( + "odometry_publish_frequency", 50).first; + std::chrono::duration odomPer{0.0}; + if (odometryFreq > 0) + { + odomPer = std::chrono::duration(1 / odometryFreq); + this->dataPtr->odometryPubPeriod = + std::chrono::duration_cast(odomPer); + } + gzdbg << "Publishing odometry to " << odometryTopic + << " with period " << odomPer.count() << " seconds." << std::endl; + + this->dataPtr->trackOrientation = _sdf->Get( "track_orientation", math::Quaterniond::Identity).first; @@ -423,6 +455,10 @@ void TrackController::PreUpdate( this->dataPtr->prevVelocity, this->dataPtr->prevPrevVelocity, _info.dt); + // Integrate track position + const double dtSec = std::chrono::duration(_info.dt).count(); + this->dataPtr->position += ( this->dataPtr->limitedVelocity * dtSec ); + this->dataPtr->prevPrevVelocity = this->dataPtr->prevVelocity; this->dataPtr->prevVelocity = this->dataPtr->limitedVelocity; @@ -433,6 +469,71 @@ void TrackController::PreUpdate( } } +////////////////////////////////////////////////// +void TrackController::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager & /*_ecm*/) +{ + // Nothing left to do if paused. + if (_info.paused) + return; + + // Throttle publishing + auto diff = _info.simTime - this->dataPtr->lastOdometryPubTime; + if (diff < this->dataPtr->odometryPubPeriod) + { + return; + } + this->dataPtr->lastOdometryPubTime = _info.simTime; + + + // Construct the odometry message and publish it: + // + // Only odometry info is published (i.e. no other kinematic state info such + // as acceleration or jerk), as these are the only known values. + // E.g. at timestep 'k': + // - For an ideal system: (position k) = (position k-1) + (velocity k-1) * dt, + // - And (velocity k) is known from the velocity command (possibly limited by + // the SpeedLimiter). + // However, since this is a velocity-resolved controler, (acceleration k) + // and (jerk k) are unknown, e.g.: + // (acceleration k) = ( (velocity k+1) - (velocity k) ) / dt + // in which (velocity k+1) is unknown in timestep k. + // + // Note that, in case of a lower publish frequency than the simulation + // frequency, a similar issue exists for the velocity, since only the + // instantaneous velocity is known at each time step, and not the average + // velocity. E.g. consider: + // + // Time 0 1 2 3 4 5 + // Velocity 10 10 10 10 0 0 + // Position 0 10 20 30 40 40 + // + // with publish at: + // - time 0: position 0 and velocity 10 + // - time 5: position 40 and velocity 0 + // + // For '(pos k) = (pos k-1) + (vel k-1) * dt' to hold, with k = time 5 + // and k-1 = time 0, the reported velocity at time 0 should be '8': + // (40 - 0) / 5 = 8 (i.e. the average velocity over time 0 to 5), + // instead of the reported (instantaneous) velocity '10'. + // + // Imo. this error is acceptable, as real life sensors (e.g. encoder and + // resolver) also report instantaneous values for position and velocity. + // + msgs::Odometry msg; + + // Set the time stamp in the header + msg.mutable_header()->mutable_stamp()->CopyFrom( + convert(_info.simTime)); + + // Set position and velocity + msg.mutable_pose()->mutable_position()->set_x(this->dataPtr->position); + msg.mutable_twist()->mutable_linear()->set_x(this->dataPtr->limitedVelocity); + + this->dataPtr->odometryPub.Publish(msg); +} + + ////////////////////////////////////////////////// void TrackControllerPrivate::ComputeSurfaceProperties( const Entity& _collision1, @@ -616,7 +717,8 @@ void TrackControllerPrivate::OnCenterOfRotation(const msgs::Vector3d& _msg) GZ_ADD_PLUGIN(TrackController, System, TrackController::ISystemConfigure, - TrackController::ISystemPreUpdate) + TrackController::ISystemPreUpdate, + TrackController::ISystemPostUpdate) GZ_ADD_PLUGIN_ALIAS(TrackController, "gz::sim::systems::TrackController") diff --git a/src/systems/track_controller/TrackController.hh b/src/systems/track_controller/TrackController.hh index 2e366a15e1..ff0e95f323 100644 --- a/src/systems/track_controller/TrackController.hh +++ b/src/systems/track_controller/TrackController.hh @@ -82,6 +82,14 @@ namespace systems /// rotation command will only act for the given number of seconds and the /// track will be stopped if no command arrives before this timeout. /// + /// `` The topic on which the track odometry (i.e. position + /// and instantaneous velocity) is published. This can be used e.g. to + /// simulate a conveyor with encoder feedback. + /// Defaults to `/model/${model_name}/link/${link_name}/odometry`. + /// + /// `` the frequency (in Hz) at which the + /// odometry messages are published. Defaults to 50 Hz. + /// /// ``/`` Min/max velocity of the track (m/s). /// If not specified, the velocity is not limited (however the physics will, /// in the end, have some implicit limit). @@ -96,7 +104,8 @@ namespace systems class TrackController : public System, public ISystemConfigure, - public ISystemPreUpdate + public ISystemPreUpdate, + public ISystemPostUpdate { /// \brief Constructor public: TrackController(); @@ -112,8 +121,12 @@ namespace systems // Documentation inherited public: void PreUpdate( - const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) override; + const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + // Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; /// \brief Private data pointer private: std::unique_ptr dataPtr; diff --git a/test/integration/tracked_vehicle_system.cc b/test/integration/tracked_vehicle_system.cc index 64b4fbc0f9..919939f79c 100644 --- a/test/integration/tracked_vehicle_system.cc +++ b/test/integration/tracked_vehicle_system.cc @@ -62,6 +62,28 @@ void verifyPose(const math::Pose3d& pose1, const math::Pose3d& pose2) EXPECT_ANGLE_NEAR(pose1.Rot().Yaw(), pose2.Rot().Yaw(), 1e-1); } +/// \brief Helper function to wait until a predicate is true or a timeout occurs +/// \tparam Pred Predicate function of type bool() +/// \param[in] _timeoutMs Timeout in milliseconds +template +bool waitUntil(int _timeoutMs, Pred _pred) +{ + using namespace std::chrono; + auto tStart = steady_clock::now(); + auto sleepDur = milliseconds(std::min(100, _timeoutMs)); + auto waitDuration = milliseconds(_timeoutMs); + while (duration_cast(steady_clock::now() - tStart) < + waitDuration) + { + if (_pred()) + { + return true; + } + std::this_thread::sleep_for(sleepDur); + } + return false; +} + /// \brief Test TrackedVehicle system. This test drives a tracked robot over a /// course of obstacles and verifies that it is able to climb on/over them. class TrackedVehicleTest : public InternalFixture<::testing::Test> @@ -442,7 +464,8 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> /// \param[in] _sdfFile SDF file to load. /// \param[in] _cmdVelTopic Command velocity topic. protected: void TestConveyor(const std::string &_sdfFile, - const std::string &_cmdVelTopic) + const std::string &_cmdVelTopic, + const std::string &_odometryTopic) { // Start server ServerConfig serverConfig; @@ -452,6 +475,22 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> EXPECT_FALSE(server.Running()); EXPECT_FALSE(*server.Running(0)); + // gz::transport node + transport::Node node; + + // subscribe to odometry + msgs::Odometry odometryMsg; + unsigned int odomMsgCounter = 0; + + auto msgCb = std::function( + [&odometryMsg, &odomMsgCounter](const auto & msg) + { + odometryMsg = msg; + odomMsgCounter++; + }); + + node.Subscribe(_odometryTopic, msgCb); + // Create a system that records the vehicle poses test::Relay ecmGetterSystem; EntityComponentManager* ecm {nullptr}; @@ -495,8 +534,12 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> server.Run(true, 1000, false); + // Wait for messages + waitUntil(5000, [&]{return 50 == odomMsgCounter;}); + // Poses for 1s ASSERT_EQ(1000u, poses.size()); + ASSERT_EQ(50u, odomMsgCounter); // check that the box has not moved in X and Y directions (it will move in // Z as it falls down on the conveyor) @@ -506,10 +549,25 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + // check reported odometry pose and twist + EXPECT_NEAR(0, odometryMsg.pose().position().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().position().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().position().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().w(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().linear().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().linear().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().linear().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().z(), 1e-6); + poses.clear(); + odomMsgCounter = 0; // Publish command and check that vehicle moved - transport::Node node; auto pub = node.Advertise(_cmdVelTopic); // In this test, there is a long conveyor and a small box at its center. @@ -525,8 +583,12 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> server.Run(true, 1000, false); + // Wait for messages + waitUntil(5000, [&]{return 50 == odomMsgCounter;}); + // Poses for 1s ASSERT_EQ(1000u, poses.size()); + ASSERT_EQ(50u, odomMsgCounter); EXPECT_NEAR(0.125, poses.back().Pos().X(), 1e-1); EXPECT_NEAR(poses[0].Pos().Y(), poses.back().Pos().Y(), 1e-2); @@ -535,10 +597,29 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + // check reported odometry pose and twist + EXPECT_NEAR(0.125, odometryMsg.pose().position().x(), 1e-1); + EXPECT_NEAR(0, odometryMsg.pose().position().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().position().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().w(), 1e-6); + EXPECT_NEAR(0.25, odometryMsg.twist().linear().x(), 1e-1); + EXPECT_NEAR(0, odometryMsg.twist().linear().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().linear().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().z(), 1e-6); + server.Run(true, 1000, false); + // Wait for messages + waitUntil(5000, [&]{return 100 == odomMsgCounter;}); + // Poses for 2s ASSERT_EQ(2000u, poses.size()); + ASSERT_EQ(100u, odomMsgCounter); EXPECT_NEAR(0.5, poses.back().Pos().X(), 1e-1); EXPECT_NEAR(poses[0].Pos().Y(), poses.back().Pos().Y(), 1e-2); @@ -547,10 +628,29 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + // check reported odometry pose and twist + EXPECT_NEAR(0.5, odometryMsg.pose().position().x(), 1e-1); + EXPECT_NEAR(0, odometryMsg.pose().position().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().position().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().w(), 1e-6); + EXPECT_NEAR(0.5, odometryMsg.twist().linear().x(), 1e-1); + EXPECT_NEAR(0, odometryMsg.twist().linear().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().linear().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().z(), 1e-6); + server.Run(true, 1000, false); + // Wait for messages + waitUntil(5000, [&]{return 150 == odomMsgCounter;}); + // Poses for 3s ASSERT_EQ(3000u, poses.size()); + ASSERT_EQ(150u, odomMsgCounter); EXPECT_NEAR(0.875, poses.back().Pos().X(), 1e-1); EXPECT_NEAR(poses[0].Pos().Y(), poses.back().Pos().Y(), 1e-2); @@ -559,6 +659,21 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + // check reported odometry pose and twist + EXPECT_NEAR(0.875, odometryMsg.pose().position().x(), 1e-1); + EXPECT_NEAR(0, odometryMsg.pose().position().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().position().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().w(), 1e-6); + EXPECT_NEAR(0.25, odometryMsg.twist().linear().x(), 1e-1); + EXPECT_NEAR(0, odometryMsg.twist().linear().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().linear().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().z(), 1e-6); + poses.clear(); } }; @@ -580,5 +695,6 @@ TEST_F(TrackedVehicleTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Conveyor)) this->TestConveyor( std::string(PROJECT_SOURCE_PATH) + "/test/worlds/conveyor.sdf", - "/model/conveyor/link/base_link/track_cmd_vel"); + "/model/conveyor/link/base_link/track_cmd_vel", + "/model/conveyor/link/base_link/odometry"); }