Using math::SpeedLimiter on the ackermann_steering controller. (#837)

Signed-off-by: LolaSegura <lsegura@ekumenlabs.com>

src/systems/ackermann_steering/AckermannSteering.cc

@@ -19,7 +19,6 @@
 
 #include <ignition/msgs/odometry.pb.h>
 
-#include <limits>
 #include <mutex>
 #include <set>
 #include <string>
@@ -28,6 +27,7 @@
 #include <ignition/common/Profiler.hh>
 #include <ignition/math/Quaternion.hh>
 #include <ignition/math/Angle.hh>
+#include <ignition/math/SpeedLimiter.hh>
 #include <ignition/plugin/Register.hh>
 #include <ignition/transport/Node.hh>
 
@@ -38,8 +38,6 @@
 #include "ignition/gazebo/Model.hh"
 #include "ignition/gazebo/Util.hh"
 
-#include "SpeedLimiter.hh"
-
 using namespace ignition;
 using namespace gazebo;
 using namespace systems;
@@ -164,10 +162,10 @@ class ignition::gazebo::systems::AckermannSteeringPrivate
   public: std::chrono::steady_clock::duration lastOdomTime{0};
 
   /// \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;
@@ -258,56 +256,48 @@ void AckermannSteering::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)
@@ -696,11 +686,11 @@ void AckermannSteeringPrivate::UpdateVelocity(
     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;

src/systems/ackermann_steering/CMakeLists.txt

@@ -1,7 +1,6 @@
 gz_add_system(ackermann-steering
   SOURCES
     AckermannSteering.cc
-    SpeedLimiter.cc
   PUBLIC_LINK_LIBS
     ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
 )