Skip to content

Commit 24d60ed

Browse files
MarqRazzMarqRazz
authored
Deactivate controllers with command interfaces to hardware on DEACTIVATE (ros-controls#2334)
1 parent 8c52010 commit 24d60ed

File tree

8 files changed

+497
-180
lines changed

8 files changed

+497
-180
lines changed

‎controller_manager/src/controller_manager.cpp‎

Lines changed: 49 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -2669,9 +2669,9 @@ std::vector<std::string> ControllerManager::get_controller_names()
26692669
void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period)
26702670
{
26712671
periodicity_stats_.AddMeasurement(1.0 / period.seconds());
2672-
auto [ok, failed_hardware_names] = resource_manager_->read(time, period);
2672+
auto [result, failed_hardware_names] = resource_manager_->read(time, period);
26732673

2674-
if (!ok)
2674+
if (result != hardware_interface::return_type::OK)
26752675
{
26762676
rt_buffer_.deactivate_controllers_list.clear();
26772677
// Determine controllers to stop
@@ -2938,9 +2938,9 @@ controller_interface::return_type ControllerManager::update(
29382938

29392939
void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration & period)
29402940
{
2941-
auto [ok, failed_hardware_names] = resource_manager_->write(time, period);
2941+
auto [result, failed_hardware_names] = resource_manager_->write(time, period);
29422942

2943-
if (!ok)
2943+
if (result == hardware_interface::return_type::ERROR)
29442944
{
29452945
rt_buffer_.deactivate_controllers_list.clear();
29462946
// Determine controllers to stop
@@ -2968,6 +2968,51 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration
29682968
deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list);
29692969
// TODO(destogl): do auto-start of broadcasters
29702970
}
2971+
else if (result == hardware_interface::return_type::DEACTIVATE)
2972+
{
2973+
rt_buffer_.deactivate_controllers_list.clear();
2974+
auto loaded_controllers = get_loaded_controllers();
2975+
// Only stop controllers with active command interfaces to the failed_hardware_names
2976+
for (const auto & hardware_name : failed_hardware_names)
2977+
{
2978+
auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name);
2979+
for (const auto & controller : controllers)
2980+
{
2981+
auto controller_spec = std::find_if(
2982+
loaded_controllers.begin(), loaded_controllers.end(),
2983+
[&](const controller_manager::ControllerSpec & spec)
2984+
{ return spec.c->get_name() == controller; });
2985+
if (controller_spec == loaded_controllers.end())
2986+
{
2987+
RCLCPP_WARN(
2988+
get_logger(),
2989+
"Deactivate failed to find controller [%s] in loaded controllers. "
2990+
"This can happen due to multiple returns of 'DEACTIVATE' from [%s] write()",
2991+
controller.c_str(), hardware_name.c_str());
2992+
continue;
2993+
}
2994+
std::vector<std::string> command_interface_names;
2995+
extract_command_interfaces_for_controller(
2996+
*controller_spec, resource_manager_, command_interface_names);
2997+
// if this controller has command interfaces add it to the deactivate_controllers_list
2998+
if (!command_interface_names.empty())
2999+
{
3000+
rt_buffer_.deactivate_controllers_list.push_back(controller);
3001+
}
3002+
}
3003+
}
3004+
RCLCPP_ERROR_EXPRESSION(
3005+
get_logger(), !rt_buffer_.deactivate_controllers_list.empty(),
3006+
"Deactivating controllers [%s] as their command interfaces are tied to DEACTIVATEing "
3007+
"hardware components",
3008+
rt_buffer_.get_concatenated_string(rt_buffer_.deactivate_controllers_list).c_str());
3009+
std::vector<ControllerSpec> & rt_controller_list =
3010+
rt_controllers_wrapper_.update_and_get_used_by_rt_list();
3011+
3012+
perform_hardware_command_mode_change(
3013+
rt_controller_list, {}, rt_buffer_.deactivate_controllers_list, "write");
3014+
deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list);
3015+
}
29713016
}
29723017

29733018
std::vector<ControllerSpec> &

‎controller_manager/test/test_controller_manager_hardware_error_handling.cpp‎

