Using math::SpeedLimiter on the diff_drive controller.

src/systems/diff_drive/CMakeLists.txt

@@ -1,7 +1,6 @@
 gz_add_system(diff-drive
   SOURCES
     DiffDrive.cc
-    SpeedLimiter.cc
   PUBLIC_LINK_LIBS
     ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
 )

src/systems/diff_drive/DiffDrive.cc

@@ -27,6 +27,7 @@
 #include <ignition/common/Profiler.hh>
 #include <ignition/math/DiffDriveOdometry.hh>
 #include <ignition/math/Quaternion.hh>
+#include <ignition/math/SpeedLimiter.hh>
 #include <ignition/plugin/Register.hh>
 #include <ignition/transport/Node.hh>
 
@@ -36,8 +37,6 @@
 #include "ignition/gazebo/Link.hh"
 #include "ignition/gazebo/Model.hh"
 
-#include "SpeedLimiter.hh"
-
 using namespace ignition;
 using namespace gazebo;
 using namespace systems;
@@ -123,10 +122,10 @@ class ignition::gazebo::systems::DiffDrivePrivate
   public: transport::Node::Publisher tfPub;
 
   /// \brief Linear velocity limiter.
-  public: std::unique_ptr<SpeedLimiter> limiterLin;
+  public: std::unique_ptr<ignition::math::SpeedLimiter> limiterLin;
 
   /// \brief Angular velocity limiter.
-  public: std::unique_ptr<SpeedLimiter> limiterAng;
+  public: std::unique_ptr<ignition::math::SpeedLimiter> limiterAng;
 
   /// \brief Previous control command.
   public: Commands last0Cmd;
@@ -197,56 +196,48 @@ void DiffDrive::Configure(const Entity &_entity,
   this->dataPtr->wheelRadius = _sdf->Get<double>("wheel_radius",
       this->dataPtr->wheelRadius).first;
 
-  // Parse speed limiter parameters.
-  bool hasVelocityLimits     = false;
-  bool hasAccelerationLimits = false;
-  bool hasJerkLimits         = false;
-  double minVel              = std::numeric_limits<double>::lowest();
-  double maxVel              = std::numeric_limits<double>::max();
-  double minAccel            = std::numeric_limits<double>::lowest();
-  double maxAccel            = std::numeric_limits<double>::max();
-  double minJerk             = std::numeric_limits<double>::lowest();
-  double maxJerk             = std::numeric_limits<double>::max();
+  // Instantiate the speed limiters.
+  this->dataPtr->limiterLin = std::make_unique<ignition::math::SpeedLimiter>();
+  this->dataPtr->limiterAng = std::make_unique<ignition::math::SpeedLimiter>();
 
+  // Parse speed limiter parameters.
   if (_sdf->HasElement("min_velocity"))
   {
-    minVel = _sdf->Get<double>("min_velocity");
-    hasVelocityLimits = true;
+    const double minVel = _sdf->Get<double>("min_velocity");
+    this->dataPtr->limiterLin->SetMinVelocity(minVel);
+    this->dataPtr->limiterAng->SetMinVelocity(minVel);
   }
   if (_sdf->HasElement("max_velocity"))
   {
-    maxVel = _sdf->Get<double>("max_velocity");
-    hasVelocityLimits = true;
+    const double maxVel = _sdf->Get<double>("max_velocity");
+    this->dataPtr->limiterLin->SetMaxVelocity(maxVel);
+    this->dataPtr->limiterAng->SetMaxVelocity(maxVel);
   }
   if (_sdf->HasElement("min_acceleration"))
   {
-    minAccel = _sdf->Get<double>("min_acceleration");
-    hasAccelerationLimits = true;
+    const double minAccel = _sdf->Get<double>("min_acceleration");
+    this->dataPtr->limiterLin->SetMinAcceleration(minAccel);
+    this->dataPtr->limiterAng->SetMinAcceleration(minAccel);
   }
   if (_sdf->HasElement("max_acceleration"))
   {
-    maxAccel = _sdf->Get<double>("max_acceleration");
-    hasAccelerationLimits = true;
+    const double maxAccel = _sdf->Get<double>("max_acceleration");
+    this->dataPtr->limiterLin->SetMaxAcceleration(maxAccel);
+    this->dataPtr->limiterAng->SetMaxAcceleration(maxAccel);
   }
   if (_sdf->HasElement("min_jerk"))
   {
-    minJerk = _sdf->Get<double>("min_jerk");
-    hasJerkLimits = true;
+    const double minJerk = _sdf->Get<double>("min_jerk");
+    this->dataPtr->limiterLin->SetMinJerk(minJerk);
+    this->dataPtr->limiterAng->SetMinJerk(minJerk);
   }
   if (_sdf->HasElement("max_jerk"))
   {
-    maxJerk = _sdf->Get<double>("max_jerk");
-    hasJerkLimits = true;
+    const double maxJerk = _sdf->Get<double>("max_jerk");
+    this->dataPtr->limiterLin->SetMaxJerk(maxJerk);
+    this->dataPtr->limiterAng->SetMaxJerk(maxJerk);
   }
 
-  // Instantiate the speed limiters.
-  this->dataPtr->limiterLin = std::make_unique<SpeedLimiter>(
-    hasVelocityLimits, hasAccelerationLimits, hasJerkLimits,
-    minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk);
-
-  this->dataPtr->limiterAng = std::make_unique<SpeedLimiter>(
-    hasVelocityLimits, hasAccelerationLimits, hasJerkLimits,
-    minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk);
 
   double odomFreq = _sdf->Get<double>("odom_publish_frequency", 50).first;
   if (odomFreq > 0)
@@ -502,11 +493,11 @@ void DiffDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_info,
     angVel = this->targetVel.angular().z();
   }
 
-  const double dt = std::chrono::duration<double>(_info.dt).count();
-
   // Limit the target velocity if needed.
-  this->limiterLin->Limit(linVel, this->last0Cmd.lin, this->last1Cmd.lin, dt);
-  this->limiterAng->Limit(angVel, this->last0Cmd.ang, this->last1Cmd.ang, dt);
+  this->limiterLin->Limit(
+      linVel, this->last0Cmd.lin, this->last1Cmd.lin, _info.dt);
+  this->limiterAng->Limit(
+      angVel, this->last0Cmd.ang, this->last1Cmd.ang, _info.dt);
 
   // Update history of commands.
   this->last1Cmd = last0Cmd;