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) {