Lines changed: 293 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@ class TestableControllerManager : public controller_manager::ControllerManager
4747
FRIEND_TEST(TestControllerManagerWithTestableCM, check_cached_controllers_for_hardware);
4848
FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_error);
4949
FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_error);
50+
FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_deactivate);
5051
FRIEND_TEST(TestControllerManagerWithTestableCM, stop_controllers_on_multiple_hardware_error);
5152

5253
public:
@@ -428,6 +429,95 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er
428429
EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher);
429430
EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher);
430431
}
432+
433+
// Simulate deactivate in read() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME
434+
// by setting first command interface to READ_DEACTIVATE_VALUE
435+
// (this should be the same as returning ERROR)
436+
test_controller_actuator->set_first_command_interface_value_to =
437+
test_constants::READ_DEACTIVATE_VALUE;
438+
test_controller_system->set_first_command_interface_value_to =
439+
test_constants::READ_DEACTIVATE_VALUE;
440+
{
441+
auto previous_counter_lower = test_controller_actuator->internal_counter + 1;
442+
auto previous_counter_higher = test_controller_system->internal_counter + 1;
443+
auto new_counter = test_broadcaster_sensor->internal_counter + 1;
444+
445+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
446+
447+
EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower)
448+
<< "Execute without errors to write value";
449+
EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher)
450+
<< "Execute without errors to write value";
451+
EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower)
452+
<< "Execute without errors to write value";
453+
EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
454+
<< "Execute without errors to write value";
455+
}
456+
457+
{
458+
auto previous_counter_lower = test_controller_actuator->internal_counter;
459+
auto previous_counter_higher = test_controller_system->internal_counter;
460+
auto new_counter = test_broadcaster_sensor->internal_counter + 1;
461+
462+
EXPECT_NO_THROW(cm_->read(time_, PERIOD));
463+
EXPECT_EQ(
464+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
465+
test_controller_actuator->get_lifecycle_state().id());
466+
EXPECT_EQ(
467+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
468+
test_controller_system->get_lifecycle_state().id());
469+
EXPECT_EQ(
470+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
471+
test_broadcaster_all->get_lifecycle_state().id());
472+
EXPECT_EQ(
473+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
474+
test_broadcaster_sensor->get_lifecycle_state().id());
475+
476+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
477+
EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower)
478+
<< "Execute has read error and it is not updated";
479+
EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher)
480+
<< "Execute has read error and it is not updated";
481+
EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_lower)
482+
<< "Broadcaster for all interfaces is not updated";
483+
EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
484+
<< "Execute without errors to write value";
485+
}
486+
487+
// Recover hardware and activate again all controllers
488+
{
489+
ASSERT_EQ(
490+
cm_->resource_manager_->set_component_state(
491+
ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active),
492+
hardware_interface::return_type::OK);
493+
ASSERT_EQ(
494+
cm_->resource_manager_->set_component_state(
495+
ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME, state_active),
496+
hardware_interface::return_type::OK);
497+
auto status_map = cm_->resource_manager_->get_components_status();
498+
ASSERT_EQ(
499+
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
500+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
501+
ASSERT_EQ(
502+
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
503+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
504+
505+
auto previous_counter_lower = test_controller_actuator->internal_counter;
506+
auto previous_counter = test_controller_system->internal_counter;
507+
auto previous_counter_higher = test_broadcaster_sensor->internal_counter;
508+
509+
switch_test_controllers(
510+
{TEST_CONTROLLER_SYSTEM_NAME, TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}, {},
511+
strictness);
512+
513+
EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower);
514+
EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher);
515+
EXPECT_GT(test_controller_system->internal_counter, previous_counter);
516+
EXPECT_LE(test_controller_system->internal_counter, previous_counter_higher);
517+
EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_lower);
518+
EXPECT_LE(test_broadcaster_all->internal_counter, previous_counter_higher);
519+
EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher);
520+
}
431521
}
432522

433523
TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error)
@@ -745,6 +835,209 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e
745835
}
746836
}
747837

