Remove usage of get_ordered_interfaces but update parameter validation instead

admittance_controller/include/admittance_controller/admittance_controller.hpp

@@ -82,15 +82,6 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
   size_t num_joints_ = 0;
   std::vector<std::string> command_joint_names_;
 
-  // The interfaces are defined as the types in 'allowed_interface_types_' member.
-  // For convenience, for each type the interfaces are ordered so that i-th position
-  // matches i-th index in joint_names_
-  template <typename T>
-  using InterfaceReferences = std::vector<std::vector<std::reference_wrapper<T>>>;
-
-  InterfaceReferences<hardware_interface::LoanedCommandInterface> joint_command_interface_;
-  InterfaceReferences<hardware_interface::LoanedStateInterface> joint_state_interface_;
-
   bool has_position_state_interface_ = false;
   bool has_velocity_state_interface_ = false;
   bool has_acceleration_state_interface_ = false;

admittance_controller/src/admittance_controller.cpp

@@ -226,16 +226,14 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
     }
   }
 
-  // Check if only allowed interface types are used and initialize storage to avoid memory
-  // allocation during activation
+  // Check if only allowed interface types are used
   auto contains_interface_type =
     [](const std::vector<std::string> & interface_type_list, const std::string & interface_type)
   {
     return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) !=
            interface_type_list.end();
   };
 
-  joint_command_interface_.resize(allowed_interface_types_.size());
   for (const auto & interface : admittance_->parameters_.command_interfaces)
   {
     auto it =
@@ -257,9 +255,7 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
   has_effort_command_interface_ = contains_interface_type(
     admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_EFFORT);
 
-  // Check if only allowed interface types are used and initialize storage to avoid memory
-  // allocation during activation
-  joint_state_interface_.resize(allowed_interface_types_.size());
+  // Check if only allowed interface types are used
   for (const auto & interface : admittance_->parameters_.state_interfaces)
   {
     auto it =
@@ -364,37 +360,6 @@ controller_interface::CallbackReturn AdmittanceController::on_activate(
     return controller_interface::CallbackReturn::ERROR;
   }
 
-  // order all joints in the storage
-  for (const auto & interface : admittance_->parameters_.state_interfaces)
-  {
-    auto it =
-      std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface);
-    auto index = static_cast<size_t>(std::distance(allowed_interface_types_.begin(), it));
-    if (!controller_interface::get_ordered_interfaces(
-          state_interfaces_, admittance_->parameters_.joints, interface,
-          joint_state_interface_[index]))
-    {
-      RCLCPP_ERROR(
-        get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", num_joints_,
-        interface.c_str(), joint_state_interface_[index].size());
-      return CallbackReturn::ERROR;
-    }
-  }
-  for (const auto & interface : admittance_->parameters_.command_interfaces)
-  {
-    auto it =
-      std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface);
-    auto index = static_cast<size_t>(std::distance(allowed_interface_types_.begin(), it));
-    if (!controller_interface::get_ordered_interfaces(
-          command_interfaces_, command_joint_names_, interface, joint_command_interface_[index]))
-    {
-      RCLCPP_ERROR(
-        get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", num_joints_,
-        interface.c_str(), joint_command_interface_[index].size());
-      return CallbackReturn::ERROR;
-    }
-  }
-
   // update parameters if any have changed
   admittance_->apply_parameters_update();
 
@@ -522,11 +487,6 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate(
     }
   }
 
-  for (size_t index = 0; index < allowed_interface_types_.size(); ++index)
-  {
-    joint_command_interface_[index].clear();
-    joint_state_interface_[index].clear();
-  }
   release_interfaces();
   admittance_->reset(num_joints_);
 

effort_controllers/CMakeLists.txt

@@ -38,6 +38,8 @@ if(BUILD_TESTING)
   find_package(controller_manager REQUIRED)
   find_package(ros2_control_test_assets REQUIRED)
 
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+
   ament_add_gmock(test_load_joint_group_effort_controller
     test/test_load_joint_group_effort_controller.cpp
   )

effort_controllers/src/joint_group_effort_controller.cpp

