From dc7633887e7ccde904dec2734b71942367ff8df3 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Aug 2025 08:22:16 +0000 Subject: [PATCH 1/4] Declare parameters --- control_toolbox/src/pid_ros.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 84dbeabd..779a9155 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -260,6 +260,7 @@ bool PidROS::initialize_from_ros_parameters() u_max = UMAX_INFINITY; u_min = -UMAX_INFINITY; bool antiwindup = false; + bool saturation = false; std::string antiwindup_strat_str = "legacy"; bool all_params_available = true; @@ -274,9 +275,17 @@ bool PidROS::initialize_from_ros_parameters() all_params_available &= get_double_param(param_prefix_ + "tracking_time_constant", tracking_time_constant); + get_boolean_param(param_prefix_ + "saturation", saturation); + if (!saturation) + { + u_max = UMAX_INFINITY; + u_min = -UMAX_INFINITY; + } get_boolean_param(param_prefix_ + "antiwindup", antiwindup); get_string_param(param_prefix_ + "antiwindup_strategy", antiwindup_strat_str); declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(false)); + declare_param( + param_prefix_ + "activate_state_publisher", rclcpp::ParameterValue(rt_state_pub_ != nullptr)); if (all_params_available) { From 4577aefc268cfa68629eaed6612e543c068abb75 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Aug 2025 08:39:50 +0000 Subject: [PATCH 2/4] Fix default value --- control_toolbox/src/pid_ros.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 779a9155..24c3dee2 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -394,7 +394,9 @@ bool PidROS::initialize_from_args( rclcpp::ParameterValue(antiwindup_strat.tracking_time_constant)); declare_param( param_prefix_ + "error_deadband", rclcpp::ParameterValue(antiwindup_strat.error_deadband)); - declare_param(param_prefix_ + "saturation", rclcpp::ParameterValue(true)); + declare_param( + param_prefix_ + "saturation", + rclcpp::ParameterValue(std::isfinite(gains.u_max_) || std::isfinite(gains.u_min_))); declare_param( param_prefix_ + "antiwindup_strategy", rclcpp::ParameterValue(gains.antiwindup_strat_.to_string())); From b448c365d244c5690de1f750c09f88842b08c7ee Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Aug 2025 09:22:31 +0000 Subject: [PATCH 3/4] Make default value of saturation ROS param dynamic --- control_toolbox/src/pid_ros.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 24c3dee2..2654b580 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -260,7 +260,6 @@ bool PidROS::initialize_from_ros_parameters() u_max = UMAX_INFINITY; u_min = -UMAX_INFINITY; bool antiwindup = false; - bool saturation = false; std::string antiwindup_strat_str = "legacy"; bool all_params_available = true; @@ -275,6 +274,7 @@ bool PidROS::initialize_from_ros_parameters() all_params_available &= get_double_param(param_prefix_ + "tracking_time_constant", tracking_time_constant); + bool saturation = std::isfinite(u_max) || std::isfinite(u_min); get_boolean_param(param_prefix_ + "saturation", saturation); if (!saturation) { From 0634ee940f5d688dea9cf21beb7075fffceac489 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 13 Aug 2025 17:11:30 +0000 Subject: [PATCH 4/4] Fix the tests --- .../test/pid_ros_parameters_tests.cpp | 28 ++++++++++++------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 127ad6ba..6c39f3f5 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -605,42 +605,50 @@ TEST(PidParametersTest, GetParametersTest) TEST(PidParametersTest, GetParametersFromParams) { rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - - TestablePidROS pid(node, "", "", false); + const bool ACTIVATE_STATE_PUBLISHER = false; + TestablePidROS pid(node, "", "", ACTIVATE_STATE_PUBLISHER); ASSERT_FALSE(pid.initialize_from_ros_parameters()); rclcpp::Parameter param_p; ASSERT_TRUE(node->get_parameter("p", param_p)); - ASSERT_TRUE(std::isnan(param_p.get_value())); + EXPECT_TRUE(std::isnan(param_p.get_value())); rclcpp::Parameter param_i; ASSERT_TRUE(node->get_parameter("i", param_i)); - ASSERT_TRUE(std::isnan(param_i.get_value())); + EXPECT_TRUE(std::isnan(param_i.get_value())); rclcpp::Parameter param_d; ASSERT_TRUE(node->get_parameter("d", param_d)); - ASSERT_TRUE(std::isnan(param_d.get_value())); + EXPECT_TRUE(std::isnan(param_d.get_value())); rclcpp::Parameter param_i_clamp_max; ASSERT_TRUE(node->get_parameter("i_clamp_max", param_i_clamp_max)); - ASSERT_TRUE(std::isnan(param_i_clamp_max.get_value())); + EXPECT_TRUE(std::isnan(param_i_clamp_max.get_value())); rclcpp::Parameter param_i_clamp_min; ASSERT_TRUE(node->get_parameter("i_clamp_min", param_i_clamp_min)); - ASSERT_TRUE(std::isnan(param_i_clamp_min.get_value())); + EXPECT_TRUE(std::isnan(param_i_clamp_min.get_value())); rclcpp::Parameter param_u_clamp_max; ASSERT_TRUE(node->get_parameter("u_clamp_max", param_u_clamp_max)); - ASSERT_TRUE(std::isinf(param_u_clamp_max.get_value())); + EXPECT_TRUE(std::isinf(param_u_clamp_max.get_value())); rclcpp::Parameter param_u_clamp_min; ASSERT_TRUE(node->get_parameter("u_clamp_min", param_u_clamp_min)); - ASSERT_TRUE(std::isinf(param_u_clamp_min.get_value())); + EXPECT_TRUE(std::isinf(param_u_clamp_min.get_value())); + + rclcpp::Parameter param_saturation; + ASSERT_TRUE(node->get_parameter("saturation", param_saturation)); + EXPECT_FALSE(param_saturation.get_value()); rclcpp::Parameter param_tracking_time_constant; ASSERT_TRUE(node->get_parameter("tracking_time_constant", param_tracking_time_constant)); - ASSERT_TRUE(std::isnan(param_tracking_time_constant.get_value())); + EXPECT_TRUE(std::isnan(param_tracking_time_constant.get_value())); + + rclcpp::Parameter param_activate_state_publisher; + ASSERT_TRUE(node->get_parameter("activate_state_publisher", param_activate_state_publisher)); + EXPECT_EQ(param_activate_state_publisher.get_value(), ACTIVATE_STATE_PUBLISHER); } TEST(PidParametersTest, MultiplePidInstances)