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 dfb0663fd6..ac34b81eca 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 a1145be47d..58d989ccca 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] + } }