@@ -29,26 +29,19 @@ JointGroupEffortController::JointGroupEffortController()
 
 controller_interface::CallbackReturn JointGroupEffortController::on_init()
 {
-  auto ret = forward_command_controller::ForwardCommandController::on_init();
-  if (ret != controller_interface::CallbackReturn::SUCCESS)
-  {
-    return ret;
-  }
-
   try
   {
     // Explicitly set the interface parameter declared by the forward_command_controller
     // to match the value set in the JointGroupEffortController constructor.
-    get_node()->set_parameter(
-      rclcpp::Parameter("interface_name", hardware_interface::HW_IF_EFFORT));
+    auto_declare<std::string>("interface_name", interface_name_);
   }
   catch (const std::exception & e)
   {
     fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
-    return controller_interface::CallbackReturn::ERROR;
+    return CallbackReturn::ERROR;
   }
 
-  return controller_interface::CallbackReturn::SUCCESS;
+  return forward_command_controller::ForwardCommandController::on_init();
 }
 
 controller_interface::CallbackReturn JointGroupEffortController::on_deactivate(

effort_controllers/test/config/test_joint_group_effort_controller.yaml

@@ -0,0 +1,3 @@
+test_joint_group_effort_controller:
+  ros__parameters:
+    joints: ["joint1"]

effort_controllers/test/test_joint_group_effort_controller.cpp

@@ -36,10 +36,14 @@ void JointGroupEffortControllerTest::SetUp()
 
 void JointGroupEffortControllerTest::TearDown() { controller_.reset(nullptr); }
 
-void JointGroupEffortControllerTest::SetUpController()
+void JointGroupEffortControllerTest::SetUpController(
+  const std::vector<rclcpp::Parameter> & parameters)
 {
-  const auto result = controller_->init(
-    "test_joint_group_effort_controller", "", 0, "", controller_->define_custom_node_options());
+  auto node_options = controller_->define_custom_node_options();
+  node_options.parameter_overrides(parameters);
+
+  const auto result =
+    controller_->init("test_joint_group_effort_controller", "", 0, "", node_options);
   ASSERT_EQ(result, controller_interface::return_type::OK);
 
   std::vector<LoanedCommandInterface> command_ifs;
@@ -50,54 +54,20 @@ void JointGroupEffortControllerTest::SetUpController()
   executor.add_node(controller_->get_node()->get_node_base_interface());
 }
 
-TEST_F(JointGroupEffortControllerTest, JointsParameterNotSet)
-{
-  SetUpController();
-
-  // configure failed, 'joints' parameter not set
-  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
-}
-
-TEST_F(JointGroupEffortControllerTest, JointsParameterIsEmpty)
-{
-  SetUpController();
-  controller_->get_node()->set_parameter({"joints", std::vector<std::string>()});
-
-  // configure failed, 'joints' is empty
-  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
-}
-
 TEST_F(JointGroupEffortControllerTest, ConfigureAndActivateParamsSuccess)
 {
-  SetUpController();
-  controller_->get_node()->set_parameter({"joints", joint_names_});
+  SetUpController({rclcpp::Parameter("joints", joint_names_)});
 
   // configure successful
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 }
 
-TEST_F(JointGroupEffortControllerTest, ActivateWithWrongJointsNamesFails)
-{
-  SetUpController();
-  controller_->get_node()->set_parameter({"joints", std::vector<std::string>{"joint1", "joint4"}});
-
-  // activate failed, 'joint4' is not a valid joint name for the hardware
-  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
-  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
-  ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
-
-  controller_->get_node()->set_parameter({"joints", std::vector<std::string>{"joint1", "joint2"}});
-
-  // activate should succeed now
-  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
-  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
-}
-
 TEST_F(JointGroupEffortControllerTest, CommandSuccessTest)
 {
-  SetUpController();
-  controller_->get_node()->set_parameter({"joints", joint_names_});
+  SetUpController({rclcpp::Parameter("joints", joint_names_)});
+
+  // configure successful
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 
   // update successful though no command has been send yet
@@ -128,8 +98,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest)
 
 TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest)
 {
-  SetUpController();
-  controller_->get_node()->set_parameter({"joints", joint_names_});
+  SetUpController({rclcpp::Parameter("joints", joint_names_)});
+
+  // configure successful
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 
   // send command with wrong number of joints
@@ -150,8 +121,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest)
 
 TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest)
 {
-  SetUpController();
-  controller_->get_node()->set_parameter({"joints", joint_names_});
+  SetUpController({rclcpp::Parameter("joints", joint_names_)});
+
+  // configure successful
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 
   // update successful, no command received yet
@@ -167,8 +139,7 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest)
 
 TEST_F(JointGroupEffortControllerTest, CommandCallbackTest)
 {
-  SetUpController();
-  controller_->get_node()->set_parameter({"joints", joint_names_});
+  SetUpController({rclcpp::Parameter("joints", joint_names_)});
 
   // default values
   ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1);
