From 66f531a6ec313e45f9b2d2435206cc78404885f5 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 1 Nov 2024 19:52:12 +0000 Subject: [PATCH] diff_drive: Use speed_limiter from control_toolbox --- diff_drive_controller/CMakeLists.txt | 2 +- .../diff_drive_controller.hpp | 6 +- .../diff_drive_controller/speed_limiter.hpp | 105 ------------- diff_drive_controller/package.xml | 1 + .../src/diff_drive_controller.cpp | 4 +- diff_drive_controller/src/speed_limiter.cpp | 139 ------------------ 6 files changed, 7 insertions(+), 250 deletions(-) delete mode 100644 diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp delete mode 100644 diff_drive_controller/src/speed_limiter.cpp diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 4eeb98b9d2..6796025346 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -6,6 +6,7 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") endif() set(THIS_PACKAGE_INCLUDE_DEPENDS + control_toolbox controller_interface generate_parameter_library geometry_msgs @@ -35,7 +36,6 @@ generate_parameter_library(diff_drive_controller_parameters add_library(diff_drive_controller SHARED src/diff_drive_controller.cpp src/odometry.cpp - src/speed_limiter.cpp ) target_compile_features(diff_drive_controller PUBLIC cxx_std_17) target_include_directories(diff_drive_controller PUBLIC diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index ac34b81eca..dfb0663fd6 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -26,9 +26,9 @@ #include #include +#include "control_toolbox/speed_limiter.hpp" #include "controller_interface/controller_interface.hpp" #include "diff_drive_controller/odometry.hpp" -#include "diff_drive_controller/speed_limiter.hpp" #include "diff_drive_controller/visibility_control.h" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -135,8 +135,8 @@ class DiffDriveController : public controller_interface::ControllerInterface std::queue previous_commands_; // last two commands // speed limiters - SpeedLimiter limiter_linear_; - SpeedLimiter limiter_angular_; + control_toolbox::SpeedLimiter limiter_linear_; + control_toolbox::SpeedLimiter limiter_angular_; bool publish_limited_velocity_ = false; std::shared_ptr> limited_velocity_publisher_ = nullptr; diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp deleted file mode 100644 index f6fe439f5d..0000000000 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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: Enrique Fernández - */ - -#ifndef DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ -#define DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ - -#include - -namespace diff_drive_controller -{ -class SpeedLimiter -{ -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_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); - - /** - * \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] 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] - * \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] 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] 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_; - - // Acceleration limits: - double min_acceleration_; - double max_acceleration_; - - // Jerk limits: - double min_jerk_; - double max_jerk_; -}; - -} // namespace diff_drive_controller - -#endif // DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 45ab80d5cb..2b295a653b 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -12,6 +12,7 @@ generate_parameter_library backward_ros + control_toolbox controller_interface geometry_msgs hardware_interface diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 66da6d6738..a1145be47d 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -296,13 +296,13 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; publish_limited_velocity_ = params_.publish_limited_velocity; - limiter_linear_ = SpeedLimiter( + limiter_linear_ = control_toolbox::SpeedLimiter( params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits, params_.linear.x.has_jerk_limits, params_.linear.x.min_velocity, params_.linear.x.max_velocity, params_.linear.x.min_acceleration, params_.linear.x.max_acceleration, params_.linear.x.min_jerk, params_.linear.x.max_jerk); - limiter_angular_ = SpeedLimiter( + limiter_angular_ = control_toolbox::SpeedLimiter( params_.angular.z.has_velocity_limits, params_.angular.z.has_acceleration_limits, params_.angular.z.has_jerk_limits, params_.angular.z.min_velocity, params_.angular.z.max_velocity, params_.angular.z.min_acceleration, diff --git a/diff_drive_controller/src/speed_limiter.cpp b/diff_drive_controller/src/speed_limiter.cpp deleted file mode 100644 index 0f82c2cc3b..0000000000 --- a/diff_drive_controller/src/speed_limiter.cpp +++ /dev/null @@ -1,139 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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: Enrique Fernández - */ - -#include -#include - -#include "diff_drive_controller/speed_limiter.hpp" - -namespace diff_drive_controller -{ -SpeedLimiter::SpeedLimiter( - bool has_velocity_limits, bool has_acceleration_limits, bool has_jerk_limits, double min_velocity, - double max_velocity, double min_acceleration, double max_acceleration, double min_jerk, - double max_jerk) -: has_velocity_limits_(has_velocity_limits), - has_acceleration_limits_(has_acceleration_limits), - has_jerk_limits_(has_jerk_limits), - min_velocity_(min_velocity), - max_velocity_(max_velocity), - min_acceleration_(min_acceleration), - max_acceleration_(max_acceleration), - min_jerk_(min_jerk), - max_jerk_(max_jerk) -{ - // Check if limits are valid, max must be specified, min defaults to -max if unspecified - if (has_velocity_limits_) - { - if (std::isnan(max_velocity_)) - { - throw std::runtime_error("Cannot apply velocity limits if max_velocity is not specified"); - } - if (std::isnan(min_velocity_)) - { - min_velocity_ = -max_velocity_; - } - } - if (has_acceleration_limits_) - { - if (std::isnan(max_acceleration_)) - { - throw std::runtime_error( - "Cannot apply acceleration limits if max_acceleration is not specified"); - } - if (std::isnan(min_acceleration_)) - { - min_acceleration_ = -max_acceleration_; - } - } - if (has_jerk_limits_) - { - if (std::isnan(max_jerk_)) - { - throw std::runtime_error("Cannot apply jerk limits if max_jerk is not specified"); - } - if (std::isnan(min_jerk_)) - { - min_jerk_ = -max_jerk_; - } - } -} - -double SpeedLimiter::limit(double & v, double v0, double v1, double dt) -{ - const double tmp = v; - - limit_jerk(v, v0, v1, dt); - limit_acceleration(v, v0, dt); - limit_velocity(v); - - return tmp != 0.0 ? v / tmp : 1.0; -} - -double SpeedLimiter::limit_velocity(double & v) -{ - const double tmp = v; - - if (has_velocity_limits_) - { - v = std::clamp(v, min_velocity_, max_velocity_); - } - - return tmp != 0.0 ? v / tmp : 1.0; -} - -double SpeedLimiter::limit_acceleration(double & v, double v0, double dt) -{ - const double tmp = v; - - if (has_acceleration_limits_) - { - const double dv_min = min_acceleration_ * dt; - const double dv_max = max_acceleration_ * dt; - - const double dv = std::clamp(v - v0, dv_min, dv_max); - - v = v0 + dv; - } - - return tmp != 0.0 ? v / tmp : 1.0; -} - -double SpeedLimiter::limit_jerk(double & v, double v0, double v1, double dt) -{ - const double tmp = v; - - if (has_jerk_limits_) - { - const double dv = v - v0; - const double dv0 = v0 - v1; - - const double dt2 = 2. * dt * dt; - - const double da_min = min_jerk_ * dt2; - const double da_max = max_jerk_ * dt2; - - const double da = std::clamp(dv - dv0, da_min, da_max); - - v = v0 + dv0 + da; - } - - return tmp != 0.0 ? v / tmp : 1.0; -} - -} // namespace diff_drive_controller