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 36ba2d6874..aeca57a245 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 @@ -123,8 +123,8 @@ class DiffDriveController : public controller_interface::ControllerInterface std::queue previous_commands_; // last two commands // speed limiters - SpeedLimiter limiter_linear_; - SpeedLimiter limiter_angular_; + std::unique_ptr limiter_linear_; + std::unique_ptr 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 4fa08fcdba..937e848460 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -40,7 +40,7 @@ class SpeedLimiter * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 */ - [[deprecated]] SpeedLimiter( + [[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(), @@ -79,16 +79,16 @@ class SpeedLimiter * >= 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 */ - 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()) + 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, diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 6957864321..a21e221268 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -45,14 +45,7 @@ using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; using lifecycle_msgs::msg::State; -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()) -{ -} +DiffDriveController::DiffDriveController() : controller_interface::ControllerInterface() {} const char * DiffDriveController::feedback_type() const { @@ -241,9 +234,9 @@ controller_interface::return_type DiffDriveController::update( auto & last_command = previous_commands_.back().twist; auto & second_to_last_command = previous_commands_.front().twist; - limiter_linear_.limit( + limiter_linear_->limit( linear_command, last_command.linear.x, second_to_last_command.linear.x, 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(); @@ -365,13 +358,13 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( std::numeric_limits::quiet_NaN(); } // END DEPRECATED - limiter_linear_ = SpeedLimiter( + limiter_linear_ = 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); - limiter_angular_ = SpeedLimiter( + 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,