diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index 2d96fa4c..b8106946 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -68,7 +68,7 @@ class PidROS * */ template - explicit PidROS( + [[deprecated("Use overloads with explicit prefixes for params and topics")]] explicit PidROS( std::shared_ptr node_ptr, std::string prefix = std::string(""), bool prefix_is_for_params = false) : PidROS( @@ -77,14 +77,84 @@ class PidROS prefix_is_for_params) { } + template + explicit PidROS(std::shared_ptr node_ptr, const std::string & param_prefix) + : PidROS( + node_ptr->get_node_base_interface(), node_ptr->get_node_logging_interface(), + node_ptr->get_node_parameters_interface(), node_ptr->get_node_topics_interface(), + param_prefix, "", false) + { + } + /*! + * \brief Constructor of PidROS class. + * + * The node is passed to this class to handler the ROS parameters, this class allows + * to add a prefix to the pid parameters + * + * \param node Any ROS node + * \param param_prefix prefix to add to the pid parameters. + * \param topic_prefix prefix to add to the state publisher. If it starts with `~/`, topic will be local under the namespace of the node. If it starts with `/` or an alphanumeric character, topic will be in global namespace. + * + */ + template + explicit PidROS( + std::shared_ptr node_ptr, const std::string & param_prefix, + const std::string & topic_prefix) + : PidROS( + node_ptr->get_node_base_interface(), node_ptr->get_node_logging_interface(), + node_ptr->get_node_parameters_interface(), node_ptr->get_node_topics_interface(), + param_prefix, topic_prefix, true) + { + } + /*! + * \brief Constructor of PidROS class. + * + * The node is passed to this class to handler the ROS parameters, this class allows + * to add a prefix to the pid parameters + * + * \param node Any ROS node + * \param param_prefix prefix to add to the pid parameters. + * \param topic_prefix prefix to add to the state publisher. If it starts with `~/`, topic will be local under the namespace of the node. If it starts with `/` or an alphanumeric character, topic will be in global namespace. + * \param activate_state_publisher If true, the publisher will be enabled after initialization. + * + */ + template + explicit PidROS( + std::shared_ptr node_ptr, std::string param_prefix, std::string topic_prefix, + bool activate_state_publisher) + : PidROS( + node_ptr->get_node_base_interface(), node_ptr->get_node_logging_interface(), + node_ptr->get_node_parameters_interface(), node_ptr->get_node_topics_interface(), + param_prefix, topic_prefix, activate_state_publisher) + { + } - PidROS( + [[deprecated("Use overloads with explicit prefixes for params and topics")]] PidROS( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params, rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, std::string prefix = std::string(""), bool prefix_is_for_params = false); + /*! + * \brief Constructor of PidROS class with node_interfaces + * + * \param node_base Node base interface pointer. + * \param node_logging Node logging interface pointer. + * \param node_params Node parameters interface pointer. + * \param topics_interface Node topics interface pointer. + * \param param_prefix Prefix to add to the PID parameters. This string is not manipulated, i.e., probably should end with `.`. + * \param topic_prefix Prefix to add to the state publisher. This string is not manipulated, i.e., probably should end with `/`. If it starts with `~/`, topic will be local under the namespace of the node. If it starts with `/` or an alphanumeric character, topic will be in global namespace. + * \param activate_state_publisher If true, the publisher will be enabled after initialization. + */ + PidROS( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, + const std::string & param_prefix, const std::string & topic_prefix, + bool activate_state_publisher); + /*! * \brief Initialize the PID controller and set the parameters * \param p The proportional gain. @@ -473,7 +543,7 @@ class PidROS * If not stated explicitly using "/" or "~", prefix is interpreted as global, i.e., * "/" will be added in front of topic prefix */ - void set_prefixes(const std::string & topic_prefix); + [[deprecated]] void set_prefixes(const std::string & topic_prefix); rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_; diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 2d85caa4..2732e569 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -54,6 +54,12 @@ PidROS::PidROS( node_params_(node_params), topics_interface_(topics_interface) { + // note: deprecation on templated constructor does not show up + RCLCPP_WARN( + node_logging->get_logger(), + "PidROS constructor with node and prefix is deprecated, use overloads with explicit " + "prefixes for params and topics"); + if (prefix_is_for_params) { param_prefix_ = prefix; @@ -97,6 +103,39 @@ PidROS::PidROS( rt_state_pub_.reset( new realtime_tools::RealtimePublisher(state_pub_)); } + +PidROS::PidROS( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, + const std::string & param_prefix, const std::string & topic_prefix, bool activate_state_publisher) +: topic_prefix_(topic_prefix), + param_prefix_(param_prefix), + node_base_(node_base), + node_logging_(node_logging), + node_params_(node_params), + topics_interface_(topics_interface) +{ + // Add a trailing "." + if (!param_prefix_.empty() && param_prefix_.back() != '.') + { + param_prefix_.append("."); + } + // Add a trailing "/" + if (!topic_prefix_.empty() && topic_prefix_.back() != '/') + { + topic_prefix_.append("/"); + } + + if (activate_state_publisher) + { + state_pub_ = rclcpp::create_publisher( + topics_interface_, topic_prefix_ + "pid_state", rclcpp::SensorDataQoS()); + rt_state_pub_.reset( + new realtime_tools::RealtimePublisher(state_pub_)); + } +} #pragma GCC diagnostic pop void PidROS::set_prefixes(const std::string & topic_prefix) @@ -329,7 +368,6 @@ bool PidROS::initialize_from_args( if (pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat)) { const Pid::Gains gains = pid_.get_gains(); - declare_param(param_prefix_ + "p", rclcpp::ParameterValue(gains.p_gain_)); declare_param(param_prefix_ + "i", rclcpp::ParameterValue(gains.i_gain_)); declare_param(param_prefix_ + "d", rclcpp::ParameterValue(gains.d_gain_)); @@ -352,6 +390,9 @@ bool PidROS::initialize_from_args( param_prefix_ + "antiwindup_strategy", rclcpp::ParameterValue(gains.antiwindup_strat_.to_string())); declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term)); + declare_param( + param_prefix_ + "activate_state_publisher", + rclcpp::ParameterValue(rt_state_pub_ != nullptr)); set_parameter_event_callback(); return true; @@ -636,6 +677,25 @@ void PidROS::set_parameter_event_callback() gains.antiwindup_strat_.error_deadband = parameter.get_value(); changed = true; } + else if (param_name == param_prefix_ + "activate_state_publisher") + { + if (parameter.get_value()) + { + std::string topic_name = topic_prefix_ + "pid_state"; + RCLCPP_INFO( + node_logging_->get_logger(), "Activate publisher: `%s` ...", topic_name.c_str()); + state_pub_ = rclcpp::create_publisher( + topics_interface_, topic_name, rclcpp::SensorDataQoS()); + rt_state_pub_.reset( + new realtime_tools::RealtimePublisher(state_pub_)); + } + else + { + RCLCPP_INFO(node_logging_->get_logger(), "Deactivate publisher..."); + state_pub_.reset(); + rt_state_pub_.reset(); + } + } } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 0712d6f0..127ad6ba 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -30,21 +30,17 @@ using rclcpp::executors::MultiThreadedExecutor; class TestablePidROS : public control_toolbox::PidROS { FRIEND_TEST(PidParametersTest, InitPidTest); - FRIEND_TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot); - FRIEND_TEST(PidParametersTest, InitPid_when_prefix_for_params_then_dont_replace_slash_with_dot); - FRIEND_TEST( - PidParametersTest, - InitPid_when_not_prefix_for_params_then_replace_slash_with_dot_leading_slash); - FRIEND_TEST( - PidParametersTest, - InitPid_when_prefix_for_params_then_dont_replace_slash_with_dot_leading_slash); + FRIEND_TEST(PidParametersTest, InitPid_no_prefix); + FRIEND_TEST(PidParametersTest, InitPid_prefix); + FRIEND_TEST(PidParametersTest, InitPid_param_prefix_only); + FRIEND_TEST(PidParametersTest, InitPid_topic_prefix_only); public: template TestablePidROS( - std::shared_ptr node_ptr, std::string prefix = std::string(""), - bool prefix_is_for_params = false) - : control_toolbox::PidROS(node_ptr, prefix, prefix_is_for_params) + std::shared_ptr node_ptr, std::string param_prefix = std::string(""), + std::string topic_prefix = std::string(""), bool activate_state_publisher = false) + : control_toolbox::PidROS(node_ptr, param_prefix, topic_prefix, activate_state_publisher) { } }; @@ -126,11 +122,11 @@ void check_set_parameters( ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); } -TEST(PidParametersTest, InitPidTest) +TEST(PidParametersTest, InitPid_no_prefix) { rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - TestablePidROS pid(node); + TestablePidROS pid(node, "", "", false); ASSERT_EQ(pid.topic_prefix_, ""); ASSERT_EQ(pid.param_prefix_, ""); @@ -142,7 +138,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) { rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - TestablePidROS pid(node); + TestablePidROS pid(node, "", "", false); const double P = 1.0; const double I = 2.0; @@ -191,77 +187,56 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::LEGACY); } -TEST(PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot) +TEST(PidParametersTest, InitPid_param_prefix_only) { - const std::string INPUT_PREFIX = "slash/to/dots"; - const std::string RESULTING_TOPIC_PREFIX = INPUT_PREFIX + "/"; - const std::string RESULTING_PARAM_PREFIX = "slash.to.dots."; + const std::string PARAM_PREFIX = "some_param_prefix"; + const std::string TOPIC_PREFIX = ""; rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - TestablePidROS pid(node, INPUT_PREFIX); // default is false + TestablePidROS pid(node, PARAM_PREFIX, TOPIC_PREFIX, false); - ASSERT_EQ(pid.topic_prefix_, RESULTING_TOPIC_PREFIX); - ASSERT_EQ(pid.param_prefix_, RESULTING_PARAM_PREFIX); + ASSERT_EQ(pid.topic_prefix_, TOPIC_PREFIX); + ASSERT_EQ(pid.param_prefix_, PARAM_PREFIX + "."); - check_set_parameters(node, pid, RESULTING_PARAM_PREFIX); + check_set_parameters(node, pid, PARAM_PREFIX + "."); } -TEST(PidParametersTest, InitPid_when_prefix_for_params_then_dont_replace_slash_with_dot) +TEST(PidParametersTest, InitPid_topic_prefix_only) { - const std::string INPUT_PREFIX = "slash/to/dots"; - const std::string RESULTING_TOPIC_PREFIX = "/" + INPUT_PREFIX + "/"; - const std::string RESULTING_PARAM_PREFIX = INPUT_PREFIX + "."; + const std::string PARAM_PREFIX = ""; + const std::string TOPIC_PREFIX = "some_topic_prefix"; rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - TestablePidROS pid(node, INPUT_PREFIX, true); // prefix is for parameters + TestablePidROS pid(node, PARAM_PREFIX, TOPIC_PREFIX, false); - ASSERT_EQ(pid.topic_prefix_, RESULTING_TOPIC_PREFIX); - ASSERT_EQ(pid.param_prefix_, RESULTING_PARAM_PREFIX); + ASSERT_EQ(pid.topic_prefix_, TOPIC_PREFIX + "/"); + ASSERT_EQ(pid.param_prefix_, PARAM_PREFIX); - check_set_parameters(node, pid, RESULTING_PARAM_PREFIX); + check_set_parameters(node, pid, PARAM_PREFIX); } -TEST( - PidParametersTest, InitPid_when_not_prefix_for_params_then_replace_slash_with_dot_leading_slash) +TEST(PidParametersTest, InitPid_prefix) { - const std::string INPUT_PREFIX = "/slash/to/dots"; - const std::string RESULTING_TOPIC_PREFIX = INPUT_PREFIX + "/"; - const std::string RESULTING_PARAM_PREFIX = "slash.to.dots."; + const std::string PARAM_PREFIX = "some_param_prefix"; + const std::string TOPIC_PREFIX = "some_topic_prefix"; rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - TestablePidROS pid(node, INPUT_PREFIX); // default is false + TestablePidROS pid(node, PARAM_PREFIX, TOPIC_PREFIX, false); - ASSERT_EQ(pid.topic_prefix_, RESULTING_TOPIC_PREFIX); - ASSERT_EQ(pid.param_prefix_, RESULTING_PARAM_PREFIX); + ASSERT_EQ(pid.topic_prefix_, TOPIC_PREFIX + "/"); + ASSERT_EQ(pid.param_prefix_, PARAM_PREFIX + "."); - check_set_parameters(node, pid, RESULTING_PARAM_PREFIX); -} - -TEST( - PidParametersTest, InitPid_when_prefix_for_params_then_dont_replace_slash_with_dot_leading_slash) -{ - const std::string INPUT_PREFIX = "/slash/to/dots"; - const std::string RESULTING_TOPIC_PREFIX = INPUT_PREFIX + "/"; - const std::string RESULTING_PARAM_PREFIX = INPUT_PREFIX.substr(1) + "."; - - rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - - TestablePidROS pid(node, INPUT_PREFIX, true); // prefix is for parameters - - ASSERT_EQ(pid.topic_prefix_, RESULTING_TOPIC_PREFIX); - ASSERT_EQ(pid.param_prefix_, RESULTING_PARAM_PREFIX); - - check_set_parameters(node, pid, RESULTING_PARAM_PREFIX); + check_set_parameters(node, pid, PARAM_PREFIX + "."); } TEST(PidParametersTest, SetParametersTest) { rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - TestablePidROS pid(node); + TestablePidROS pid(node, "", "", false); const double P = 1.0; const double I = 2.0; @@ -316,6 +291,9 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("save_i_term", SAVE_I_TERM))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("activate_state_publisher", true))); + ASSERT_TRUE(set_result.successful); // process callbacks rclcpp::spin_some(node->get_node_base_interface()); @@ -338,7 +316,7 @@ TEST(PidParametersTest, SetBadParametersTest) { rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - TestablePidROS pid(node); + TestablePidROS pid(node, "", "", false); const double P = 1.0; const double I = 2.0; @@ -462,7 +440,7 @@ TEST(PidParametersTest, GetParametersTest) { rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - TestablePidROS pid(node); + TestablePidROS pid(node, "", "", false); const double P = 1.0; const double I = 2.0; @@ -525,11 +503,42 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_TRUE(node->get_parameter("save_i_term", param)); ASSERT_EQ(param.get_value(), false); + + ASSERT_TRUE(node->get_parameter("activate_state_publisher", param)); + ASSERT_EQ(param.get_value(), false); + } + { + // test activate_state_publisher + rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); + + TestablePidROS pid(node, "", "", true); + const double P = 1.0; + const double I = 2.0; + const double D = 3.0; + const double I_MAX = 10.0; + const double I_MIN = -10.0; + const double U_MAX = 10.0; + const double U_MIN = -10.0; + const double TRK_TC = 4.0; + const bool ANTIWINDUP = true; + + AntiWindupStrategy ANTIWINDUP_STRAT; + ANTIWINDUP_STRAT.type = AntiWindupStrategy::LEGACY; + ANTIWINDUP_STRAT.i_max = I_MAX; + ANTIWINDUP_STRAT.i_min = I_MIN; + ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; + ANTIWINDUP_STRAT.legacy_antiwindup = ANTIWINDUP; + + ASSERT_TRUE(pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); + + rclcpp::Parameter param; + ASSERT_TRUE(node->get_parameter("activate_state_publisher", param)); + ASSERT_EQ(param.get_value(), true); } { rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - TestablePidROS pid(node); + TestablePidROS pid(node, "", "", false); const double P = 1.0; const double I = 2.0; @@ -597,7 +606,7 @@ TEST(PidParametersTest, GetParametersFromParams) { rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); - TestablePidROS pid(node); + TestablePidROS pid(node, "", "", false); ASSERT_FALSE(pid.initialize_from_ros_parameters()); @@ -638,8 +647,8 @@ TEST(PidParametersTest, MultiplePidInstances) { rclcpp::Node::SharedPtr node = std::make_shared("multiple_pid_instances"); - TestablePidROS pid_1(node, "PID_1"); - TestablePidROS pid_2(node, "PID_2"); + TestablePidROS pid_1(node, "PID_1"); // missing trailing dot should be auto-added + TestablePidROS pid_2(node, "PID_2."); // Note the trailing dot in the prefix const double P = 1.0; const double I = 2.0; @@ -657,14 +666,13 @@ TEST(PidParametersTest, MultiplePidInstances) ANTIWINDUP_STRAT.legacy_antiwindup = false; ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); - ANTIWINDUP_STRAT.legacy_antiwindup = true; - ASSERT_NO_THROW(pid_2.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); + ASSERT_NO_THROW(pid_2.initialize_from_args(2 * P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); rclcpp::Parameter param_1, param_2; ASSERT_TRUE(node->get_parameter("PID_1.p", param_1)); - ASSERT_EQ(param_1.get_value(), P); + EXPECT_EQ(param_1.get_value(), P); ASSERT_TRUE(node->get_parameter("PID_2.p", param_2)); - ASSERT_EQ(param_2.get_value(), P); + EXPECT_EQ(param_2.get_value(), 2 * P); } int main(int argc, char ** argv) diff --git a/control_toolbox/test/pid_ros_publisher_tests.cpp b/control_toolbox/test/pid_ros_publisher_tests.cpp index 2752d5b3..c434587f 100644 --- a/control_toolbox/test/pid_ros_publisher_tests.cpp +++ b/control_toolbox/test/pid_ros_publisher_tests.cpp @@ -36,12 +36,51 @@ using rclcpp::executors::MultiThreadedExecutor; TEST(PidPublisherTest, PublishTest) { - const size_t ATTEMPTS = 100; + const size_t ATTEMPTS = 10; + const std::chrono::milliseconds DELAY(250); + + auto node = std::make_shared("pid_publisher_test"); + + control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node, "", "", true); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; + antiwindup_strat.i_max = 5.0; + antiwindup_strat.i_min = -5.0; + antiwindup_strat.legacy_antiwindup = false; + antiwindup_strat.tracking_time_constant = 1.0; + pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, false); + + bool callback_called = false; + control_msgs::msg::PidState::SharedPtr last_state_msg; + auto state_callback = [&](const control_msgs::msg::PidState::SharedPtr) + { callback_called = true; }; + + auto state_sub = node->create_subscription( + "/pid_state", rclcpp::SensorDataQoS(), state_callback); + + double command = pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); + EXPECT_EQ(-1.5, command); + + // wait for callback + for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) + { + pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); + rclcpp::spin_some(node); + std::this_thread::sleep_for(DELAY); + } + + ASSERT_TRUE(callback_called); +} + +TEST(PidPublisherTest, PublishTest_start_deactivated) +{ + const size_t ATTEMPTS = 10; const std::chrono::milliseconds DELAY(250); auto node = std::make_shared("pid_publisher_test"); - control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node); + control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node, "", "", false); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::LEGACY; @@ -62,6 +101,115 @@ TEST(PidPublisherTest, PublishTest) double command = pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); EXPECT_EQ(-1.5, command); + // wait for callback + for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) + { + pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); + rclcpp::spin_some(node); + std::this_thread::sleep_for(DELAY); + } + ASSERT_FALSE(callback_called); + + // activate publisher + rcl_interfaces::msg::SetParametersResult set_result; + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("activate_state_publisher", true))); + ASSERT_TRUE(set_result.successful); + + // wait for callback + for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) + { + pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); + rclcpp::spin_some(node); + std::this_thread::sleep_for(DELAY); + } + ASSERT_TRUE(callback_called); + + // deactivate publisher again + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("activate_state_publisher", false))); + ASSERT_TRUE(set_result.successful); + rclcpp::spin_some(node); // process callbacks to ensure that no further messages are received + callback_called = false; + + // wait for callback + for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) + { + pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); + rclcpp::spin_some(node); + std::this_thread::sleep_for(DELAY); + } + ASSERT_FALSE(callback_called); +} + +TEST(PidPublisherTest, PublishTest_prefix) +{ + const size_t ATTEMPTS = 10; + const std::chrono::milliseconds DELAY(250); + + auto node = std::make_shared("pid_publisher_test"); + + // test with a prefix for the topic without trailing / (should be auto-added) + control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node, "", "global", true); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; + antiwindup_strat.i_max = 5.0; + antiwindup_strat.i_min = -5.0; + antiwindup_strat.legacy_antiwindup = false; + antiwindup_strat.tracking_time_constant = 1.0; + pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, false); + + bool callback_called = false; + control_msgs::msg::PidState::SharedPtr last_state_msg; + auto state_callback = [&](const control_msgs::msg::PidState::SharedPtr) + { callback_called = true; }; + + auto state_sub = node->create_subscription( + "/global/pid_state", rclcpp::SensorDataQoS(), state_callback); + + double command = pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); + EXPECT_EQ(-1.5, command); + + // wait for callback + for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) + { + pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); + rclcpp::spin_some(node); + std::this_thread::sleep_for(DELAY); + } + + ASSERT_TRUE(callback_called); +} + +TEST(PidPublisherTest, PublishTest_local_prefix) +{ + const size_t ATTEMPTS = 10; + const std::chrono::milliseconds DELAY(250); + + auto node = std::make_shared("pid_publisher_test"); + + control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node, "", "~/local/", true); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::LEGACY; + antiwindup_strat.i_max = 5.0; + antiwindup_strat.i_min = -5.0; + antiwindup_strat.legacy_antiwindup = false; + antiwindup_strat.tracking_time_constant = 1.0; + pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, false); + + bool callback_called = false; + control_msgs::msg::PidState::SharedPtr last_state_msg; + auto state_callback = [&](const control_msgs::msg::PidState::SharedPtr) + { callback_called = true; }; + + auto state_sub = node->create_subscription( + "~/local/pid_state", rclcpp::SensorDataQoS(), state_callback); + + double command = pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); + EXPECT_EQ(-1.5, command); + // wait for callback for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) { @@ -80,7 +228,7 @@ TEST(PidPublisherTest, PublishTestLifecycle) auto node = std::make_shared("pid_publisher_test"); - control_toolbox::PidROS pid_ros(node); + control_toolbox::PidROS pid_ros(node, "", "", true); auto state_pub_lifecycle_ = std::dynamic_pointer_cast>( diff --git a/doc/migration.rst b/doc/migration.rst index 04399699..20234dd1 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -8,6 +8,7 @@ This list summarizes important changes between Humble (previous) and Jazzy (curr This list was created in June 2025 (tag 4.4.0), earlier changes may not be included. -Pid/PidRos +Pid/PidROS *********************************************************** * The parameters ``antiwindup``, ``i_clamp_max``, and ``i_clamp_min`` will be removed. The anti-windup behavior is now configured via the ``AntiWindupStrategy`` enum. (`#298 `_). +* The constructors of ``PidROS`` have changed and ``prefix_is_for_params`` argument has been deprecated. Use explicit parameter and topic prefix instead (`#431 `_). diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 30e0c1f9..52286f08 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -7,3 +7,8 @@ This list summarizes the changes between Humble (previous) and Jazzy (current) r .. note:: This list was created in June 2025 (tag 4.4.0), earlier changes may not be included. + +Pid/PidROS +*********************************************************** +* Added a saturation feature to PID output and two anti-windup techniques (back calculation and conditional integration) (`#298 `_). +* Added a constructor argument to ``PidROS`` to control if the PID state publisher is initially active or not. Can be changed during runtime by using ``activate_state_publisher`` parameter. (`#431 `_).