A method to get node options to setup the controller node #api-breaki…

…ng (#1169)

controller_interface/CMakeLists.txt

@@ -40,6 +40,8 @@ if(BUILD_TESTING)
   )
 
   ament_add_gmock(test_controller_with_options test/test_controller_with_options.cpp)
+  install(FILES test/test_controller_node_options.yaml
+      DESTINATION test)
   target_link_libraries(test_controller_with_options
     controller_interface
   )

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -113,10 +113,9 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   void release_interfaces();
 
   CONTROLLER_INTERFACE_PUBLIC
-  virtual return_type init(
+  return_type init(
     const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
-    const std::string & node_namespace = "",
-    const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().enable_logger_service(true));
+    const std::string & node_namespace, const rclcpp::NodeOptions & node_options);
 
   /// Custom configure method to read additional parameters for controller-nodes
   /*
@@ -159,6 +158,23 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   CONTROLLER_INTERFACE_PUBLIC
   const std::string & get_robot_description() const;
 
+  /**
+   * Method used by the controller_manager for base NodeOptions to instantiate the Lifecycle node
+   * of the controller upon loading the controller.
+   *
+   * \note The controller_manager will modify these NodeOptions in case a params file is passed
+   * by the spawner to load the controller parameters or when controllers are loaded in simulation
+   * (see ros2_control#1311, ros2_controllers#698 , ros2_controllers#795,ros2_controllers#966 for
+   * more details)
+   *
+   * @returns NodeOptions required for the configuration of the controller lifecycle node
+   */
+  CONTROLLER_INTERFACE_PUBLIC
+  virtual rclcpp::NodeOptions define_custom_node_options() const
+  {
+    return rclcpp::NodeOptions().enable_logger_service(true);
+  }
+
   /// Declare and initialize a parameter with a type.
   /**
    *

controller_interface/src/controller_interface_base.cpp

@@ -28,10 +28,10 @@ return_type ControllerInterfaceBase::init(
   const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
   const std::string & node_namespace, const rclcpp::NodeOptions & node_options)
 {
+  urdf_ = urdf;
   node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
     controller_name, node_namespace, node_options,
     false);  // disable LifecycleNode service interfaces
-  urdf_ = urdf;
 
   try
   {

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

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

controller_interface/test/test_controller_node_options.yaml

@@ -0,0 +1,6 @@
+controller_name:
+  ros__parameters:
+    parameter_list:
+      parameter1: 20.0
+      parameter2: 23.14
+      parameter3: -52.323

controller_interface/test/test_controller_with_options.cpp

@@ -16,7 +16,7 @@
 
 #include <gtest/gtest.h>
 #include <string>
-
+#include "ament_index_cpp/get_package_prefix.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 class FriendControllerWithOptions : public controller_with_options::ControllerWithOptions
@@ -42,11 +42,14 @@ 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());
   EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides());
+  EXPECT_TRUE(node_options.arguments().empty());
   // checks that the parameters have been correctly processed
   EXPECT_EQ(controller.params.size(), 3u);
   EXPECT_EQ(controller.params["parameter1"], 1.);
@@ -55,6 +58,98 @@ TEST(ControllerWithOption, init_with_overrides)
   rclcpp::shutdown();
 }
 
+TEST(ControllerWithOption, init_with_node_options_arguments_parameters)
+{
+  char const * const argv[] = {""};
+  int argc = arrlen(argv);
+  rclcpp::init(argc, argv);
+  // creates the controller
+  FriendControllerWithOptions controller;
+  auto controller_node_options = controller.define_custom_node_options();
+  controller_node_options.arguments(
+    {"--ros-args", "-p", "parameter_list.parameter1:=1.", "-p", "parameter_list.parameter2:=2.",
+     "-p", "parameter_list.parameter3:=3."});
+  EXPECT_EQ(
+    controller.init("controller_name", "", 50.0, "", controller_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());
+  EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides());
+  EXPECT_EQ(7lu, node_options.arguments().size());
+  // checks that the parameters have been correctly processed
+  EXPECT_EQ(controller.params.size(), 3u);
+  EXPECT_EQ(controller.params["parameter1"], 1.);
+  EXPECT_EQ(controller.params["parameter2"], 2.);
+  EXPECT_EQ(controller.params["parameter3"], 3.);
+  rclcpp::shutdown();
+}
+
+TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file)
+{
+  char const * const argv[] = {""};
+  int argc = arrlen(argv);
+  rclcpp::init(argc, argv);
+  // creates the controller
+  FriendControllerWithOptions controller;
+  const std::string params_file_path = ament_index_cpp::get_package_prefix("controller_interface") +
+                                       "/test/test_controller_node_options.yaml";
+  std::cerr << params_file_path << std::endl;
+  auto controller_node_options = controller.define_custom_node_options();
+  controller_node_options.arguments({"--ros-args", "--params-file", params_file_path});
+  EXPECT_EQ(
+    controller.init("controller_name", "", 50.0, "", controller_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());
+  EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides());
+  EXPECT_EQ(3lu, node_options.arguments().size());
+  // checks that the parameters have been correctly processed
+  EXPECT_EQ(controller.params.size(), 3u);
+  EXPECT_EQ(controller.params["parameter1"], 20.0);
+  EXPECT_EQ(controller.params["parameter2"], 23.14);
+  EXPECT_EQ(controller.params["parameter3"], -52.323);
+  bool use_sim_time(true);
+  controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false);
+  ASSERT_FALSE(use_sim_time);
+  rclcpp::shutdown();
+}
+
+TEST(
+  ControllerWithOption, init_with_node_options_arguments_parameters_file_and_override_command_line)
+{
+  char const * const argv[] = {""};
+  int argc = arrlen(argv);
+  rclcpp::init(argc, argv);
+  // creates the controller
+  FriendControllerWithOptions controller;
+  const std::string params_file_path = ament_index_cpp::get_package_prefix("controller_interface") +
+                                       "/test/test_controller_node_options.yaml";
+  std::cerr << params_file_path << std::endl;
+  auto controller_node_options = controller.define_custom_node_options();
+  controller_node_options.arguments(
+    {"--ros-args", "--params-file", params_file_path, "-p", "parameter_list.parameter1:=562.785",
+     "-p", "use_sim_time:=true"});
+  EXPECT_EQ(
+    controller.init("controller_name", "", 50.0, "", controller_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());
+  EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides());
+  EXPECT_EQ(7lu, node_options.arguments().size());
+  // checks that the parameters have been correctly processed
+  EXPECT_EQ(controller.params.size(), 3u);
+  EXPECT_EQ(controller.params["parameter1"], 562.785);
+  EXPECT_EQ(controller.params["parameter2"], 23.14);
+  EXPECT_EQ(controller.params["parameter3"], -52.323);
+  bool use_sim_time(false);
+  controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false);
+  ASSERT_TRUE(use_sim_time);
+  rclcpp::shutdown();
+}
+
 TEST(ControllerWithOption, init_without_overrides)
 {
   // mocks the declaration of overrides parameters in a yaml file
@@ -63,7 +158,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());

controller_interface/test/test_controller_with_options.hpp

@@ -36,36 +36,19 @@ 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 = "",
-    const rclcpp::NodeOptions & node_options =
-      rclcpp::NodeOptions()
-        .allow_undeclared_parameters(true)
-        .automatically_declare_parameters_from_overrides(true)) override
-  {
-    ControllerInterface::init(controller_name, urdf, cm_update_rate, node_namespace, node_options);
-
-    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
+  {
+    return rclcpp::NodeOptions()
+      .allow_undeclared_parameters(true)
+      .automatically_declare_parameters_from_overrides(true);
   }
 
   controller_interface::InterfaceConfiguration command_interface_configuration() const override

controller_manager/src/controller_manager.cpp

@@ -2606,7 +2606,7 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options(
   auto check_for_element = [](const auto & list, const auto & element)
   { return std::find(list.begin(), list.end(), element) != list.end(); };
 
-  rclcpp::NodeOptions controller_node_options = rclcpp::NodeOptions().enable_logger_service(true);
+  rclcpp::NodeOptions controller_node_options = controller.c->define_custom_node_options();
   std::vector<std::string> node_options_arguments = controller_node_options.arguments();
   const std::string ros_args_arg = "--ros-args";
   if (controller.info.parameters_file.has_value())

controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp

@@ -31,14 +31,6 @@ TestControllerFailedInit::on_init()
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
 }
 
-controller_interface::return_type TestControllerFailedInit::init(
-  const std::string & /* controller_name */, const std::string & /* urdf */,
-  unsigned int /*cm_update_rate*/, const std::string & /*node_namespace*/,
-  const rclcpp::NodeOptions & /*node_options*/)
-{
-  return controller_interface::return_type::ERROR;
-}
-
 controller_interface::return_type TestControllerFailedInit::update(
   const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
 {