@@ -211,8 +182,7 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest)
 
 TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest)
 {
-  SetUpController();
-  controller_->get_node()->set_parameter({"joints", joint_names_});
+  SetUpController({rclcpp::Parameter("joints", joint_names_)});
 
   // configure successful
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);

effort_controllers/test/test_joint_group_effort_controller.hpp

@@ -46,7 +46,7 @@ class JointGroupEffortControllerTest : public ::testing::Test
   void SetUp();
   void TearDown();
 
-  void SetUpController();
+  void SetUpController(const std::vector<rclcpp::Parameter> & parameters = {});
 
 protected:
   std::unique_ptr<FriendJointGroupEffortController> controller_;

effort_controllers/test/test_load_joint_group_effort_controller.cpp

@@ -32,10 +32,14 @@ TEST(TestLoadJointGroupVelocityController, load_controller)
   controller_manager::ControllerManager cm(
     executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager");
 
-  ASSERT_NE(
-    cm.load_controller(
-      "test_joint_group_effort_controller", "effort_controllers/JointGroupEffortController"),
-    nullptr);
+  const std::string test_file_path =
+    std::string(TEST_FILES_DIRECTORY) + "/config/test_joint_group_effort_controller.yaml";
+
+  cm.set_parameter({"test_joint_group_effort_controller.params_file", test_file_path});
+  cm.set_parameter(
+    {"test_joint_group_effort_controller.type", "effort_controllers/JointGroupEffortController"});
+
+  ASSERT_NE(cm.load_controller("test_joint_group_effort_controller"), nullptr);
 
   rclcpp::shutdown();
 }

forward_command_controller/CMakeLists.txt

@@ -58,6 +58,8 @@ if(BUILD_TESTING)
   find_package(controller_manager REQUIRED)
   find_package(ros2_control_test_assets REQUIRED)
 
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+
   ament_add_gmock(test_load_forward_command_controller
     test/test_load_forward_command_controller.cpp
   )

forward_command_controller/src/forward_command_controller_parameters.yaml

@@ -1,11 +1,17 @@
 forward_command_controller:
   joints: {
     type: string_array,
-    default_value: [],
     description: "Name of the joints to control",
+    validation: {
+      not_empty<>: []
+    },
+    read_only: true
   }
   interface_name: {
     type: string,
-    default_value: "",
     description: "Name of the interface to command",
+    validation: {
+      not_empty<>: []
+    },
+    read_only: true
   }

forward_command_controller/src/forward_controllers_base.cpp

@@ -112,22 +112,6 @@ controller_interface::InterfaceConfiguration ForwardControllersBase::state_inter
 controller_interface::CallbackReturn ForwardControllersBase::on_activate(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
-  //  check if we have all resources defined in the "points" parameter
-  //  also verify that we *only* have the resources defined in the "points" parameter
-  // ATTENTION(destogl): Shouldn't we use ordered interface all the time?
-  std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
-    ordered_interfaces;
-  if (
-    !controller_interface::get_ordered_interfaces(
-      command_interfaces_, command_interface_types_, std::string(""), ordered_interfaces) ||
-    command_interface_types_.size() != ordered_interfaces.size())
-  {
-    RCLCPP_ERROR(
-      get_node()->get_logger(), "Expected %zu command interfaces, got %zu",
-      command_interface_types_.size(), ordered_interfaces.size());
-    return controller_interface::CallbackReturn::ERROR;
-  }
-
   // reset command buffer if a command came through callback when controller was inactive
   // Try to set default value in command.
   // If this fails, then another command will be received soon anyways.

forward_command_controller/src/multi_interface_forward_command_controller_parameters.yaml

@@ -1,11 +1,17 @@
 multi_interface_forward_command_controller:
   joint: {
     type: string,
-    default_value: "",
     description: "Name of the joint to control",
+    validation: {
+      not_empty<>: []
+    },
+    read_only: true
   }
   interface_names: {
     type: string_array,
-    default_value: [],
     description: "Names of the interfaces to command",
+    validation: {
+      not_empty<>: []
+    },
+    read_only: true
   }

forward_command_controller/test/config/test_forward_command_controller.yaml

@@ -0,0 +1,4 @@
+test_forward_command_controller:
+  ros__parameters:
+    joints: ["joint1"]
+    interface_name: "position"

forward_command_controller/test/config/test_multi_interface_forward_command_controller.yaml

@@ -0,0 +1,4 @@
+test_multi_interface_forward_command_controller:
+  ros__parameters:
+    joint: "joint1"
+    interface_names: ["position"]