Skip to content

Commit 00e2b8b

Browse files
mergify[bot]mergify[bot]
authored
Avoid deadlocks for failed command switching (#2774) (#2797)
1 parent 691dea7 commit 00e2b8b

File tree

4 files changed

+160
-0
lines changed

4 files changed

+160
-0
lines changed

‎controller_manager/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -161,6 +161,7 @@ if(BUILD_TESTING)
161161
${controller_manager_msgs_TARGETS}
162162
)
163163
set_tests_properties(test_controller_manager_srvs PROPERTIES TIMEOUT 120)
164+
164165
ament_add_gmock(test_controller_manager_urdf_passing
165166
test/test_controller_manager_urdf_passing.cpp
166167
)

‎controller_manager/src/controller_manager.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2964,6 +2964,8 @@ void ControllerManager::manage_switch()
29642964
{
29652965
RCLCPP_ERROR(get_logger(), "Error while performing mode switch.");
29662966
// If the hardware switching fails, there is no point in continuing to switch controllers
2967+
switch_params_.do_switch = false;
2968+
switch_params_.cv.notify_all();
29672969
return;
29682970
}
29692971
execution_time_.switch_perform_mode_time =

‎controller_manager/test/test_controller_manager_srvs.cpp‎

Lines changed: 149 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414

1515
#include <memory>
1616
#include <random>
17+
#include <regex>
1718
#include <string>
1819
#include <vector>
1920

