From dc20cdc20047dc1fc0663a339e8e8fae198d5138 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Tue, 12 Dec 2023 11:08:51 +0100 Subject: [PATCH 1/3] Move 'manage_switch' method and 'do_switch' flag to more appropriate positions. --- controller_manager/src/controller_manager.cpp | 65 +++++++++---------- 1 file changed, 32 insertions(+), 33 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e7a2293672..5cd33b1f17 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1336,37 +1336,6 @@ 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) @@ -1579,8 +1548,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 +2004,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) { From e31cfc4da104b04d8c322963aa851cd7faa582f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 18 Dec 2023 10:50:59 +0100 Subject: [PATCH 2/3] Make variable name clearer: rename 'request' to 'controller_name'. --- controller_manager/src/controller_manager.cpp | 38 ++++++++++--------- 1 file changed, 20 insertions(+), 18 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 5cd33b1f17..e2655b53d0 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1341,17 +1341,17 @@ void ControllerManager::deactivate_controllers( 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; @@ -1362,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()); } } } @@ -1380,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; @@ -1399,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( @@ -1408,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 @@ -1417,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); } } @@ -1427,17 +1427,17 @@ 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; @@ -1470,7 +1470,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; } @@ -1480,7 +1480,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; } @@ -1514,7 +1515,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; } @@ -1540,7 +1542,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()) From 771f4abb1f513c5b745058a8acbad15355d3c150 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Fri, 12 Jan 2024 16:20:35 +0100 Subject: [PATCH 3/3] Remove duplicated 'controller_name' variable --- controller_manager/src/controller_manager.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e2655b53d0..f588720b3d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1441,7 +1441,6 @@ void ControllerManager::activate_controllers( 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()); @@ -2010,13 +2009,13 @@ 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_)) + 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(); + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); deactivate_controllers(rt_controller_list, deactivate_request_);