Skip to content

Commit d687860

Browse files
saikishormergify[bot]
saikishor
authored and
mergify[bot]
committed
Fail early for the resource conflicts between the activating controllers (#2760)
(cherry picked from commit f8903b3)
1 parent 4d0ea5f commit d687860

File tree

4 files changed

+188
-8
lines changed

4 files changed

+188
-8
lines changed

‎controller_manager/include/controller_manager/controller_manager.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -458,12 +458,13 @@ class ControllerManager : public rclcpp::Node
458458
*
459459
* \param[in] controllers list with controllers.
460460
* \param[in] activation_list list with controllers to activate.
461+
* \param[in] deactivation_list list with controllers to deactivate.
461462
* \param[out] message describing the result of the check.
462463
* \return return_type::OK if all interfaces are available, otherwise return_type::ERROR.
463464
*/
464465
controller_interface::return_type check_for_interfaces_availability_to_activate(
465466
const std::vector<ControllerSpec> & controllers, const std::vector<std::string> activation_list,
466-
std::string & message);
467+
const std::vector<std::string> deactivation_list, std::string & message);
467468

468469
/**
469470
* @brief Inserts a controller into an ordered list based on dependencies to compute the

‎controller_manager/src/controller_manager.cpp‎

Lines changed: 55 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1980,7 +1980,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
19801980

19811981
if (
19821982
check_for_interfaces_availability_to_activate(
1983-
controllers, switch_params_.activate_request, message) !=
1983+
controllers, switch_params_.activate_request, switch_params_.deactivate_request, message) !=
19841984
controller_interface::return_type::OK)
19851985
{
19861986
clear_requests();
@@ -2456,7 +2456,7 @@ void ControllerManager::activate_controllers(
24562456
}
24572457
// Now prepare and perform the stop interface switching as this is needed for exclusive
24582458
// interfaces
2459-
if (
2459+
else if (
24602460
!failed_controllers_command_interfaces.empty() &&
24612461
(!resource_manager_->prepare_command_mode_switch({}, failed_controllers_command_interfaces) ||
24622462
!resource_manager_->perform_command_mode_switch({}, failed_controllers_command_interfaces)))
@@ -2963,6 +2963,8 @@ void ControllerManager::manage_switch()
29632963
switch_params_.deactivate_command_interface_request))
29642964
{
29652965
RCLCPP_ERROR(get_logger(), "Error while performing mode switch.");
2966+
// If the hardware switching fails, there is no point in continuing to switch controllers
2967+
return;
29662968
}
29672969
execution_time_.switch_perform_mode_time =
29682970
std::chrono::duration<double, std::micro>(std::chrono::steady_clock::now() - start_time)
@@ -3926,10 +3928,37 @@ void ControllerManager::publish_activity()
39263928

39273929
controller_interface::return_type ControllerManager::check_for_interfaces_availability_to_activate(
39283930
const std::vector<ControllerSpec> & controllers, const std::vector<std::string> activation_list,
3929-
std::string & message)
3931+
const std::vector<std::string> deactivation_list, std::string & message)
39303932
{
3933+
std::vector<std::string> future_unavailable_cmd_interfaces = {};
3934+
std::vector<std::string> future_available_cmd_interfaces = {};
3935+
for (const auto & controller_name : deactivation_list)
3936+
{
3937+
auto controller_it = std::find_if(
3938+
controllers.begin(), controllers.end(),
3939+
std::bind(controller_name_compare, std::placeholders::_1, controller_name));
3940+
if (controller_it == controllers.end())
3941+
{
3942+
message = fmt::format(
3943+
FMT_COMPILE("Unable to find the deactivation controller : '{}' within the controller list"),
3944+
controller_name);
3945+
RCLCPP_ERROR(get_logger(), "%s", message.c_str());
3946+
return controller_interface::return_type::ERROR;
3947+
}
3948+
const auto controller_cmd_interfaces =
3949+
controller_it->c->command_interface_configuration().names;
3950+
for (const auto & cmd_itf : controller_cmd_interfaces)
3951+
{
3952+
future_available_cmd_interfaces.push_back(cmd_itf);
3953+
}
3954+
}
39313955
for (const auto & controller_name : activation_list)
39323956
{
3957+
if (ros2_control::has_item(deactivation_list, controller_name))
3958+
{
3959+
// skip controllers that are being deactivated and activated in the same request
3960+
continue;
3961+
}
39333962
auto controller_it = std::find_if(
39343963
controllers.begin(), controllers.end(),
39353964
std::bind(controller_name_compare, std::placeholders::_1, controller_name));
@@ -3959,6 +3988,29 @@ controller_interface::return_type ControllerManager::check_for_interfaces_availa
39593988
RCLCPP_WARN(get_logger(), "%s", message.c_str());
39603989
return controller_interface::return_type::ERROR;
39613990
}
3991+
if (
3992+
resource_manager_->command_interface_is_claimed(cmd_itf) &&
3993+
!ros2_control::has_item(future_available_cmd_interfaces, cmd_itf))
3994+
{
3995+
message = fmt::format(
3996+
FMT_COMPILE(
3997+
"Unable to activate controller '{}' since the "
3998+
"command interface '{}' is currently claimed by another controller."),
3999+
controller_it->info.name, cmd_itf);
4000+
RCLCPP_WARN(get_logger(), "%s", message.c_str());
4001+
return controller_interface::return_type::ERROR;
4002+
}
4003+
if (ros2_control::has_item(future_unavailable_cmd_interfaces, cmd_itf))
4004+
{
4005+
message = fmt::format(
4006+
FMT_COMPILE(
4007+
"Unable to activate controller '{}' since the "
4008+
"command interface '{}' will be used by another controller that is being activated."),
4009+
controller_it->info.name, cmd_itf);
4010+
RCLCPP_WARN(get_logger(), "%s", message.c_str());
4011+
return controller_interface::return_type::ERROR;
4012+
}
4013+
future_unavailable_cmd_interfaces.push_back(cmd_itf);
39624014
}
39634015
for (const auto & state_itf : controller_state_interfaces)
39644016
{

‎controller_manager/test/test_controller_manager_srvs.cpp‎

Lines changed: 127 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2551,3 +2551,130 @@ TEST_F(TestControllerManagerSrvs, switch_controller_controllers_taking_long_time
25512551
EXPECT_EQ(old_counter_2 + 1, test_controller_2->internal_counter);
25522552
EXPECT_EQ(old_counter_3 + 1, test_controller_3->internal_counter);
25532553
}
2554+
2555+
TEST_F(TestControllerManagerSrvs, switch_controller_test_two_controllers_using_same_interface)
2556+
{
2557+
// create server client and request
2558+
rclcpp::executors::SingleThreadedExecutor srv_executor;
2559+
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
2560+
srv_executor.add_node(srv_node);
2561+
rclcpp::Client<ListControllers>::SharedPtr client =
2562+
srv_node->create_client<ListControllers>("test_controller_manager/list_controllers");
2563+
auto request = std::make_shared<ListControllers::Request>();
2564+
2565+
// create set of controllers
2566+
static constexpr char TEST_CONTROLLER_1[] = "test_controller_1";
2567+
static constexpr char TEST_CONTROLLER_2[] = "test_controller_2";
2568+
2569+
// create non-chained controller
2570+
auto test_controller_1 = std::make_shared<TestController>();
2571+
controller_interface::InterfaceConfiguration cmd_cfg = {
2572+
controller_interface::interface_configuration_type::INDIVIDUAL,
2573+
{"joint1/position", "joint1/max_velocity"}};
2574+
controller_interface::InterfaceConfiguration state_cfg = {
2575+
controller_interface::interface_configuration_type::INDIVIDUAL,
2576+
{"joint1/position", "joint1/velocity"}};
2577+
test_controller_1->set_command_interface_configuration(cmd_cfg);
2578+
test_controller_1->set_state_interface_configuration(state_cfg);
2579+
2580+
auto test_controller_2 = std::make_shared<TestController>();
2581+
cmd_cfg = {
2582+
controller_interface::interface_configuration_type::INDIVIDUAL,
2583+
{"joint1/position", "joint2/velocity"}};
2584+
state_cfg = {
2585+
controller_interface::interface_configuration_type::INDIVIDUAL,
2586+
{"joint1/position", "joint2/velocity"}};
2587+
test_controller_2->set_command_interface_configuration(cmd_cfg);
2588+
test_controller_2->set_state_interface_configuration(state_cfg);
2589+
2590+
// add controllers
2591+
cm_->add_controller(
2592+
test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME);
2593+
cm_->add_controller(
2594+
test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME);
2595+
2596+
// get controller list before configure
2597+
auto result = call_service_and_wait(*client, request, srv_executor);
2598+
// check chainable controller
2599+
ASSERT_EQ(2u, result->controller.size());
2600+
ASSERT_EQ(result->controller[0].name, TEST_CONTROLLER_1);
2601+
ASSERT_EQ(result->controller[1].name, TEST_CONTROLLER_2);
2602+
2603+
// configure controllers
2604+
for (const auto & controller : {TEST_CONTROLLER_1, TEST_CONTROLLER_2})
2605+
{
2606+
cm_->configure_controller(controller);
2607+
}
2608+
2609+
// get controller list after configure
2610+
result = call_service_and_wait(*client, request, srv_executor);
2611+
ASSERT_EQ(2u, result->controller.size());
2612+
2613+
// reordered controllers
2614+
ASSERT_EQ(result->controller[0].name, TEST_CONTROLLER_2);
2615+
ASSERT_EQ(result->controller[1].name, TEST_CONTROLLER_1);
2616+
2617+
// Check individual activation and they should work fine
2618+
auto res = cm_->switch_controller(
2619+
{TEST_CONTROLLER_1}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2620+
rclcpp::Duration(0, 0));
2621+
ASSERT_EQ(res, controller_interface::return_type::OK);
2622+
2623+
ASSERT_EQ(
2624+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2625+
test_controller_1->get_lifecycle_state().id());
2626+
ASSERT_EQ(
2627+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2628+
test_controller_2->get_lifecycle_state().id());
2629+
2630+
res = cm_->switch_controller(
2631+
{}, {TEST_CONTROLLER_1}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2632+
rclcpp::Duration(0, 0));
2633+
ASSERT_EQ(res, controller_interface::return_type::OK);
2634+
2635+
ASSERT_EQ(
2636+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2637+
test_controller_1->get_lifecycle_state().id());
2638+
ASSERT_EQ(
2639+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2640+
test_controller_2->get_lifecycle_state().id());
2641+
2642+
// Now test activating controller_2
2643+
res = cm_->switch_controller(
2644+
{TEST_CONTROLLER_2}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2645+
rclcpp::Duration(0, 0));
2646+
ASSERT_EQ(res, controller_interface::return_type::OK);
2647+
2648+
ASSERT_EQ(
2649+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2650+
test_controller_1->get_lifecycle_state().id());
2651+
ASSERT_EQ(
2652+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2653+
test_controller_2->get_lifecycle_state().id());
2654+
2655+
res = cm_->switch_controller(
2656+
{}, {TEST_CONTROLLER_2}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2657+
rclcpp::Duration(0, 0));
2658+
ASSERT_EQ(res, controller_interface::return_type::OK);
2659+
2660+
ASSERT_EQ(
2661+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2662+
test_controller_1->get_lifecycle_state().id());
2663+
ASSERT_EQ(
2664+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2665+
test_controller_2->get_lifecycle_state().id());
2666+
2667+
// Now try activating both the controllers at once, it should fail as they are using same
2668+
// interface
2669+
res = cm_->switch_controller(
2670+
{TEST_CONTROLLER_1, TEST_CONTROLLER_2}, {},
2671+
controller_manager_msgs::srv::SwitchController::Request::STRICT, false, rclcpp::Duration(0, 0));
2672+
ASSERT_EQ(res, controller_interface::return_type::ERROR);
2673+
2674+
ASSERT_EQ(
2675+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2676+
test_controller_1->get_lifecycle_state().id());
2677+
ASSERT_EQ(
2678+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2679+
test_controller_2->get_lifecycle_state().id());
2680+
}

‎controller_manager/test/test_release_interfaces.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -81,7 +81,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface)
8181
auto switch_future = std::async(
8282
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
8383
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));
84-
ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
84+
ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
8585
<< "switch_controller should be blocking until next update cycle";
8686
ControllerManagerRunner cm_runner(this);
8787
EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());
@@ -185,7 +185,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface)
185185
auto switch_future = std::async(
186186
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
187187
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));
188-
ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
188+
ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
189189
<< "switch_controller should be blocking until next update cycle";
190190
ControllerManagerRunner cm_runner(this);
191191
EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());
@@ -206,12 +206,12 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface)
206206
auto switch_future = std::async(
207207
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
208208
start_controllers, stop_controllers, BEST_EFFORT, true, rclcpp::Duration(0, 0));
209-
ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
209+
ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
210210
<< "switch_controller should be blocking until next update cycle";
211211
ControllerManagerRunner cm_runner(this);
212212
EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());
213213
ASSERT_EQ(
214-
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
214+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
215215
abstract_test_controller1.c->get_lifecycle_state().id());
216216
ASSERT_EQ(
217217
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,

0 commit comments

Comments
 (0)