@@ -2678,3 +2679,151 @@ TEST_F(TestControllerManagerSrvs, switch_controller_test_two_controllers_using_s
26782679
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
26792680
test_controller_2->get_lifecycle_state().id());
26802681
}
2682+
2683+
class TestControllerManagerSrvsWithFailingPerformSwitch : public TestControllerManagerSrvs
2684+
{
2685+
void SetUp()
2686+
{
2687+
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
2688+
const std::string robot_description = ros2_control_test_assets::minimal_robot_urdf;
2689+
const std::regex plugin_regex(R"((<plugin>.*?</plugin>\s*))");
2690+
const std::string replacement = "$1<param name=\"fail_on_perform_mode_switch\">true</param>\n";
2691+
const std::string result = std::regex_replace(robot_description, plugin_regex, replacement);
2692+
2693+
cm_ = std::make_shared<controller_manager::ControllerManager>(
2694+
std::make_unique<hardware_interface::ResourceManager>(
2695+
result, rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface(), true),
2696+
executor_, TEST_CM_NAME);
2697+
run_updater_ = false;
2698+
time_ = rclcpp::Time(0, 0, cm_->get_trigger_clock()->get_clock_type());
2699+
SetUpSrvsCMExecutor();
2700+
}
2701+
};
2702+
2703+
TEST_F(
2704+
TestControllerManagerSrvsWithFailingPerformSwitch, switch_controller_with_failing_perform_switch)
2705+
{
2706+
// create server client and request
2707+
rclcpp::executors::SingleThreadedExecutor srv_executor;
2708+
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
2709+
srv_executor.add_node(srv_node);
2710+
rclcpp::Client<ListControllers>::SharedPtr client =
2711+
srv_node->create_client<ListControllers>("test_controller_manager/list_controllers");
2712+
auto request = std::make_shared<ListControllers::Request>();
2713+
2714+
// create set of controllers
2715+
static constexpr char TEST_CONTROLLER_1[] = "test_controller_1";
2716+
static constexpr char TEST_CONTROLLER_2[] = "test_controller_2";
2717+
2718+
// create non-chained controller
2719+
auto test_controller_1 = std::make_shared<TestController>();
2720+
controller_interface::InterfaceConfiguration cmd_cfg = {
2721+
controller_interface::interface_configuration_type::INDIVIDUAL,
2722+
{"joint1/position", "joint1/max_velocity"}};
2723+
controller_interface::InterfaceConfiguration state_cfg = {
2724+
controller_interface::interface_configuration_type::INDIVIDUAL,
2725+
{"joint1/position", "joint1/velocity"}};
2726+
test_controller_1->set_command_interface_configuration(cmd_cfg);
2727+
test_controller_1->set_state_interface_configuration(state_cfg);
2728+
2729+
auto test_controller_2 = std::make_shared<TestController>();
2730+
cmd_cfg = {
2731+
controller_interface::interface_configuration_type::INDIVIDUAL,
2732+
{"joint2/max_acceleration", "joint2/velocity"}};
2733+
state_cfg = {
2734+
controller_interface::interface_configuration_type::INDIVIDUAL,
2735+
{"joint2/position", "joint2/velocity"}};
2736+
test_controller_2->set_command_interface_configuration(cmd_cfg);
2737+
test_controller_2->set_state_interface_configuration(state_cfg);
2738+
2739+
// add controllers
2740+
cm_->add_controller(
2741+
test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME);
2742+
cm_->add_controller(
2743+
test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME);
2744+
2745+
// get controller list before configure
2746+
auto result = call_service_and_wait(*client, request, srv_executor);
2747+
// check chainable controller
2748+
ASSERT_EQ(2u, result->controller.size());
2749+
ASSERT_EQ(result->controller[0].name, TEST_CONTROLLER_1);
2750+
ASSERT_EQ(result->controller[1].name, TEST_CONTROLLER_2);
2751+
2752+
// configure controllers
2753+
for (const auto & controller : {TEST_CONTROLLER_1, TEST_CONTROLLER_2})
2754+
{
2755+
cm_->configure_controller(controller);
2756+
}
2757+
2758+
// get controller list after configure
2759+
result = call_service_and_wait(*client, request, srv_executor);
2760+
ASSERT_EQ(2u, result->controller.size());
2761+
2762+
// reordered controllers
2763+
ASSERT_EQ(result->controller[0].name, TEST_CONTROLLER_2);
2764+
ASSERT_EQ(result->controller[1].name, TEST_CONTROLLER_1);
2765+
2766+
// Check individual activation
2767+
auto res = cm_->switch_controller(
2768+
{TEST_CONTROLLER_1}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2769+
rclcpp::Duration(100, 0));
2770+
ASSERT_EQ(res, controller_interface::return_type::ERROR);
2771+
2772+
ASSERT_EQ(
2773+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2774+
test_controller_1->get_lifecycle_state().id());
2775+
ASSERT_EQ(
2776+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2777+
test_controller_2->get_lifecycle_state().id());
2778+
2779+
res = cm_->switch_controller(
2780+
{TEST_CONTROLLER_1}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, false,
2781+
rclcpp::Duration(0, 0));
2782+
ASSERT_EQ(res, controller_interface::return_type::ERROR);
2783+
2784+
ASSERT_EQ(
2785+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2786+
test_controller_1->get_lifecycle_state().id());
2787+
ASSERT_EQ(
2788+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2789+
test_controller_2->get_lifecycle_state().id());
2790+
2791+
// Now test activating controller_2
2792+
res = cm_->switch_controller(
2793+
{TEST_CONTROLLER_2}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2794+
rclcpp::Duration(0, 0));
2795+
ASSERT_EQ(res, controller_interface::return_type::OK);
2796+
2797+
ASSERT_EQ(
2798+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2799+
test_controller_1->get_lifecycle_state().id());
2800+
ASSERT_EQ(
2801+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2802+
test_controller_2->get_lifecycle_state().id());
2803+
2804+
res = cm_->switch_controller(
2805+
{}, {TEST_CONTROLLER_2}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2806+
rclcpp::Duration(0, 0));
2807+
ASSERT_EQ(res, controller_interface::return_type::OK);
2808+
2809+
ASSERT_EQ(
2810+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2811+
test_controller_1->get_lifecycle_state().id());
2812+
ASSERT_EQ(
2813+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2814+
test_controller_2->get_lifecycle_state().id());
2815+
2816+
// Now try activating both the controllers at once, it should fail as the perform switch fails for
2817+
// one of the controller
2818+
res = cm_->switch_controller(
2819+
{TEST_CONTROLLER_1, TEST_CONTROLLER_2}, {},
2820+
controller_manager_msgs::srv::SwitchController::Request::STRICT, false, rclcpp::Duration(0, 0));
2821+
ASSERT_EQ(res, controller_interface::return_type::ERROR);
2822+
2823+
ASSERT_EQ(
2824+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2825+
test_controller_1->get_lifecycle_state().id());
2826+
ASSERT_EQ(
2827+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2828+
test_controller_2->get_lifecycle_state().id());
2829+
}

‎hardware_interface_testing/test/test_components/test_actuator.cpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -111,6 +111,14 @@ class TestActuator : public ActuatorInterface
111111
const std::vector<std::string> & /*start_interfaces*/,
112112
const std::vector<std::string> & /*stop_interfaces*/) override
113113
{
114+
if (get_hardware_info().hardware_parameters.count("fail_on_perform_mode_switch"))
115+
{
116+
if (hardware_interface::parse_bool(
117+
get_hardware_info().hardware_parameters.at("fail_on_perform_mode_switch")))
118+
{
119+
return hardware_interface::return_type::ERROR;
120+
}
121+
}
114122
position_state_ += 0.1;
115123
return hardware_interface::return_type::OK;
116124
}

0 commit comments

Comments
 (0)