Added speed limiters

tricycle_controller/CMakeLists.txt

@@ -26,7 +26,8 @@ add_library(
   SHARED
   src/tricycle_controller.cpp
   src/odometry.cpp
-  src/speed_limiter.cpp
+  src/traction_limiter.cpp
+  src/steering_limiter.cpp
 )
 target_include_directories(
   ${PROJECT_NAME}

tricycle_controller/include/tricycle_controller/steering_limiter.hpp

@@ -0,0 +1,98 @@
+// Copyright 2022 Pixel Robotics.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*
+ * Author: Tony Najjar
+ */
+
+#ifndef TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_
+#define TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_
+
+#include <cmath>
+
+namespace tricycle_controller
+{
+
+class SteeringLimiter
+{
+public:
+  /**
+   * \brief Constructor
+   * \param [in] min_position Minimum position [m] or [rad]
+   * \param [in] max_position Maximum position [m] or [rad]
+   * \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]
+   */
+  SteeringLimiter(
+    double min_position = -M_PI_2, double max_position = M_PI_2,
+    double min_velocity = 0.0, double max_velocity = std::numeric_limits<double>::max(),
+    double min_acceleration = 0.0, double max_acceleration = std::numeric_limits<double>::max());
+
+  /**
+   * \brief Limit the position, velocity and acceleration
+   * \param [in, out] p  position [m] or [rad]
+   * \param [in]      p0 Previous position to p  [m] or [rad]
+   * \param [in]      p1 Previous position to p0 [m] or [rad]
+   * \param [in]      dt Time step [s]
+   * \return Limiting factor (1.0 if none)
+   */
+  double limit(double & p, double p0, double p1, double dt);
+
+  /**
+   * \brief Limit the jerk
+   * \param [in, out] p  position [m] or [rad]
+   * \param [in]      p0 Previous position to p  [m] or [rad]
+   * \param [in]      p1 Previous position to p0 [m] or [rad]
+   * \param [in]      dt Time step [s]
+   * \return Limiting factor (1.0 if none)
+   */
+  double limit_position(double & p);
+
+  /**
+   * \brief Limit the velocity
+   * \param [in, out] p position [m]
+   * \return Limiting factor (1.0 if none)
+   */
+  double limit_velocity(double & p, double p0, double dt);
+
+  /**
+   * \brief Limit the acceleration
+   * \param [in, out] p  Position [m] or [rad]
+   * \param [in]      p0 Previous position [m] or [rad]
+   * \param [in]      dt Time step [s]
+   * \return Limiting factor (1.0 if none)
+   */
+  double limit_acceleration(double & p, double p0, double p1, double dt);
+
+private:
+
+  // Position limits:
+  double min_position_;
+  double max_position_;
+
+  // Velocity limits:
+  double min_velocity_;
+  double max_velocity_;
+
+  // Acceleration limits:
+  double min_acceleration_;
+  double max_acceleration_;
+
+};
+
+}  // namespace tricycle_controller
+
+#endif  // TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_

tricycle_controller/include/tricycle_controller/speed_limiter.hpp → tricycle_controller/include/tricycle_controller/traction_limiter.hpp

@@ -1,4 +1,4 @@
-// Copyright 2020 PAL Robotics S.L.
+// Copyright 2022 Pixel Robotics.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -13,80 +13,75 @@
 // limitations under the License.
 
 /*
- * Author: Enrique Fernández
+ * Author: Tony Najjar
  */
 
-#ifndef TRICYCLE_CONTROLLER__SPEED_LIMITER_HPP_
-#define TRICYCLE_CONTROLLER__SPEED_LIMITER_HPP_
+#ifndef TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_
+#define TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_
 
 #include <cmath>
 
 namespace tricycle_controller
 {
-class SpeedLimiter
+
+class TractionLimiter
 {
 public:
   /**
    * \brief Constructor
-   * \param [in] has_velocity_limits     if true, applies velocity limits
-   * \param [in] has_acceleration_limits if true, applies acceleration limits
-   * \param [in] has_jerk_limits         if true, applies jerk limits
-   * \param [in] min_velocity Minimum velocity [m/s], usually <= 0
-   * \param [in] max_velocity Maximum velocity [m/s], usually >= 0
-   * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0
-   * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0
+   * \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
    */
-  SpeedLimiter(
-    bool has_velocity_limits = false, bool has_acceleration_limits = false,
-    bool has_jerk_limits = false, double min_velocity = NAN, double max_velocity = NAN,
-    double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN,
-    double max_jerk = NAN);
+  TractionLimiter(
+    double min_velocity = 0.0, double max_velocity = std::numeric_limits<double>::max(),
+    double min_acceleration = 0.0, double max_acceleration = std::numeric_limits<double>::max(),
+    double min_deceleration = 0.0, double max_deceleration = std::numeric_limits<double>::max(),
+    double min_jerk = 0.0, double max_jerk = std::numeric_limits<double>::max());
 
   /**
    * \brief Limit the velocity and acceleration
-   * \param [in, out] v  Velocity [m/s]
-   * \param [in]      v0 Previous velocity to v  [m/s]
-   * \param [in]      v1 Previous velocity to v0 [m/s]
+   * \param [in, out] v  Velocity [m/s] or [rad/s]
+   * \param [in]      v0 Previous velocity to v  [m/s] or [rad/s]
+   * \param [in]      v1 Previous velocity to v0 [m/s] or [rad/s]
    * \param [in]      dt Time step [s]
    * \return Limiting factor (1.0 if none)
    */
   double limit(double & v, double v0, double v1, double dt);
 
   /**
    * \brief Limit the velocity
-   * \param [in, out] v Velocity [m/s]
+   * \param [in, out] v Velocity [m/s] or [rad/s]
    * \return Limiting factor (1.0 if none)
    */
   double limit_velocity(double & v);
 
   /**
    * \brief Limit the acceleration
-   * \param [in, out] v  Velocity [m/s]
-   * \param [in]      v0 Previous velocity [m/s]
+   * \param [in, out] v  Velocity [m/s] or [rad/s]
+   * \param [in]      v0 Previous velocity [m/s] or [rad/s]
    * \param [in]      dt Time step [s]
    * \return Limiting factor (1.0 if none)
    */
   double limit_acceleration(double & v, double v0, double dt);
 
   /**
    * \brief Limit the jerk
-   * \param [in, out] v  Velocity [m/s]
-   * \param [in]      v0 Previous velocity to v  [m/s]
-   * \param [in]      v1 Previous velocity to v0 [m/s]
+   * \param [in, out] v  Velocity [m/s] or [rad/s]
+   * \param [in]      v0 Previous velocity to v  [m/s] or [rad/s]
+   * \param [in]      v1 Previous velocity to v0 [m/s] or [rad/s]
    * \param [in]      dt Time step [s]
    * \return Limiting factor (1.0 if none)
    * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control
    */
   double limit_jerk(double & v, double v0, double v1, double dt);
 
 private:
-  // Enable/Disable velocity/acceleration/jerk limits:
-  bool has_velocity_limits_;
-  bool has_acceleration_limits_;
-  bool has_jerk_limits_;
-
   // Velocity limits:
   double min_velocity_;
   double max_velocity_;
@@ -95,11 +90,15 @@ class SpeedLimiter
   double min_acceleration_;
   double max_acceleration_;
 
+  // Deceleration limits:
+  double min_deceleration_;
+  double max_deceleration_;
+
   // Jerk limits:
   double min_jerk_;
   double max_jerk_;
 };
 
 }  // namespace tricycle_controller
 
-#endif  // TRICYCLE_CONTROLLER__SPEED_LIMITER_HPP_
+#endif  // TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_

tricycle_controller/include/tricycle_controller/tricycle_controller.hpp

@@ -41,7 +41,8 @@
 #include "realtime_tools/realtime_publisher.h"
 #include "tf2_msgs/msg/tf_message.hpp"
 #include "tricycle_controller/odometry.hpp"
-#include "tricycle_controller/speed_limiter.hpp"
+#include "tricycle_controller/traction_limiter.hpp"
+#include "tricycle_controller/steering_limiter.hpp"
 #include "tricycle_controller/visibility_control.h"
 
 namespace tricycle_controller
@@ -160,11 +161,11 @@ class TricycleController : public controller_interface::ControllerInterface
 
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_odom_service_;
 
-  std::queue<TwistStamped> previous_commands_;  // last two commands
+  std::queue<AckermannDrive> previous_commands_;  // last two commands
 
   // speed limiters
-  SpeedLimiter limiter_linear_;
-  SpeedLimiter limiter_angular_;
+  TractionLimiter limiter_traction_;
+  SteeringLimiter limiter_steering_;
 
   rclcpp::Time previous_update_timestamp_{0};