Add enable_feedforward parameter

pid_controller/doc/userdoc.rst

@@ -74,6 +74,9 @@ Services
 
 - <controller_name>/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_name>/controller_state  [control_msgs/msg/MultiDOFStateStamped]
@@ -85,6 +88,10 @@ The PID controller uses the `generate_parameter_library <https://github.com/Pick
 
 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 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
 
 

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<std::shared_ptr<ControllerMeasuredStateMsg>> measured_state_;
 
   rclcpp::Service<ControllerModeSrvType>::SharedPtr set_feedforward_control_service_;
-  realtime_tools::RealtimeBuffer<feedforward_mode_type> control_mode_;
+  realtime_tools::RealtimeBuffer<bool> feedforward_mode_enabled_;
 
   using ControllerStatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
 

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<ControllerMeasuredStateMsg>(
       "~/measured_state", subscribers_qos, measured_state_callback);
   }
+
   std::shared_ptr<ControllerMeasuredStateMsg> measured_state_msg =
     std::make_shared<ControllerMeasuredStateMsg>();
   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<ControllerModeSrvType::Request> request,
       std::shared_ptr<ControllerModeSrvType::Response> 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_)

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: {

pid_controller/test/test_pid_controller.cpp

@@ -21,8 +21,6 @@
 #include <string>
 #include <vector>
 
-using pid_controller::feedforward_mode_type;
-
 class PidControllerTest : public PidControllerFixture<TestablePidController>
 {
 };
@@ -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,16 +338,16 @@ 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));
   }
 
   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});

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);

pid_controller/test/test_pid_controller_dual_interface.cpp

@@ -20,8 +20,6 @@
 #include <string>
 #include <vector>
 
-using pid_controller::feedforward_mode_type;
-
 class PidControllerDualInterfaceTest : public PidControllerFixture<TestablePidController>
 {
 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_ = {

pid_controller/test/test_pid_controller_preceding.cpp

@@ -21,8 +21,6 @@
 #include <string>
 #include <vector>
 
-using pid_controller::feedforward_mode_type;
-
 class PidControllerTest : public PidControllerFixture<TestablePidController>
 {
 };