Skip to content

Commit

Permalink
[CM] Better readability and maintainability: rename variables, move c…
Browse files Browse the repository at this point in the history 
…ode to more logical places 🔧 (#1227)
  • Loading branch information
@destogl
destogl authored Jan 20, 2024
1 parent abceece commit 6a98586
Showing 1 changed file with 52 additions and 52 deletions.
104 changes: 52 additions & 52 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1336,53 +1336,22 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
return to.back().c;
}

void ControllerManager::manage_switch()
{
// Ask hardware interfaces to change mode
if (!resource_manager_->perform_command_mode_switch(
activate_command_interface_request_, deactivate_command_interface_request_))
{
RCLCPP_ERROR(get_logger(), "Error while performing mode switch.");
}

std::vector<ControllerSpec> & rt_controller_list =
rt_controllers_wrapper_.update_and_get_used_by_rt_list();

deactivate_controllers(rt_controller_list, deactivate_request_);

switch_chained_mode(to_chained_mode_request_, true);
switch_chained_mode(from_chained_mode_request_, false);

// activate controllers once the switch is fully complete
if (!switch_params_.activate_asap)
{
activate_controllers(rt_controller_list, activate_request_);
}
else
{
// activate controllers as soon as their required joints are done switching
activate_controllers_asap(rt_controller_list, activate_request_);
}

// TODO(destogl): move here "do_switch = false"
}

