Skip to content

Commit

Permalink
change the bug fix implementation to not use goto
Browse files Browse the repository at this point in the history
  • Loading branch information
@TakashiSato
TakashiSato committed Jun 26, 2024
1 parent df56d4f commit fcc1b96
Showing 1 changed file with 132 additions and 90 deletions.
222 changes: 132 additions & 90 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -963,115 +963,157 @@ controller_interface::return_type ControllerManager::switch_controller(

const std::vector<ControllerSpec> & controllers = rt_controllers_wrapper_.get_updated_list(guard);

LABEL_CHECK_REQUEST:
// Set these interfaces as unavailable when clearing requests to avoid leaving them in available
// state without the controller being in active state
for (const auto & controller_name : to_chained_mode_request_)
enum class CreateRequestResult
{
resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
}
to_chained_mode_request_.clear();
from_chained_mode_request_.clear();

// if a preceding controller is deactivated, all first-level controllers should be switched 'from'
// chained mode
propagate_deactivation_of_chained_mode(controllers);
OK,
ERROR,
RETRY
};

// check if controllers should be switched 'to' chained mode when controllers are activated
for (auto ctrl_it = activate_request_.begin(); ctrl_it != activate_request_.end(); ++ctrl_it)
const auto check_de_activate_request_and_create_chained_mode_request =
[this, &strictness, &controllers]() -> CreateRequestResult
{
auto controller_it = std::find_if(
controllers.begin(), controllers.end(),
std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it));
controller_interface::return_type status = controller_interface::return_type::OK;

// if controller is not inactive then do not do any following-controllers checks
if (!is_controller_inactive(controller_it->c))
const auto clear_chained_mode_requests = [this]()
{
RCLCPP_WARN(
get_logger(),
"Controller with name '%s' is not inactive so its following "
"controllers do not have to be checked, because it cannot be activated.",
controller_it->info.name.c_str());
status = controller_interface::return_type::ERROR;
}
else
{
status = check_following_controllers_for_activate(controllers, strictness, controller_it);
}
// Set these interfaces as unavailable when clearing requests to avoid leaving them in
// available state without the controller being in active state
for (const auto & controller_name : to_chained_mode_request_)
{
resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
}
to_chained_mode_request_.clear();
from_chained_mode_request_.clear();
};

// if a preceding controller is deactivated, all first-level controllers should be switched
// 'from' chained mode
propagate_deactivation_of_chained_mode(controllers);

if (status != controller_interface::return_type::OK)
// check if controllers should be switched 'to' chained mode when controllers are activated
for (auto ctrl_it = activate_request_.begin(); ctrl_it != activate_request_.end(); ++ctrl_it)
{
RCLCPP_WARN(
get_logger(),
"Could not activate controller with name '%s'. Check above warnings for more details. "
"Check the state of the controllers and their required interfaces using "
"`ros2 control list_controllers -v` CLI to get more information.",
(*ctrl_it).c_str());
if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT)
auto controller_it = std::find_if(
controllers.begin(), controllers.end(),
std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it));
controller_interface::return_type status = controller_interface::return_type::OK;

// if controller is not inactive then do not do any following-controllers checks
if (!is_controller_inactive(controller_it->c))
{
// TODO(destogl): automatic manipulation of the chain:
// || strictness ==
// controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN);
// remove controller that can not be activated from the activation request and step-back
// iterator to correctly step to the next element in the list in the loop
activate_request_.erase(ctrl_it);
goto LABEL_CHECK_REQUEST;
RCLCPP_WARN(
get_logger(),
"Controller with name '%s' is not inactive so its following "
"controllers do not have to be checked, because it cannot be activated.",
controller_it->info.name.c_str());
status = controller_interface::return_type::ERROR;
}
if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
else
{
RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
// reset all lists
clear_requests();
return controller_interface::return_type::ERROR;
status = check_following_controllers_for_activate(controllers, strictness, controller_it);
}
}
}

// check if controllers should be deactivated if used in chained mode
for (auto ctrl_it = deactivate_request_.begin(); ctrl_it != deactivate_request_.end(); ++ctrl_it)
{
auto controller_it = std::find_if(
controllers.begin(), controllers.end(),
std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it));
controller_interface::return_type status = controller_interface::return_type::OK;

