TractionLimiter: Fix wrong input checks (#1341)

(cherry picked from commit 28dd7e1)

tricycle_controller/CMakeLists.txt

@@ -82,6 +82,12 @@ if(BUILD_TESTING)
     controller_manager
     ros2_control_test_assets
   )
+
+  ament_add_gmock(test_traction_limiter
+    test/test_traction_limiter.cpp)
+  target_link_libraries(test_traction_limiter
+    tricycle_controller
+  )
 endif()
 
 install(

tricycle_controller/include/tricycle_controller/traction_limiter.hpp

@@ -28,14 +28,25 @@ class TractionLimiter
 public:
   /**
    * \brief Constructor
+   *
+   * Parameters are applied symmetrically for both directions, i.e., are applied
+   * to the absolute value of the corresponding quantity.
+   *
+   * \warning
+   * - Setting min_velocity: the robot can't stand still
+   *
+   * - Setting min_deceleration/min_acceleration: the robot can't move with constant velocity
+   *
+   * - Setting min_jerk: the robot can't move with constant acceleration
+   *
    * \param [in] min_velocity Minimum velocity [m/s] or [rad/s]
    * \param [in] max_velocity Maximum velocity [m/s] or [rad/s]
    * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2]
    * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2]
    * \param [in] min_deceleration Minimum deceleration [m/s^2] or [rad/s^2]
    * \param [in] max_deceleration Maximum deceleration [m/s^2] or [rad/s^2]
-   * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0
-   * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0
+   * \param [in] min_jerk Minimum jerk [m/s^3]
+   * \param [in] max_jerk Maximum jerk [m/s^3]
    */
   TractionLimiter(
     double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN,

tricycle_controller/src/traction_limiter.cpp

@@ -44,33 +44,53 @@ TractionLimiter::TractionLimiter(
   if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0;
 
   if (!std::isnan(min_deceleration_) && std::isnan(max_deceleration_)) max_deceleration_ = 1000.0;
-  if (!std::isnan(max_deceleration_) && std::isnan(min_acceleration_)) min_deceleration_ = 0.0;
+  if (!std::isnan(max_deceleration_) && std::isnan(min_deceleration_)) min_deceleration_ = 0.0;
 
   if (!std::isnan(min_jerk_) && std::isnan(max_jerk_)) max_jerk_ = 1000.0;
   if (!std::isnan(max_jerk_) && std::isnan(min_jerk_)) min_jerk_ = 0.0;
 
   const std::string error =
-    "The positive limit will be applied to both directions. Setting different limits for positive "
+    " The positive limit will be applied to both directions. Setting different limits for positive "
     "and negative directions is not supported. Actuators are "
     "assumed to have the same constraints in both directions";
   if (min_velocity_ < 0 || max_velocity_ < 0)
   {
     throw std::invalid_argument("Velocity cannot be negative." + error);
   }
 
+  if (min_velocity_ > max_velocity_)
+  {
+    throw std::invalid_argument("Min velocity cannot be greater than max velocity.");
+  }
+
   if (min_acceleration_ < 0 || max_acceleration_ < 0)
   {
-    throw std::invalid_argument("Acceleration cannot be negative." + error);
+    throw std::invalid_argument("Acceleration limits cannot be negative." + error);
+  }
+
+  if (min_acceleration_ > max_acceleration_)
+  {
+    throw std::invalid_argument("Min acceleration cannot be greater than max acceleration.");
   }
 
   if (min_deceleration_ < 0 || max_deceleration_ < 0)
   {
-    throw std::invalid_argument("Deceleration cannot be negative." + error);
+    throw std::invalid_argument("Deceleration limits cannot be negative." + error);
+  }
+
+  if (min_deceleration_ > max_deceleration_)
+  {
+    throw std::invalid_argument("Min deceleration cannot be greater than max deceleration.");
   }
 
   if (min_jerk_ < 0 || max_jerk_ < 0)
   {
-    throw std::invalid_argument("Jerk cannot be negative." + error);
+    throw std::invalid_argument("Jerk limits cannot be negative." + error);
+  }
+
+  if (min_jerk_ > max_jerk_)
+  {
+    throw std::invalid_argument("Min jerk cannot be greater than max jerk.");
   }
 }