Skip to content

Commit cca2070

Browse files
saikishorsaikishor
authored
Deactivate the controllers when they return error similar to hardware (ros-controls#1499)
1 parent 16fbde3 commit cca2070

File tree

4 files changed

+130
-3
lines changed

4 files changed

+130
-3
lines changed

‎controller_manager/doc/userdoc.rst‎

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -225,6 +225,10 @@ Note that not all controllers have to be restarted, e.g., broadcasters.
225225
Restarting hardware
226226
^^^^^^^^^^^^^^^^^^^^^
227227

228-
If hardware gets restarted then you should go through its lifecycle again.
229-
This can be simply achieved by returning ``ERROR`` from ``write`` and ``read`` methods of interface implementation.
230-
**NOT IMPLEMENTED YET - PLEASE STOP/RESTART ALL CONTROLLERS MANUALLY FOR NOW** The controller manager detects that and stops all the controllers that are commanding that hardware and restarts broadcasters that are listening to its states.
228+
If hardware gets restarted then you should go through its lifecycle again in order to reconfigure and export the interfaces
229+
230+
Hardware and Controller Errors
231+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
232+
233+
If the hardware during it's ``read`` or ``write`` method returns ``return_type::ERROR``, the controller manager will stop all controllers that are using the hardware's command and state interfaces.
234+
Likewise, if a controller returns ``return_type::ERROR`` from its ``update`` method, the controller manager will deactivate the respective controller. In future, the controller manager will try to start any fallback controllers if available.

‎controller_manager/src/controller_manager.cpp‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2023,6 +2023,7 @@ controller_interface::return_type ControllerManager::update(
20232023
++update_loop_counter_;
20242024
update_loop_counter_ %= update_rate_;
20252025

2026+
std::vector<std::string> failed_controllers_list;
20262027
for (const auto & loaded_controller : rt_controller_list)
20272028
{
20282029
// TODO(v-lopez) we could cache this information
@@ -2061,11 +2062,25 @@ controller_interface::return_type ControllerManager::update(
20612062

20622063
if (controller_ret != controller_interface::return_type::OK)
20632064
{
2065+
failed_controllers_list.push_back(loaded_controller.info.name);
20642066
ret = controller_ret;
20652067
}
20662068
}
20672069
}
20682070
}
2071+
if (!failed_controllers_list.empty())
2072+
{
2073+
std::string failed_controllers;
2074+
for (const auto & controller : failed_controllers_list)
2075+
{
2076+
failed_controllers += "\n\t- " + controller;
2077+
}
2078+
RCLCPP_ERROR(
2079+
get_logger(), "Deactivating following controllers as their update resulted in an error :%s",
2080+
failed_controllers.c_str());
2081+
2082+
deactivate_controllers(rt_controller_list, failed_controllers_list);
2083+
}
20692084

20702085
// there are controllers to (de)activate
20712086
if (switch_params_.do_switch)

‎controller_manager/test/test_controller/test_controller.cpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,14 @@ controller_interface::return_type TestController::update(
7676
{
7777
for (size_t i = 0; i < command_interfaces_.size(); ++i)
7878
{
79+
if (!std::isfinite(external_commands_for_testing_[i]))
80+
{
81+
RCLCPP_ERROR(
82+
get_node()->get_logger(),
83+
"External command value for command interface '%s' is not finite",
84+
command_interfaces_[i].get_name().c_str());
85+
return controller_interface::return_type::ERROR;
86+
}
7987
RCLCPP_INFO(
8088
get_node()->get_logger(), "Setting value of command interface '%s' to %f",
8189
command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]);

‎controller_manager/test/test_controller_manager_hardware_error_handling.cpp‎

Lines changed: 100 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -405,6 +405,106 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er
405405
}
406406
}
407407

408+
TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error)
409+
{
410+
auto strictness = GetParam().strictness;
411+
SetupAndConfigureControllers(strictness);
412+
413+
rclcpp_lifecycle::State state_active(
414+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
415+
hardware_interface::lifecycle_state_names::ACTIVE);
416+
417+
{
418+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
419+
EXPECT_GE(test_controller_actuator->internal_counter, 1u)
420+
<< "Controller is started at the end of update";
421+
EXPECT_GE(test_controller_system->internal_counter, 1u)
422+
<< "Controller is started at the end of update";
423+
EXPECT_GE(test_broadcaster_all->internal_counter, 1u)
424+
<< "Controller is started at the end of update";
425+
EXPECT_GE(test_broadcaster_sensor->internal_counter, 1u)
426+
<< "Controller is started at the end of update";
427+
}
428+
429+
EXPECT_EQ(
430+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id());
431+
EXPECT_EQ(
432+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id());
433+
EXPECT_EQ(
434+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id());
435+
EXPECT_EQ(
436+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id());
437+
438+
// Execute first time without any errors
439+
{
440+
auto new_counter = test_controller_actuator->internal_counter + 1;
441+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
442+
EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors";
443+
EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors";
444+
EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors";
445+
EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) << "Execute without errors";
446+
}
447+
448+
// Simulate error in update method of the controllers but not in hardware
449+
test_controller_actuator->external_commands_for_testing_[0] =
450+
std::numeric_limits<double>::quiet_NaN();
451+
test_controller_system->external_commands_for_testing_[0] =
452+
std::numeric_limits<double>::quiet_NaN();
453+
{
454+
auto new_counter = test_controller_actuator->internal_counter + 1;
455+
EXPECT_EQ(controller_interface::return_type::ERROR, cm_->update(time_, PERIOD));
456+
EXPECT_EQ(test_controller_actuator->internal_counter, new_counter)
457+
<< "Executes the current cycle and returns ERROR";
458+
EXPECT_EQ(
459+
test_controller_actuator->get_state().id(),
460+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
461+
EXPECT_EQ(test_controller_system->internal_counter, new_counter)
462+
<< "Executes the current cycle and returns ERROR";
463+
EXPECT_EQ(
464+
test_controller_system->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
465+
EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter)
466+
<< "Execute without errors to write value";
467+
EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
468+
<< "Execute without errors to write value";
469+
}
470+
471+
{
472+
auto previous_counter = test_controller_actuator->internal_counter;
473+
auto new_counter = test_controller_system->internal_counter + 1;
474+
475+
EXPECT_EQ(
476+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
477+
test_controller_actuator->get_state().id());
478+
EXPECT_EQ(
479+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id());
480+
EXPECT_EQ(
481+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id());
482+
EXPECT_EQ(
483+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id());
484+
485+
EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
486+
EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter)
487+
<< "Cannot execute as it should be currently deactivated";
488+
EXPECT_EQ(test_controller_system->internal_counter, previous_counter)
489+
<< "Cannot execute as it should be currently deactivated";
490+
EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter)
491+
<< "Broadcaster all interfaces without errors";
492+
EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
493+
<< "Execute without errors to write value";
494+
495+
// The states shouldn't change as there are no more controller errors
496+
EXPECT_EQ(
497+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
498+
test_controller_actuator->get_state().id());
499+
EXPECT_EQ(
500+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id());
501+
EXPECT_EQ(
502+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id());
503+
EXPECT_EQ(
504+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id());
505+
}
506+
}
507+
408508
TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_error)
409509
{
410510
auto strictness = GetParam().strictness;

0 commit comments

Comments
 (0)