make init method arguments mandatory instead of using default

ros-controls · Jan 18, 2024 · d95e914 · d95e914
1 parent d20b971
commit d95e914
Show file tree

Hide file tree

Showing 7 changed files with 51 additions and 39 deletions.
diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp
@@ -115,7 +115,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   CONTROLLER_INTERFACE_PUBLIC
   virtual return_type init(
     const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
-    const std::string & node_namespace = "");
+    const std::string & node_namespace, const rclcpp::NodeOptions & node_options);
 
   /// Custom configure method to read additional parameters for controller-nodes
   /*

diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp
@@ -26,11 +26,11 @@ namespace controller_interface
 {
 return_type ControllerInterfaceBase::init(
   const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
-  const std::string & node_namespace)
+  const std::string & node_namespace, const rclcpp::NodeOptions & node_options)
 {
   urdf_ = urdf;
   node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
-    controller_name, node_namespace, define_custom_node_options(),
+    controller_name, node_namespace, node_options,
     false);  // disable LifecycleNode service interfaces
 
   try

diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp
@@ -21,7 +21,10 @@ TEST_F(ChainableControllerInterfaceTest, default_returns)
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK);
+  const auto node_options = controller.define_custom_node_options();
+  ASSERT_EQ(
+    controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options),
+    controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   EXPECT_TRUE(controller.is_chainable());
@@ -33,7 +36,10 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK);
+  const auto node_options = controller.define_custom_node_options();
+  ASSERT_EQ(
+    controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options),
+    controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   auto reference_interfaces = controller.export_reference_interfaces();
@@ -50,7 +56,10 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correc
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK);
+  const auto node_options = controller.define_custom_node_options();
+  ASSERT_EQ(
+    controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options),
+    controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   // expect empty return because storage is not resized
@@ -64,7 +73,10 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK);
+  const auto node_options = controller.define_custom_node_options();
+  ASSERT_EQ(
+    controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options),
+    controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   controller.set_name_prefix_of_reference_interfaces("some_not_correct_interface_prefix");
@@ -79,7 +91,10 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK);
+  const auto node_options = controller.define_custom_node_options();
+  ASSERT_EQ(
+    controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options),
+    controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   auto reference_interfaces = controller.export_reference_interfaces();
@@ -126,7 +141,10 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic)
   TestableChainableControllerInterface controller;
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK);
+  const auto node_options = controller.define_custom_node_options();
+  ASSERT_EQ(
+    controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options),
+    controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   EXPECT_FALSE(controller.is_in_chained_mode());

diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp
@@ -38,7 +38,10 @@ TEST(TestableControllerInterface, init)
   ASSERT_THROW(controller.get_node(), std::runtime_error);
 
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 10.0), controller_interface::return_type::OK);
+  const auto node_options = controller.define_custom_node_options();
+  ASSERT_EQ(
+    controller.init(TEST_CONTROLLER_NAME, "", 10.0, "", node_options),
+    controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
   // update_rate is set to default 0
@@ -60,7 +63,10 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
 
   TestableControllerInterface controller;
   // initialize, create node
-  ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 1.0), controller_interface::return_type::OK);
+  const auto node_options = controller.define_custom_node_options();
+  ASSERT_EQ(
+    controller.init(TEST_CONTROLLER_NAME, "", 1.0, "", node_options),
+    controller_interface::return_type::OK);
 
   // initialize executor to be able to get parameter update
   auto executor =
@@ -122,8 +128,10 @@ TEST(TestableControllerInterfaceInitError, init_with_error)
   TestableControllerInterfaceInitError controller;
 
   // initialize, create node
+  const auto node_options = controller.define_custom_node_options();
   ASSERT_EQ(
-    controller.init(TEST_CONTROLLER_NAME, "", 100.0), controller_interface::return_type::ERROR);
+    controller.init(TEST_CONTROLLER_NAME, "", 100.0, "", node_options),
+    controller_interface::return_type::ERROR);
 
   rclcpp::shutdown();
 }
@@ -137,8 +145,10 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure)
   TestableControllerInterfaceInitFailure controller;
 
   // initialize, create node
+  const auto node_options = controller.define_custom_node_options();
   ASSERT_EQ(
-    controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::ERROR);
+    controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options),
+    controller_interface::return_type::ERROR);
 
   rclcpp::shutdown();
 }

diff --git a/controller_interface/test/test_controller_with_options.cpp b/controller_interface/test/test_controller_with_options.cpp
@@ -42,7 +42,9 @@ TEST(ControllerWithOption, init_with_overrides)
   rclcpp::init(argc, argv);
   // creates the controller
   FriendControllerWithOptions controller;
-  EXPECT_EQ(controller.init("controller_name", "", 50.0), controller_interface::return_type::OK);
+  EXPECT_EQ(
+    controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()),
+    controller_interface::return_type::OK);
   // checks that the node options have been updated
   const auto & node_options = controller.get_node()->get_node_options();
   EXPECT_TRUE(node_options.allow_undeclared_parameters());
@@ -63,7 +65,9 @@ TEST(ControllerWithOption, init_without_overrides)
   rclcpp::init(argc, argv);
   // creates the controller
   FriendControllerWithOptions controller;
-  EXPECT_EQ(controller.init("controller_name", "", 50.0), controller_interface::return_type::ERROR);
+  EXPECT_EQ(
+    controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()),
+    controller_interface::return_type::ERROR);
   // checks that the node options have been updated
   const auto & node_options = controller.get_node()->get_node_options();
   EXPECT_TRUE(node_options.allow_undeclared_parameters());

diff --git a/controller_interface/test/test_controller_with_options.hpp b/controller_interface/test/test_controller_with_options.hpp
@@ -36,32 +36,12 @@ class ControllerWithOptions : public controller_interface::ControllerInterface
   ControllerWithOptions() = default;
   LifecycleNodeInterface::CallbackReturn on_init() override
   {
-    return LifecycleNodeInterface::CallbackReturn::SUCCESS;
-  }
-
-  controller_interface::return_type init(
-    const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
-    const std::string & node_namespace = "") override
-  {
-    ControllerInterface::init(controller_name, urdf, cm_update_rate, node_namespace);
-
-    switch (on_init())
-    {
-      case LifecycleNodeInterface::CallbackReturn::SUCCESS:
-        break;
-      case LifecycleNodeInterface::CallbackReturn::ERROR:
-      case LifecycleNodeInterface::CallbackReturn::FAILURE:
-        return controller_interface::return_type::ERROR;
-    }
     if (get_node()->get_parameters("parameter_list", params))
     {
       RCLCPP_INFO_STREAM(get_node()->get_logger(), "I found " << params.size() << " parameters.");
-      return controller_interface::return_type::OK;
-    }
-    else
-    {
-      return controller_interface::return_type::ERROR;
+      return LifecycleNodeInterface::CallbackReturn::SUCCESS;
     }
+    return LifecycleNodeInterface::CallbackReturn::FAILURE;
   }
 
   rclcpp::NodeOptions define_custom_node_options() const override

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -1302,8 +1302,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
 
   if (
     controller.c->init(
-      controller.info.name, robot_description_, get_update_rate(), get_namespace()) ==
-    controller_interface::return_type::ERROR)
+      controller.info.name, robot_description_, get_update_rate(), get_namespace(),
+      controller.c->define_custom_node_options()) == controller_interface::return_type::ERROR)
   {
     to.clear();
     RCLCPP_ERROR(