From d88594ed656f30c428031252ff764f6eb5bbe408 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 1 Dec 2024 20:13:06 +0000 Subject: [PATCH 01/14] 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 4b3ff4f77f..cd9f4c16a4 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 @@ -37,7 +38,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 74526b199c..2ac8e6b376 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 0602d91898..e1d4c991d9 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 d21cac602d..4877403418 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 From a71881d4932ff3d9b9c9a5826db5d6bcf4db9516 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 1 Dec 2024 20:13:06 +0000 Subject: [PATCH 02/14] Readd a deprecated speed_limiter --- .../diff_drive_controller/speed_limiter.hpp | 106 ++++++++++++++++++ 1 file changed, 106 insertions(+) create mode 100644 diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp new file mode 100644 index 0000000000..24a9c66647 --- /dev/null +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -0,0 +1,106 @@ +// 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_ + +// TODO(christophfroehlich) remove this file after the next release + +#include "control_toolbox/speed_limiter.hpp" + +namespace diff_drive_controller +{ +class [[deprecated("Use control_toolbox/speed_limiter instead")]] 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) + : speed_limiter_( + has_velocity_limits, has_acceleration_limits, has_jerk_limits, min_velocity, max_velocity, + min_acceleration, max_acceleration, min_jerk, max_jerk) + { + } + + /** + * \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) + { + 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) { return speed_limiter_.limit_velocity(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) + { + return speed_limiter_.limit_acceleration(v, v0, 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) + { + return speed_limiter_.limit_jerk(v, v0, v1, dt); + } + +private: + control_toolbox::SpeedLimiter speed_limiter_; // Instance of the new SpeedLimiter +}; + +} // namespace diff_drive_controller + +#endif // DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ From d3630100c5994f65929d1e1786c6aa3c93dce783 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 1 Dec 2024 20:13:06 +0000 Subject: [PATCH 03/14] Use the renamed and merged RateLimiter --- diff_drive_controller/CMakeLists.txt | 5 +- .../custom_validators.hpp | 64 +++++++++++ .../diff_drive_controller.hpp | 6 +- .../diff_drive_controller/speed_limiter.hpp | 36 +++++-- .../src/diff_drive_controller.cpp | 45 +++++++- .../src/diff_drive_controller_parameter.yaml | 100 ++++++++++++++++-- 6 files changed, 233 insertions(+), 23 deletions(-) create mode 100644 diff_drive_controller/include/diff_drive_controller/custom_validators.hpp diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index cd9f4c16a4..925112e3bb 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -33,6 +33,7 @@ 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 @@ -44,7 +45,9 @@ 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. 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 2ac8e6b376..74526b199c 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 - control_toolbox::SpeedLimiter limiter_linear_; - control_toolbox::SpeedLimiter limiter_angular_; + SpeedLimiter limiter_linear_; + 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 index 24a9c66647..9c33d2bdb1 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -21,11 +21,11 @@ // TODO(christophfroehlich) remove this file after the next release -#include "control_toolbox/speed_limiter.hpp" +#include "control_toolbox/rate_limiter.hpp" namespace diff_drive_controller { -class [[deprecated("Use control_toolbox/speed_limiter instead")]] SpeedLimiter +class SpeedLimiter { public: /** @@ -41,14 +41,28 @@ class [[deprecated("Use control_toolbox/speed_limiter instead")]] SpeedLimiter * \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, + bool has_velocity_limits = true, bool has_acceleration_limits = true, + bool has_jerk_limits = true, double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN, double max_jerk = NAN) - : speed_limiter_( - has_velocity_limits, has_acceleration_limits, has_jerk_limits, min_velocity, max_velocity, - min_acceleration, max_acceleration, min_jerk, max_jerk) { + // START DEPRECATED + if (!has_velocity_limits) + { + min_velocity = max_velocity = NAN; + } + if (!has_acceleration_limits) + { + min_acceleration = max_acceleration = NAN; + } + if (!has_jerk_limits) + { + min_jerk = max_jerk = NAN; + } + // END DEPRECATED + speed_limiter_ = control_toolbox::RateLimiter( + min_velocity, max_velocity, min_acceleration, max_acceleration, min_acceleration, + max_acceleration, min_jerk, max_jerk); } /** @@ -69,7 +83,7 @@ class [[deprecated("Use control_toolbox/speed_limiter instead")]] SpeedLimiter * \param [in, out] v Velocity [m/s] * \return Limiting factor (1.0 if none) */ - double limit_velocity(double & v) { return speed_limiter_.limit_velocity(v); } + double limit_velocity(double & v) { return speed_limiter_.limit_value(v); } /** * \brief Limit the acceleration @@ -80,7 +94,7 @@ class [[deprecated("Use control_toolbox/speed_limiter instead")]] SpeedLimiter */ double limit_acceleration(double & v, double v0, double dt) { - return speed_limiter_.limit_acceleration(v, v0, dt); + return speed_limiter_.limit_first_derivative(v, v0, dt); } /** @@ -94,11 +108,11 @@ class [[deprecated("Use control_toolbox/speed_limiter instead")]] SpeedLimiter */ double limit_jerk(double & v, double v0, double v1, double dt) { - return speed_limiter_.limit_jerk(v, v0, v1, dt); + return speed_limiter_.limit_second_derivative(v, v0, v1, dt); } private: - control_toolbox::SpeedLimiter speed_limiter_; // Instance of the new SpeedLimiter + control_toolbox::RateLimiter speed_limiter_; // Instance of the new RateLimiter }; } // namespace diff_drive_controller diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 4877403418..70fa509bcd 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -296,13 +296,54 @@ 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_ = control_toolbox::SpeedLimiter( + // TODO(christophfroehlich) remove deprecated parameters + // START DEPRECATED + if (!params_.linear.x.has_velocity_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_velocity_limits parameter is deprecated. Set the respective limits to NAN"); + } + if (!params_.linear.x.has_acceleration_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_acceleration_limits parameter is deprecated. Set the respective limits to " + "NAN"); + } + if (!params_.linear.x.has_jerk_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_jerk_limits parameter is deprecated. Set the respective limits to NAN"); + } + if (!params_.angular.z.has_velocity_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_velocity_limits parameter is deprecated. Set the respective limits to NAN"); + } + if (!params_.angular.z.has_acceleration_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_acceleration_limits parameter is deprecated. Set the respective limits to " + "NAN"); + } + if (!params_.angular.z.has_jerk_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_jerk_limits parameter is deprecated. Set the respective limits to 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); - limiter_angular_ = control_toolbox::SpeedLimiter( + 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, diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 755259cc2a..538f874c1c 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] + } } From dee3cbc9e57ab59bad6e9e5a4a100b5f64343edf Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 1 Dec 2024 20:13:06 +0000 Subject: [PATCH 04/14] Remove wrong hint in parameter description --- diff_drive_controller/doc/parameters_context.yaml | 4 ---- 1 file changed, 4 deletions(-) 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. From 164d8067f5675c236e8887c89191214fccd0f87a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 1 Dec 2024 20:13:06 +0000 Subject: [PATCH 05/14] Remove outdated comment --- .../include/diff_drive_controller/speed_limiter.hpp | 2 -- 1 file changed, 2 deletions(-) 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 9c33d2bdb1..6aab78b84e 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -19,8 +19,6 @@ #ifndef DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ #define DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ -// TODO(christophfroehlich) remove this file after the next release - #include "control_toolbox/rate_limiter.hpp" namespace diff_drive_controller From 1612cdefa968f9def863025f7aa5602125a81cf4 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 1 Dec 2024 20:13:06 +0000 Subject: [PATCH 06/14] Update docs --- doc/migration.rst | 1 + doc/release_notes.rst | 2 ++ 2 files changed, 3 insertions(+) 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 974bca880f..b79c221d8b 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -12,6 +12,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 ***************************** From a7f6c94d9b5b5206d7e93f17502f5656b597679a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 1 Dec 2024 20:13:06 +0000 Subject: [PATCH 07/14] Cleanup cmath includes --- .../diff_drive_controller/diff_drive_controller.hpp | 1 - .../include/diff_drive_controller/odometry.hpp | 2 -- diff_drive_controller/src/odometry.cpp | 10 ++++++---- 3 files changed, 6 insertions(+), 7 deletions(-) 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 74526b199c..757f22a740 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/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)); } } From 6b07b0d5cb08af7ec95cce25b8f704d7e2a3c6b6 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 1 Dec 2024 20:13:06 +0000 Subject: [PATCH 08/14] Use instead of c-style NAN --- .../diff_drive_controller/speed_limiter.hpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) 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 6aab78b84e..a053711c0b 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -19,6 +19,8 @@ #ifndef DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ #define DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ +#include + #include "control_toolbox/rate_limiter.hpp" namespace diff_drive_controller @@ -40,22 +42,25 @@ class SpeedLimiter */ SpeedLimiter( bool has_velocity_limits = true, bool has_acceleration_limits = true, - bool has_jerk_limits = true, double min_velocity = NAN, double max_velocity = NAN, - double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN, - double max_jerk = NAN) + bool has_jerk_limits = true, double min_velocity = std::numeric_limits::quiet_NaN(), + double max_velocity = std::numeric_limits::quiet_NaN(), + double min_acceleration = 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()) { // START DEPRECATED if (!has_velocity_limits) { - min_velocity = max_velocity = NAN; + min_velocity = max_velocity = std::numeric_limits::quiet_NaN(); } if (!has_acceleration_limits) { - min_acceleration = max_acceleration = NAN; + min_acceleration = max_acceleration = std::numeric_limits::quiet_NaN(); } if (!has_jerk_limits) { - min_jerk = max_jerk = NAN; + min_jerk = max_jerk = std::numeric_limits::quiet_NaN(); } // END DEPRECATED speed_limiter_ = control_toolbox::RateLimiter( From c77f8733c572e10cf1e2b331be3d7222dbb2c205 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 1 Dec 2024 20:13:06 +0000 Subject: [PATCH 09/14] Update param description --- .../src/diff_drive_controller_parameter.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 538f874c1c..dc1109e043 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -116,17 +116,17 @@ diff_drive_controller: has_velocity_limits: { type: bool, default_value: true, - description: "deprecated, set the respective limits to NAN for deactivation" + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_acceleration_limits: { type: bool, default_value: true, - description: "deprecated, set the respective limits to NAN for deactivation" + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_jerk_limits: { type: bool, default_value: true, - description: "deprecated, set the respective limits to NAN for deactivation" + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } max_velocity: { type: double, @@ -198,17 +198,17 @@ diff_drive_controller: has_velocity_limits: { type: bool, default_value: true, - description: "deprecated, set the respective limits to NAN for deactivation" + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_acceleration_limits: { type: bool, default_value: true, - description: "deprecated, set the respective limits to NAN for deactivation" + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_jerk_limits: { type: bool, default_value: true, - description: "deprecated, set the respective limits to NAN for deactivation" + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } max_velocity: { type: double, From ffea38a3d02b6e4e5352ea0266c8339c75f0d8b0 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 1 Dec 2024 20:13:06 +0000 Subject: [PATCH 10/14] Add new deceleration parameters --- .../diff_drive_controller/speed_limiter.hpp | 46 +++++++++++++++---- .../src/diff_drive_controller.cpp | 39 ++++++++++++---- .../config/test_diff_drive_controller.yaml | 33 +++++++------ 3 files changed, 84 insertions(+), 34 deletions(-) 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 a053711c0b..63745b1810 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -35,37 +35,67 @@ 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 */ - SpeedLimiter( + [[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 min_acceleration = 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()) { - // START DEPRECATED if (!has_velocity_limits) { min_velocity = max_velocity = std::numeric_limits::quiet_NaN(); } if (!has_acceleration_limits) { - min_acceleration = max_acceleration = std::numeric_limits::quiet_NaN(); + max_deceleration = max_acceleration = std::numeric_limits::quiet_NaN(); } if (!has_jerk_limits) { min_jerk = max_jerk = std::numeric_limits::quiet_NaN(); } - // END DEPRECATED speed_limiter_ = control_toolbox::RateLimiter( - min_velocity, max_velocity, min_acceleration, max_acceleration, min_acceleration, - max_acceleration, min_jerk, max_jerk); + 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] 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] 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( + 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); } /** diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 70fa509bcd..1eaf187964 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 { @@ -303,6 +310,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( RCLCPP_WARN( logger, "[deprecated] has_velocity_limits parameter is deprecated. 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) { @@ -310,18 +319,25 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( logger, "[deprecated] has_acceleration_limits parameter is deprecated. 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. 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. 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) { @@ -329,25 +345,30 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( logger, "[deprecated] has_acceleration_limits parameter is deprecated. 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. 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/test/config/test_diff_drive_controller.yaml b/diff_drive_controller/test/config/test_diff_drive_controller.yaml index 606bacbf4f..64b63e8c98 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_deceleration: .NAN + linear.x.max_acceleration: .NAN + linear.x.max_deceleration_reverse: .NAN + linear.x.max_acceleration_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_deceleration: .NAN + angular.z.max_acceleration: .NAN + angular.z.max_deceleration_reverse: .NAN + angular.z.max_acceleration_reverse: .NAN + angular.z.max_jerk: .NAN + angular.z.min_jerk: .NAN From 1b69fc97c302d57fd09d614e16a015d2e37cedd1 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 1 Dec 2024 20:52:53 +0000 Subject: [PATCH 11/14] Add a simple test loading limits from parameter file --- diff_drive_controller/CMakeLists.txt | 5 ++ .../config/test_diff_drive_controller.yaml | 8 +-- .../test_diff_drive_controller_limits.yaml | 42 +++++++++++++++ .../test_configure_diff_drive_controller.cpp | 51 +++++++++++++++++++ 4 files changed, 102 insertions(+), 4 deletions(-) create mode 100644 diff_drive_controller/test/config/test_diff_drive_controller_limits.yaml create mode 100644 diff_drive_controller/test/test_configure_diff_drive_controller.cpp diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 925112e3bb..6aacfb7572 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -81,6 +81,11 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) + ament_add_gmock(test_configure_diff_drive_controller test/test_configure_diff_drive_controller.cpp) + ament_target_dependencies(test_configure_diff_drive_controller + controller_manager + ros2_control_test_assets + ) endif() install( 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 64b63e8c98..7707f2050f 100644 --- a/diff_drive_controller/test/config/test_diff_drive_controller.yaml +++ b/diff_drive_controller/test/config/test_diff_drive_controller.yaml @@ -25,18 +25,18 @@ test_diff_drive_controller: linear.x.max_velocity: .NAN linear.x.min_velocity: .NAN - linear.x.max_deceleration: .NAN linear.x.max_acceleration: .NAN - linear.x.max_deceleration_reverse: .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.max_velocity: .NAN angular.z.min_velocity: .NAN - angular.z.max_deceleration: .NAN angular.z.max_acceleration: .NAN - angular.z.max_deceleration_reverse: .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/config/test_diff_drive_controller_limits.yaml b/diff_drive_controller/test/config/test_diff_drive_controller_limits.yaml new file mode 100644 index 0000000000..2ce8beb550 --- /dev/null +++ b/diff_drive_controller/test/config/test_diff_drive_controller_limits.yaml @@ -0,0 +1,42 @@ +test_diff_drive_controller: + ros__parameters: + left_wheel_names: ["left_wheels"] + right_wheel_names: ["right_wheels"] + + wheel_separation: 0.40 + wheel_radius: 0.02 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + position_feedback: false + open_loop: true + enable_odom_tf: true + + cmd_vel_timeout: 0.5 # seconds + publish_limited_velocity: true + velocity_rolling_window_size: 10 + + linear.x.max_velocity: 10.0 + linear.x.min_velocity: -10.0 + linear.x.max_acceleration: 10.0 + linear.x.max_deceleration: -20.0 + linear.x.max_acceleration_reverse: -10.0 + linear.x.max_deceleration_reverse: 20.0 + linear.x.max_jerk: 50.0 + linear.x.min_jerk: -50.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_configure_diff_drive_controller.cpp b/diff_drive_controller/test/test_configure_diff_drive_controller.cpp new file mode 100644 index 0000000000..8b856e7d4c --- /dev/null +++ b/diff_drive_controller/test/test_configure_diff_drive_controller.cpp @@ -0,0 +1,51 @@ +// Copyright 2020 PAL Robotics SL. +// +// 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. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadDiffDriveController, load_configure_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::diffbot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/config/test_diff_drive_controller_limits.yaml"; + + cm.set_parameter({"test_diff_drive_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_diff_drive_controller.type", "diff_drive_controller/DiffDriveController"}); + auto ctr = cm.load_controller("test_diff_drive_controller"); + ASSERT_NE(ctr, nullptr); + ASSERT_EQ( + ctr->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} From 9bc6d41cf17d6357ebb940a3ee671addb8704724 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Dec 2024 20:14:32 +0000 Subject: [PATCH 12/14] Add a simple test with acceleration limits --- diff_drive_controller/CMakeLists.txt | 8 +- .../test_diff_drive_controller_limits.yaml | 42 -- .../test_configure_diff_drive_controller.cpp | 51 --- .../test/test_diff_drive_controller.cpp | 398 +++++++++--------- .../test/test_diff_drive_controller.hpp | 221 ++++++++++ 5 files changed, 421 insertions(+), 299 deletions(-) delete mode 100644 diff_drive_controller/test/config/test_diff_drive_controller_limits.yaml delete mode 100644 diff_drive_controller/test/test_configure_diff_drive_controller.cpp create mode 100644 diff_drive_controller/test/test_diff_drive_controller.hpp diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 6aacfb7572..d94b6e3ce0 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -60,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 ) @@ -81,11 +82,6 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) - ament_add_gmock(test_configure_diff_drive_controller test/test_configure_diff_drive_controller.cpp) - ament_target_dependencies(test_configure_diff_drive_controller - controller_manager - ros2_control_test_assets - ) endif() install( diff --git a/diff_drive_controller/test/config/test_diff_drive_controller_limits.yaml b/diff_drive_controller/test/config/test_diff_drive_controller_limits.yaml deleted file mode 100644 index 2ce8beb550..0000000000 --- a/diff_drive_controller/test/config/test_diff_drive_controller_limits.yaml +++ /dev/null @@ -1,42 +0,0 @@ -test_diff_drive_controller: - ros__parameters: - left_wheel_names: ["left_wheels"] - right_wheel_names: ["right_wheels"] - - wheel_separation: 0.40 - wheel_radius: 0.02 - - wheel_separation_multiplier: 1.0 - left_wheel_radius_multiplier: 1.0 - right_wheel_radius_multiplier: 1.0 - - odom_frame_id: odom - base_frame_id: base_link - pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - position_feedback: false - open_loop: true - enable_odom_tf: true - - cmd_vel_timeout: 0.5 # seconds - publish_limited_velocity: true - velocity_rolling_window_size: 10 - - linear.x.max_velocity: 10.0 - linear.x.min_velocity: -10.0 - linear.x.max_acceleration: 10.0 - linear.x.max_deceleration: -20.0 - linear.x.max_acceleration_reverse: -10.0 - linear.x.max_deceleration_reverse: 20.0 - linear.x.max_jerk: 50.0 - linear.x.min_jerk: -50.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_configure_diff_drive_controller.cpp b/diff_drive_controller/test/test_configure_diff_drive_controller.cpp deleted file mode 100644 index 8b856e7d4c..0000000000 --- a/diff_drive_controller/test/test_configure_diff_drive_controller.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// 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. - -#include -#include - -#include "controller_manager/controller_manager.hpp" -#include "hardware_interface/resource_manager.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/utilities.hpp" -#include "ros2_control_test_assets/descriptions.hpp" - -TEST(TestLoadDiffDriveController, load_configure_controller) -{ - std::shared_ptr executor = - std::make_shared(); - - controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::diffbot_urdf, true, "test_controller_manager"); - const std::string test_file_path = - std::string(TEST_FILES_DIRECTORY) + "/config/test_diff_drive_controller_limits.yaml"; - - cm.set_parameter({"test_diff_drive_controller.params_file", test_file_path}); - cm.set_parameter( - {"test_diff_drive_controller.type", "diff_drive_controller/DiffDriveController"}); - auto ctr = cm.load_controller("test_diff_drive_controller"); - ASSERT_NE(ctr, nullptr); - ASSERT_EQ( - ctr->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); -} - -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/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 72ae4dbfd7..6ebbb9f464 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -14,206 +14,7 @@ #include -#include -#include -#include -#include -#include - -#include "diff_drive_controller/diff_drive_controller.hpp" -#include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/executors.hpp" - -using CallbackReturn = controller_interface::CallbackReturn; -using hardware_interface::HW_IF_POSITION; -using hardware_interface::HW_IF_VELOCITY; -using hardware_interface::LoanedCommandInterface; -using hardware_interface::LoanedStateInterface; -using lifecycle_msgs::msg::State; -using testing::SizeIs; - -namespace -{ -const std::vector left_wheel_names = {"left_wheel_joint"}; -const std::vector right_wheel_names = {"right_wheel_joint"}; -} // namespace - -class TestableDiffDriveController : public diff_drive_controller::DiffDriveController -{ -public: - using DiffDriveController::DiffDriveController; - std::shared_ptr getLastReceivedTwist() - { - std::shared_ptr ret; - received_velocity_msg_ptr_.get(ret); - return ret; - } - - /** - * @brief wait_for_twist block until a new twist is received. - * Requires that the executor is not spinned elsewhere between the - * message publication and the call to this function - */ - void wait_for_twist( - rclcpp::Executor & executor, - const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) - { - auto until = get_node()->get_clock()->now() + timeout; - while (get_node()->get_clock()->now() < until) - { - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::microseconds(10)); - } - } - - /** - * @brief Used to get the real_time_odometry_publisher to verify its contents - * - * @return Copy of realtime_odometry_publisher_ object - */ - std::shared_ptr> - get_rt_odom_publisher() - { - return realtime_odometry_publisher_; - } -}; - -class TestDiffDriveController : public ::testing::Test -{ -protected: - static void SetUpTestCase() { rclcpp::init(0, nullptr); } - - void SetUp() override - { - controller_ = std::make_unique(); - - pub_node = std::make_shared("velocity_publisher"); - velocity_publisher = pub_node->create_publisher( - controller_name + "/cmd_vel", rclcpp::SystemDefaultsQoS()); - } - - static void TearDownTestCase() { rclcpp::shutdown(); } - - /// Publish velocity msgs - /** - * linear - magnitude of the linear command in the geometry_msgs::twist message - * angular - the magnitude of the angular command in geometry_msgs::twist message - */ - void publish(double linear, double angular) - { - int wait_count = 0; - auto topic = velocity_publisher->get_topic_name(); - while (pub_node->count_subscribers(topic) == 0) - { - if (wait_count >= 5) - { - auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; - throw std::runtime_error(error_msg); - } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - ++wait_count; - } - - geometry_msgs::msg::TwistStamped velocity_message; - velocity_message.header.stamp = pub_node->get_clock()->now(); - velocity_message.twist.linear.x = linear; - velocity_message.twist.angular.z = angular; - velocity_publisher->publish(velocity_message); - } - - /// \brief wait for the subscriber and publisher to completely setup - void waitForSetup() - { - constexpr std::chrono::seconds TIMEOUT{2}; - auto clock = pub_node->get_clock(); - auto start = clock->now(); - while (velocity_publisher->get_subscription_count() <= 0) - { - if ((clock->now() - start) > TIMEOUT) - { - FAIL(); - } - rclcpp::spin_some(pub_node); - } - } - - void assignResourcesPosFeedback() - { - std::vector state_ifs; - state_ifs.emplace_back(left_wheel_pos_state_); - state_ifs.emplace_back(right_wheel_pos_state_); - - std::vector command_ifs; - command_ifs.emplace_back(left_wheel_vel_cmd_); - command_ifs.emplace_back(right_wheel_vel_cmd_); - - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); - } - - void assignResourcesVelFeedback() - { - std::vector state_ifs; - state_ifs.emplace_back(left_wheel_vel_state_); - state_ifs.emplace_back(right_wheel_vel_state_); - - std::vector command_ifs; - command_ifs.emplace_back(left_wheel_vel_cmd_); - command_ifs.emplace_back(right_wheel_vel_cmd_); - - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); - } - - controller_interface::return_type InitController( - const std::vector left_wheel_joints_init = left_wheel_names, - const std::vector right_wheel_joints_init = right_wheel_names, - const std::vector & parameters = {}, const std::string ns = "") - { - auto node_options = rclcpp::NodeOptions(); - std::vector parameter_overrides; - - parameter_overrides.push_back( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_joints_init))); - parameter_overrides.push_back( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_joints_init))); - // default parameters - parameter_overrides.push_back( - rclcpp::Parameter("wheel_separation", rclcpp::ParameterValue(1.0))); - parameter_overrides.push_back(rclcpp::Parameter("wheel_radius", rclcpp::ParameterValue(0.1))); - - parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); - node_options.parameter_overrides(parameter_overrides); - - return controller_->init(controller_name, urdf_, 0, ns, node_options); - } - - const std::string controller_name = "test_diff_drive_controller"; - std::unique_ptr controller_; - - std::vector position_values_ = {0.1, 0.2}; - std::vector velocity_values_ = {0.01, 0.02}; - - hardware_interface::StateInterface left_wheel_pos_state_{ - left_wheel_names[0], HW_IF_POSITION, &position_values_[0]}; - hardware_interface::StateInterface right_wheel_pos_state_{ - right_wheel_names[0], HW_IF_POSITION, &position_values_[1]}; - hardware_interface::StateInterface left_wheel_vel_state_{ - left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; - hardware_interface::StateInterface right_wheel_vel_state_{ - right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; - hardware_interface::CommandInterface left_wheel_vel_cmd_{ - left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; - hardware_interface::CommandInterface right_wheel_vel_cmd_{ - right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; - - rclcpp::Node::SharedPtr pub_node; - rclcpp::Publisher::SharedPtr velocity_publisher; - - const std::string urdf_ = ""; -}; +#include "test_diff_drive_controller.hpp" TEST_F(TestDiffDriveController, init_fails_without_parameters) { @@ -448,6 +249,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( @@ -575,3 +564,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/diff_drive_controller/test/test_diff_drive_controller.hpp b/diff_drive_controller/test/test_diff_drive_controller.hpp new file mode 100644 index 0000000000..b5d1b88422 --- /dev/null +++ b/diff_drive_controller/test/test_diff_drive_controller.hpp @@ -0,0 +1,221 @@ +// Copyright 2020 PAL Robotics SL. +// +// 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. + +#ifndef TEST_DIFF_DRIVE_CONTROLLER_HPP_ +#define TEST_DIFF_DRIVE_CONTROLLER_HPP_ + +#include + +#include +#include +#include +#include + +#include "diff_drive_controller/diff_drive_controller.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" + +using CallbackReturn = controller_interface::CallbackReturn; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using lifecycle_msgs::msg::State; +using testing::SizeIs; + +namespace +{ +const std::vector left_wheel_names = {"left_wheel_joint"}; +const std::vector right_wheel_names = {"right_wheel_joint"}; +} // namespace + +class TestableDiffDriveController : public diff_drive_controller::DiffDriveController +{ +public: + using DiffDriveController::DiffDriveController; + std::shared_ptr getLastReceivedTwist() + { + std::shared_ptr ret; + received_velocity_msg_ptr_.get(ret); + return ret; + } + + /** + * @brief wait_for_twist block until a new twist is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + */ + void wait_for_twist( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + /** + * @brief Used to get the real_time_odometry_publisher to verify its contents + * + * @return Copy of realtime_odometry_publisher_ object + */ + std::shared_ptr> + get_rt_odom_publisher() + { + return realtime_odometry_publisher_; + } +}; + +class TestDiffDriveController : public ::testing::Test +{ +protected: + 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"); + velocity_publisher = pub_node->create_publisher( + controller_name + "/cmd_vel", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + /// Publish velocity msgs + /** + * linear - magnitude of the linear command in the geometry_msgs::twist message + * angular - the magnitude of the angular command in geometry_msgs::twist message + */ + void publish(double linear, double angular) + { + int wait_count = 0; + auto topic = velocity_publisher->get_topic_name(); + while (pub_node->count_subscribers(topic) == 0) + { + if (wait_count >= 5) + { + auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + + geometry_msgs::msg::TwistStamped velocity_message; + velocity_message.header.stamp = pub_node->get_clock()->now(); + velocity_message.twist.linear.x = linear; + velocity_message.twist.angular.z = angular; + velocity_publisher->publish(velocity_message); + } + + /// \brief wait for the subscriber and publisher to completely setup + void waitForSetup() + { + constexpr std::chrono::seconds TIMEOUT{2}; + auto clock = pub_node->get_clock(); + auto start = clock->now(); + while (velocity_publisher->get_subscription_count() <= 0) + { + if ((clock->now() - start) > TIMEOUT) + { + FAIL(); + } + rclcpp::spin_some(pub_node); + } + } + + void assignResourcesPosFeedback() + { + std::vector state_ifs; + state_ifs.emplace_back(left_wheel_pos_state_); + state_ifs.emplace_back(right_wheel_pos_state_); + + std::vector command_ifs; + command_ifs.emplace_back(left_wheel_vel_cmd_); + command_ifs.emplace_back(right_wheel_vel_cmd_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void assignResourcesVelFeedback() + { + std::vector state_ifs; + state_ifs.emplace_back(left_wheel_vel_state_); + state_ifs.emplace_back(right_wheel_vel_state_); + + std::vector command_ifs; + command_ifs.emplace_back(left_wheel_vel_cmd_); + command_ifs.emplace_back(right_wheel_vel_cmd_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + controller_interface::return_type InitController( + const std::vector left_wheel_joints_init = left_wheel_names, + const std::vector right_wheel_joints_init = right_wheel_names, + const std::vector & parameters = {}, const std::string ns = "") + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + parameter_overrides.push_back( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_joints_init))); + parameter_overrides.push_back( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_joints_init))); + // default parameters + parameter_overrides.push_back( + rclcpp::Parameter("wheel_separation", rclcpp::ParameterValue(1.0))); + parameter_overrides.push_back(rclcpp::Parameter("wheel_radius", rclcpp::ParameterValue(0.1))); + + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + + return controller_->init(controller_name, urdf_, 0, ns, node_options); + } + + std::string controller_name; + std::unique_ptr controller_; + + std::vector position_values_ = {0.1, 0.2}; + std::vector velocity_values_ = {0.01, 0.02}; + + hardware_interface::StateInterface left_wheel_pos_state_{ + left_wheel_names[0], HW_IF_POSITION, &position_values_[0]}; + hardware_interface::StateInterface right_wheel_pos_state_{ + right_wheel_names[0], HW_IF_POSITION, &position_values_[1]}; + hardware_interface::StateInterface left_wheel_vel_state_{ + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; + hardware_interface::StateInterface right_wheel_vel_state_{ + right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; + hardware_interface::CommandInterface left_wheel_vel_cmd_{ + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; + hardware_interface::CommandInterface right_wheel_vel_cmd_{ + right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; + + rclcpp::Node::SharedPtr pub_node; + rclcpp::Publisher::SharedPtr velocity_publisher; + + const std::string urdf_ = ""; +}; + +#endif // TEST_DIFF_DRIVE_CONTROLLER_HPP_ From 89b4db34cc87766038957bea687ac58ef882d2fa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 6 Dec 2024 19:51:37 +0100 Subject: [PATCH 13/14] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- .../include/diff_drive_controller/speed_limiter.hpp | 3 --- diff_drive_controller/src/diff_drive_controller.cpp | 12 ++++++------ 2 files changed, 6 insertions(+), 9 deletions(-) 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 63745b1810..4fa08fcdba 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -69,9 +69,6 @@ class SpeedLimiter /** * \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] max_acceleration_reverse Maximum acceleration in reverse direction [m/s^2], usually diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 937e8a93d9..a6620e8a25 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -310,7 +310,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { RCLCPP_WARN( logger, - "[deprecated] has_velocity_limits parameter is deprecated. Set the respective limits to NAN"); + "[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(); } @@ -318,7 +318,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { RCLCPP_WARN( logger, - "[deprecated] has_acceleration_limits parameter is deprecated. Set the respective limits to " + "[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 = @@ -328,7 +328,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { RCLCPP_WARN( logger, - "[deprecated] has_jerk_limits parameter is deprecated. Set the respective limits to NAN"); + "[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(); } @@ -336,7 +336,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { RCLCPP_WARN( logger, - "[deprecated] has_velocity_limits parameter is deprecated. Set the respective limits to NAN"); + "[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(); } @@ -344,7 +344,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { RCLCPP_WARN( logger, - "[deprecated] has_acceleration_limits parameter is deprecated. Set the respective limits to " + "[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 = @@ -354,7 +354,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { RCLCPP_WARN( logger, - "[deprecated] has_jerk_limits parameter is deprecated. Set the respective limits to NAN"); + "[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(); } From 6d21da92ef58a7164d5c6a872b8e1f3addaee294 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 6 Dec 2024 18:58:11 +0000 Subject: [PATCH 14/14] Code format --- .../src/diff_drive_controller.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index a6620e8a25..703d1af775 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -310,7 +310,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { RCLCPP_WARN( logger, - "[deprecated] has_velocity_limits parameter is deprecated, instead set the respective limits to NAN"); + "[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(); } @@ -318,7 +319,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { RCLCPP_WARN( logger, - "[deprecated] has_acceleration_limits parameter is deprecated, instead set the respective limits to " + "[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 = @@ -328,7 +330,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { RCLCPP_WARN( logger, - "[deprecated] has_jerk_limits parameter is deprecated, instead set the respective limits to NAN"); + "[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(); } @@ -336,7 +339,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { RCLCPP_WARN( logger, - "[deprecated] has_velocity_limits parameter is deprecated, instead set the respective limits to NAN"); + "[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(); } @@ -344,7 +348,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { RCLCPP_WARN( logger, - "[deprecated] has_acceleration_limits parameter is deprecated, instead set the respective limits to " + "[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 = @@ -354,7 +359,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { RCLCPP_WARN( logger, - "[deprecated] has_jerk_limits parameter is deprecated, instead set the respective limits to NAN"); + "[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(); }