// if controller is not active then skip preceding-controllers checks
if (!is_controller_active(controller_it->c))
{
RCLCPP_WARN(
get_logger(), "Controller with name '%s' can not be deactivated since it is not active.",
controller_it->info.name.c_str());
status = controller_interface::return_type::ERROR;
}
else
{
status = check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it);
if (status != controller_interface::return_type::OK)
{
RCLCPP_WARN(
get_logger(),
"Could not activate controller with name '%s'. Check above warnings for more details. "
"Check the state of the controllers and their required interfaces using "
"`ros2 control list_controllers -v` CLI to get more information.",
(*ctrl_it).c_str());
if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT)
{
// TODO(destogl): automatic manipulation of the chain:
// || strictness ==
// controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN);
// remove controller that can not be activated from the activation request and step-back
// iterator to correctly step to the next element in the list in the loop
activate_request_.erase(ctrl_it);
// reset chained mode request lists and will retry the creation of the request
clear_chained_mode_requests();
return CreateRequestResult::RETRY;
}
if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
{
RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
// reset all lists
clear_requests();
return CreateRequestResult::ERROR;
}
}
}

if (status != controller_interface::return_type::OK)
// check if controllers should be deactivated if used in chained mode
for (auto ctrl_it = deactivate_request_.begin(); ctrl_it != deactivate_request_.end();
++ctrl_it)
{
RCLCPP_WARN(
get_logger(),
"Could not deactivate controller with name '%s'. Check above warnings for more details. "
"Check the state of the controllers and their required interfaces using "
"`ros2 control list_controllers -v` CLI to get more information.",
(*ctrl_it).c_str());
if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT)
auto controller_it = std::find_if(
controllers.begin(), controllers.end(),
std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it));
controller_interface::return_type status = controller_interface::return_type::OK;

// if controller is not active then skip preceding-controllers checks
if (!is_controller_active(controller_it->c))
{
// remove controller that can not be activated from the activation request and step-back
// iterator to correctly step to the next element in the list in the loop
deactivate_request_.erase(ctrl_it);
goto LABEL_CHECK_REQUEST;
RCLCPP_WARN(
get_logger(), "Controller with name '%s' can not be deactivated since it is not active.",
controller_it->info.name.c_str());
status = controller_interface::return_type::ERROR;
}
if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
else
{
RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
// reset all lists
clear_requests();
return controller_interface::return_type::ERROR;
status =
check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it);
}

if (status != controller_interface::return_type::OK)
{
RCLCPP_WARN(
get_logger(),
"Could not deactivate controller with name '%s'. Check above warnings for more details. "
"Check the state of the controllers and their required interfaces using "
"`ros2 control list_controllers -v` CLI to get more information.",
(*ctrl_it).c_str());
if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT)
{
// remove controller that can not be activated from the activation request and step-back
// iterator to correctly step to the next element in the list in the loop
deactivate_request_.erase(ctrl_it);
// reset chained mode request lists and will retry the creation of the request
clear_chained_mode_requests();
return CreateRequestResult::RETRY;
}
if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
{
RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
// reset all lists
clear_requests();
return CreateRequestResult::ERROR;
}
}
}

return CreateRequestResult::OK;
};

// Validate the (de)activate request and create from/to chained mode requests as needed.
// If the strictness value is STRICT, return an error for any invalid request.
// If the strictness value is BEST_EFFORT, remove any controllers that cannot be
// (de)activated from the request and proceed. However, any changes to the
// (de)activate request will affect the outcome of the check and creation process,
// so retry from the beginning.
while (true)
{
const auto result = check_de_activate_request_and_create_chained_mode_request();
if (result == CreateRequestResult::RETRY)
{
continue;
}
if (result == CreateRequestResult::ERROR)
{
return controller_interface::return_type::ERROR;
}
// if result == CreateRequestResult::OK -> break the loop
break;
}

for (const auto & controller : controllers)
Expand Down

0 comments on commit fcc1b96

Please sign in to comment.