diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 376cef3362..0d1b88a55a 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -150,6 +150,7 @@ if(BUILD_TESTING) ) target_link_libraries(test_controller_manager_urdf_passing controller_manager + test_controller ros2_control_test_assets::ros2_control_test_assets ) ament_target_dependencies(test_controller_manager_urdf_passing diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index f5ee6854a1..c0a22fe5bd 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -322,7 +322,7 @@ class ControllerManager : public rclcpp::Node std::vector get_controller_names(); std::pair split_command_interface( const std::string & command_interface); - void subscribe_to_robot_description_topic(); + void init_controller_manager(); /** * Clear request lists used when switching controllers. The lists are shared between "callback" @@ -546,6 +546,7 @@ class ControllerManager : public rclcpp::Node std::string robot_description_; rclcpp::Subscription::SharedPtr robot_description_subscription_; + rclcpp::TimerBase::SharedPtr robot_description_notification_timer_; struct SwitchParams { diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ab57c5a196..28fb970126 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -267,31 +267,7 @@ ControllerManager::ControllerManager( std::make_shared>( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) { - if (!get_parameter("update_rate", update_rate_)) - { - RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); - } - - robot_description_ = ""; - // TODO(destogl): remove support at the end of 2023 - get_parameter("robot_description", robot_description_); - if (robot_description_.empty()) - { - subscribe_to_robot_description_topic(); - } - else - { - RCLCPP_WARN( - 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_services(); - } - - diagnostics_updater_.setHardwareID("ros2_control"); - diagnostics_updater_.add( - "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); + init_controller_manager(); } ControllerManager::ControllerManager( @@ -308,24 +284,25 @@ ControllerManager::ControllerManager( std::make_shared>( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) { + init_controller_manager(); +} + +void ControllerManager::init_controller_manager() +{ + // Get parameters needed for RT "update" loop to work if (!get_parameter("update_rate", update_rate_)) { RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } - if (resource_manager_->is_urdf_already_loaded()) - { - init_services(); - } - subscribe_to_robot_description_topic(); - - diagnostics_updater_.setHardwareID("ros2_control"); - diagnostics_updater_.add( - "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); -} + robot_description_notification_timer_ = create_wall_timer( + std::chrono::seconds(1), + [&]() + { + RCLCPP_WARN( + get_logger(), "Waiting for data on '~/robot_description' topic to finish initialization"); + }); -void ControllerManager::subscribe_to_robot_description_topic() -{ // set QoS to transient local to get messages that have already been published // (if robot state publisher starts before controller manager) robot_description_subscription_ = create_subscription( @@ -334,6 +311,11 @@ void ControllerManager::subscribe_to_robot_description_topic() RCLCPP_INFO( get_logger(), "Subscribing to '%s' topic for robot description.", robot_description_subscription_->get_topic_name()); + + // Setup diagnostics + diagnostics_updater_.setHardwareID("ros2_control"); + diagnostics_updater_.add( + "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); } void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) @@ -341,36 +323,28 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & RCLCPP_INFO(get_logger(), "Received robot description from topic."); RCLCPP_DEBUG( get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); - // TODO(Manuel): errors should probably be caught since we don't want controller_manager node - // 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( - get_logger(), - "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot " - "description file."); - return; - } - init_resource_manager(robot_description_); - init_services(); - } - catch (std::runtime_error & e) + robot_description_ = robot_description.data; + if (resource_manager_->is_urdf_already_loaded()) { - RCLCPP_ERROR_STREAM( + RCLCPP_WARN( get_logger(), - "The published robot description file (urdf) seems not to be genuine. The following error " - "was caught:" - << e.what()); + "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot " + "description file."); + return; } + init_resource_manager(robot_description_); } void ControllerManager::init_resource_manager(const std::string & robot_description) { - // TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong... - resource_manager_->load_urdf(robot_description); + if (!resource_manager_->load_and_initialize_components(robot_description)) + { + RCLCPP_WARN( + get_logger(), + "URDF validation went wrong check the previous output. This might only mean that interfaces " + "defined in URDF and exported by the hardware do not match. Therefore continue initializing " + "controller manager..."); + } // Get all components and if they are not defined in parameters activate them automatically auto components_to_activate = resource_manager_->get_components_status(); @@ -414,57 +388,22 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED)); // inactive (configured) - // BEGIN: Keep old functionality on for backwards compatibility (Remove at the end of 2023) - std::vector configure_components_on_start = std::vector({}); - get_parameter("configure_components_on_start", configure_components_on_start); - if (!configure_components_on_start.empty()) - { - RCLCPP_WARN( - get_logger(), - "Parameter 'configure_components_on_start' is deprecated. " - "Use 'hardware_components_initial_state.inactive' instead, to set component's initial " - "state to 'inactive'. Don't use this parameters in combination with the new " - "'hardware_components_initial_state' parameter structure."); - set_components_to_state( - "configure_components_on_start", - rclcpp_lifecycle::State( - State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); - } - // END: Keep old functionality on humble backwards compatibility (Remove at the end of 2023) - else - { - set_components_to_state( - "hardware_components_initial_state.inactive", - rclcpp_lifecycle::State( - State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); - } + set_components_to_state( + "hardware_components_initial_state.inactive", + rclcpp_lifecycle::State( + State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); - // BEGIN: Keep old functionality on for backwards compatibility (Remove at the end of 2023) - std::vector activate_components_on_start = std::vector({}); - get_parameter("activate_components_on_start", activate_components_on_start); + // activate all other components rclcpp_lifecycle::State active_state( State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); - if (!activate_components_on_start.empty()) + for (const auto & [component, state] : components_to_activate) { - RCLCPP_WARN( - get_logger(), - "Parameter 'activate_components_on_start' is deprecated. " - "Components are activated per default. Don't use this parameters in combination with the new " - "'hardware_components_initial_state' parameter structure."); - for (const auto & component : activate_components_on_start) - { - resource_manager_->set_component_state(component, active_state); - } - } - // END: Keep old functionality on humble for backwards compatibility (Remove at the end of 2023) - else - { - // activate all other components - for (const auto & [component, state] : components_to_activate) - { - resource_manager_->set_component_state(component, active_state); - } + resource_manager_->set_component_state(component, active_state); } + + // Init CM services first after the URDF is loaded an components are set + init_services(); + robot_description_notification_timer_->cancel(); } void ControllerManager::init_services() @@ -838,6 +777,15 @@ controller_interface::return_type ControllerManager::switch_controller( const std::vector & deactivate_controllers, int strictness, bool activate_asap, const rclcpp::Duration & timeout) { + if (!resource_manager_->is_urdf_already_loaded()) + { + RCLCPP_ERROR( + get_logger(), + "Resource Manager is not initialized yet! Please provide robot description on " + "'~/robot_description' topic before trying to switch controllers."); + return controller_interface::return_type::ERROR; + } + switch_params_ = SwitchParams(); if (!deactivate_request_.empty() || !activate_request_.empty()) @@ -877,14 +825,15 @@ controller_interface::return_type ControllerManager::switch_controller( strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; } - RCLCPP_DEBUG(get_logger(), "Switching controllers:"); + RCLCPP_DEBUG(get_logger(), "Activating controllers:"); for (const auto & controller : activate_controllers) { - RCLCPP_DEBUG(get_logger(), "- Activating controller '%s'", controller.c_str()); + RCLCPP_DEBUG(get_logger(), " - %s", controller.c_str()); } + RCLCPP_DEBUG(get_logger(), "Deactivating controllers:"); for (const auto & controller : deactivate_controllers) { - RCLCPP_DEBUG(get_logger(), "- Deactivating controller '%s'", controller.c_str()); + RCLCPP_DEBUG(get_logger(), " - %s", controller.c_str()); } const auto list_controllers = [this, strictness]( @@ -1222,18 +1171,24 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::OK; } - if ( - !activate_command_interface_request_.empty() || !deactivate_command_interface_request_.empty()) + RCLCPP_DEBUG(get_logger(), "Request for command interfaces from activating controllers:"); + for (const auto & interface : activate_command_interface_request_) { - if (!resource_manager_->prepare_command_mode_switch( - activate_command_interface_request_, deactivate_command_interface_request_)) - { - RCLCPP_ERROR( - get_logger(), - "Could not switch controllers since prepare command mode switch was rejected."); - clear_requests(); - return controller_interface::return_type::ERROR; - } + RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); + } + RCLCPP_DEBUG(get_logger(), "Request for command interfaces from deactivating controllers:"); + for (const auto & interface : deactivate_command_interface_request_) + { + RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); + } + + if (!resource_manager_->prepare_command_mode_switch( + activate_command_interface_request_, deactivate_command_interface_request_)) + { + RCLCPP_ERROR( + get_logger(), "Could not switch controllers since prepare command mode switch was rejected."); + clear_requests(); + return controller_interface::return_type::ERROR; } // start the atomic controller switching diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 7bf5cae628..8b6608bbb3 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -65,40 +65,16 @@ class ControllerManagerFixture : public ::testing::Test { public: explicit ControllerManagerFixture( - const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, - const bool & pass_urdf_as_parameter = false) - : robot_description_(robot_description), pass_urdf_as_parameter_(pass_urdf_as_parameter) + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf) + : robot_description_(robot_description) { executor_ = std::make_shared(); - // We want to be able to create a ResourceManager where no urdf file has been passed to - if (robot_description_.empty()) + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + // We want to be able to not pass robot description immediately + if (!robot_description_.empty()) { - cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); - } - else - { - // can be removed later, needed if we want to have the deprecated way of passing the robot - // description file to the controller manager covered by tests - if (pass_urdf_as_parameter_) - { - cm_ = std::make_shared( - std::make_unique(robot_description_, true, true), - executor_, TEST_CM_NAME); - } - else - { - // TODO(Manuel) : passing via topic not working in test setup, tested cm does - // not receive msg. Have to check this... - - // this is just a workaround to skip passing - cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); - // mimic topic call - auto msg = std_msgs::msg::String(); - msg.data = robot_description_; - cm_->robot_description_callback(msg); - } + pass_robot_description_to_cm_and_rm(robot_description_); } time_ = rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()); } @@ -134,6 +110,17 @@ class ControllerManagerFixture : public ::testing::Test } } + void pass_robot_description_to_cm_and_rm( + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf) + { + // TODO(Manuel) : passing via topic not working in test setup, tested cm does + // not receive msg. Have to check this... + // this is just a workaround to skip passing - mimic topic call + auto msg = std_msgs::msg::String(); + msg.data = robot_description; + cm_->robot_description_callback(msg); + } + void switch_test_controllers( const std::vector & start_controllers, const std::vector & stop_controllers, const int strictness, @@ -156,7 +143,6 @@ class ControllerManagerFixture : public ::testing::Test std::thread updater_; bool run_updater_; const std::string robot_description_; - const bool pass_urdf_as_parameter_; rclcpp::Time time_; }; diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 5c2ebd14f6..d8a9785052 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -20,8 +20,12 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" - #include "ros2_control_test_assets/descriptions.hpp" +#include "test_controller/test_controller.hpp" + +const auto CONTROLLER_NAME = "test_controller"; +using test_controller::TEST_CONTROLLER_CLASS_NAME; +using strvec = std::vector; class TestControllerManagerWithTestableCM; @@ -29,11 +33,22 @@ class TestableControllerManager : public controller_manager::ControllerManager { friend TestControllerManagerWithTestableCM; - FRIEND_TEST(TestControllerManagerWithTestableCM, initial_no_load_urdf_called); - FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); - FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed); - FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); - FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); + FRIEND_TEST( + TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called); + FRIEND_TEST( + TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback); + FRIEND_TEST( + TestControllerManagerWithTestableCM, + load_and_initialize_components_called_after_invalid_urdf_passed); + FRIEND_TEST( + TestControllerManagerWithTestableCM, + when_starting_broadcaster_expect_error_before_rm_is_initialized_with_robot_description); + FRIEND_TEST( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_with_robot_description); + FRIEND_TEST( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_after_some_time); public: TestableControllerManager( @@ -53,35 +68,156 @@ class TestControllerManagerWithTestableCM { public: // create cm with no urdf - TestControllerManagerWithTestableCM() - : ControllerManagerFixture("", false) + TestControllerManagerWithTestableCM() : ControllerManagerFixture("") {} + + void prepare_controller() { + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + test_controller_ = std::make_shared(); + cm_->add_controller(test_controller_, CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME); + ASSERT_NE(test_controller_, nullptr); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller_->get_state().id()); + } + + void configure_and_try_switch_that_returns_error() + { + // configure controller + cm_->configure_controller(CONTROLLER_NAME); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id()); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + + switch_test_controllers( + strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::ready, + controller_interface::return_type::ERROR); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id()); + } + + void try_successful_switch() + { + switch_test_controllers( + strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::timeout, + controller_interface::return_type::OK); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_->get_state().id()); } + + std::shared_ptr test_controller_; }; -TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called) +TEST_P(TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called) { ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); } -TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback) +TEST_P(TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback) { ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); - // mimic callback - auto msg = std_msgs::msg::String(); - msg.data = ros2_control_test_assets::minimal_robot_urdf; - cm_->robot_description_callback(msg); + pass_robot_description_to_cm_and_rm(); ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); } -TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed) +TEST_P( + TestControllerManagerWithTestableCM, + load_and_initialize_components_called_after_invalid_urdf_passed) { ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); - // mimic callback - auto msg = std_msgs::msg::String(); - msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf; - cm_->robot_description_callback(msg); + pass_robot_description_to_cm_and_rm( + ros2_control_test_assets::minimal_robot_missing_command_keys_urdf); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); +} + +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_broadcaster_expect_error_before_rm_is_initialized_with_robot_description) +{ + prepare_controller(); + + // setup state interfaces to claim from controllers + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller_->set_state_interface_configuration(state_itfs_cfg); + + configure_and_try_switch_that_returns_error(); + + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); + + try_successful_switch(); +} + +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_with_robot_description) +{ + prepare_controller(); + + // setup command interfaces to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller_->set_command_interface_configuration(cmd_itfs_cfg); + + configure_and_try_switch_that_returns_error(); + + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); + + try_successful_switch(); +} + +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_after_some_time) +{ + prepare_controller(); + + // setup state interfaces to claim from controllers + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller_->set_state_interface_configuration(state_itfs_cfg); + + // setup command interfaces to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller_->set_command_interface_configuration(cmd_itfs_cfg); + + configure_and_try_switch_that_returns_error(); + + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + + pass_robot_description_to_cm_and_rm(); ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); + + try_successful_switch(); } INSTANTIATE_TEST_SUITE_P( diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index 0b4b107ee1..9e28863f19 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -67,8 +67,6 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs std::make_unique(), executor_, TEST_CM_NAME); run_updater_ = false; - cm_->set_parameter( - rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); cm_->set_parameter(rclcpp::Parameter( "hardware_components_initial_state.unconfigured", std::vector({TEST_SYSTEM_HARDWARE_NAME}))); @@ -76,16 +74,8 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs "hardware_components_initial_state.inactive", std::vector({TEST_SENSOR_HARDWARE_NAME}))); - std::string robot_description = ""; - cm_->get_parameter("robot_description", robot_description); - if (robot_description.empty()) - { - throw std::runtime_error( - "Unable to initialize resource manager, no robot description found."); - } - auto msg = std_msgs::msg::String(); - msg.data = robot_description_; + msg.data = ros2_control_test_assets::minimal_robot_urdf; cm_->robot_description_callback(msg); SetUpSrvsCMExecutor(); @@ -207,7 +197,8 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs TEST_F(TestControllerManagerHWManagementSrvs, list_hardware_components) { // Default status after start: - // checks if "configure_components_on_start" and "activate_components_on_start" are correctly read + // checks if "hardware_components_initial_state.unconfigured" and + // "hardware_components_initial_state.inactive" are correctly read list_hardware_components_and_check( // actuator, sensor, system @@ -372,21 +363,8 @@ class TestControllerManagerHWManagementSrvsWithoutParams std::make_unique(), executor_, TEST_CM_NAME); run_updater_ = false; - // TODO(destogl): separate this to init_tests method where parameter can be set for each test - // separately - cm_->set_parameter( - rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); - - std::string robot_description = ""; - cm_->get_parameter("robot_description", robot_description); - if (robot_description.empty()) - { - throw std::runtime_error( - "Unable to initialize resource manager, no robot description found."); - } - auto msg = std_msgs::msg::String(); - msg.data = robot_description_; + msg.data = ros2_control_test_assets::minimal_robot_urdf; cm_->robot_description_callback(msg); SetUpSrvsCMExecutor(); @@ -395,8 +373,9 @@ class TestControllerManagerHWManagementSrvsWithoutParams TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activation_of_all_hardware) { - // "configure_components_on_start" and "activate_components_on_start" are not set (empty) - // therefore all hardware components are active + // "hardware_components_initial_state.unconfigured" and + // "hardware_components_initial_state.inactive" are not set (empty). Therefore, all hardware + // components are active. list_hardware_components_and_check( // actuator, sensor, system std::vector( @@ -416,64 +395,3 @@ TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activati {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); } - -// BEGIN: Remove at the end of 2023 -class TestControllerManagerHWManagementSrvsOldParameters -: public TestControllerManagerHWManagementSrvs -{ -public: - void SetUp() override - { - executor_ = std::make_shared(); - cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); - run_updater_ = false; - - cm_->set_parameter( - rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); - cm_->set_parameter(rclcpp::Parameter( - "activate_components_on_start", std::vector({TEST_ACTUATOR_HARDWARE_NAME}))); - cm_->set_parameter(rclcpp::Parameter( - "configure_components_on_start", std::vector({TEST_SENSOR_HARDWARE_NAME}))); - - std::string robot_description = ""; - cm_->get_parameter("robot_description", robot_description); - if (robot_description.empty()) - { - throw std::runtime_error( - "Unable to initialize resource manager, no robot description found."); - } - - auto msg = std_msgs::msg::String(); - msg.data = robot_description_; - cm_->robot_description_callback(msg); - - SetUpSrvsCMExecutor(); - } -}; - -TEST_F(TestControllerManagerHWManagementSrvsOldParameters, list_hardware_components) -{ - // Default status after start: - // checks if "configure_components_on_start" and "activate_components_on_start" are correctly read - - list_hardware_components_and_check( - // actuator, sensor, system - std::vector( - {LFC_STATE::PRIMARY_STATE_ACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE, - LFC_STATE::PRIMARY_STATE_UNCONFIGURED}), - std::vector({ACTIVE, INACTIVE, UNCONFIGURED}), - std::vector>>({ - // is available - {{true, true}, {true, true, true}}, // actuator - {{}, {true}}, // sensor - {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system - }), - std::vector>>({ - // is claimed - {{false, false}, {false, false, false}}, // actuator - {{}, {false}}, // sensor - {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system - })); -} -// END: Remove at the end of 2023 diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index 390a7365f0..ed443ea3d4 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -28,8 +28,8 @@ using test_controller::TEST_CONTROLLER_CLASS_NAME; using ::testing::_; using ::testing::Return; -const auto controller_name1 = "test_controller1"; -const auto controller_name2 = "test_controller2"; +const auto CONTROLLER_NAME_1 = "test_controller1"; +const auto CONTROLLER_NAME_2 = "test_controller2"; using strvec = std::vector; class TestLoadController : public ControllerManagerFixture @@ -53,7 +53,7 @@ TEST_F(TestLoadController, load_controller_failed_init) TEST_F(TestLoadController, configuring_non_loaded_controller_fails) { // try configure non-loaded controller - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::ERROR); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); } TEST_F(TestLoadController, can_set_and_get_non_default_update_rate) @@ -82,7 +82,7 @@ class TestLoadedController : public TestLoadController { TestLoadController::SetUp(); - controller_if = cm_->load_controller(controller_name1, TEST_CONTROLLER_CLASS_NAME); + controller_if = cm_->load_controller(CONTROLLER_NAME_1, TEST_CONTROLLER_CLASS_NAME); ASSERT_NE(controller_if, nullptr); } @@ -93,7 +93,7 @@ class TestLoadedController : public TestLoadController controller_interface::return_type::OK) { switch_test_controllers( - strvec{controller_name1}, strvec{}, strictness, expected_future_status, + strvec{CONTROLLER_NAME_1}, strvec{}, strictness, expected_future_status, expected_interface_status); } @@ -104,7 +104,7 @@ class TestLoadedController : public TestLoadController controller_interface::return_type::OK) { switch_test_controllers( - strvec{}, strvec{controller_name1}, strictness, expected_future_status, + strvec{}, strvec{CONTROLLER_NAME_1}, strictness, expected_future_status, expected_interface_status); } }; @@ -121,7 +121,7 @@ TEST_P(TestLoadedControllerParametrized, load_and_configure_one_known_controller EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); - cm_->configure_controller(controller_name1); + cm_->configure_controller(CONTROLLER_NAME_1); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); } @@ -129,7 +129,7 @@ TEST_P(TestLoadedControllerParametrized, can_start_configured_controller) { const auto test_param = GetParam(); - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); start_test_controller(test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); } @@ -138,7 +138,7 @@ TEST_P(TestLoadedControllerParametrized, can_stop_active_controller) { const auto test_param = GetParam(); - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); start_test_controller(test_param.strictness); @@ -164,7 +164,7 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) // Activate configured controller { ControllerManagerRunner cm_runner(this); - cm_->configure_controller(controller_name1); + cm_->configure_controller(CONTROLLER_NAME_1); } start_test_controller(test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); @@ -180,11 +180,11 @@ TEST_P(TestLoadedControllerParametrized, can_not_configure_active_controller) { const auto test_param = GetParam(); - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); start_test_controller(test_param.strictness); // Can not configure active controller - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::ERROR); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); } @@ -205,7 +205,7 @@ TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) test_param.strictness, std::future_status::ready, test_param.expected_return); // Can not configure finalize controller - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::ERROR); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, controller_if->get_state().id()); } @@ -213,7 +213,7 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_u { const auto test_param = GetParam(); - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); start_test_controller(test_param.strictness); @@ -227,7 +227,7 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_u test_controller->cleanup_calls = &cleanup_calls; // Configure from inactive state: controller can no be cleaned-up test_controller->simulate_cleanup_failure = true; - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::ERROR); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); EXPECT_EQ(0u, cleanup_calls); } @@ -236,7 +236,7 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure { const auto test_param = GetParam(); - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); start_test_controller(test_param.strictness); @@ -251,7 +251,7 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure test_controller->simulate_cleanup_failure = false; { ControllerManagerRunner cm_runner(this); - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); } ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); EXPECT_EQ(1u, cleanup_calls); @@ -270,8 +270,8 @@ class SwitchTest const auto UNSPECIFIED = 0; const auto EMPTY_STR_VEC = strvec{}; const auto NONEXISTENT_CONTROLLER = strvec{"nonexistent_controller"}; -const auto VALID_CONTROLLER = strvec{controller_name1}; -const auto VALID_PLUS_NONEXISTENT_CONTROLLERS = strvec{controller_name1, "nonexistent_controller"}; +const auto VALID_CONTROLLER = strvec{CONTROLLER_NAME_1}; +const auto VALID_PLUS_NONEXISTENT_CONTROLLERS = strvec{CONTROLLER_NAME_1, "nonexistent_controller"}; TEST_P(SwitchTest, EmptyListOrNonExistentTest) { @@ -375,10 +375,10 @@ class TestTwoLoadedControllers : public TestLoadController, void SetUp() override { TestLoadController::SetUp(); - controller_if1 = cm_->load_controller(controller_name1, TEST_CONTROLLER_CLASS_NAME); + controller_if1 = cm_->load_controller(CONTROLLER_NAME_1, TEST_CONTROLLER_CLASS_NAME); ASSERT_NE(controller_if1, nullptr); EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); - controller_if2 = cm_->load_controller(controller_name2, TEST_CONTROLLER_CLASS_NAME); + controller_if2 = cm_->load_controller(CONTROLLER_NAME_2, TEST_CONTROLLER_CLASS_NAME); ASSERT_NE(controller_if2, nullptr); EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); ASSERT_EQ( @@ -390,10 +390,10 @@ class TestTwoLoadedControllers : public TestLoadController, TEST_F(TestTwoLoadedControllers, load_and_configure_two_known_controllers) { - cm_->configure_controller(controller_name1); + cm_->configure_controller(CONTROLLER_NAME_1); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); - cm_->configure_controller(controller_name2); + cm_->configure_controller(CONTROLLER_NAME_2); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); } @@ -401,11 +401,11 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) { const auto test_param = GetParam(); - cm_->configure_controller(controller_name1); + cm_->configure_controller(CONTROLLER_NAME_1); // Start controller #1 RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller #1"); - switch_test_controllers(strvec{controller_name1}, strvec{}, test_param.strictness); + switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); @@ -418,10 +418,10 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) // TODO(destogl): fix this test for BEST_EFFORT - probably related to: // https://github.com/ros-controls/ros2_control/pull/582#issuecomment-1029931561 // switch_test_controllers( - // strvec{controller_name2}, strvec{controller_name1}, test_param.strictness, + // strvec{CONTROLLER_NAME_2}, strvec{CONTROLLER_NAME_1}, test_param.strictness, // std::future_status::ready, controller_interface::return_type::ERROR); switch_test_controllers( - strvec{controller_name2}, strvec{controller_name1}, STRICT, std::future_status::ready, + strvec{CONTROLLER_NAME_2}, strvec{CONTROLLER_NAME_1}, STRICT, std::future_status::ready, controller_interface::return_type::ERROR); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ( @@ -429,31 +429,31 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) { ControllerManagerRunner cm_runner(this); - cm_->configure_controller(controller_name2); + cm_->configure_controller(CONTROLLER_NAME_2); } // Stop controller 1 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1"); - switch_test_controllers(strvec{}, strvec{controller_name1}, test_param.strictness); + switch_test_controllers(strvec{}, strvec{CONTROLLER_NAME_1}, test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); // Start controller 1 again RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller #1"); - switch_test_controllers(strvec{controller_name1}, strvec{}, test_param.strictness); + switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); // Stop controller 1, start controller 2 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1, starting controller #2"); switch_test_controllers( - strvec{controller_name2}, strvec{controller_name1}, test_param.strictness); + strvec{CONTROLLER_NAME_2}, strvec{CONTROLLER_NAME_1}, test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if2->get_state().id()); // Stop controller 2 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #2"); - switch_test_controllers(strvec{}, strvec{controller_name2}, test_param.strictness); + switch_test_controllers(strvec{}, strvec{CONTROLLER_NAME_2}, test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); } diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 1ecc3164b8..03917dcd6e 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -31,6 +31,7 @@ using ::testing::Return; using namespace std::chrono_literals; class TestLoadController : public ControllerManagerFixture { +public: void SetUp() override { ControllerManagerFixture::SetUp(); @@ -274,3 +275,82 @@ TEST_F(TestLoadController, unload_on_kill) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } + +class TestLoadControllerWithoutRobotDescription +: public ControllerManagerFixture +{ +public: + TestLoadControllerWithoutRobotDescription() + : ControllerManagerFixture("") + { + } + + void SetUp() override + { + ControllerManagerFixture::SetUp(); + + update_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(10), + [&]() + { + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); + }); + + update_executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + update_executor_->add_node(cm_); + update_executor_spin_future_ = + std::async(std::launch::async, [this]() -> void { update_executor_->spin(); }); + + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + std::this_thread::sleep_for(50ms); + } + + void TearDown() override { update_executor_->cancel(); } + + rclcpp::TimerBase::SharedPtr robot_description_sending_timer_; + +protected: + rclcpp::TimerBase::SharedPtr update_timer_; + + // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks + std::shared_ptr update_executor_; + std::future update_executor_spin_future_; +}; + +TEST_F(TestLoadControllerWithoutRobotDescription, when_no_robot_description_spawner_times_out) +{ + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 256) + << "could not spawn controller because not robot description and not services for controller " + "manager are active"; +} + +TEST_F( + TestLoadControllerWithoutRobotDescription, + controller_starting_with_later_load_of_robot_description) +{ + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + // Delay sending robot description + robot_description_sending_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(2500), [&]() { pass_robot_description_to_cm_and_rm(); }); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0) + << "could not activate control because not robot description"; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } +} diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 743e548a4c..b7b7a414bb 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -88,18 +88,19 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * interfaces ought to be validated. Defaults to true. * \param[in] load_and_initialize_components boolean argument indicating whether to load and * initialize the components present in the parsed URDF. Defaults to true. + * \returns false if URDF validation has failed. */ - void load_urdf( + bool load_and_initialize_components( const std::string & urdf, bool validate_interfaces = true, bool load_and_initialize_components = true); /** - * @brief if the resource manager load_urdf(...) function has been called this returns true. - * We want to permit to load the urdf later on but we currently don't want to permit multiple - * calls to load_urdf (reloading/loading different urdf). + * @brief if the resource manager load_and_initialize_components(...) function has been called + * this returns true. We want to permit to load the urdf later on but we currently don't want to + * permit multiple calls to load_and_initialize_components (reloading/loading different urdf). * - * @return true if resource manager's load_urdf() has been already called. - * @return false if resource manager's load_urdf() has not been yet called. + * @return true if resource manager's load_and_initialize_components() has been already called. + * @return false if resource manager's load_and_initialize_components() has not been yet called. */ bool is_urdf_already_loaded() const; @@ -408,7 +409,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager bool state_interface_exists(const std::string & key) const; private: - void validate_storage(const std::vector & hardware_info) const; + bool validate_storage(const std::vector & hardware_info) const; void release_command_interface(const std::string & key); @@ -423,7 +424,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager // Structure to store read and write status so it is not initialized in the real-time loop HardwareReadWriteStatus read_write_status; - bool is_urdf_loaded__ = false; + bool is_urdf_loaded_ = false; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 2e8ccc7b1f..ccb531f6f3 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -60,7 +60,7 @@ auto trigger_and_print_hardware_state_transition = } else { - RCUTILS_LOG_INFO_NAMED( + RCUTILS_LOG_WARN_NAMED( "resource_manager", "Failed to '%s' hardware '%s'", transition_name.c_str(), hardware_name.c_str()); } @@ -433,6 +433,7 @@ class ResourceStorage template void import_state_interfaces(HardwareT & hardware) { + // TODO(destogl): add here exception catch block - this can otherwise crash the whole system auto interfaces = hardware.export_state_interfaces(); std::vector interface_names; interface_names.reserve(interfaces.size()); @@ -450,6 +451,7 @@ class ResourceStorage template void import_command_interfaces(HardwareT & hardware) { + // TODO(destogl): add here exception catch block - this can otherwise crash the whole system auto interfaces = hardware.export_command_interfaces(); hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); } @@ -551,7 +553,7 @@ class ResourceStorage } else { - RCUTILS_LOG_WARN_NAMED( + RCUTILS_LOG_ERROR_NAMED( "resource_manager", "Sensor hardware component '%s' from plugin '%s' failed to initialize.", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); @@ -742,7 +744,7 @@ ResourceManager::ResourceManager( rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) : resource_storage_(std::make_unique(update_rate, clock_interface)) { - load_urdf(urdf, validate_interfaces); + load_and_initialize_components(urdf, validate_interfaces); if (activate_all) { @@ -756,15 +758,18 @@ ResourceManager::ResourceManager( } // CM API: Called in "callback/slow"-thread -void ResourceManager::load_urdf( +bool ResourceManager::load_and_initialize_components( const std::string & urdf, bool validate_interfaces, bool load_and_initialize_components) { - is_urdf_loaded__ = true; + bool result = true; + const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + is_urdf_loaded_ = true; + if (load_and_initialize_components) { for (const auto & individual_hardware_info : hardware_info) @@ -790,16 +795,18 @@ void ResourceManager::load_urdf( // throw on missing state and command interfaces, not specified keys are being ignored if (validate_interfaces) { - validate_storage(hardware_info); + result = validate_storage(hardware_info); } std::lock_guard guard(resources_lock_); read_write_status.failed_hardware_names.reserve( resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + resource_storage_->systems_.size()); + + return result; } -bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } +bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded_; } // CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) @@ -1430,7 +1437,7 @@ bool ResourceManager::state_interface_exists(const std::string & key) const // BEGIN: private methods -void ResourceManager::validate_storage( +bool ResourceManager::validate_storage( const std::vector & hardware_info) const { std::vector missing_state_keys = {}; @@ -1486,20 +1493,29 @@ void ResourceManager::validate_storage( if (!missing_state_keys.empty() || !missing_command_keys.empty()) { - std::string err_msg = "Wrong state or command interface configuration.\n"; - err_msg += "missing state interfaces:\n"; + std::string message = "Wrong state or command interface configuration.\n"; + message += "missing state interfaces:\n"; for (const auto & missing_key : missing_state_keys) { - err_msg += "' " + missing_key + " '" + "\t"; + message += "' " + missing_key + " '" + "\t"; } - err_msg += "\nmissing command interfaces:\n"; + message += "\nmissing command interfaces:\n"; for (const auto & missing_key : missing_command_keys) { - err_msg += "' " + missing_key + " '" + "\t"; + message += "' " + missing_key + " '" + "\t"; } - throw std::runtime_error(err_msg); + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "Discrepancy between robot description file (urdf) and actually exported HW interfaces." + "If this is not intentional - fix it! " + "Details: %s", + message.c_str()); + + return false; } + + return true; } // END: private methods diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 4bb9e0c5fe..4e5a21a427 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -98,25 +98,25 @@ TEST_F(ResourceManagerTest, initialization_with_urdf) TEST_F(ResourceManagerTest, post_initialization_with_urdf) { TestableResourceManager rm; - ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf)); + ASSERT_NO_THROW(rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf)); } TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation) { - // If the the hardware can not be initialized and load_urdf tried to validate the interfaces a - // runtime exception is thrown + // If the the hardware can not be initialized and load_and_initialize_components tried to validate + // the interfaces a runtime exception is thrown TestableResourceManager rm; - ASSERT_THROW( - rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true), - std::runtime_error); + EXPECT_FALSE(rm.load_and_initialize_components( + ros2_control_test_assets::minimal_unitilizable_robot_urdf, true)); } TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) { - // If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces, - // the interface should not show up + // If the the hardware can not be initialized and load_and_initialize_components didn't try to + // validate the interfaces, the interface should not show up TestableResourceManager rm; - EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); + EXPECT_TRUE(rm.load_and_initialize_components( + ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); // test actuator EXPECT_FALSE(rm.state_interface_exists("joint1/position")); @@ -168,15 +168,17 @@ TEST_F(ResourceManagerTest, initialization_with_wrong_urdf) { // missing state keys { - EXPECT_THROW( - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), - std::exception); + auto rm = TestableResourceManager(); + EXPECT_FALSE(rm.load_and_initialize_components( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf)); + ASSERT_TRUE(rm.is_urdf_already_loaded()); } // missing command keys { - EXPECT_THROW( - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_missing_command_keys_urdf), - std::exception); + auto rm = TestableResourceManager(); + EXPECT_FALSE(rm.load_and_initialize_components( + ros2_control_test_assets::minimal_robot_missing_command_keys_urdf)); + ASSERT_TRUE(rm.is_urdf_already_loaded()); } } @@ -199,31 +201,23 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) } } -TEST_F(ResourceManagerTest, no_load_urdf_function_called) +TEST_F(ResourceManagerTest, no_load_and_initialize_components_function_called) { TestableResourceManager rm; ASSERT_FALSE(rm.is_urdf_already_loaded()); } -TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_invalid) -{ - TestableResourceManager rm; - EXPECT_THROW( - rm.load_urdf(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), std::exception); - ASSERT_TRUE(rm.is_urdf_already_loaded()); -} - -TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_valid) +TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_valid) { TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); ASSERT_TRUE(rm.is_urdf_already_loaded()); } -TEST_F(ResourceManagerTest, can_load_urdf_later) +TEST_F(ResourceManagerTest, can_load_and_initialize_components_later) { TestableResourceManager rm; ASSERT_FALSE(rm.is_urdf_already_loaded()); - rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf); + rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf); ASSERT_TRUE(rm.is_urdf_already_loaded()); } diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index a2f6195c67..d824a989dc 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -258,6 +258,7 @@ const auto hardware_resources_missing_state_keys = +