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
@@ -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");
}