diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst index fc94357ab3..3d476e5eb4 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 setting the ``feedforward_gain`` parameter to a non-zero value. + Publishers ,,,,,,,,,,, - /controller_state [control_msgs/msg/MultiDOFStateStamped] @@ -85,6 +88,10 @@ The PID controller uses the `generate_parameter_library > 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 782d39dfdc..5bfd6435aa 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); + feedforward_mode_enabled_.initRT(false); try { @@ -96,6 +96,8 @@ void PidController::update_parameters() return; } params_ = param_listener_->get_params(); + + feedforward_mode_enabled_.writeFromNonRT(params_.enable_feedforward); } controller_interface::CallbackReturn PidController::configure_parameters() @@ -230,6 +232,7 @@ controller_interface::CallbackReturn PidController::on_configure( measured_state_subscriber_ = get_node()->create_subscription( "~/measured_state", subscribers_qos, measured_state_callback); } + std::shared_ptr measured_state_msg = std::make_shared(); reset_controller_measured_state_msg(measured_state_msg, reference_and_state_dof_names_); @@ -243,14 +246,13 @@ 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); - } + feedforward_mode_enabled_.writeFromNonRT(request->data); + + RCLCPP_ERROR( + get_node()->get_logger(), + "This service will be deprecated in favour of setting the ``feedforward_gain`` parameter to " + "a non-zero value."); + response->success = true; }; @@ -512,7 +514,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 (*(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 02448f2a19..8903085667 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. (Will be deprecated in favour of setting feedforward_gain to a non-zero value.)" + } 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..2a3b99e969 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,68 @@ 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_->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()), feedforward_mode_type::OFF); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.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_->feedforward_mode_enabled_.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_->feedforward_mode_enabled_.readFromRT()), false); +} + +TEST_F(PidControllerTest, test_feedforward_mode_parameter) +{ + SetUpController(); + + // Check updating mode during on_configure + + // initially set to OFF + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + 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_->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_->feedforward_mode_enabled_.readFromRT()), true); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + 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_->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_->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_->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_->feedforward_mode_enabled_.readFromRT()), false); } /** @@ -239,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_->feedforward_mode_enabled_.readFromRT()), false); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -258,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_->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()); @@ -293,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_->feedforward_mode_enabled_.readFromRT()), false); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -301,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_->feedforward_mode_enabled_.writeFromNonRT(true); + EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); for (size_t i = 0; i < dof_command_values_.size(); ++i) { @@ -315,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_->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()); @@ -355,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_->feedforward_mode_enabled_.readFromRT()), false); // update reference interface which will be used for calculation const double ref_interface_value = 5.0; @@ -567,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_->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}); @@ -626,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_->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.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); diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp index e006986473..6db9171bcb 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_->feedforward_mode_enabled_.writeFromNonRT(true); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.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 { };