From 74161da6d59fbf182e09a8cae0574429a7785d9d Mon Sep 17 00:00:00 2001 From: Pascal Auf der Maur Date: Fri, 21 Feb 2025 18:13:27 +0100 Subject: [PATCH 1/8] Add enable feedforward parameter --- pid_controller/src/pid_controller.cpp | 10 ++++++++++ pid_controller/src/pid_controller.yaml | 5 +++++ pid_controller/test/test_pid_controller.cpp | 16 ++++++++++++++++ pid_controller/test/test_pid_controller.hpp | 1 + 4 files changed, 32 insertions(+) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 782d39dfdc..d32f0639a0 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -230,6 +230,16 @@ controller_interface::CallbackReturn PidController::on_configure( measured_state_subscriber_ = get_node()->create_subscription( "~/measured_state", subscribers_qos, measured_state_callback); } + + if (params_.enable_feedforward) + { + control_mode_.writeFromNonRT(feedforward_mode_type::ON); + } + else + { + control_mode_.writeFromNonRT(feedforward_mode_type::OFF); + } + std::shared_ptr measured_state_msg = std::make_shared(); reset_controller_measured_state_msg(measured_state_msg, reference_and_state_dof_names_); diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 02448f2a19..eb4d09885f 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -43,6 +43,11 @@ pid_controller: default_value: false, description: "Use external states from a topic instead from state interfaces." } + enable_feedforward: { + type: bool, + default_value: false, + description: "Enables feedforward gain on start." + } gains: __map_dof_names: p: { diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 1c0494a002..3d008fec77 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -222,6 +222,22 @@ TEST_F(PidControllerTest, test_feedforward_mode_service) ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); } +TEST_F(PidControllerTest, test_feedforward_mode_parameter) +{ + SetUpController(); + + // initially set to OFF + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + // Reconfigure after setting parameter to true + ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), NODE_SUCCESS); + EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", true}).successful); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); +} + /** * @brief Check the update logic in non chained mode with feedforward OFF * diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 1451ae919b..f852601e45 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -54,6 +54,7 @@ class TestablePidController : public pid_controller::PidController FRIEND_TEST(PidControllerTest, activate_success); FRIEND_TEST(PidControllerTest, reactivate_success); FRIEND_TEST(PidControllerTest, test_feedforward_mode_service); + FRIEND_TEST(PidControllerTest, test_feedforward_mode_parameter); FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_off); FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward_gain); FRIEND_TEST(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update); From 64220fd0aff8f2ff94ca248928e9debc95a6526e Mon Sep 17 00:00:00 2001 From: pascalau Date: Tue, 25 Feb 2025 09:28:11 +0100 Subject: [PATCH 2/8] Enable changing mode during runtime --- pid_controller/src/pid_controller.cpp | 18 ++++++------ pid_controller/test/test_pid_controller.cpp | 31 +++++++++++++++++++++ 2 files changed, 40 insertions(+), 9 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index d32f0639a0..ed0b4cb135 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -96,6 +96,15 @@ void PidController::update_parameters() return; } params_ = param_listener_->get_params(); + + if (params_.enable_feedforward) + { + control_mode_.writeFromNonRT(feedforward_mode_type::ON); + } + else + { + control_mode_.writeFromNonRT(feedforward_mode_type::OFF); + } } controller_interface::CallbackReturn PidController::configure_parameters() @@ -231,15 +240,6 @@ controller_interface::CallbackReturn PidController::on_configure( "~/measured_state", subscribers_qos, measured_state_callback); } - if (params_.enable_feedforward) - { - control_mode_.writeFromNonRT(feedforward_mode_type::ON); - } - else - { - control_mode_.writeFromNonRT(feedforward_mode_type::OFF); - } - std::shared_ptr measured_state_msg = std::make_shared(); reset_controller_measured_state_msg(measured_state_msg, reference_and_state_dof_names_); diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 3d008fec77..62abfe7e3b 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -226,6 +226,8 @@ TEST_F(PidControllerTest, test_feedforward_mode_parameter) { SetUpController(); + // Check updating mode during on_configure + // initially set to OFF ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -236,6 +238,35 @@ TEST_F(PidControllerTest, test_feedforward_mode_parameter) EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", true}).successful); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), NODE_SUCCESS); + EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", false}).successful); + + // initially set to ON + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + // Check updating mode during update_and_write_commands + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + // Switch to ON + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", true}).successful); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + // Switch to OFF + EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", false}).successful); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); } /** From e4d9efc8ce989e4ab0f1c5510b745b7779a9b8ff Mon Sep 17 00:00:00 2001 From: pascalau Date: Thu, 27 Feb 2025 12:05:12 +0100 Subject: [PATCH 3/8] Remove enum and add deprecation warning in userdoc and rosout --- pid_controller/doc/userdoc.rst | 3 ++ .../include/pid_controller/pid_controller.hpp | 8 +--- pid_controller/src/pid_controller.cpp | 27 ++++------- pid_controller/src/pid_controller.yaml | 2 +- pid_controller/test/test_pid_controller.cpp | 48 +++++++++---------- .../test_pid_controller_dual_interface.cpp | 6 +-- .../test/test_pid_controller_preceding.cpp | 2 - 7 files changed, 39 insertions(+), 57 deletions(-) diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst index fc94357ab3..a8182b0271 100644 --- a/pid_controller/doc/userdoc.rst +++ b/pid_controller/doc/userdoc.rst @@ -74,6 +74,9 @@ Services - /set_feedforward_control [std_srvs/srv/SetBool] +.. warning:: + This service is being deprecated in favour of the ``enable_feedforward`` parameter. + Publishers ,,,,,,,,,,, - /controller_state [control_msgs/msg/MultiDOFStateStamped] diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 39ec58be6b..cd28c0386c 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -36,12 +36,6 @@ namespace pid_controller { -enum class feedforward_mode_type : std::uint8_t -{ - OFF = 0, - ON = 1, -}; - class PidController : public controller_interface::ChainableControllerInterface { public: @@ -94,7 +88,7 @@ class PidController : public controller_interface::ChainableControllerInterface realtime_tools::RealtimeBuffer> measured_state_; rclcpp::Service::SharedPtr set_feedforward_control_service_; - realtime_tools::RealtimeBuffer control_mode_; + realtime_tools::RealtimeBuffer control_mode_; using ControllerStatePublisher = realtime_tools::RealtimePublisher; diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index ed0b4cb135..521a39b967 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -74,7 +74,7 @@ PidController::PidController() : controller_interface::ChainableControllerInterf controller_interface::CallbackReturn PidController::on_init() { - control_mode_.initRT(feedforward_mode_type::OFF); + control_mode_.initRT(false); try { @@ -97,14 +97,7 @@ void PidController::update_parameters() } params_ = param_listener_->get_params(); - if (params_.enable_feedforward) - { - control_mode_.writeFromNonRT(feedforward_mode_type::ON); - } - else - { - control_mode_.writeFromNonRT(feedforward_mode_type::OFF); - } + control_mode_.writeFromNonRT(params_.enable_feedforward); } controller_interface::CallbackReturn PidController::configure_parameters() @@ -253,14 +246,12 @@ controller_interface::CallbackReturn PidController::on_configure( const std::shared_ptr request, std::shared_ptr response) { - if (request->data) - { - control_mode_.writeFromNonRT(feedforward_mode_type::ON); - } - else - { - control_mode_.writeFromNonRT(feedforward_mode_type::OFF); - } + control_mode_.writeFromNonRT(request->data); + + RCLCPP_ERROR( + get_logger(), + "This service will be deprecated in favour of the 'enable_feedforward' parameter."); + response->success = true; }; @@ -522,7 +513,7 @@ controller_interface::return_type PidController::update_and_write_commands( if (std::isfinite(reference_interfaces_[i]) && std::isfinite(measured_state_values_[i])) { // calculate feed-forward - if (*(control_mode_.readFromRT()) == feedforward_mode_type::ON) + if (*(control_mode_.readFromRT())) { // two interfaces if (reference_interfaces_.size() == 2 * dof_) diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index eb4d09885f..974e317656 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -46,7 +46,7 @@ pid_controller: enable_feedforward: { type: bool, default_value: false, - description: "Enables feedforward gain on start." + description: "Enables feedforward gain." } gains: __map_dof_names: diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 62abfe7e3b..37d02f85ba 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -21,8 +21,6 @@ #include #include -using pid_controller::feedforward_mode_type; - class PidControllerTest : public PidControllerFixture { }; @@ -205,21 +203,21 @@ TEST_F(PidControllerTest, test_feedforward_mode_service) executor.add_node(service_caller_node_->get_node_base_interface()); // initially set to OFF - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // should stay false - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); // set to true ASSERT_NO_THROW(call_service(true, executor)); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), true); // set back to false ASSERT_NO_THROW(call_service(false, executor)); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); } TEST_F(PidControllerTest, test_feedforward_mode_parameter) @@ -229,44 +227,44 @@ TEST_F(PidControllerTest, test_feedforward_mode_parameter) // Check updating mode during on_configure // initially set to OFF - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); // Reconfigure after setting parameter to true ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), NODE_SUCCESS); EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", true}).successful); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), true); ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), NODE_SUCCESS); EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", false}).successful); // initially set to ON - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), true); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); // Check updating mode during update_and_write_commands ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); // Switch to ON ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", true}).successful); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), true); // Switch to OFF EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", false}).successful); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); } /** @@ -286,7 +284,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_FALSE(controller_->is_in_chained_mode()); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), false); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -305,7 +303,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), false); EXPECT_EQ( controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); @@ -340,7 +338,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_FALSE(controller_->is_in_chained_mode()); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), false); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -348,8 +346,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward controller_->set_reference(dof_command_values_); - controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + controller_->control_mode_.writeFromNonRT(true); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), true); for (size_t i = 0; i < dof_command_values_.size(); ++i) { @@ -362,7 +360,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), true); EXPECT_EQ( controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); @@ -402,7 +400,7 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(controller_->is_in_chained_mode()); // feedforward mode is off as default, use this for convenience - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), false); // update reference interface which will be used for calculation const double ref_interface_value = 5.0; @@ -614,8 +612,8 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) ASSERT_TRUE(controller_->is_in_chained_mode()); // turn on feedforward - controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + controller_->control_mode_.writeFromNonRT(true); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), true); // send a message to update reference interface controller_->set_reference({target_value}); @@ -673,7 +671,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain) ASSERT_TRUE(controller_->is_in_chained_mode()); // feedforward by default is OFF - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); // send a message to update reference interface controller_->set_reference({target_value}); diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp index e006986473..0fc512017e 100644 --- a/pid_controller/test/test_pid_controller_dual_interface.cpp +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -20,8 +20,6 @@ #include #include -using pid_controller::feedforward_mode_type; - class PidControllerDualInterfaceTest : public PidControllerFixture { public: @@ -80,8 +78,8 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i ASSERT_TRUE(controller_->is_in_chained_mode()); // turn on feedforward - controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + controller_->control_mode_.writeFromNonRT(true); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), true); // set up the reference interface, controller_->reference_interfaces_ = { diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index 602303ef9e..31629ed7b7 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -21,8 +21,6 @@ #include #include -using pid_controller::feedforward_mode_type; - class PidControllerTest : public PidControllerFixture { }; From 35f04088d839952969c6b50af6f7fa35f7407905 Mon Sep 17 00:00:00 2001 From: pascalau Date: Thu, 27 Feb 2025 12:28:18 +0100 Subject: [PATCH 4/8] Fix typo in deprecation warning --- pid_controller/src/pid_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 521a39b967..802719d48b 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -249,7 +249,7 @@ controller_interface::CallbackReturn PidController::on_configure( control_mode_.writeFromNonRT(request->data); RCLCPP_ERROR( - get_logger(), + get_node()->get_logger(), "This service will be deprecated in favour of the 'enable_feedforward' parameter."); response->success = true; From 4a40d1ea6ae7a48766f07b2a290e00f0953e499b Mon Sep 17 00:00:00 2001 From: pascalau Date: Mon, 3 Mar 2025 11:30:54 +0100 Subject: [PATCH 5/8] Add deprecation warnings for parameter and rename control_mode_ --- pid_controller/doc/userdoc.rst | 5 +- .../include/pid_controller/pid_controller.hpp | 2 +- pid_controller/src/pid_controller.cpp | 8 ++-- pid_controller/src/pid_controller.yaml | 2 +- pid_controller/test/test_pid_controller.cpp | 46 +++++++++---------- .../test_pid_controller_dual_interface.cpp | 4 +- 6 files changed, 35 insertions(+), 32 deletions(-) diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst index a8182b0271..6c35cbebaa 100644 --- a/pid_controller/doc/userdoc.rst +++ b/pid_controller/doc/userdoc.rst @@ -75,7 +75,7 @@ Services - /set_feedforward_control [std_srvs/srv/SetBool] .. warning:: - This service is being deprecated in favour of the ``enable_feedforward`` parameter. + This service is being deprecated in favour of setting the ``feedforward_gain`` parameter to a non-zero value. Publishers ,,,,,,,,,,, @@ -90,6 +90,9 @@ List of parameters ========================= .. generate_parameter_library_details:: ../src/pid_controller.yaml +.. warning:: + The parameter ``enable_feedforward`` is being deprecated in favour of setting the ``feedforward_gain`` parameter to a non-zero value. + This might cause different behaviour if currently the ``feedforward_gain`` is set to a non-zero value and not activated. An example parameter file ========================= diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index cd28c0386c..34a50a3687 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -88,7 +88,7 @@ class PidController : public controller_interface::ChainableControllerInterface realtime_tools::RealtimeBuffer> measured_state_; rclcpp::Service::SharedPtr set_feedforward_control_service_; - realtime_tools::RealtimeBuffer control_mode_; + realtime_tools::RealtimeBuffer feedforward_mode_enabled_; using ControllerStatePublisher = realtime_tools::RealtimePublisher; diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 802719d48b..9c20369b56 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -74,7 +74,7 @@ PidController::PidController() : controller_interface::ChainableControllerInterf controller_interface::CallbackReturn PidController::on_init() { - control_mode_.initRT(false); + feedforward_mode_enabled_.initRT(false); try { @@ -97,7 +97,7 @@ void PidController::update_parameters() } params_ = param_listener_->get_params(); - control_mode_.writeFromNonRT(params_.enable_feedforward); + feedforward_mode_enabled_.writeFromNonRT(params_.enable_feedforward); } controller_interface::CallbackReturn PidController::configure_parameters() @@ -246,7 +246,7 @@ controller_interface::CallbackReturn PidController::on_configure( const std::shared_ptr request, std::shared_ptr response) { - control_mode_.writeFromNonRT(request->data); + feedforward_mode_enabled_.writeFromNonRT(request->data); RCLCPP_ERROR( get_node()->get_logger(), @@ -513,7 +513,7 @@ controller_interface::return_type PidController::update_and_write_commands( if (std::isfinite(reference_interfaces_[i]) && std::isfinite(measured_state_values_[i])) { // calculate feed-forward - if (*(control_mode_.readFromRT())) + if (*(feedforward_mode_enabled_.readFromRT())) { // two interfaces if (reference_interfaces_.size() == 2 * dof_) diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 974e317656..8903085667 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -46,7 +46,7 @@ pid_controller: enable_feedforward: { type: bool, default_value: false, - description: "Enables feedforward gain." + description: "Enables feedforward gain. (Will be deprecated in favour of setting feedforward_gain to a non-zero value.)" } gains: __map_dof_names: diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 37d02f85ba..2a3b99e969 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -203,21 +203,21 @@ TEST_F(PidControllerTest, test_feedforward_mode_service) executor.add_node(service_caller_node_->get_node_base_interface()); // initially set to OFF - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // should stay false - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); // set to true ASSERT_NO_THROW(call_service(true, executor)); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), true); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); // set back to false ASSERT_NO_THROW(call_service(false, executor)); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); } TEST_F(PidControllerTest, test_feedforward_mode_parameter) @@ -227,44 +227,44 @@ TEST_F(PidControllerTest, test_feedforward_mode_parameter) // Check updating mode during on_configure // initially set to OFF - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); // Reconfigure after setting parameter to true ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), NODE_SUCCESS); EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", true}).successful); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), true); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), NODE_SUCCESS); EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", false}).successful); // initially set to ON - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), true); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); // Check updating mode during update_and_write_commands ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); // Switch to ON ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", true}).successful); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), true); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); // Switch to OFF EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", false}).successful); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); } /** @@ -284,7 +284,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_FALSE(controller_->is_in_chained_mode()); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), false); + EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -303,7 +303,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), false); + EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); EXPECT_EQ( controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); @@ -338,7 +338,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_FALSE(controller_->is_in_chained_mode()); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), false); + EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -346,8 +346,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward controller_->set_reference(dof_command_values_); - controller_->control_mode_.writeFromNonRT(true); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), true); + controller_->feedforward_mode_enabled_.writeFromNonRT(true); + EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); for (size_t i = 0; i < dof_command_values_.size(); ++i) { @@ -360,7 +360,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), true); + EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); EXPECT_EQ( controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); @@ -400,7 +400,7 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(controller_->is_in_chained_mode()); // feedforward mode is off as default, use this for convenience - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), false); + EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); // update reference interface which will be used for calculation const double ref_interface_value = 5.0; @@ -612,8 +612,8 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) ASSERT_TRUE(controller_->is_in_chained_mode()); // turn on feedforward - controller_->control_mode_.writeFromNonRT(true); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), true); + controller_->feedforward_mode_enabled_.writeFromNonRT(true); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); // send a message to update reference interface controller_->set_reference({target_value}); @@ -671,7 +671,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain) ASSERT_TRUE(controller_->is_in_chained_mode()); // feedforward by default is OFF - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), false); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); // send a message to update reference interface controller_->set_reference({target_value}); diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp index 0fc512017e..6db9171bcb 100644 --- a/pid_controller/test/test_pid_controller_dual_interface.cpp +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -78,8 +78,8 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i ASSERT_TRUE(controller_->is_in_chained_mode()); // turn on feedforward - controller_->control_mode_.writeFromNonRT(true); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), true); + controller_->feedforward_mode_enabled_.writeFromNonRT(true); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); // set up the reference interface, controller_->reference_interfaces_ = { From bc6a3a6ee8164900b936cd154c5786e144c48b8f Mon Sep 17 00:00:00 2001 From: pascalau Date: Mon, 3 Mar 2025 11:42:49 +0100 Subject: [PATCH 6/8] Update error message in service call --- pid_controller/src/pid_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 9c20369b56..5bfd6435aa 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -250,7 +250,8 @@ controller_interface::CallbackReturn PidController::on_configure( RCLCPP_ERROR( get_node()->get_logger(), - "This service will be deprecated in favour of the 'enable_feedforward' parameter."); + "This service will be deprecated in favour of setting the ``feedforward_gain`` parameter to " + "a non-zero value."); response->success = true; }; From 9d81b4e15b540684d729a4aba37cf12e9d52d977 Mon Sep 17 00:00:00 2001 From: Pascal Auf der Maur <47889885+pascalau@users.noreply.github.com> Date: Wed, 5 Mar 2025 09:21:44 +0100 Subject: [PATCH 7/8] Move deprecation warning about parameter to the top MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- pid_controller/doc/userdoc.rst | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst index 6c35cbebaa..c80472df69 100644 --- a/pid_controller/doc/userdoc.rst +++ b/pid_controller/doc/userdoc.rst @@ -88,11 +88,12 @@ The PID controller uses the `generate_parameter_library Date: Wed, 5 Mar 2025 10:51:58 +0100 Subject: [PATCH 8/8] Clarify different behaviour in the future --- pid_controller/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst index c80472df69..3d476e5eb4 100644 --- a/pid_controller/doc/userdoc.rst +++ b/pid_controller/doc/userdoc.rst @@ -90,7 +90,7 @@ List of parameters ========================= .. warning:: The parameter ``enable_feedforward`` is being deprecated in favor of setting the ``feedforward_gain`` parameter to a non-zero value. - This might cause different behavior if currently the ``feedforward_gain`` is set to a non-zero value and not activated. + This might cause different behavior in the future if currently the ``feedforward_gain`` is set to a non-zero value and not activated. .. generate_parameter_library_details:: ../src/pid_controller.yaml