diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 47345192a2..9340c61163 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1438,7 +1438,6 @@ void ControllerManager::switch_chained_mode( auto controller = found_it->c; if (!is_controller_active(*controller)) { -<<<<<<< HEAD if (controller->set_chained_mode(to_chained_mode)) { if (to_chained_mode) @@ -1451,9 +1450,7 @@ void ControllerManager::switch_chained_mode( } } else -======= if (!controller->set_chained_mode(to_chained_mode)) ->>>>>>> 1cc73c2 (Fix multiple chainable controller activation bug (#1401)) { RCLCPP_ERROR( get_logger(), diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 503321969c..db31709b9e 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -1520,166 +1520,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_large_chained_controller_tree) } RCLCPP_INFO(srv_node->get_logger(), "Check successful!"); } -<<<<<<< HEAD -======= - -TEST_F(TestControllerManagerSrvs, list_hardware_interfaces_srv) -{ - // create server client and request - rclcpp::executors::SingleThreadedExecutor srv_executor; - rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); - srv_executor.add_node(srv_node); - rclcpp::Client::SharedPtr client = - srv_node->create_client( - "test_controller_manager/list_hardware_interfaces"); - auto request = std::make_shared(); - - // create chained controller - auto test_chained_controller = std::make_shared(); - controller_interface::InterfaceConfiguration chained_cmd_cfg = { - controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}}; - controller_interface::InterfaceConfiguration chained_state_cfg = { - controller_interface::interface_configuration_type::INDIVIDUAL, - {"joint1/position", "joint1/velocity"}}; - test_chained_controller->set_command_interface_configuration(chained_cmd_cfg); - test_chained_controller->set_state_interface_configuration(chained_state_cfg); - test_chained_controller->set_reference_interface_names({"joint1/position", "joint1/velocity"}); - // create non-chained controller - auto test_controller = std::make_shared(); - controller_interface::InterfaceConfiguration cmd_cfg = { - controller_interface::interface_configuration_type::INDIVIDUAL, - {std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position", - std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity", - "joint2/velocity"}}; - controller_interface::InterfaceConfiguration state_cfg = { - controller_interface::interface_configuration_type::INDIVIDUAL, - {"joint1/position", "joint1/velocity"}}; - test_controller->set_command_interface_configuration(cmd_cfg); - test_controller->set_state_interface_configuration(state_cfg); - - // add controllers - cm_->add_controller( - test_chained_controller, test_chainable_controller::TEST_CONTROLLER_NAME, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->add_controller( - test_controller, test_controller::TEST_CONTROLLER_NAME, - test_controller::TEST_CONTROLLER_CLASS_NAME); - // get hardware interface list before configure and loading - auto initial_result = call_service_and_wait(*client, request, srv_executor); - // As there is no controller, so all the interfaces should be available and unclaimed - for (const auto & cmd_itrf : initial_result->command_interfaces) - { - ASSERT_TRUE(cmd_itrf.is_available); - ASSERT_FALSE(cmd_itrf.is_claimed); - } - - // configure controllers - cm_->configure_controller(test_chainable_controller::TEST_CONTROLLER_NAME); - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); - - // The interfaces should be same available and unclaimed until we activate the controllers - auto result = call_service_and_wait(*client, request, srv_executor); - - ASSERT_EQ(2u, result->command_interfaces.size() - initial_result->command_interfaces.size()); - // There will be no increase in state interfaces - ASSERT_EQ(0u, result->state_interfaces.size() - initial_result->state_interfaces.size()); - // As there is no controller, so all the interfaces should be available and unclaimed - for (const auto & cmd_itrf : result->command_interfaces) - { - // The controller command interface shouldn't be active until it's controller is active - if ( - cmd_itrf.name == - std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position" || - cmd_itrf.name == - std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity") - { - ASSERT_FALSE(cmd_itrf.is_available); - } - else - { - ASSERT_TRUE(cmd_itrf.is_available); - } - ASSERT_FALSE(cmd_itrf.is_claimed); - } - auto find_interface_in_list = [](const std::string & interface, auto & hw_interface_info) - { - return std::find_if( - hw_interface_info.begin(), hw_interface_info.end(), - [&](auto info) { return info.name == interface; }) != hw_interface_info.end(); - }; - ASSERT_TRUE(find_interface_in_list( - std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position", - result->command_interfaces)); - ASSERT_TRUE(find_interface_in_list( - std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity", - result->command_interfaces)); - - // activate controllers - cm_->switch_controller( - {test_chainable_controller::TEST_CONTROLLER_NAME}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - - // On activating this chainable controller, we should see that there are 2 more command interfaces - // as this chainable controllers exports 2 reference interfaces - result = call_service_and_wait(*client, request, srv_executor); - - // There will be no increase upon activation - ASSERT_EQ(2u, result->command_interfaces.size() - initial_result->command_interfaces.size()); - ASSERT_EQ(0u, result->state_interfaces.size() - initial_result->state_interfaces.size()); - - for (const auto & cmd_itrf : result->command_interfaces) - { - ASSERT_TRUE(cmd_itrf.is_available); - // This interface is claimed by the chainable controller - if (cmd_itrf.name == "joint1/position") - { - EXPECT_TRUE(cmd_itrf.is_claimed); - } - else if ( - cmd_itrf.name == - std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position" || - cmd_itrf.name == - std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity") - { - // As these interfaces are exposed by the chainable controller and not yet claimed by other - // controllers - ASSERT_FALSE(cmd_itrf.is_claimed); - } - else - { - // Case for rest of the controllers - ASSERT_FALSE(cmd_itrf.is_claimed); - } - } - - cm_->switch_controller( - {test_controller::TEST_CONTROLLER_NAME}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - - // Now as another controller using the interfaces of chainable controller is activated, those - // interfaces should reflect as claimed - result = call_service_and_wait(*client, request, srv_executor); - - for (const auto & cmd_itrf : result->command_interfaces) - { - ASSERT_TRUE(cmd_itrf.is_available); - // This interface is claimed by the chainable controller - if ( - cmd_itrf.name == "joint1/position" || cmd_itrf.name == "joint2/velocity" || - cmd_itrf.name == - std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position" || - cmd_itrf.name == - std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity") - { - ASSERT_TRUE(cmd_itrf.is_claimed); - } - else - { - // Case for rest of the controllers - ASSERT_FALSE(cmd_itrf.is_claimed); - } - } -} TEST_F(TestControllerManagerSrvs, activate_chained_controllers_one_by_one) { @@ -1877,4 +1717,3 @@ TEST_F(TestControllerManagerSrvs, activate_chained_controllers_all_at_once) RCLCPP_ERROR(srv_node->get_logger(), "Check successful!"); } ->>>>>>> 1cc73c2 (Fix multiple chainable controller activation bug (#1401))