From 8f8b22e4ee834cbecbfa7956afe39bfeab63ced9 Mon Sep 17 00:00:00 2001 From: hjh059 Date: Sun, 14 Dec 2025 20:00:05 +0800 Subject: [PATCH 1/2] RateLimiter: Don't update parameters before input checks #437 --- .../include/control_toolbox/rate_limiter.hpp | 78 ++++++++++++------- 1 file changed, 48 insertions(+), 30 deletions(-) diff --git a/control_toolbox/include/control_toolbox/rate_limiter.hpp b/control_toolbox/include/control_toolbox/rate_limiter.hpp index 63a830af..79258290 100644 --- a/control_toolbox/include/control_toolbox/rate_limiter.hpp +++ b/control_toolbox/include/control_toolbox/rate_limiter.hpp @@ -171,68 +171,86 @@ void RateLimiter::set_params( T min_first_derivative_pos, T max_first_derivative_neg, T min_second_derivative, T max_second_derivative) { - min_value_ = min_value; - max_value_ = max_value; - min_first_derivative_neg_ = min_first_derivative_neg; - max_first_derivative_pos_ = max_first_derivative_pos; - min_first_derivative_pos_ = min_first_derivative_pos; - max_first_derivative_neg_ = max_first_derivative_neg; - min_second_derivative_ = min_second_derivative; - max_second_derivative_ = max_second_derivative; - - if (std::isnan(max_value_)) + T tmp_min_value = min_value; + T tmp_max_value = max_value; + T tmp_min_first_derivative_neg = min_first_derivative_neg; + T tmp_max_first_derivative_pos = max_first_derivative_pos; + T tmp_min_first_derivative_pos = min_first_derivative_pos; + T tmp_max_first_derivative_neg = max_first_derivative_neg; + T tmp_min_second_derivative = min_second_derivative; + T tmp_max_second_derivative = max_second_derivative; + auto tmp_has_value_limits = has_value_limits_; + auto tmp_has_first_derivative_limits = has_first_derivative_limits_; + auto tmp_has_second_derivative_limits = has_second_derivative_limits_; + + if (std::isnan(tmp_max_value)) { - has_value_limits_ = false; + tmp_has_value_limits = false; } - if (std::isnan(min_value_)) + if (std::isnan(tmp_min_value)) { - min_value_ = -max_value_; + tmp_min_value = -tmp_max_value; } - if (has_value_limits_ && min_value_ > max_value_) + if (tmp_has_value_limits && tmp_min_value > tmp_max_value) { throw std::invalid_argument("Invalid value limits"); } - if (std::isnan(max_first_derivative_pos_)) + if (std::isnan(tmp_max_first_derivative_pos)) { - has_first_derivative_limits_ = false; + tmp_has_first_derivative_limits = false; } - if (std::isnan(min_first_derivative_neg_)) + if (std::isnan(tmp_min_first_derivative_neg)) { - min_first_derivative_neg_ = -max_first_derivative_pos_; + tmp_min_first_derivative_neg = -tmp_max_first_derivative_pos; } - if (has_first_derivative_limits_ && min_first_derivative_neg_ > max_first_derivative_pos_) + if ( + tmp_has_first_derivative_limits && tmp_min_first_derivative_neg > tmp_max_first_derivative_pos) { throw std::invalid_argument("Invalid first derivative limits"); } - if (has_first_derivative_limits_) + if (tmp_has_first_derivative_limits) { - if (std::isnan(max_first_derivative_neg_)) + if (std::isnan(tmp_max_first_derivative_neg)) { - max_first_derivative_neg_ = max_first_derivative_pos_; + tmp_max_first_derivative_neg = tmp_max_first_derivative_pos; } - if (std::isnan(min_first_derivative_pos_)) + if (std::isnan(tmp_min_first_derivative_pos)) { - min_first_derivative_pos_ = min_first_derivative_neg_; + tmp_min_first_derivative_pos = tmp_min_first_derivative_neg; } - if (has_first_derivative_limits_ && min_first_derivative_pos_ > max_first_derivative_neg_) + if ( + tmp_has_first_derivative_limits && + tmp_min_first_derivative_pos > tmp_max_first_derivative_neg) { throw std::invalid_argument("Invalid first derivative limits"); } } - if (std::isnan(max_second_derivative_)) + if (std::isnan(tmp_max_second_derivative)) { - has_second_derivative_limits_ = false; + tmp_has_second_derivative_limits = false; } - if (std::isnan(min_second_derivative_)) + if (std::isnan(tmp_min_second_derivative)) { - min_second_derivative_ = -max_second_derivative_; + tmp_min_second_derivative = -tmp_max_second_derivative; } - if (has_second_derivative_limits_ && min_second_derivative_ > max_second_derivative_) + if (tmp_has_second_derivative_limits && tmp_min_second_derivative > tmp_max_second_derivative) { throw std::invalid_argument("Invalid second derivative limits"); } + + min_value_ = tmp_min_value; + max_value_ = tmp_max_value; + min_first_derivative_neg_ = tmp_min_first_derivative_neg; + max_first_derivative_pos_ = tmp_max_first_derivative_pos; + min_first_derivative_pos_ = tmp_min_first_derivative_pos; + max_first_derivative_neg_ = tmp_max_first_derivative_neg; + min_second_derivative_ = tmp_min_second_derivative; + max_second_derivative_ = tmp_max_second_derivative; + has_value_limits_ = tmp_has_value_limits; + has_first_derivative_limits_ = tmp_has_first_derivative_limits; + has_second_derivative_limits_ = tmp_has_second_derivative_limits; } template From be681a01224e52d0158453168b44f70f3a72636b Mon Sep 17 00:00:00 2001 From: hjh059 Date: Mon, 15 Dec 2025 20:09:25 +0800 Subject: [PATCH 2/2] fixup! RateLimiter: Don't update parameters before input checks #437 --- .../include/control_toolbox/rate_limiter.hpp | 61 ++++++++----------- 1 file changed, 25 insertions(+), 36 deletions(-) diff --git a/control_toolbox/include/control_toolbox/rate_limiter.hpp b/control_toolbox/include/control_toolbox/rate_limiter.hpp index 79258290..24b2159c 100644 --- a/control_toolbox/include/control_toolbox/rate_limiter.hpp +++ b/control_toolbox/include/control_toolbox/rate_limiter.hpp @@ -171,83 +171,72 @@ void RateLimiter::set_params( T min_first_derivative_pos, T max_first_derivative_neg, T min_second_derivative, T max_second_derivative) { - T tmp_min_value = min_value; - T tmp_max_value = max_value; - T tmp_min_first_derivative_neg = min_first_derivative_neg; - T tmp_max_first_derivative_pos = max_first_derivative_pos; - T tmp_min_first_derivative_pos = min_first_derivative_pos; - T tmp_max_first_derivative_neg = max_first_derivative_neg; - T tmp_min_second_derivative = min_second_derivative; - T tmp_max_second_derivative = max_second_derivative; auto tmp_has_value_limits = has_value_limits_; auto tmp_has_first_derivative_limits = has_first_derivative_limits_; auto tmp_has_second_derivative_limits = has_second_derivative_limits_; - if (std::isnan(tmp_max_value)) + if (std::isnan(max_value)) { tmp_has_value_limits = false; } - if (std::isnan(tmp_min_value)) + if (std::isnan(min_value)) { - tmp_min_value = -tmp_max_value; + min_value = -max_value; } - if (tmp_has_value_limits && tmp_min_value > tmp_max_value) + if (tmp_has_value_limits && min_value > max_value) { throw std::invalid_argument("Invalid value limits"); } - if (std::isnan(tmp_max_first_derivative_pos)) + if (std::isnan(max_first_derivative_pos)) { tmp_has_first_derivative_limits = false; } - if (std::isnan(tmp_min_first_derivative_neg)) + if (std::isnan(min_first_derivative_neg)) { - tmp_min_first_derivative_neg = -tmp_max_first_derivative_pos; + min_first_derivative_neg = -max_first_derivative_pos; } - if ( - tmp_has_first_derivative_limits && tmp_min_first_derivative_neg > tmp_max_first_derivative_pos) + if (tmp_has_first_derivative_limits && min_first_derivative_neg > max_first_derivative_pos) { throw std::invalid_argument("Invalid first derivative limits"); } if (tmp_has_first_derivative_limits) { - if (std::isnan(tmp_max_first_derivative_neg)) + if (std::isnan(max_first_derivative_neg)) { - tmp_max_first_derivative_neg = tmp_max_first_derivative_pos; + max_first_derivative_neg = max_first_derivative_pos; } - if (std::isnan(tmp_min_first_derivative_pos)) + if (std::isnan(min_first_derivative_pos)) { - tmp_min_first_derivative_pos = tmp_min_first_derivative_neg; + min_first_derivative_pos = min_first_derivative_neg; } - if ( - tmp_has_first_derivative_limits && - tmp_min_first_derivative_pos > tmp_max_first_derivative_neg) + if (tmp_has_first_derivative_limits && min_first_derivative_pos > max_first_derivative_neg) { throw std::invalid_argument("Invalid first derivative limits"); } } - if (std::isnan(tmp_max_second_derivative)) + if (std::isnan(max_second_derivative)) { tmp_has_second_derivative_limits = false; } - if (std::isnan(tmp_min_second_derivative)) + if (std::isnan(min_second_derivative)) { - tmp_min_second_derivative = -tmp_max_second_derivative; + min_second_derivative = -max_second_derivative; } - if (tmp_has_second_derivative_limits && tmp_min_second_derivative > tmp_max_second_derivative) + if (tmp_has_second_derivative_limits && min_second_derivative > max_second_derivative) { throw std::invalid_argument("Invalid second derivative limits"); } - min_value_ = tmp_min_value; - max_value_ = tmp_max_value; - min_first_derivative_neg_ = tmp_min_first_derivative_neg; - max_first_derivative_pos_ = tmp_max_first_derivative_pos; - min_first_derivative_pos_ = tmp_min_first_derivative_pos; - max_first_derivative_neg_ = tmp_max_first_derivative_neg; - min_second_derivative_ = tmp_min_second_derivative; - max_second_derivative_ = tmp_max_second_derivative; + min_value_ = min_value; + max_value_ = max_value; + min_first_derivative_neg_ = min_first_derivative_neg; + max_first_derivative_pos_ = max_first_derivative_pos; + min_first_derivative_pos_ = min_first_derivative_pos; + max_first_derivative_neg_ = max_first_derivative_neg; + min_second_derivative_ = min_second_derivative; + max_second_derivative_ = max_second_derivative; has_value_limits_ = tmp_has_value_limits; has_first_derivative_limits_ = tmp_has_first_derivative_limits; has_second_derivative_limits_ = tmp_has_second_derivative_limits;