void ControllerManager::deactivate_controllers(
const std::vector<ControllerSpec> & rt_controller_list,
const std::vector<std::string> controllers_to_deactivate)
{
// deactivate controllers
for (const auto & request : controllers_to_deactivate)
for (const auto & controller_name : controllers_to_deactivate)
{
auto found_it = std::find_if(
rt_controller_list.begin(), rt_controller_list.end(),
std::bind(controller_name_compare, std::placeholders::_1, request));
std::bind(controller_name_compare, std::placeholders::_1, controller_name));
if (found_it == rt_controller_list.end())
{
RCLCPP_ERROR(
get_logger(),
"Got request to deactivate controller '%s' but it is not in the realtime controller list",
request.c_str());
controller_name.c_str());
continue;
}
auto controller = found_it->c;
Expand All @@ -1393,13 +1362,13 @@ void ControllerManager::deactivate_controllers(
// if it is a chainable controller, make the reference interfaces unavailable on deactivation
if (controller->is_chainable())
{
resource_manager_->make_controller_reference_interfaces_unavailable(request);
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",
request.c_str(), new_state.label().c_str());
controller_name.c_str(), new_state.label().c_str());
}
}
}
Expand All @@ -1411,26 +1380,26 @@ void ControllerManager::switch_chained_mode(
std::vector<ControllerSpec> & rt_controller_list =
rt_controllers_wrapper_.update_and_get_used_by_rt_list();

for (const auto & request : chained_mode_switch_list)
for (const auto & controller_name : chained_mode_switch_list)
{
auto found_it = std::find_if(
rt_controller_list.begin(), rt_controller_list.end(),
std::bind(controller_name_compare, std::placeholders::_1, request));
std::bind(controller_name_compare, std::placeholders::_1, controller_name));
if (found_it == rt_controller_list.end())
{
RCLCPP_FATAL(
get_logger(),
"Got request to turn %s chained mode for controller '%s', but controller is not in the "
"realtime controller list. (This should never happen!)",
(to_chained_mode ? "ON" : "OFF"), request.c_str());
(to_chained_mode ? "ON" : "OFF"), controller_name.c_str());
continue;
}
auto controller = found_it->c;
if (!is_controller_active(*controller))
{
// if it is a chainable controller, make the reference interfaces available on preactivation
// (This is needed when you activate a couple of chainable controller altogether)
resource_manager_->make_controller_reference_interfaces_available(request);
resource_manager_->make_controller_reference_interfaces_available(controller_name);
if (!controller->set_chained_mode(to_chained_mode))
{
RCLCPP_ERROR(
Expand All @@ -1439,7 +1408,7 @@ void ControllerManager::switch_chained_mode(
"it! The control will probably not work as expected. Try to restart all controllers. "
"If "
"the error persist check controllers' individual configuration.",
(to_chained_mode ? "ON" : "OFF"), request.c_str());
(to_chained_mode ? "ON" : "OFF"), controller_name.c_str());
}
}
else
Expand All @@ -1448,7 +1417,7 @@ void ControllerManager::switch_chained_mode(
get_logger(),
"Got request to turn %s chained mode for controller '%s', but this can not happen if "
"controller is in '%s' state. (This should never happen!)",
(to_chained_mode ? "ON" : "OFF"), request.c_str(),
(to_chained_mode ? "ON" : "OFF"), controller_name.c_str(),
hardware_interface::lifecycle_state_names::ACTIVE);
}
}
Expand All @@ -1458,21 +1427,20 @@ void ControllerManager::activate_controllers(
const std::vector<ControllerSpec> & rt_controller_list,
const std::vector<std::string> controllers_to_activate)
{
for (const auto & request : controllers_to_activate)
for (const auto & controller_name : controllers_to_activate)
{
auto found_it = std::find_if(
rt_controller_list.begin(), rt_controller_list.end(),
std::bind(controller_name_compare, std::placeholders::_1, request));
std::bind(controller_name_compare, std::placeholders::_1, controller_name));
if (found_it == rt_controller_list.end())
{
RCLCPP_ERROR(
get_logger(),
"Got request to activate controller '%s' but it is not in the realtime controller list",
request.c_str());
controller_name.c_str());
continue;
}
auto controller = found_it->c;
auto controller_name = found_it->info.name;
// reset the next update cycle time for newly activated controllers
*found_it->next_update_cycle_time =
rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
Expand Down Expand Up @@ -1501,7 +1469,7 @@ void ControllerManager::activate_controllers(
RCLCPP_ERROR(
get_logger(),
"Resource conflict for controller '%s'. Command interface '%s' is already claimed.",
request.c_str(), command_interface.c_str());
controller_name.c_str(), command_interface.c_str());
assignment_successful = false;
break;
}
Expand All @@ -1511,7 +1479,8 @@ void ControllerManager::activate_controllers(
}
catch (const std::exception & e)
{
RCLCPP_ERROR(get_logger(), "Can't activate controller '%s': %s", request.c_str(), e.what());
RCLCPP_ERROR(
get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what());
assignment_successful = false;
break;
}
Expand Down Expand Up @@ -1545,7 +1514,8 @@ void ControllerManager::activate_controllers(
}
catch (const std::exception & e)
{
RCLCPP_ERROR(get_logger(), "Can't activate controller '%s': %s", request.c_str(), e.what());
RCLCPP_ERROR(
get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what());
assignment_successful = false;
break;
}
Expand All @@ -1571,16 +1541,14 @@ void ControllerManager::activate_controllers(
// if it is a chainable controller, make the reference interfaces available on activation
if (controller->is_chainable())
{
resource_manager_->make_controller_reference_interfaces_available(request);
resource_manager_->make_controller_reference_interfaces_available(controller_name);
}

if (controller->is_async())
{
async_controller_threads_.at(controller_name)->activate();
}
}
// All controllers activated, switching done
switch_params_.do_switch = false;
}

void ControllerManager::activate_controllers_asap(
Expand Down Expand Up @@ -2037,6 +2005,38 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration &
}
}

void ControllerManager::manage_switch()
{
// Ask hardware interfaces to change mode
if (!resource_manager_->perform_command_mode_switch(
activate_command_interface_request_, deactivate_command_interface_request_))
{
RCLCPP_ERROR(get_logger(), "Error while performing mode switch.");
}

std::vector<ControllerSpec> & rt_controller_list =
rt_controllers_wrapper_.update_and_get_used_by_rt_list();

deactivate_controllers(rt_controller_list, deactivate_request_);

switch_chained_mode(to_chained_mode_request_, true);
switch_chained_mode(from_chained_mode_request_, false);

// activate controllers once the switch is fully complete
if (!switch_params_.activate_asap)
{
activate_controllers(rt_controller_list, activate_request_);
}
else
{
// activate controllers as soon as their required joints are done switching
activate_controllers_asap(rt_controller_list, activate_request_);
}

// All controllers switched --> switching done
switch_params_.do_switch = false;
}

controller_interface::return_type ControllerManager::update(
const rclcpp::Time & time, const rclcpp::Duration & period)
{
Expand Down

0 comments on commit 6a98586

Please sign in to comment.