diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 3dabed59cc..ca0068ad88 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -1,8 +1,6 @@ name: Coverage Build on: workflow_dispatch: - branches: - - master push: branches: - master @@ -31,11 +29,7 @@ jobs: package-name: controller_interface controller_manager - controller_manager_msgs hardware_interface - ros2controlcli - ros2_control - ros2_control_test_assets transmission_interface vcs-repo-file-url: | diff --git a/README.md b/README.md index b68c11705c..0231a1c0e7 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,7 @@ # ros2_control [![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) +[![codecov](https://codecov.io/gh/ros-controls/ros2_control/graph/badge.svg?token=idvm1zJXOf)](https://codecov.io/gh/ros-controls/ros2_control) This package is a part of the ros2_control framework. For more, please check the [documentation](https://control.ros.org/). diff --git a/codecov.yml b/codecov.yml index 0db154faa5..d8a5fde3e0 100644 --- a/codecov.yml +++ b/codecov.yml @@ -14,12 +14,12 @@ fixes: comment: layout: "diff, flags, files" behavior: default +ignore: + - "**/test" flags: unittests: paths: - controller_interface - controller_manager - hardware_interface - - joint_limits_interface - - test_robot_hardware - transmission_interface diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a48069acfb..28c8c8d80a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -148,9 +148,6 @@ std::vector get_following_controller_names( return following_controllers; } - // If the controller is not configured, return empty - if (!(is_controller_active(controller_it->c) || is_controller_inactive(controller_it->c))) - return following_controllers; const auto cmd_itfs = controller_it->c->command_interface_configuration().names; for (const auto & itf : cmd_itfs) { @@ -209,13 +206,11 @@ std::vector get_preceding_controller_names( } for (const auto & ctrl : controllers) { - // If the controller is not configured, return empty - if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c))) - return preceding_controllers; auto cmd_itfs = ctrl.c->command_interface_configuration().names; for (const auto & itf : cmd_itfs) { - if (itf.find(controller_name) != std::string::npos) + auto split_pos = itf.find_first_of('/'); + if ((split_pos != std::string::npos) && (itf.substr(0, split_pos) == controller_name)) { preceding_controllers.push_back(ctrl.info.name); auto ctrl_names = get_preceding_controller_names(ctrl.info.name, controllers); @@ -768,10 +763,35 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } resource_manager_->import_controller_reference_interfaces(controller_name, interfaces); + } + + // Now let's reorder the controllers + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); + const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); + + // Copy all controllers from the 'from' list to the 'to' list + to = from; + + // Reordering the controllers + std::sort( + to.begin(), to.end(), + std::bind( + &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2, + to)); - // TODO(destogl): check and resort controllers in the vector + RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:"); + for (const auto & ctrl : to) + { + RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.info.name.c_str()); } + // switch lists + rt_controllers_wrapper_.switch_updated_list(guard); + // clear unused list + rt_controllers_wrapper_.get_unused_list(guard).clear(); + return controller_interface::return_type::OK; } @@ -1233,19 +1253,6 @@ controller_interface::return_type ControllerManager::switch_controller( } } - // Reordering the controllers - std::sort( - to.begin(), to.end(), - std::bind( - &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2, - to)); - - RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:"); - for (const auto & ctrl : to) - { - RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.info.name.c_str()); - } - // switch lists rt_controllers_wrapper_.switch_updated_list(guard); // clear unused list @@ -2435,13 +2442,6 @@ bool ControllerManager::controller_sorting( const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b, const std::vector & controllers) { - // If the controllers are not active, then should be at the end of the list - if (!is_controller_active(ctrl_a.c) || !is_controller_active(ctrl_b.c)) - { - if (is_controller_active(ctrl_a.c)) return true; - return false; - } - const std::vector cmd_itfs = ctrl_a.c->command_interface_configuration().names; const std::vector state_itfs = ctrl_a.c->state_interface_configuration().names; if (cmd_itfs.empty() || !ctrl_a.c->is_chainable()) @@ -2513,7 +2513,11 @@ bool ControllerManager::controller_sorting( // TODO(saikishor): deal with the state interface chaining in the sorting algorithm auto state_it = std::find_if( state_itfs.begin(), state_itfs.end(), - [ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); }); + [ctrl_b](auto itf) + { + auto index = itf.find_first_of('/'); + return ((index != std::string::npos) && (itf.substr(0, index) == ctrl_b.info.name)); + }); if (state_it != state_itfs.end()) { return false; diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index d21957a0b4..b02c2e65ef 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -32,33 +32,13 @@ TestChainableController::TestChainableController() controller_interface::InterfaceConfiguration TestChainableController::command_interface_configuration() const { - if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - return cmd_iface_cfg_; - } - else - { - throw std::runtime_error( - "Can not get command interface configuration until the controller is configured."); - } + return cmd_iface_cfg_; } controller_interface::InterfaceConfiguration TestChainableController::state_interface_configuration() const { - if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - return state_iface_cfg_; - } - else - { - throw std::runtime_error( - "Can not get state interface configuration until the controller is configured."); - } + return state_iface_cfg_; } controller_interface::return_type TestChainableController::update_reference_from_subscribers( diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 06f76bd044..97670e9a7a 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -31,32 +31,12 @@ TestController::TestController() controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const { - if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - return cmd_iface_cfg_; - } - else - { - throw std::runtime_error( - "Can not get command interface configuration until the controller is configured."); - } + return cmd_iface_cfg_; } controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const { - if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - return state_iface_cfg_; - } - else - { - throw std::runtime_error( - "Can not get state interface configuration until the controller is configured."); - } + return state_iface_cfg_; } controller_interface::return_type TestController::update( diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index e317726585..eb001ce70f 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -104,8 +104,11 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); - cm_->configure_controller(TEST_CONTROLLER2_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + cm_->configure_controller(TEST_CONTROLLER2_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -217,7 +220,10 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) test_controller->get_node()->set_parameter({"update_rate", 4}); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -296,7 +302,10 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd rclcpp::Parameter update_rate_parameter("update_rate", static_cast(ctrl_update_rate)); test_controller->get_node()->set_parameter(update_rate_parameter); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -389,7 +398,10 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 7873adaacf..6429f82a44 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -261,27 +261,28 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) // get controller list after configure result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(2u, result->controller.size()); + // At this stage, the controllers are already reordered // check chainable controller ASSERT_EQ(result->controller[0].state, "inactive"); ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 0u); - ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 1u); + ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 3u); ASSERT_EQ(result->controller[0].required_state_interfaces.size(), 2u); - ASSERT_EQ(result->controller[0].is_chainable, true); + ASSERT_EQ(result->controller[0].is_chainable, false); ASSERT_EQ(result->controller[0].is_chained, false); - ASSERT_EQ(result->controller[0].reference_interfaces.size(), 2u); - ASSERT_EQ("joint1/position", result->controller[0].reference_interfaces[0]); - ASSERT_EQ("joint1/velocity", result->controller[0].reference_interfaces[1]); + ASSERT_EQ(result->controller[0].reference_interfaces.size(), 0u); + ASSERT_EQ(result->controller[0].chain_connections.size(), 1u); - ASSERT_EQ(result->controller[0].chain_connections.size(), 0u); // check test controller ASSERT_EQ(result->controller[1].state, "inactive"); ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 0u); - ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 3u); + ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 1u); ASSERT_EQ(result->controller[1].required_state_interfaces.size(), 2u); - ASSERT_EQ(result->controller[1].is_chainable, false); + ASSERT_EQ(result->controller[1].is_chainable, true); ASSERT_EQ(result->controller[1].is_chained, false); - ASSERT_EQ(result->controller[1].reference_interfaces.size(), 0u); - ASSERT_EQ(result->controller[1].chain_connections.size(), 1u); + ASSERT_EQ(result->controller[1].reference_interfaces.size(), 2u); + ASSERT_EQ(result->controller[1].chain_connections.size(), 0u); + ASSERT_EQ("joint1/position", result->controller[1].reference_interfaces[0]); + ASSERT_EQ("joint1/velocity", result->controller[1].reference_interfaces[1]); // activate controllers cm_->switch_controller( {test_chainable_controller::TEST_CONTROLLER_NAME}, {}, @@ -603,22 +604,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(6u, result->controller.size()); - // activate controllers - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_1}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2, - TEST_CHAINED_CONTROLLER_4}, - {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, - rclcpp::Duration(0, 0)); - cm_->switch_controller( - {test_controller::TEST_CONTROLLER_NAME}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - // get controller list after activate - result = call_service_and_wait(*client, request, srv_executor); - - ASSERT_EQ(6u, result->controller.size()); // reordered controllers ASSERT_EQ(result->controller[0].name, "test_controller_name"); ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_5); @@ -776,25 +761,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(8u, result->controller.size()); - // activate controllers - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_1}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_3}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2, - TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_7}, - {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, - rclcpp::Duration(0, 0)); - cm_->switch_controller( - {test_controller::TEST_CONTROLLER_NAME}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - // get controller list after activate - result = call_service_and_wait(*client, request, srv_executor); - - ASSERT_EQ(8u, result->controller.size()); // reordered controllers ASSERT_EQ(result->controller[0].name, "test_controller_name"); ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_7); @@ -1009,29 +975,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(10u, result->controller.size()); - // activate controllers - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_1}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_4}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_7}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2, - TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_8}, - {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, - rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CONTROLLER_1, TEST_CONTROLLER_2}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - // get controller list after activate - result = call_service_and_wait(*client, request, srv_executor); - - ASSERT_EQ(10u, result->controller.size()); - auto get_ctrl_pos = [result](const std::string & controller_name) -> int { auto it = std::find_if( diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index ca2f7f6b1c..390a7365f0 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -162,7 +162,10 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); // Activate configured controller - cm_->configure_controller(controller_name1); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(controller_name1); + } start_test_controller(test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); } @@ -246,7 +249,10 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure test_controller->cleanup_calls = &cleanup_calls; // Configure from inactive state test_controller->simulate_cleanup_failure = false; - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + } ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); EXPECT_EQ(1u, cleanup_calls); } @@ -421,7 +427,10 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); - cm_->configure_controller(controller_name2); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(controller_name2); + } // Stop controller 1 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1"); diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 9d2351db36..7b62af105b 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -362,6 +362,11 @@ return_type GenericSystem::prepare_command_mode_switch( { hardware_interface::return_type ret_val = hardware_interface::return_type::OK; + if (!calculate_dynamics_) + { + return ret_val; + } + const size_t FOUND_ONCE_FLAG = 1000000; std::vector joint_found_in_x_requests_; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 36ace2b78f..5c5d5af3e6 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -767,8 +767,7 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac { if (individual_hardware_info.type == actuator_type) { - std::lock_guard guard(resource_interfaces_lock_); - std::lock_guard guard_claimed(claimed_command_interfaces_lock_); + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); resource_storage_->load_and_initialize_actuator(individual_hardware_info); } if (individual_hardware_info.type == sensor_type) @@ -778,8 +777,7 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac } if (individual_hardware_info.type == system_type) { - std::lock_guard guard(resource_interfaces_lock_); - std::lock_guard guard_claimed(claimed_command_interfaces_lock_); + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); resource_storage_->load_and_initialize_system(individual_hardware_info); } } @@ -843,8 +841,7 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con void ResourceManager::import_controller_reference_interfaces( const std::string & controller_name, std::vector & interfaces) { - std::lock_guard guard(resource_interfaces_lock_); - std::lock_guard guard_claimed(claimed_command_interfaces_lock_); + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); auto interface_names = resource_storage_->add_command_interfaces(interfaces); resource_storage_->controllers_reference_interfaces_map_[controller_name] = interface_names; } @@ -898,8 +895,7 @@ void ResourceManager::remove_controller_reference_interfaces(const std::string & resource_storage_->controllers_reference_interfaces_map_.at(controller_name); resource_storage_->controllers_reference_interfaces_map_.erase(controller_name); - std::lock_guard guard(resource_interfaces_lock_); - std::lock_guard guard_claimed(claimed_command_interfaces_lock_); + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); resource_storage_->remove_command_interfaces(interface_names); } diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 1aec4a6074..57b5796d0d 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -526,6 +526,46 @@ class TestGenericSystem : public ::testing::Test )"; + valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_ = + R"( + + + mock_components/GenericSystem + true + + + + + 3.45 + + + + + + + + 2.78 + + + + + + + + + 2.78 + + + + + + + + + + +)"; + disabled_commands_ = R"( @@ -562,6 +602,7 @@ class TestGenericSystem : public ::testing::Test std::string sensor_with_initial_value_; std::string gpio_with_initial_value_; std::string hardware_system_2dof_standard_interfaces_with_different_control_modes_; + std::string valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_; std::string disabled_commands_; }; @@ -1879,3 +1920,43 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) ASSERT_EQ(0.0, j1v_s.get_value()); ASSERT_EQ(0.11, j1p_c.get_value()); } + +TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tags) +{ + auto check_prepare_command_mode_switch = [&](const std::string & urdf) + { + TestableResourceManager rm( + ros2_control_test_assets::urdf_head + urdf + ros2_control_test_assets::urdf_tail); + auto start_interfaces = rm.command_interface_keys(); + std::vector stop_interfaces; + return rm.prepare_command_mode_switch(start_interfaces, stop_interfaces); + }; + + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_asymetric_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_standard_interfaces_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_other_interface_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_)); + ASSERT_TRUE( + check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_True_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_mimic_joint_)); + ASSERT_TRUE( + check_prepare_command_mode_switch(hardware_system_2dof_standard_interfaces_with_offset_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_)); + ASSERT_TRUE(check_prepare_command_mode_switch(valid_urdf_ros2_control_system_robot_with_gpio_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_)); + ASSERT_TRUE(check_prepare_command_mode_switch(sensor_with_initial_value_)); + ASSERT_TRUE(check_prepare_command_mode_switch(gpio_with_initial_value_)); + ASSERT_FALSE(check_prepare_command_mode_switch( + hardware_system_2dof_standard_interfaces_with_different_control_modes_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_)); + ASSERT_TRUE(check_prepare_command_mode_switch(disabled_commands_)); +}