Odometry topic for the track controller system

examples/worlds/conveyor.sdf

@@ -121,6 +121,11 @@
  <size>5 0.2 0.1</size>
  </box>
  </geometry>
+ <material>
+ <ambient>0.05 0.05 0.70 1</ambient>
+ <diffuse>0.05 0.05 0.70 1</diffuse>
+ <specular>0.8 0.8 0.8 1</specular>
+ </material>
  </visual>
  <visual name='visual_1'>
  <pose relative_to='base_link'>2.5 0 0 -1.570796327 0 0</pose>
@@ -130,6 +135,11 @@
  <radius>0.05</radius>
  </cylinder>
  </geometry>
+ <material>
+ <ambient>0.05 0.05 0.70 1</ambient>
+ <diffuse>0.05 0.05 0.70 1</diffuse>
+ <specular>0.8 0.8 0.8 1</specular>
+ </material>
  </visual>
  <visual name='visual_2'>
  <pose relative_to='base_link'>-2.5 0 0 -1.570796327 0 0</pose>
@@ -139,6 +149,11 @@
  <radius>0.05</radius>
  </cylinder>
  </geometry>
+ <material>
+ <ambient>0.05 0.05 0.70 1</ambient>
+ <diffuse>0.05 0.05 0.70 1</diffuse>
+ <specular>0.8 0.8 0.8 1</specular>
+ </material>
  </visual>
  <gravity>1</gravity>
  <kinematic>0</kinematic>
@@ -147,6 +162,7 @@
  <plugin filename="gz-sim-track-controller-system"
  name="gz::sim::systems::TrackController">
  <link>base_link</link>
+ <odometry_publish_frequency>1</odometry_publish_frequency>
  <!--debug>true</debug-->
  </plugin>
 
@@ -157,7 +173,7 @@
  <match field="data">87</match>
  </input>
  <output type="gz.msgs.Double" topic="/model/conveyor/link/base_link/track_cmd_vel">
- data: 10.0
+ data: 1.0
  </output>
  </plugin>
 
@@ -185,7 +201,7 @@
  </model>
 
  <model name='box'>
- <pose frame=''>0 0 1 0 0 0</pose>
+ <pose>0 0 1 0 0 0</pose>
  <link name='base_link'>
  <inertial>
  <mass>1.06</mass>
@@ -206,7 +222,9 @@
  </box>
  </geometry>
  <material>
- <ambient>1 1 1 1</ambient>
+ <ambient>0.60 0.0 0.0 1</ambient>
+ <diffuse>0.60 0.0 0.0 1</diffuse>
+ <specular>0.8 0.8 0.8 1</specular>
  </material>
  </visual>
  <collision name='main_collision'>
@@ -273,6 +291,25 @@
  </gz-gui>
  </plugin>
 
+ <plugin name='World control' filename='WorldControl'>
+ <gz-gui>
+ <title>World control</title>
+ <property type='bool' key='showTitleBar'>0</property>
+ <property type='bool' key='resizable'>0</property>
+ <property type='double' key='height'>72</property>
+ <property type='double' key='width'>150</property>
+ <property type='double' key='z'>1</property>
+ <property type='string' key='state'>floating</property>
+ <anchors target='3D View'>
+ <line own='left' target='left'/>
+ <line own='bottom' target='bottom'/>
+ </anchors>
+ </gz-gui>
+ <play_pause>1</play_pause>
+ <step>1</step>
+ <start_paused>0</start_paused>
+ </plugin>
+
  <!-- World statistics -->
  <plugin filename="WorldStats" name="World stats">
  <gz-gui>

src/systems/track_controller/TrackController.cc

@@ -25,10 +25,12 @@
 
 #include <gz/msgs/double.pb.h>
 #include <gz/msgs/marker.pb.h>
+#include <gz/msgs/odometry.pb.h>
 #include <gz/msgs/Utility.hh>
 
 #include <gz/math/eigen3.hh>
 #include <gz/math/SpeedLimiter.hh>
+#include <gz/math/Helpers.hh>
 
 #include <gz/plugin/Register.hh>
 #include <gz/transport/Node.hh>
@@ -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<Entity, math::Pose3d> 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 <odometry_publish_frequency>.
+ 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<std::string>(
+ "odometry_topic", kDefaultOdometryTopic).first, kDefaultOdometryTopic});
+ this->dataPtr->odometryPub =
+ this->dataPtr->node.Advertise<msgs::Odometry>(odometryTopic);
+
+ double odometryFreq = _sdf->Get<double>(
+ "odometry_publish_frequency", 50).first;
+ std::chrono::duration<double> odomPer{0.0};
+ if (odometryFreq > 0)
+ {
+ odomPer = std::chrono::duration<double>(1 / odometryFreq);
+ this->dataPtr->odometryPubPeriod =
+ std::chrono::duration_cast<std::chrono::steady_clock::duration>(odomPer);
+ }
+ gzdbg << "Publishing odometry to " << odometryTopic
+ << " with period " << odomPer.count() << " seconds." << std::endl;
+
+
  this->dataPtr->trackOrientation = _sdf->Get<math::Quaterniond>(
  "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<double>(_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<msgs::Time>(_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")

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.
  ///
+ /// `<odometry_topic>` 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`.
+ ///
+ /// `<odometry_publish_frequency>` the frequency (in Hz) at which the
+ /// odometry messages are published. Defaults to 50 Hz.
+ ///
  /// `<min_velocity>`/`<max_velocity>` 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<TrackControllerPrivate> dataPtr;