Add enable_feedforward parameter

pid_controller/src/pid_controller.cpp

@@ -230,6 +230,16 @@ controller_interface::CallbackReturn PidController::on_configure(
     measured_state_subscriber_ = get_node()->create_subscription<ControllerMeasuredStateMsg>(
       "~/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<ControllerMeasuredStateMsg> measured_state_msg =
     std::make_shared<ControllerMeasuredStateMsg>();
   reset_controller_measured_state_msg(measured_state_msg, reference_and_state_dof_names_);

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

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
  *

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