Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add controller exception handling in controller manager #1507

Merged
merged 7 commits into from
May 3, 2024
Merged

Add controller exception handling in controller manager #1507

Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
6507874
catch exceptions during node cleanup
saikishor Apr 25, 2024
63eb75e
catch exceptions during node initialization
saikishor Apr 25, 2024
083b829
Catch exceptions during the node update cycle
saikishor Apr 25, 2024
fd7e845
catch exceptions while configuring the controllers
saikishor Apr 25, 2024
95f5689
catch exceptions while deactivating the controller
saikishor Apr 25, 2024
5b539dd
catch exceptions while activating the controller
saikishor Apr 25, 2024
5546fd6
Minor compilation fixes
saikishor Apr 25, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
182 changes: 150 additions & 32 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
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
Loading