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

[CM] Better readability and maintainability: rename variables, move code to more logical places 🔧 #1227

Merged
merged 3 commits into from
Jan 20, 2024
+52 −52
Merged

[CM] Better readability and maintainability: rename variables, move code to more logical places 🔧 #1227

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
dc20cdc
Move 'manage_switch' method and 'do_switch' flag to more appropriate …
destogl Dec 12, 2023
e31cfc4
Make variable name clearer: rename 'request' to 'controller_name'.
destogl Dec 18, 2023
771f4ab
Remove duplicated 'controller_name' variable
destogl Jan 12, 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
104 changes: 52 additions & 52 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
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)
destogl marked this conversation as resolved.
Show resolved Hide resolved
{
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;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this changes behaviour, nicht wahr?

I see it was moved into manage_switch() to the end

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No, it should not change behavior, but just put it in a more logical place.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actually here it might be wrong. Because the "switching" is not (theoretically( finished here if one add something more to the mange_switch method.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think what you said makes sense. It should be at the end of the manage_switch method.

}

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
Loading