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

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*/)
 {