Skip to content

Commit

Permalink
refactor (de)activate request check
Browse files Browse the repository at this point in the history
  • Loading branch information
@TakashiSato
TakashiSato committed Jun 26, 2024
1 parent 005a79f commit 0115236
Show file tree
Hide file tree
Showing 2 changed files with 178 additions and 126 deletions.
42 changes: 42 additions & 0 deletions controller_manager/include/controller_manager/controller_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -329,6 +329,13 @@ class ControllerManager : public rclcpp::Node
std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;

private:
enum class CheckDeActivateRequestResult
{
OK,
ERROR,
RETRY
};

std::vector<std::string> get_controller_names();
std::pair<std::string, std::string> split_command_interface(
const std::string & command_interface);
Expand All @@ -339,6 +346,7 @@ class ControllerManager : public rclcpp::Node
* and "control loop" threads.
*/
void clear_requests();
void clear_chained_mode_requests();

/**
* If a controller is deactivated all following controllers (if any exist) should be switched
Expand Down Expand Up @@ -377,6 +385,23 @@ class ControllerManager : public rclcpp::Node
const std::vector<ControllerSpec> & controllers, int strictness,
const ControllersListIterator controller_it);

/**
* Check if all activate requests are valid.
* Perform a detailed check internally by calling check_following_controllers_for_activate.
*
* \param[in] controllers list with controllers.
* \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all following
* controllers will be automatically added to the activate request list if they are not in the
* deactivate request.
*
* \returns If all activate requests pass the check, return CheckDeActivateRequestResult::OK. If
* an error occurs during the check, the processing will differ based on the value of strictness:
* if BEST_EFFORT, erase the target controller from the activate_request and return
* CheckDeActivateRequestResult::RETRY; if STRICT, return CheckDeActivateRequestResult::ERROR.
*/
CheckDeActivateRequestResult check_activate_requests(
const std::vector<ControllerSpec> & controllers, int strictness);

/// Check if all the preceding controllers will be in inactive state after controllers' switch.
/**
* Check that all preceding controllers of the @controller_it
Expand All @@ -400,6 +425,23 @@ class ControllerManager : public rclcpp::Node
const std::vector<ControllerSpec> & controllers, int strictness,
const ControllersListIterator controller_it);

/**
* Check if all deactivate requests are valid.
* Perform a detailed check internally by calling check_preceeding_controllers_for_deactivate.
*
* \param[in] controllers list with controllers.
* \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all following
* controllers will be automatically added to the activate request list if they are not in the
* deactivate request.
*
* \returns If all deactivate requests pass the check, return CheckDeActivateRequestResult::OK. If
* an error occurs during the check, the processing will differ based on the value of strictness:
* if BEST_EFFORT, erase the target controller from the deactivate_request and return
* CheckDeActivateRequestResult::RETRY; if STRICT, return CheckDeActivateRequestResult::ERROR.
*/
CheckDeActivateRequestResult check_deactivate_requests(
const std::vector<ControllerSpec> & controllers, int strictness);

/**
* @brief Inserts a controller into an ordered list based on dependencies to compute the
* controller chain.
Expand Down
262 changes: 136 additions & 126 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -831,16 +831,21 @@ void ControllerManager::clear_requests()
{
deactivate_request_.clear();
activate_request_.clear();
// Set these interfaces as unavailable when clearing requests to avoid leaving them in available
// state without the controller being in active state
clear_chained_mode_requests();
activate_command_interface_request_.clear();
deactivate_command_interface_request_.clear();
}

void ControllerManager::clear_chained_mode_requests()
{
// 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();
activate_command_interface_request_.clear();
deactivate_command_interface_request_.clear();
}

controller_interface::return_type ControllerManager::switch_controller(
Expand Down Expand Up @@ -963,136 +968,25 @@ controller_interface::return_type ControllerManager::switch_controller(

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

enum class CreateRequestResult
{
OK,
ERROR,
RETRY
};

const auto check_de_activate_request_and_create_chained_mode_request =
[this, &strictness, &controllers]() -> CreateRequestResult
[this, &strictness, &controllers]() -> CheckDeActivateRequestResult
{
const auto clear_chained_mode_requests = [this]()
{
// 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);

// 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)
if (const auto result = check_activate_requests(controllers, strictness);
result != CheckDeActivateRequestResult::OK)
{
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))
{
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);
}

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;
}
}
return result;
}

// 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)
if (const auto result = check_deactivate_requests(controllers, strictness);
result != CheckDeActivateRequestResult::OK)
{
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 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 result;
}

return CreateRequestResult::OK;
return CheckDeActivateRequestResult::OK;
};

// Validate the (de)activate request and create from/to chained mode requests as needed.
Expand All @@ -1104,15 +998,15 @@ controller_interface::return_type ControllerManager::switch_controller(
while (true)
{
const auto result = check_de_activate_request_and_create_chained_mode_request();
if (result == CreateRequestResult::RETRY)
if (result == CheckDeActivateRequestResult::RETRY)
{
continue;
}
if (result == CreateRequestResult::ERROR)
if (result == CheckDeActivateRequestResult::ERROR)
{
return controller_interface::return_type::ERROR;
}
// if result == CreateRequestResult::OK -> break the loop
// if result == CheckDeActivateRequestResult::OK -> break the loop
break;
}

Expand Down Expand Up @@ -2550,6 +2444,68 @@ controller_interface::return_type ControllerManager::check_following_controllers
return controller_interface::return_type::OK;
};

ControllerManager::CheckDeActivateRequestResult ControllerManager::check_activate_requests(
const std::vector<ControllerSpec> & controllers, int strictness)
{
// 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)
{
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) &&
std::find(deactivate_request_.begin(), deactivate_request_.end(), *ctrl_it) ==
deactivate_request_.end())
{
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);
}

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 CheckDeActivateRequestResult::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 CheckDeActivateRequestResult::ERROR;
}
}
}

return CheckDeActivateRequestResult::OK;
}

controller_interface::return_type ControllerManager::check_preceeding_controllers_for_deactivate(
const std::vector<ControllerSpec> & controllers, int /*strictness*/,
const ControllersListIterator controller_it)
Expand Down Expand Up @@ -2636,6 +2592,60 @@ controller_interface::return_type ControllerManager::check_preceeding_controller
return controller_interface::return_type::OK;
}

ControllerManager::CheckDeActivateRequestResult ControllerManager::check_deactivate_requests(
const std::vector<ControllerSpec> & controllers, int strictness)
{
// 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 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 CheckDeActivateRequestResult::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 CheckDeActivateRequestResult::ERROR;
}
}
}

return CheckDeActivateRequestResult::OK;
}

void ControllerManager::controller_activity_diagnostic_callback(
diagnostic_updater::DiagnosticStatusWrapper & stat)
{
Expand Down

0 comments on commit 0115236

Please sign in to comment.