838+
TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_deactivate)
839+
{
840+
auto strictness = GetParam().strictness;
841+
SetupAndConfigureControllers(strictness);
842+
843+
rclcpp_lifecycle::State state_active(
844+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
845+
hardware_interface::lifecycle_state_names::ACTIVE);
846+
847+
{
848+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
849+
EXPECT_GE(test_controller_actuator->internal_counter, 1u)
850+
<< "Controller is started at the end of update";
851+
EXPECT_GE(test_controller_system->internal_counter, 1u)
852+
<< "Controller is started at the end of update";
853+
EXPECT_GE(test_broadcaster_all->internal_counter, 1u)
854+
<< "Controller is started at the end of update";
855+
EXPECT_GE(test_broadcaster_sensor->internal_counter, 1u)
856+
<< "Controller is started at the end of update";
857+
}
858+
859+
EXPECT_EQ(
860+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
861+
test_controller_actuator->get_lifecycle_state().id());
862+
EXPECT_EQ(
863+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
864+
test_controller_system->get_lifecycle_state().id());
865+
EXPECT_EQ(
866+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
867+
test_broadcaster_all->get_lifecycle_state().id());
868+
EXPECT_EQ(
869+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
870+
test_broadcaster_sensor->get_lifecycle_state().id());
871+
872+
// Execute first time without any errors
873+
{
874+
auto new_counter = test_controller_actuator->internal_counter + 1;
875+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
876+
EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors";
877+
EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors";
878+
EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors";
879+
EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) << "Execute without errors";
880+
}
881+
882+
// Simulate deactivate in write() on TEST_ACTUATOR_HARDWARE_NAME by setting first command
883+
// interface to WRITE_DEACTIVATE_VALUE
884+
test_controller_actuator->set_first_command_interface_value_to =
885+
test_constants::WRITE_DEACTIVATE_VALUE;
886+
{
887+
auto new_counter = test_controller_actuator->internal_counter + 1;
888+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
889+
EXPECT_EQ(test_controller_actuator->internal_counter, new_counter)
890+
<< "Execute without errors to write value";
891+
EXPECT_EQ(test_controller_system->internal_counter, new_counter)
892+
<< "Execute without errors to write value";
893+
EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter)
894+
<< "Execute without errors to write value";
895+
EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
896+
<< "Execute without errors to write value";
897+
}
898+
899+
{
900+
auto previous_counter = test_controller_actuator->internal_counter;
901+
auto new_counter = test_controller_system->internal_counter + 1;
902+
903+
// here happens deactivate in hardware and
904+
// "actuator controller" is deactivated but "broadcaster all" should stay active
905+
EXPECT_NO_THROW(cm_->write(time_, PERIOD));
906+
EXPECT_EQ(
907+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
908+
test_controller_actuator->get_lifecycle_state().id());
909+
EXPECT_EQ(
910+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
911+
test_controller_system->get_lifecycle_state().id());
912+
EXPECT_EQ(
913+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
914+
test_broadcaster_all->get_lifecycle_state().id());
915+
EXPECT_EQ(
916+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
917+
test_broadcaster_sensor->get_lifecycle_state().id());
918+
919+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
920+
EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter)
921+
<< "Execute without errors to write value";
922+
EXPECT_EQ(test_controller_system->internal_counter, new_counter)
923+
<< "Execute without errors to write value";
924+
EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter)
925+
<< "Execute without errors to write value";
926+
EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
927+
<< "Execute without errors to write value";
928+
}
929+
930+
// Recover hardware and activate again all controllers
931+
{
932+
ASSERT_EQ(
933+
cm_->resource_manager_->set_component_state(
934+
ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active),
935+
hardware_interface::return_type::OK);
936+
auto status_map = cm_->resource_manager_->get_components_status();
937+
ASSERT_EQ(
938+
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
939+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
940+
941+
auto previous_counter_lower = test_controller_actuator->internal_counter;
942+
auto previous_counter_higher = test_controller_system->internal_counter;
943+
944+
switch_test_controllers({TEST_CONTROLLER_ACTUATOR_NAME}, {}, strictness);
945+
946+
EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower);
947+
EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher);
948+
EXPECT_GT(test_controller_system->internal_counter, previous_counter_higher);
949+
EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_higher);
950+
EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher);
951+
}
952+
953+
// Simulate deactivate in write() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME
954+
// by setting first command interface to WRITE_DEACTIVATE_VALUE
955+
test_controller_actuator->set_first_command_interface_value_to =
956+
test_constants::WRITE_DEACTIVATE_VALUE;
957+
test_controller_system->set_first_command_interface_value_to =
958+
test_constants::WRITE_DEACTIVATE_VALUE;
959+
{
960+
auto previous_counter_lower = test_controller_actuator->internal_counter + 1;
961+
auto previous_counter_higher = test_controller_system->internal_counter + 1;
962+
963+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
964+
965+
EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower)
966+
<< "Execute without errors to write value";
967+
EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher)
968+
<< "Execute without errors to write value";
969+
EXPECT_EQ(test_broadcaster_all->internal_counter, previous_counter_higher)
970+
<< "Execute without errors to write value";
971+
EXPECT_EQ(test_broadcaster_sensor->internal_counter, previous_counter_higher)
972+
<< "Execute without errors to write value";
973+
}
974+
975+
{
976+
auto previous_counter_lower = test_controller_actuator->internal_counter;
977+
auto previous_counter_higher = test_controller_system->internal_counter;
978+
auto new_counter = test_broadcaster_sensor->internal_counter + 1;
979+
980+
// here happens deactivate in hardware and
981+
// "actuator controller" is deactivated but "broadcaster's" should stay active
982+
EXPECT_NO_THROW(cm_->write(time_, PERIOD));
983+
EXPECT_EQ(
984+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
985+
test_controller_actuator->get_lifecycle_state().id());
986+
EXPECT_EQ(
987+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
988+
test_controller_system->get_lifecycle_state().id());
989+
EXPECT_EQ(
990+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
991+
test_broadcaster_all->get_lifecycle_state().id());
992+
EXPECT_EQ(
993+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
994+
test_broadcaster_sensor->get_lifecycle_state().id());
995+
996+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
997+
EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower)
998+
<< "Execute has write error and it is not updated";
999+
EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher)
1000+
<< "Execute has write error and it is not updated";
1001+
EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter)
1002+
<< "Broadcaster for all interfaces is not updated";
1003+
EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
1004+
<< "Execute without errors to write value";
1005+
}
1006+
1007+
// Recover hardware and activate again all controllers
1008+
{
1009+
ASSERT_EQ(
1010+
cm_->resource_manager_->set_component_state(
1011+
ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME, state_active),
1012+
hardware_interface::return_type::OK);
1013+
ASSERT_EQ(
1014+
cm_->resource_manager_->set_component_state(
1015+
ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME, state_active),
1016+
hardware_interface::return_type::OK);
1017+
auto status_map = cm_->resource_manager_->get_components_status();
1018+
ASSERT_EQ(
1019+
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
1020+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
1021+
ASSERT_EQ(
1022+
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
1023+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
1024+
1025+
auto previous_counter_lower = test_controller_actuator->internal_counter;
1026+
auto previous_counter = test_controller_system->internal_counter;
1027+
auto previous_counter_higher = test_broadcaster_sensor->internal_counter;
1028+
1029+
switch_test_controllers(
1030+
{TEST_CONTROLLER_SYSTEM_NAME, TEST_CONTROLLER_ACTUATOR_NAME}, {}, strictness);
1031+
1032+
EXPECT_GT(test_controller_actuator->internal_counter, previous_counter_lower);
1033+
EXPECT_LE(test_controller_actuator->internal_counter, previous_counter_higher);
1034+
EXPECT_GT(test_controller_system->internal_counter, previous_counter);
1035+
EXPECT_LE(test_controller_system->internal_counter, previous_counter_higher);
1036+
EXPECT_GT(test_broadcaster_all->internal_counter, previous_counter_higher);
1037+
EXPECT_GT(test_broadcaster_sensor->internal_counter, previous_counter_higher);
1038+
}
1039+
}
1040+
7481041
INSTANTIATE_TEST_SUITE_P(
7491042
test_strict_best_effort, TestControllerManagerWithTestableCM,
7501043
testing::Values(strict, best_effort));

‎hardware_interface/include/hardware_interface/resource_manager.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ class ControllerManager;
4545

4646
struct HardwareReadWriteStatus
4747
{
48-
bool ok;
48+
return_type result;
4949
std::vector<std::string> failed_hardware_names;
5050
};
5151

0 commit comments

Comments
 (0)