From d32665a8cc0584682e28c6dc8efa392c83df654c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 16 Dec 2024 11:52:36 +0100 Subject: [PATCH] Update command limiter of diff_drive_controller (#1315) --- diff_drive_controller/CMakeLists.txt | 10 +- .../doc/parameters_context.yaml | 4 - .../custom_validators.hpp | 64 ++++++ .../diff_drive_controller.hpp | 1 - .../diff_drive_controller/odometry.hpp | 2 - .../diff_drive_controller/speed_limiter.hpp | 97 ++++++--- diff_drive_controller/package.xml | 1 + .../src/diff_drive_controller.cpp | 86 +++++++- .../src/diff_drive_controller_parameter.yaml | 100 ++++++++- diff_drive_controller/src/odometry.cpp | 10 +- diff_drive_controller/src/speed_limiter.cpp | 139 ------------ .../config/test_diff_drive_controller.yaml | 33 ++- .../test/test_diff_drive_controller.cpp | 205 +++++++++++++++++- doc/migration.rst | 1 + doc/release_notes.rst | 2 + 15 files changed, 540 insertions(+), 215 deletions(-) create mode 100644 diff_drive_controller/include/diff_drive_controller/custom_validators.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 4b3ff4f77f..d94b6e3ce0 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -8,6 +8,7 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") endif() set(THIS_PACKAGE_INCLUDE_DEPENDS + control_toolbox controller_interface generate_parameter_library geometry_msgs @@ -32,19 +33,21 @@ add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) generate_parameter_library(diff_drive_controller_parameters src/diff_drive_controller_parameter.yaml + include/diff_drive_controller/custom_validators.hpp ) 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 $ $ ) -target_link_libraries(diff_drive_controller PUBLIC diff_drive_controller_parameters) +target_link_libraries(diff_drive_controller + PUBLIC + diff_drive_controller_parameters) ament_target_dependencies(diff_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -57,7 +60,8 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_diff_drive_controller - test/test_diff_drive_controller.cpp) + test/test_diff_drive_controller.cpp + ) target_link_libraries(test_diff_drive_controller diff_drive_controller ) diff --git a/diff_drive_controller/doc/parameters_context.yaml b/diff_drive_controller/doc/parameters_context.yaml index 81e92806f5..d950f7f7e9 100644 --- a/diff_drive_controller/doc/parameters_context.yaml +++ b/diff_drive_controller/doc/parameters_context.yaml @@ -1,9 +1,5 @@ linear.x: | Joint limits structure for the linear ``x``-axis. - The limiter ignores position limits. - For details see ``joint_limits`` package from ros2_control repository. angular.z: | Joint limits structure for the rotation about ``z``-axis. - The limiter ignores position limits. - For details see ``joint_limits`` package from ros2_control repository. diff --git a/diff_drive_controller/include/diff_drive_controller/custom_validators.hpp b/diff_drive_controller/include/diff_drive_controller/custom_validators.hpp new file mode 100644 index 0000000000..53fae54560 --- /dev/null +++ b/diff_drive_controller/include/diff_drive_controller/custom_validators.hpp @@ -0,0 +1,64 @@ +// Copyright 2024 AIT - Austrian Institute of Technology GmbH +// +// 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. + +// TODO(christophfroehlich) remove this file and use it from control_toolbox once +// https://github.com/PickNikRobotics/generate_parameter_library/pull/213 is merged and released + +#ifndef DIFF_DRIVE_CONTROLLER__CUSTOM_VALIDATORS_HPP_ +#define DIFF_DRIVE_CONTROLLER__CUSTOM_VALIDATORS_HPP_ + +#include + +#include + +#include +#include +#include + +namespace diff_drive_controller +{ + +/** + * @brief gt_eq, but check only if the value is not NaN + */ +template +tl::expected gt_eq_or_nan(rclcpp::Parameter const & parameter, T expected_value) +{ + auto param_value = parameter.as_double(); + if (!std::isnan(param_value)) + { + // check only if the value is not NaN + return rsl::gt_eq(parameter, expected_value); + } + return {}; +} + +/** + * @brief lt_eq, but check only if the value is not NaN + */ +template +tl::expected lt_eq_or_nan(rclcpp::Parameter const & parameter, T expected_value) +{ + auto param_value = parameter.as_double(); + if (!std::isnan(param_value)) + { + // check only if the value is not NaN + return rsl::lt_eq(parameter, expected_value); + } + return {}; +} + +} // namespace diff_drive_controller + +#endif // DIFF_DRIVE_CONTROLLER__CUSTOM_VALIDATORS_HPP_ 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 4fc4d37d4b..85f4fb23b0 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 @@ -20,7 +20,6 @@ #define DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_ #include -#include #include #include #include diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index edca431d3d..ae4417a788 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -22,8 +22,6 @@ #ifndef DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_ #define DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_ -#include - #include "rclcpp/time.hpp" // \note The versions conditioning is added here to support the source-compatibility with Humble #if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp index f6fe439f5d..4fa08fcdba 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -19,7 +19,9 @@ #ifndef DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ #define DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ -#include +#include + +#include "control_toolbox/rate_limiter.hpp" namespace diff_drive_controller { @@ -33,16 +35,65 @@ class SpeedLimiter * \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_deceleration Maximum deceleration [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 */ + [[deprecated]] SpeedLimiter( + bool has_velocity_limits = true, bool has_acceleration_limits = true, + bool has_jerk_limits = true, double min_velocity = std::numeric_limits::quiet_NaN(), + double max_velocity = std::numeric_limits::quiet_NaN(), + double max_deceleration = std::numeric_limits::quiet_NaN(), + double max_acceleration = std::numeric_limits::quiet_NaN(), + double min_jerk = std::numeric_limits::quiet_NaN(), + double max_jerk = std::numeric_limits::quiet_NaN()) + { + if (!has_velocity_limits) + { + min_velocity = max_velocity = std::numeric_limits::quiet_NaN(); + } + if (!has_acceleration_limits) + { + max_deceleration = max_acceleration = std::numeric_limits::quiet_NaN(); + } + if (!has_jerk_limits) + { + min_jerk = max_jerk = std::numeric_limits::quiet_NaN(); + } + speed_limiter_ = control_toolbox::RateLimiter( + min_velocity, max_velocity, max_deceleration, max_acceleration, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), min_jerk, + max_jerk); + } + + /** + * \brief Constructor + * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 + * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 + * \param [in] max_acceleration_reverse Maximum acceleration in reverse direction [m/s^2], usually + * <= 0 + * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 + * \param [in] max_deceleration Maximum deceleration [m/s^2], usually <= 0 + * \param [in] max_deceleration_reverse Maximum deceleration in reverse direction [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); + double min_velocity = std::numeric_limits::quiet_NaN(), + double max_velocity = std::numeric_limits::quiet_NaN(), + double max_acceleration_reverse = std::numeric_limits::quiet_NaN(), + double max_acceleration = std::numeric_limits::quiet_NaN(), + double max_deceleration = std::numeric_limits::quiet_NaN(), + double max_deceleration_reverse = std::numeric_limits::quiet_NaN(), + double min_jerk = std::numeric_limits::quiet_NaN(), + double max_jerk = std::numeric_limits::quiet_NaN()) + { + speed_limiter_ = control_toolbox::RateLimiter( + min_velocity, max_velocity, max_acceleration_reverse, max_acceleration, max_deceleration, + max_deceleration_reverse, min_jerk, max_jerk); + } /** * \brief Limit the velocity and acceleration @@ -52,14 +103,17 @@ class SpeedLimiter * \param [in] dt Time step [s] * \return Limiting factor (1.0 if none) */ - double limit(double & v, double v0, double v1, double dt); + double limit(double & v, double v0, double v1, double dt) + { + return speed_limiter_.limit(v, v0, v1, dt); + } /** * \brief Limit the velocity * \param [in, out] v Velocity [m/s] * \return Limiting factor (1.0 if none) */ - double limit_velocity(double & v); + double limit_velocity(double & v) { return speed_limiter_.limit_value(v); } /** * \brief Limit the acceleration @@ -68,7 +122,10 @@ class SpeedLimiter * \param [in] dt Time step [s] * \return Limiting factor (1.0 if none) */ - double limit_acceleration(double & v, double v0, double dt); + double limit_acceleration(double & v, double v0, double dt) + { + return speed_limiter_.limit_first_derivative(v, v0, dt); + } /** * \brief Limit the jerk @@ -79,25 +136,13 @@ class SpeedLimiter * \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); + double limit_jerk(double & v, double v0, double v1, double dt) + { + return speed_limiter_.limit_second_derivative(v, v0, v1, 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_; + control_toolbox::RateLimiter speed_limiter_; // Instance of the new RateLimiter }; } // namespace diff_drive_controller diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 6147cfcc84..1ed9673c79 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -24,6 +24,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 7c3c9864d2..6957864321 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -45,7 +45,14 @@ using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; using lifecycle_msgs::msg::State; -DiffDriveController::DiffDriveController() : controller_interface::ControllerInterface() {} +DiffDriveController::DiffDriveController() +: controller_interface::ControllerInterface(), + // dummy limiter, will be created in on_configure + // could be done with shared_ptr instead -> but will break ABI + limiter_angular_(std::numeric_limits::quiet_NaN()), + limiter_linear_(std::numeric_limits::quiet_NaN()) +{ +} const char * DiffDriveController::feedback_type() const { @@ -297,17 +304,78 @@ 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; + // TODO(christophfroehlich) remove deprecated parameters + // START DEPRECATED + if (!params_.linear.x.has_velocity_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_velocity_limits parameter is deprecated, instead set the respective limits " + "to NAN"); + params_.linear.x.min_velocity = params_.linear.x.max_velocity = + std::numeric_limits::quiet_NaN(); + } + if (!params_.linear.x.has_acceleration_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_acceleration_limits parameter is deprecated, instead set the respective " + "limits to " + "NAN"); + params_.linear.x.max_deceleration = params_.linear.x.max_acceleration = + params_.linear.x.max_deceleration_reverse = params_.linear.x.max_acceleration_reverse = + std::numeric_limits::quiet_NaN(); + } + if (!params_.linear.x.has_jerk_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_jerk_limits parameter is deprecated, instead set the respective limits to " + "NAN"); + params_.linear.x.min_jerk = params_.linear.x.max_jerk = + std::numeric_limits::quiet_NaN(); + } + if (!params_.angular.z.has_velocity_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_velocity_limits parameter is deprecated, instead set the respective limits " + "to NAN"); + params_.angular.z.min_velocity = params_.angular.z.max_velocity = + std::numeric_limits::quiet_NaN(); + } + if (!params_.angular.z.has_acceleration_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_acceleration_limits parameter is deprecated, instead set the respective " + "limits to " + "NAN"); + params_.angular.z.max_deceleration = params_.angular.z.max_acceleration = + params_.angular.z.max_deceleration_reverse = params_.angular.z.max_acceleration_reverse = + std::numeric_limits::quiet_NaN(); + } + if (!params_.angular.z.has_jerk_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_jerk_limits parameter is deprecated, instead set the respective limits to " + "NAN"); + params_.angular.z.min_jerk = params_.angular.z.max_jerk = + std::numeric_limits::quiet_NaN(); + } + // END DEPRECATED limiter_linear_ = 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); + params_.linear.x.min_velocity, params_.linear.x.max_velocity, + params_.linear.x.max_acceleration_reverse, params_.linear.x.max_acceleration, + params_.linear.x.max_deceleration, params_.linear.x.max_deceleration_reverse, + params_.linear.x.min_jerk, params_.linear.x.max_jerk); limiter_angular_ = 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, - params_.angular.z.max_acceleration, params_.angular.z.min_jerk, params_.angular.z.max_jerk); + params_.angular.z.min_velocity, params_.angular.z.max_velocity, + params_.angular.z.max_acceleration_reverse, params_.angular.z.max_acceleration, + params_.angular.z.max_deceleration, params_.angular.z.max_deceleration_reverse, + params_.angular.z.min_jerk, params_.angular.z.max_jerk); if (!reset()) { diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 755259cc2a..dc1109e043 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -115,75 +115,163 @@ diff_drive_controller: x: has_velocity_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_acceleration_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_jerk_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } max_velocity: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } } min_velocity: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } } max_acceleration: { type: double, default_value: .NAN, + description: "Maximum acceleration in forward direction.", + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } + } + max_deceleration: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in forward direction.", + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } } min_acceleration: { type: double, default_value: .NAN, + description: "deprecated, use max_deceleration" + } + max_acceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum acceleration in reverse direction. If not set, -max_acceleration will be used.", + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } + } + max_deceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in reverse direction. If not set, -max_deceleration will be used.", + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } } max_jerk: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } } min_jerk: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } } angular: z: has_velocity_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_acceleration_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_jerk_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } max_velocity: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } } min_velocity: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } } max_acceleration: { type: double, default_value: .NAN, + description: "Maximum acceleration in positive direction.", + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } + } + max_deceleration: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in positive direction.", + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } } min_acceleration: { type: double, default_value: .NAN, + description: "deprecated, use max_deceleration" + } + max_acceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum acceleration in negative direction. If not set, -max_acceleration will be used.", + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } + } + max_deceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in negative direction. If not set, -max_deceleration will be used.", + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } } max_jerk: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } } min_jerk: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } } diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 99bb22890d..a4a1dbd6ca 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -16,6 +16,8 @@ * Author: Enrique Fernández */ +#include + #include "diff_drive_controller/odometry.hpp" namespace diff_drive_controller @@ -134,8 +136,8 @@ void Odometry::integrateRungeKutta2(double linear, double angular) const double direction = heading_ + angular * 0.5; /// Runge-Kutta 2nd order integration: - x_ += linear * cos(direction); - y_ += linear * sin(direction); + x_ += linear * std::cos(direction); + y_ += linear * std::sin(direction); heading_ += angular; } @@ -151,8 +153,8 @@ void Odometry::integrateExact(double linear, double angular) const double heading_old = heading_; const double r = linear / angular; heading_ += angular; - x_ += r * (sin(heading_) - sin(heading_old)); - y_ += -r * (cos(heading_) - cos(heading_old)); + x_ += r * (std::sin(heading_) - std::sin(heading_old)); + y_ += -r * (std::cos(heading_) - std::cos(heading_old)); } } 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 diff --git a/diff_drive_controller/test/config/test_diff_drive_controller.yaml b/diff_drive_controller/test/config/test_diff_drive_controller.yaml index 606bacbf4f..7707f2050f 100644 --- a/diff_drive_controller/test/config/test_diff_drive_controller.yaml +++ b/diff_drive_controller/test/config/test_diff_drive_controller.yaml @@ -23,21 +23,20 @@ test_diff_drive_controller: publish_limited_velocity: true velocity_rolling_window_size: 10 - linear.x.has_velocity_limits: false - linear.x.has_acceleration_limits: false - linear.x.has_jerk_limits: false - linear.x.max_velocity: 0.0 - linear.x.min_velocity: 0.0 - linear.x.max_acceleration: 0.0 - linear.x.max_jerk: 0.0 - linear.x.min_jerk: 0.0 + linear.x.max_velocity: .NAN + linear.x.min_velocity: .NAN + linear.x.max_acceleration: .NAN + linear.x.max_deceleration: .NAN + linear.x.max_acceleration_reverse: .NAN + linear.x.max_deceleration_reverse: .NAN + linear.x.max_jerk: .NAN + linear.x.min_jerk: .NAN - angular.z.has_velocity_limits: false - angular.z.has_acceleration_limits: false - angular.z.has_jerk_limits: false - angular.z.max_velocity: 0.0 - angular.z.min_velocity: 0.0 - angular.z.max_acceleration: 0.0 - angular.z.min_acceleration: 0.0 - angular.z.max_jerk: 0.0 - angular.z.min_jerk: 0.0 + angular.z.max_velocity: .NAN + angular.z.min_velocity: .NAN + angular.z.max_acceleration: .NAN + angular.z.max_deceleration: .NAN + angular.z.max_acceleration_reverse: .NAN + angular.z.max_deceleration_reverse: .NAN + angular.z.max_jerk: .NAN + angular.z.min_jerk: .NAN diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index a984c5a5c3..f2fc671920 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -16,7 +16,6 @@ #include #include -#include #include #include @@ -86,10 +85,11 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr class TestDiffDriveController : public ::testing::Test { protected: - static void SetUpTestCase() { rclcpp::init(0, nullptr); } - void SetUp() override { + // use the name of the test as the controller name (i.e, the node name) to be able to set + // parameters from yaml for each test + controller_name = ::testing::UnitTest::GetInstance()->current_test_info()->name(); controller_ = std::make_unique(); pub_node = std::make_shared("velocity_publisher"); @@ -191,7 +191,7 @@ class TestDiffDriveController : public ::testing::Test return controller_->init(controller_name, urdf_, 0, ns, node_options); } - const std::string controller_name = "test_diff_drive_controller"; + std::string controller_name; std::unique_ptr controller_; std::vector position_values_ = {0.1, 0.2}; @@ -449,6 +449,194 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } +TEST_F(TestDiffDriveController, test_speed_limiter) +{ + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + { + rclcpp::Parameter("linear.x.max_acceleration", rclcpp::ParameterValue(2.0)), + rclcpp::Parameter("linear.x.max_deceleration", rclcpp::ParameterValue(-4.0)), + rclcpp::Parameter("linear.x.max_acceleration_reverse", rclcpp::ParameterValue(-8.0)), + rclcpp::Parameter("linear.x.max_deceleration_reverse", rclcpp::ParameterValue(10.0)), + }), + controller_interface::return_type::OK); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // send msg + publish(0.0, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + // wait for the speed limiter to fill the queue + for (int i = 0; i < 3; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(0.0, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(0.0, right_wheel_vel_cmd_.get_value(), 1e-3); + } + + const double dt = 0.001; + const double wheel_radius = 0.1; + + // we send four steps of acceleration, deceleration, reverse acceleration and reverse deceleration + { + const double linear = 1.0; + // send msg + publish(linear, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + // should be in acceleration limit + const double time_acc = linear / 2.0; + for (int i = 0; i < floor(time_acc / dt) - 1; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_GT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + EXPECT_GT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + } + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_value(), 1e-3); + + // wait for the speed limiter to fill the queue + for (int i = 0; i < 3; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(linear / wheel_radius, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(linear / wheel_radius, right_wheel_vel_cmd_.get_value()); + } + } + + { + const double linear = 0.0; + // send msg + publish(linear, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + // should be in acceleration limit + const double time_acc = -1.0 / (-4.0); + for (int i = 0; i < floor(time_acc / dt) - 1; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_LT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + EXPECT_LT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + } + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_value(), 1e-3); + + // wait for the speed limiter to fill the queue + for (int i = 0; i < 3; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_value(), 1e-3); + } + } + + { + const double linear = -1.0; + // send msg + publish(linear, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + // should be in acceleration limit + const double time_acc = -1.0 / (-8.0); + for (int i = 0; i < floor(time_acc / dt) - 1; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_LT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + EXPECT_LT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + } + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_value(), 1e-3); + + // wait for the speed limiter to fill the queue + for (int i = 0; i < 3; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_value(), 1e-3); + } + } + + { + const double linear = 0.0; + // send msg + publish(linear, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + // should be in acceleration limit + const double time_acc = 1.0 / (10.0); + for (int i = 0; i < floor(time_acc / dt) - 1; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_GT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + EXPECT_GT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + } + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_value(), 1e-3); + + // wait for the speed limiter to fill the queue + for (int i = 0; i < 3; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_value(), 1e-3); + } + } +} + TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) { ASSERT_EQ( @@ -576,3 +764,12 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); executor.cancel(); } + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/doc/migration.rst b/doc/migration.rst index 4c0e4608d6..4f603880bb 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -8,6 +8,7 @@ This list summarizes important changes between Iron (previous) and Jazzy (curren diff_drive_controller ***************************** * The twist message on ``~/cmd_vel`` is now required to be of stamped type (`#812 `_). +* Parameters ``has_velocity_limits``, ``has_acceleration_limits``, and ``has_jerk_limits`` are removed. Instead, set the respective limits to ``.NAN``. (`#1315 `_). joint_trajectory_controller ***************************** diff --git a/doc/release_notes.rst b/doc/release_notes.rst index c7149faf03..0275230fdc 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -13,6 +13,8 @@ diff_drive_controller ***************************** * The twist message on ``~/cmd_vel`` is now required to be of stamped type (`#812 `_). * Remove unused parameter ``wheels_per_side`` (`#958 `_). +* Parameters ``has_velocity_limits``, ``has_acceleration_limits``, and ``has_jerk_limits`` are removed. Instead, set the respective limits to ``.NAN``. (`#1315 `_). +* Parameters ``max_acceleration_reverse`` and ``max_deceleration_reverse`` were added to configure asymmetric acceleration/deceleration behavior. (`#1315 `_). joint_trajectory_controller *****************************