diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 735f9ab..649fc8c 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -11,10 +11,11 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wpedantic -Wshadow -Wold-style-cast) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS + control_toolbox controller_interface generate_parameter_library geometry_msgs @@ -33,24 +34,25 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -generate_parameter_library(mecanum_drive_controller_parameters - src/mecanum_drive_controller_parameter.yaml) +# get include dirs from control_toolbox for the custom validators +get_target_property(TB_INCLUDE_DIRS control_toolbox::rate_limiter_parameters + INTERFACE_INCLUDE_DIRECTORIES) +generate_parameter_library( + mecanum_drive_controller_parameters + src/mecanum_drive_controller_parameter.yaml + ${TB_INCLUDE_DIRS}/control_toolbox/custom_validators.hpp) -add_library( - mecanum_drive_controller SHARED src/mecanum_drive_controller.cpp - src/odometry.cpp src/speed_limiter.cpp) +add_library(mecanum_drive_controller SHARED src/mecanum_drive_controller.cpp + src/odometry.cpp) target_include_directories( mecanum_drive_controller - PUBLIC $ + PUBLIC $ $) -target_link_libraries(mecanum_drive_controller - PUBLIC mecanum_drive_controller_parameters) +target_link_libraries( + mecanum_drive_controller PUBLIC mecanum_drive_controller_parameters + control_toolbox::rate_limiter_parameters) ament_target_dependencies(mecanum_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. -target_compile_definitions(mecanum_drive_controller - PRIVATE "MECANUM_DRIVE_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface mecanum_drive_plugin.xml) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index d26561c..e1f3a5b 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -47,7 +47,6 @@ #include "mecanum_drive_controller/mecanum_drive_controller_parameters.hpp" #include "mecanum_drive_controller/odometry.hpp" #include "mecanum_drive_controller/speed_limiter.hpp" -#include "mecanum_drive_controller/visibility_control.h" #include "odometry.hpp" namespace mecanum_drive_controller @@ -57,46 +56,32 @@ class MecanumDriveController : public controller_interface::ControllerInterface using TwistStamped = geometry_msgs::msg::TwistStamped; public: - MECANUM_DRIVE_CONTROLLER_PUBLIC MecanumDriveController(); - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state) override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_error( const rclcpp_lifecycle::State & previous_state) override; - MECANUM_DRIVE_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_shutdown( - const rclcpp_lifecycle::State & previous_state) override; - protected: struct WheelHandle { @@ -124,6 +109,9 @@ class MecanumDriveController : public controller_interface::ControllerInterface Odometry odometry_; + // Timeout to consider cmd_vel commands old + rclcpp::Duration cmd_vel_timeout_ = rclcpp::Duration::from_seconds(0.5); + std::shared_ptr> odometry_publisher_ = nullptr; std::shared_ptr> realtime_odometry_publisher_ = nullptr; @@ -132,10 +120,6 @@ class MecanumDriveController : public controller_interface::ControllerInterface nullptr; std::shared_ptr> realtime_odometry_transform_publisher_ = nullptr; - - // Timeout to consider cmd_vel commands old - std::chrono::milliseconds cmd_vel_timeout_{500}; - bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; @@ -144,9 +128,9 @@ class MecanumDriveController : public controller_interface::ControllerInterface std::queue previous_commands_; // last two commands // speed limiters - SpeedLimiter limiter_linear_x_; - SpeedLimiter limiter_linear_y_; - SpeedLimiter limiter_angular_; + std::unique_ptr limiter_linear_x_; + std::unique_ptr limiter_linear_y_; + std::unique_ptr limiter_angular_; bool publish_limited_velocity_ = false; std::shared_ptr> limited_velocity_publisher_ = nullptr; diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.hpp index 689f54d..a13a3f3 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.hpp @@ -23,7 +23,9 @@ #ifndef MECANUM_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ #define MECANUM_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ -#include +#include + +#include "control_toolbox/rate_limiter.hpp" namespace mecanum_drive_controller { @@ -37,16 +39,62 @@ 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( - 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); + [[deprecated]] explicit SpeedLimiter( + bool has_velocity_limits = true, bool has_acceleration_limits = true, + bool has_jerk_limits = true, double min_velocity = std::numeric_limits::quiet_NaN(), + double max_velocity = std::numeric_limits::quiet_NaN(), + double max_deceleration = std::numeric_limits::quiet_NaN(), + double max_acceleration = std::numeric_limits::quiet_NaN(), + double min_jerk = std::numeric_limits::quiet_NaN(), + double max_jerk = std::numeric_limits::quiet_NaN()) + { + if (!has_velocity_limits) { + min_velocity = max_velocity = std::numeric_limits::quiet_NaN(); + } + if (!has_acceleration_limits) { + max_deceleration = max_acceleration = std::numeric_limits::quiet_NaN(); + } + if (!has_jerk_limits) { + min_jerk = max_jerk = std::numeric_limits::quiet_NaN(); + } + speed_limiter_ = control_toolbox::RateLimiter( + min_velocity, max_velocity, max_deceleration, max_acceleration, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), min_jerk, + max_jerk); + } + + /** + * \brief Constructor + * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 + * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 + * \param [in] max_acceleration_reverse Maximum acceleration in reverse direction [m/s^2], usually + * <= 0 + * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 + * \param [in] max_deceleration Maximum deceleration [m/s^2], usually <= 0 + * \param [in] max_deceleration_reverse Maximum deceleration in reverse direction [m/s^2], usually + * >= 0 + * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 + * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + * + * \note + * If max_* values are NAN, the respective limit is deactivated + * If min_* values are NAN (unspecified), defaults to -max + * If min_first_derivative_pos/max_first_derivative_neg values are NAN, symmetric limits are used + */ + explicit SpeedLimiter( + double min_velocity, double max_velocity, double max_acceleration_reverse, + double max_acceleration, double max_deceleration, double max_deceleration_reverse, + double min_jerk, double max_jerk) + { + speed_limiter_ = control_toolbox::RateLimiter( + min_velocity, max_velocity, max_acceleration_reverse, max_acceleration, max_deceleration, + max_deceleration_reverse, min_jerk, max_jerk); + } /** * \brief Limit the velocity and acceleration @@ -56,14 +104,17 @@ class SpeedLimiter * \param [in] dt Time step [s] * \return Limiting factor (1.0 if none) */ - double limit(double & v, double v0, double v1, double dt); + double limit(double & v, double v0, double v1, double dt) + { + return speed_limiter_.limit(v, v0, v1, dt); + } /** * \brief Limit the velocity * \param [in, out] v Velocity [m/s] * \return Limiting factor (1.0 if none) */ - double limit_velocity(double & v); + double limit_velocity(double & v) { return speed_limiter_.limit_value(v); } /** * \brief Limit the acceleration @@ -72,7 +123,10 @@ class SpeedLimiter * \param [in] dt Time step [s] * \return Limiting factor (1.0 if none) */ - double limit_acceleration(double & v, double v0, double dt); + double limit_acceleration(double & v, double v0, double dt) + { + return speed_limiter_.limit_first_derivative(v, v0, dt); + } /** * \brief Limit the jerk @@ -83,25 +137,13 @@ class SpeedLimiter * \return Limiting factor (1.0 if none) * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control */ - double limit_jerk(double & v, double v0, double v1, double dt); + double limit_jerk(double & v, double v0, double v1, double dt) + { + return speed_limiter_.limit_second_derivative(v, v0, v1, dt); + } private: - // Enable/Disable velocity/acceleration/jerk limits: - bool has_velocity_limits_; - bool has_acceleration_limits_; - bool has_jerk_limits_; - - // Velocity limits: - double min_velocity_; - double max_velocity_; - - // Acceleration limits: - double min_acceleration_; - double max_acceleration_; - - // Jerk limits: - double min_jerk_; - double max_jerk_; + control_toolbox::RateLimiter speed_limiter_; // Instance of the new RateLimiter }; } // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h deleted file mode 100644 index ebe6fab..0000000 --- a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. -// -// 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. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -// Copied and adapted from diff_drive_controller (https://github.com/ros-controls/ros2_controllers) - -#ifndef MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define MECANUM_DRIVE_CONTROLLER_EXPORT __attribute__((dllexport)) -#define MECANUM_DRIVE_CONTROLLER_IMPORT __attribute__((dllimport)) -#else -#define MECANUM_DRIVE_CONTROLLER_EXPORT __declspec(dllexport) -#define MECANUM_DRIVE_CONTROLLER_IMPORT __declspec(dllimport) -#endif -#ifdef MECANUM_DRIVE_CONTROLLER_BUILDING_DLL -#define MECANUM_DRIVE_CONTROLLER_PUBLIC MECANUM_DRIVE_CONTROLLER_EXPORT -#else -#define MECANUM_DRIVE_CONTROLLER_PUBLIC MECANUM_DRIVE_CONTROLLER_IMPORT -#endif -#define MECANUM_DRIVE_CONTROLLER_PUBLIC_TYPE MECANUM_DRIVE_CONTROLLER_PUBLIC -#define MECANUM_DRIVE_CONTROLLER_LOCAL -#else -#define MECANUM_DRIVE_CONTROLLER_EXPORT __attribute__((visibility("default"))) -#define MECANUM_DRIVE_CONTROLLER_IMPORT -#if __GNUC__ >= 4 -#define MECANUM_DRIVE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) -#define MECANUM_DRIVE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) -#else -#define MECANUM_DRIVE_CONTROLLER_PUBLIC -#define MECANUM_DRIVE_CONTROLLER_LOCAL -#endif -#define MECANUM_DRIVE_CONTROLLER_PUBLIC_TYPE -#endif - -#endif // MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index cd2f12b..5f7597c 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -3,23 +3,31 @@ mecanum_drive_controller 1.0.0 - Controller for a mecanum drive mobile base. + Husarion Apache License 2.0 + https://husarion.com/ + https://github.com/husarion/husarion_controllers + https://github.com/husarion/husarion_controllers/issues + Jakub Delicat + Rafał Górecki + Dawid Kmak Maciej Stępień Bence Magyar Jordan Palacios - Husarion - ament_cmake + generate_parameter_library + + control_toolbox controller_interface geometry_msgs hardware_interface nav_msgs + pluginlib rclcpp rclcpp_lifecycle rcpputils @@ -27,8 +35,6 @@ tf2 tf2_msgs - pluginlib - ament_cmake diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index c7f91fd..97fb858 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -212,11 +212,11 @@ controller_interface::return_type MecanumDriveController::update( auto & last_command = previous_commands_.back().twist; auto & second_to_last_command = previous_commands_.front().twist; - limiter_linear_x_.limit( + limiter_linear_x_->limit( linear_command_x, last_command.linear.x, second_to_last_command.linear.x, period.seconds()); - limiter_linear_y_.limit( + limiter_linear_y_->limit( linear_command_y, last_command.linear.y, second_to_last_command.linear.y, period.seconds()); - limiter_angular_.limit( + limiter_angular_->limit( angular_command, last_command.angular.z, second_to_last_command.angular.z, period.seconds()); previous_commands_.pop(); @@ -336,34 +336,87 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; publish_limited_velocity_ = params_.publish_limited_velocity; + // START DEPRECATED + if (!params_.linear.x.has_velocity_limits) { + RCLCPP_WARN( + logger, + "[deprecated] has_velocity_limits parameter is deprecated, instead set the respective limits " + "to NAN"); + params_.linear.x.min_velocity = params_.linear.x.max_velocity = + std::numeric_limits::quiet_NaN(); + } + if (!params_.linear.x.has_acceleration_limits) { + RCLCPP_WARN( + logger, + "[deprecated] has_acceleration_limits parameter is deprecated, instead set the respective " + "limits to " + "NAN"); + params_.linear.x.max_deceleration = params_.linear.x.max_acceleration = + params_.linear.x.max_deceleration_reverse = params_.linear.x.max_acceleration_reverse = + std::numeric_limits::quiet_NaN(); + } + if (!params_.linear.x.has_jerk_limits) { + RCLCPP_WARN( + logger, + "[deprecated] has_jerk_limits parameter is deprecated, instead set the respective limits to " + "NAN"); + params_.linear.x.min_jerk = params_.linear.x.max_jerk = + std::numeric_limits::quiet_NaN(); + } + if (!params_.angular.z.has_velocity_limits) { + RCLCPP_WARN( + logger, + "[deprecated] has_velocity_limits parameter is deprecated, instead set the respective limits " + "to NAN"); + params_.angular.z.min_velocity = params_.angular.z.max_velocity = + std::numeric_limits::quiet_NaN(); + } + if (!params_.angular.z.has_acceleration_limits) { + RCLCPP_WARN( + logger, + "[deprecated] has_acceleration_limits parameter is deprecated, instead set the respective " + "limits to " + "NAN"); + params_.angular.z.max_deceleration = params_.angular.z.max_acceleration = + params_.angular.z.max_deceleration_reverse = params_.angular.z.max_acceleration_reverse = + std::numeric_limits::quiet_NaN(); + } + if (!params_.angular.z.has_jerk_limits) { + RCLCPP_WARN( + logger, + "[deprecated] has_jerk_limits parameter is deprecated, instead set the respective limits to " + "NAN"); + params_.angular.z.min_jerk = params_.angular.z.max_jerk = + std::numeric_limits::quiet_NaN(); + } try { - limiter_linear_x_ = 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_linear_x_ = std::make_unique( + 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); } catch (const std::runtime_error & e) { RCLCPP_ERROR( get_node()->get_logger(), "Error configuring x linear speed limiter: %s", e.what()); } try { - limiter_linear_y_ = SpeedLimiter( - params_.linear.y.has_velocity_limits, params_.linear.y.has_acceleration_limits, - params_.linear.y.has_jerk_limits, params_.linear.y.min_velocity, - params_.linear.y.max_velocity, params_.linear.y.min_acceleration, - params_.linear.y.max_acceleration, params_.linear.y.min_jerk, params_.linear.y.max_jerk); + limiter_linear_y_ = std::make_unique( + params_.linear.y.min_velocity, params_.linear.y.max_velocity, + params_.linear.y.max_acceleration_reverse, params_.linear.y.max_acceleration, + params_.linear.y.max_deceleration, params_.linear.y.max_deceleration_reverse, + params_.linear.y.min_jerk, params_.linear.y.max_jerk); } catch (const std::runtime_error & e) { RCLCPP_ERROR( get_node()->get_logger(), "Error configuring y linear speed limiter: %s", e.what()); } try { - 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); + limiter_angular_ = std::make_unique( + 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); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_node()->get_logger(), "Error configuring angular speed limiter: %s", e.what()); } @@ -546,12 +599,6 @@ void MecanumDriveController::reset_buffers() received_velocity_msg_ptr_.writeFromNonRT(empty_msg_ptr); } -controller_interface::CallbackReturn MecanumDriveController::on_shutdown( - const rclcpp_lifecycle::State &) -{ - return controller_interface::CallbackReturn::SUCCESS; -} - void MecanumDriveController::halt() { const bool value_set_error = diff --git a/mecanum_drive_controller/src/mecanum_drive_controller_parameter.yaml b/mecanum_drive_controller/src/mecanum_drive_controller_parameter.yaml index 3c8baeb..ccd0a0d 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller_parameter.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller_parameter.yaml @@ -2,33 +2,58 @@ mecanum_drive_controller: front_left_wheel_name: { type: string, description: "Link name of the front left side wheel", + read_only: true, + validation: { + not_empty<>: null, + } } front_right_wheel_name: { type: string, description: "Link name of the front right side wheel", + read_only: true, + validation: { + not_empty<>: null, + } } rear_left_wheel_name: { type: string, description: "Link name of the rear left side wheel", + read_only: true, + validation: { + not_empty<>: null, + } } rear_right_wheel_name: { type: string, description: "Link name of the rear right side wheel", + read_only: true, + validation: { + not_empty<>: null, + } } wheel_separation_x: { type: double, default_value: 0.0, description: "Shortest distance between the front and rear wheels. If this parameter is wrong, the robot will not behave correctly in curves.", + validation: { + gt<>: [0.0] + } } wheel_separation_y: { type: double, default_value: 0.0, description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.", + validation: { + gt<>: [0.0] + } } wheel_radius: { type: double, default_value: 0.0, description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + validation: { + gt<>: [0.0] + } } wheel_separation_x_multiplier: { type: double, @@ -114,112 +139,244 @@ mecanum_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: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } min_velocity: { type: double, default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } } max_acceleration: { type: double, default_value: .NAN, + description: "Maximum acceleration in forward direction.", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + max_deceleration: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in forward direction.", + validation: { + "control_filters::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: { + "control_filters::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: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } max_jerk: { type: double, default_value: .NAN, + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } min_jerk: { type: double, default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } } y: 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: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } min_velocity: { type: double, default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } } max_acceleration: { type: double, default_value: .NAN, + description: "Maximum acceleration in forward direction.", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + max_deceleration: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in forward direction.", + validation: { + "control_filters::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: { + "control_filters::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: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } max_jerk: { type: double, default_value: .NAN, + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } min_jerk: { type: double, default_value: .NAN, + validation: { + "control_filters::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: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } min_velocity: { type: double, default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } } max_acceleration: { type: double, default_value: .NAN, + description: "Maximum acceleration in positive direction.", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + max_deceleration: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in positive direction.", + validation: { + "control_filters::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: { + "control_filters::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: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } max_jerk: { type: double, default_value: .NAN, + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } min_jerk: { type: double, default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } } diff --git a/mecanum_drive_controller/src/speed_limiter.cpp b/mecanum_drive_controller/src/speed_limiter.cpp deleted file mode 100644 index 0d3f58b..0000000 --- a/mecanum_drive_controller/src/speed_limiter.cpp +++ /dev/null @@ -1,131 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// Copyright 2022 Husarion -// -// 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 - */ - -// Copied and adapted from diff_drive_controller (https://github.com/ros-controls/ros2_controllers) -// Author: Maciej Stępień - -#include -#include - -#include "mecanum_drive_controller/speed_limiter.hpp" - -namespace mecanum_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 mecanum_drive_controller