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