diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index d02c422451..19a501dc62 100644 --- a/controller_interface/CMakeLists.txt +++ b/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 ) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 2e953ee932..6ce483d1ff 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/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. /** * diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 560feff5f9..e48e1d21b3 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/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( controller_name, node_namespace, node_options, false); // disable LifecycleNode service interfaces - urdf_ = urdf; try { diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index d81d2bfdad..47487e019c 100644 --- 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 index 24e780fab3..3976b2a81e 100644 --- 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_node_options.yaml b/controller_interface/test/test_controller_node_options.yaml new file mode 100644 index 0000000000..0c23c7f9d8 --- /dev/null +++ b/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 diff --git a/controller_interface/test/test_controller_with_options.cpp b/controller_interface/test/test_controller_with_options.cpp index 51169e945c..1e22239215 100644 --- a/controller_interface/test/test_controller_with_options.cpp +++ b/controller_interface/test/test_controller_with_options.cpp @@ -16,7 +16,7 @@ #include #include - +#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()); diff --git a/controller_interface/test/test_controller_with_options.hpp b/controller_interface/test/test_controller_with_options.hpp index 1416a7d2df..fc909df361 100644 --- a/controller_interface/test/test_controller_with_options.hpp +++ b/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 diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ab57c5a196..9b9b0dcf8f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/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 node_options_arguments = controller_node_options.arguments(); const std::string ros_args_arg = "--ros-args"; if (controller.info.parameters_file.has_value()) diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp index a106d5cbdf..1b2276bf3c 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp +++ b/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*/) { diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp index 315b0745b0..3f24df4e29 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp @@ -38,21 +38,19 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac CONTROLLER_MANAGER_PUBLIC virtual ~TestControllerFailedInit() = default; - CONTROLLER_INTERFACE_PUBLIC - 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; - controller_interface::InterfaceConfiguration command_interface_configuration() const override { return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE}; } + rclcpp::NodeOptions define_custom_node_options() const override + { + return rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); + } + controller_interface::InterfaceConfiguration state_interface_configuration() const override { return controller_interface::InterfaceConfiguration{