Skip to content

Commit

Permalink
Add controller exception handling in controller manager (#1507)
Browse files Browse the repository at this point in the history
  • Loading branch information
@saikishor
saikishor authored May 3, 2024
1 parent 5d0d496 commit c935b50
Showing 1 changed file with 150 additions and 32 deletions.
182 changes: 150 additions & 32 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -562,11 +562,26 @@ controller_interface::return_type ControllerManager::unload_controller(
get_logger(), "Controller '%s' is cleaned-up before unloading!", controller_name.c_str());
// TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
// cleaning-up controllers?
const auto new_state = controller.c->get_node()->cleanup();
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
try
{
RCLCPP_WARN(
get_logger(), "Failed to clean-up the controller '%s' before unloading!",
const auto new_state = controller.c->get_node()->cleanup();
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
{
RCLCPP_WARN(
get_logger(), "Failed to clean-up the controller '%s' before unloading!",
controller_name.c_str());
}
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
get_logger(), "Failed to clean-up the controller '%s' before unloading: %s",
controller_name.c_str(), e.what());
}
catch (...)
{
RCLCPP_ERROR(
get_logger(), "Failed to clean-up the controller '%s' before unloading",
controller_name.c_str());
}
}
Expand Down Expand Up @@ -631,22 +646,49 @@ controller_interface::return_type ControllerManager::configure_controller(
get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str());
// TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
// cleaning-up controllers?
new_state = controller->get_node()->cleanup();
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
try
{
new_state = controller->get_node()->cleanup();
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
{
RCLCPP_ERROR(
get_logger(), "Controller '%s' can not be cleaned-up before configuring",
controller_name.c_str());
return controller_interface::return_type::ERROR;
}
}
catch (...)
{
RCLCPP_ERROR(
get_logger(), "Controller '%s' can not be cleaned-up before configuring",
get_logger(), "Caught exception while cleaning-up controller '%s' before configuring",
controller_name.c_str());
return controller_interface::return_type::ERROR;
}
}

new_state = controller->configure();
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
try
{
new_state = controller->configure();
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
{
RCLCPP_ERROR(
get_logger(), "After configuring, controller '%s' is in state '%s' , expected inactive.",
controller_name.c_str(), new_state.label().c_str());
return controller_interface::return_type::ERROR;
}
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
get_logger(), "After configuring, controller '%s' is in state '%s' , expected inactive.",
controller_name.c_str(), new_state.label().c_str());
get_logger(), "Caught exception while configuring controller '%s': %s",
controller_name.c_str(), e.what());
return controller_interface::return_type::ERROR;
}
catch (...)
{
RCLCPP_ERROR(
get_logger(), "Caught unknown exception while configuring controller '%s'",
controller_name.c_str());
return controller_interface::return_type::ERROR;
}

Expand Down Expand Up @@ -1286,14 +1328,35 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
}

const rclcpp::NodeOptions controller_node_options = determine_controller_node_options(controller);
if (
controller.c->init(
controller.info.name, robot_description_, get_update_rate(), get_namespace(),
controller_node_options) == controller_interface::return_type::ERROR)
// Catch whatever exception the controller might throw
try
{
if (
controller.c->init(
controller.info.name, robot_description_, get_update_rate(), get_namespace(),
controller_node_options) == controller_interface::return_type::ERROR)
{
to.clear();
RCLCPP_ERROR(
get_logger(), "Could not initialize the controller named '%s'",
controller.info.name.c_str());
return nullptr;
}
}
catch (const std::exception & e)
{
to.clear();
RCLCPP_ERROR(
get_logger(), "Could not initialize the controller named '%s'", controller.info.name.c_str());
get_logger(), "Caught exception while initializing controller '%s': %s",
controller.info.name.c_str(), e.what());
return nullptr;
}
catch (...)
{
to.clear();
RCLCPP_ERROR(
get_logger(), "Caught unknown exception while initializing controller '%s'",
controller.info.name.c_str());
return nullptr;
}

Expand Down Expand Up @@ -1336,18 +1399,37 @@ void ControllerManager::deactivate_controllers(
auto controller = found_it->c;
if (is_controller_active(*controller))
{
const auto new_state = controller->get_node()->deactivate();
controller->release_interfaces();
// if it is a chainable controller, make the reference interfaces unavailable on deactivation
if (controller->is_chainable())
try
{
const auto new_state = controller->get_node()->deactivate();
controller->release_interfaces();

// if it is a chainable controller, make the reference interfaces unavailable on
// deactivation
if (controller->is_chainable())
{
resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
}
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
{
RCLCPP_ERROR(
get_logger(), "After deactivating, controller '%s' is in state '%s', expected Inactive",
controller_name.c_str(), new_state.label().c_str());
}
}
catch (const std::exception & e)
{
resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
RCLCPP_ERROR(
get_logger(), "Caught exception while deactivating the controller '%s': %s",
controller_name.c_str(), e.what());
continue;
}
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
catch (...)
{
RCLCPP_ERROR(
get_logger(), "After deactivating, controller '%s' is in state '%s', expected Inactive",
controller_name.c_str(), new_state.label().c_str());
get_logger(), "Caught unknown exception while deactivating the controller '%s'",
controller_name.c_str());
continue;
}
}
}
Expand Down Expand Up @@ -1503,15 +1585,32 @@ void ControllerManager::activate_controllers(
}
controller->assign_interfaces(std::move(command_loans), std::move(state_loans));

const auto new_state = controller->get_node()->activate();
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
try
{
const auto new_state = controller->get_node()->activate();
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
RCLCPP_ERROR(
get_logger(),
"After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).",
controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(),
hardware_interface::lifecycle_state_names::ACTIVE,
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
}
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
get_logger(),
"After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).",
controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(),
hardware_interface::lifecycle_state_names::ACTIVE,
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
get_logger(), "Caught exception while activating the controller '%s': %s",
controller_name.c_str(), e.what());
continue;
}
catch (...)
{
RCLCPP_ERROR(
get_logger(), "Caught unknown exception while activating the controller '%s'",
controller_name.c_str());
continue;
}

// if it is a chainable controller, make the reference interfaces available on activation
Expand Down Expand Up @@ -2050,7 +2149,26 @@ controller_interface::return_type ControllerManager::update(
{
const auto controller_actual_period =
(time - *loaded_controller.next_update_cycle_time) + controller_period;
auto controller_ret = loaded_controller.c->update(time, controller_actual_period);
auto controller_ret = controller_interface::return_type::OK;
// Catch exceptions thrown by the controller update function
try
{
controller_ret = loaded_controller.c->update(time, controller_actual_period);
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
get_logger(), "Caught exception while updating controller '%s': %s",
loaded_controller.info.name.c_str(), e.what());
controller_ret = controller_interface::return_type::ERROR;
}
catch (...)
{
RCLCPP_ERROR(
get_logger(), "Caught unknown exception while updating controller '%s'",
loaded_controller.info.name.c_str());
controller_ret = controller_interface::return_type::ERROR;
}

if (
*loaded_controller.next_update_cycle_time ==
Expand Down

0 comments on commit c935b50

Please sign in to comment.