Skip to content

Commit

Permalink
Odometry topic for the track controller system (#2021)
Browse files Browse the repository at this point in the history 
This PR adds odometry output to the track controller system, publishing position and instantaneous velocity. That way a conveyor with an encoder and/or resolver for respectively position and velocity feedback can be simulated.

The default topic is /model/<model name>/link/<link name>/odometry.
An alternative can be chosen by specifying the odometry_topic parameter of the plugin in the SDF.
The publication frequency is set through parameter odometry_publish_frequency.
---------

Signed-off-by: Johan Rutgeerts <[email protected]>
  • Loading branch information
@jrutgeer
jrutgeer authored Aug 30, 2023
1 parent 6f4bcca commit f83456f
Show file tree
Hide file tree
Showing 4 changed files with 279 additions and 11 deletions.
43 changes: 40 additions & 3 deletions examples/worlds/conveyor.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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>
Expand All @@ -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>
Expand All @@ -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>

Expand All @@ -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>

Expand Down Expand Up @@ -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>
Expand All @@ -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'>
Expand Down Expand Up @@ -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>
Expand Down
106 changes: 104 additions & 2 deletions src/systems/track_controller/TrackController.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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.
Expand All @@ -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;
Expand All @@ -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};
};

//////////////////////////////////////////////////
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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;

Expand All @@ -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,
Expand Down Expand Up @@ -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")
Expand Down
19 changes: 16 additions & 3 deletions src/systems/track_controller/TrackController.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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).
Expand All @@ -96,7 +104,8 @@ namespace systems
class TrackController
: public System,
public ISystemConfigure,
public ISystemPreUpdate
public ISystemPreUpdate,
public ISystemPostUpdate
{
/// \brief Constructor
public: TrackController();
Expand All @@ -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;
Expand Down
Loading

0 comments on commit f83456f

Please sign in to comment.