From b33e579b24c324efdd24b8f9d947845e5090b1d3 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 6 Nov 2023 18:45:34 +0000 Subject: [PATCH] Pass URDF to controllers on init (#1088) * Pass URDF to controllers on init * store the robot_description in the controller_manager class variable * added minimal test to verify the update of robot description in controllers * fix code formatting * added test with loading and unloading the controller * added changes as per bence's review comments * added get_robot_description method to the base class --------- Co-authored-by: Sai Kishor Kothakota --- .../controller_interface_base.hpp | 7 +++- .../src/controller_interface_base.cpp | 5 ++- .../test_chainable_controller_interface.cpp | 12 +++--- .../test/test_controller_interface.cpp | 8 ++-- .../test/test_controller_with_options.cpp | 4 +- .../test/test_controller_with_options.hpp | 5 ++- .../controller_manager/controller_manager.hpp | 1 + controller_manager/src/controller_manager.cpp | 13 +++--- .../test/test_controller/test_controller.hpp | 3 ++ .../test_controller_failed_init.cpp | 4 +- .../test_controller_failed_init.hpp | 3 +- .../test/test_controller_manager.cpp | 28 +++++++++++++ .../test/test_controller_manager_srvs.cpp | 41 +++++++++++++++++++ 13 files changed, 109 insertions(+), 25 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 52bc970500..96141a034b 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -114,7 +114,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual return_type init( - const std::string & controller_name, const std::string & namespace_ = "", + const std::string & controller_name, const std::string & urdf, + const std::string & namespace_ = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().enable_logger_service(true)); /// Custom configure method to read additional parameters for controller-nodes @@ -155,6 +156,9 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC bool is_async() const; + CONTROLLER_INTERFACE_PUBLIC + const std::string & get_robot_description() const; + /// Declare and initialize a parameter with a type. /** * @@ -223,6 +227,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy std::vector state_interfaces_; unsigned int update_rate_ = 0; bool is_async_ = false; + std::string urdf_ = ""; private: std::shared_ptr node_; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 926449e201..fcbc28590a 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -25,11 +25,12 @@ namespace controller_interface { return_type ControllerInterfaceBase::init( - const std::string & controller_name, const std::string & namespace_, + const std::string & controller_name, const std::string & urdf, const std::string & namespace_, const rclcpp::NodeOptions & node_options) { node_ = std::make_shared( controller_name, namespace_, node_options, false); // disable LifecycleNode service interfaces + urdf_ = urdf; try { @@ -132,4 +133,6 @@ unsigned int ControllerInterfaceBase::get_update_rate() const { return update_ra bool ControllerInterfaceBase::is_async() const { return is_async_; } +const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; } + } // namespace controller_interface diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 6ae36b39ce..9c46287ba7 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -21,7 +21,7 @@ TEST_F(ChainableControllerInterfaceTest, default_returns) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); EXPECT_TRUE(controller.is_chainable()); @@ -33,7 +33,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); auto reference_interfaces = controller.export_reference_interfaces(); @@ -50,7 +50,7 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correc TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); // expect empty return because storage is not resized @@ -64,7 +64,7 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), 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 +79,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); auto reference_interfaces = controller.export_reference_interfaces(); @@ -126,7 +126,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), 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 f27fc43283..9be0dba9f0 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -38,7 +38,7 @@ TEST(TestableControllerInterface, init) ASSERT_THROW(controller.get_node(), std::runtime_error); // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); // update_rate is set to default 0 @@ -60,7 +60,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) TestableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK); // initialize executor to be able to get parameter update auto executor = @@ -122,7 +122,7 @@ TEST(TestableControllerInterfaceInitError, init_with_error) TestableControllerInterfaceInitError controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::ERROR); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::ERROR); rclcpp::shutdown(); } @@ -136,7 +136,7 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure) TestableControllerInterfaceInitFailure controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::ERROR); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), 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 index 080b8c4f8d..55c9db65c9 100644 --- a/controller_interface/test/test_controller_with_options.cpp +++ b/controller_interface/test/test_controller_with_options.cpp @@ -42,7 +42,7 @@ TEST(ControllerWithOption, init_with_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; - EXPECT_EQ(controller.init("controller_name"), controller_interface::return_type::OK); + EXPECT_EQ(controller.init("controller_name", ""), 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 +63,7 @@ TEST(ControllerWithOption, init_without_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; - EXPECT_EQ(controller.init("controller_name"), controller_interface::return_type::ERROR); + EXPECT_EQ(controller.init("controller_name", ""), 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 e19fc18d43..892ec56f9d 100644 --- a/controller_interface/test/test_controller_with_options.hpp +++ b/controller_interface/test/test_controller_with_options.hpp @@ -40,13 +40,14 @@ class ControllerWithOptions : public controller_interface::ControllerInterface } controller_interface::return_type init( - const std::string & controller_name, const std::string & namespace_ = "", + const std::string & controller_name, const std::string & urdf, + const std::string & namespace_ = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true)) override { - ControllerInterface::init(controller_name, namespace_, node_options); + ControllerInterface::init(controller_name, urdf, namespace_, node_options); switch (on_init()) { diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index cf1e959e1f..79a9861463 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -534,6 +534,7 @@ class ControllerManager : public rclcpp::Node std::vector activate_command_interface_request_, deactivate_command_interface_request_; + std::string robot_description_; rclcpp::Subscription::SharedPtr robot_description_subscription_; struct SwitchParams diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b3a4dc8945..b6c277126e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -262,10 +262,10 @@ ControllerManager::ControllerManager( RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } - std::string robot_description = ""; + robot_description_ = ""; // TODO(destogl): remove support at the end of 2023 - get_parameter("robot_description", robot_description); - if (robot_description.empty()) + get_parameter("robot_description", robot_description_); + if (robot_description_.empty()) { subscribe_to_robot_description_topic(); } @@ -275,7 +275,7 @@ ControllerManager::ControllerManager( get_logger(), "[Deprecated] Passing the robot description parameter directly to the control_manager node " "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead."); - init_resource_manager(robot_description); + init_resource_manager(robot_description_); } diagnostics_updater_.setHardwareID("ros2_control"); @@ -332,6 +332,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & // to die if a non valid urdf is passed. However, should maybe be fine tuned. try { + robot_description_ = robot_description.data; if (resource_manager_->is_urdf_already_loaded()) { RCLCPP_WARN( @@ -340,7 +341,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & "description file."); return; } - init_resource_manager(robot_description.data.c_str()); + init_resource_manager(robot_description_); } catch (std::runtime_error & e) { @@ -1290,7 +1291,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co } if ( - controller.c->init(controller.info.name, get_namespace()) == + controller.c->init(controller.info.name, robot_description_, get_namespace()) == controller_interface::return_type::ERROR) { to.clear(); diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index f73f592037..d7b318c597 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -30,6 +30,7 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface // indicating the node name under which the controller node // is being loaded. constexpr char TEST_CONTROLLER_NAME[] = "test_controller_name"; +constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name"; // corresponds to the name listed within the pluginlib xml constexpr char TEST_CONTROLLER_CLASS_NAME[] = "controller_manager/test_controller"; class TestController : public controller_interface::ControllerInterface @@ -65,6 +66,8 @@ class TestController : public controller_interface::ControllerInterface CONTROLLER_MANAGER_PUBLIC void set_state_interface_configuration(const controller_interface::InterfaceConfiguration & cfg); + const std::string & getRobotDescription() const; + unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; // Variable where we store when cleanup was called, pointer because the controller 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 57d253d03c..ba762573c7 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 @@ -32,8 +32,8 @@ TestControllerFailedInit::on_init() } controller_interface::return_type TestControllerFailedInit::init( - const std::string & /* controller_name */, const std::string & /*namespace_*/, - const rclcpp::NodeOptions & /*node_options*/) + const std::string & /* controller_name */, const std::string & /* urdf */, + const std::string & /*namespace_*/, const rclcpp::NodeOptions & /*node_options*/) { return controller_interface::return_type::ERROR; } 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 bfc2cd3cad..69530e40b0 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 @@ -40,7 +40,8 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac CONTROLLER_INTERFACE_PUBLIC controller_interface::return_type init( - const std::string & controller_name, const std::string & namespace_ = "", + const std::string & controller_name, const std::string & urdf, + const std::string & namespace_ = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() .allow_undeclared_parameters(true) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index eb001ce70f..8769a24749 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -33,6 +33,34 @@ class TestControllerManagerWithStrictness { }; +class TestControllerManagerRobotDescription +: public ControllerManagerFixture +{ +}; + +TEST_F(TestControllerManagerRobotDescription, controller_robot_description_update) +{ + auto test_controller = std::make_shared(); + auto test_controller2 = std::make_shared(); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); + + // Now change the robot description and then load a new controller and see if the new controller + // gets the new description and the old controller still maintains the configuration + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_missing_state_keys_urdf; + cm_->robot_description_callback(msg); + cm_->add_controller( + test_controller2, test_controller::TEST_CONTROLLER2_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); + ASSERT_EQ( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf, + test_controller2->get_robot_description()); +} + TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) { const auto test_param = GetParam(); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 6429f82a44..773fbd94d8 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -457,6 +457,47 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv) EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } +TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controller) +{ + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr unload_client = + srv_node->create_client( + "test_controller_manager/unload_controller"); + + auto test_controller = std::make_shared(); + auto abstract_test_controller = cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + + // check the robot description + ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); + + // Now change the robot description and then see that the controller maintains the old URDF until + // it is unloaded and loaded again + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_missing_state_keys_urdf; + cm_->robot_description_callback(msg); + ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); + + // now unload and load the controller and see if the controller gets the new robot description + auto unload_request = std::make_shared(); + unload_request->name = test_controller::TEST_CONTROLLER_NAME; + auto result = call_service_and_wait(*unload_client, unload_request, srv_executor, true); + EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); + + // now load it and check if it got the new robot description + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + ASSERT_EQ( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf, + test_controller->get_robot_description()); +} + TEST_F(TestControllerManagerSrvs, configure_controller_srv) { rclcpp::executors::SingleThreadedExecutor srv_executor;