From 6a98586c12a47c3cd6830be1963f7365e5a1e585 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Sat, 20 Jan 2024 15:10:16 +0100 Subject: [PATCH] =?UTF-8?q?[CM]=20Better=20readability=20and=20maintainabi?= =?UTF-8?q?lity:=20rename=20variables,=20move=20code=20to=20more=20logical?= =?UTF-8?q?=20places=20=F0=9F=94=A7=20(#1227)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- controller_manager/src/controller_manager.cpp | 104 +++++++++--------- 1 file changed, 52 insertions(+), 52 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e7a2293672..f588720b3d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -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 & 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 & rt_controller_list, const std::vector 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; @@ -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()); } } } @@ -1411,18 +1380,18 @@ void ControllerManager::switch_chained_mode( std::vector & 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; @@ -1430,7 +1399,7 @@ void ControllerManager::switch_chained_mode( { // 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( @@ -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 @@ -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); } } @@ -1458,21 +1427,20 @@ void ControllerManager::activate_controllers( const std::vector & rt_controller_list, const std::vector 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()); @@ -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; } @@ -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; } @@ -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; } @@ -1571,7 +1541,7 @@ 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()) @@ -1579,8 +1549,6 @@ void ControllerManager::activate_controllers( async_controller_threads_.at(controller_name)->activate(); } } - // All controllers activated, switching done - switch_params_.do_switch = false; } void ControllerManager::activate_controllers_asap( @@ -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 & 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) {