diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 4e020fdd89..de94d4fc07 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -141,6 +141,13 @@ def main(args=None): action="store_true", required=False, ) + parser.add_argument( + "--switch-asap", + help="Option to switch the controllers in the realtime loop at the earliest possible time or in the non-realtime loop.", + required=False, + default=False, + action=argparse.BooleanOptionalAction, + ) parser.add_argument( "--controller-ros-args", help="The --ros-args to be passed to the controller node, e.g., for remapping topics. " @@ -160,6 +167,7 @@ def main(args=None): switch_timeout = args.switch_timeout strictness = SwitchController.Request.STRICT unload_controllers_upon_exit = False + switch_asap = args.switch_asap node = None if param_files: @@ -282,7 +290,7 @@ def main(args=None): [], [controller_name], strictness, - True, + switch_asap, switch_timeout, service_call_timeout, ) @@ -307,7 +315,7 @@ def main(args=None): [], controller_names, strictness, - True, + switch_asap, switch_timeout, service_call_timeout, ) @@ -341,7 +349,7 @@ def main(args=None): controller_names, [], strictness, - True, + switch_asap, switch_timeout, service_call_timeout, ) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index c9932e6936..2085cf09b2 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -163,9 +163,9 @@ There are two scripts to interact with controller manager from launch files: .. code-block:: console $ ros2 run controller_manager spawner -h - usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] [--switch-timeout SWITCH_TIMEOUT] [--service-call-timeout SERVICE_CALL_TIMEOUT] [--activate-as-group] - [--controller-ros-args CONTROLLER_ROS_ARGS] - controller_names [controller_names ...] + usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] [--switch-timeout SWITCH_TIMEOUT] + [--service-call-timeout SERVICE_CALL_TIMEOUT] [--activate-as-group] [--switch-asap | --no-switch-asap] [--controller-ros-args CONTROLLER_ROS_ARGS] + controller_names [controller_names ...] positional arguments: controller_names List of controllers @@ -186,6 +186,8 @@ There are two scripts to interact with controller manager from launch files: --service-call-timeout SERVICE_CALL_TIMEOUT Time to wait for the service response from the controller manager --activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether + --switch-asap, --no-switch-asap + Option to switch the controllers in the realtime loop at the earliest possible time or in the non-realtime loop. --controller-ros-args CONTROLLER_ROS_ARGS The --ros-args to be passed to the controller node, e.g., for remapping topics. Pass multiple times for every argument. diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 88a749f078..4191a475d8 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -40,6 +40,7 @@ #include "controller_manager_msgs/srv/unload_controller.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" +#include "hardware_interface/helpers.hpp" #include "hardware_interface/resource_manager.hpp" #include "pluginlib/class_loader.hpp" @@ -259,7 +260,7 @@ class ControllerManager : public rclcpp::Node */ void deactivate_controllers( const std::vector & rt_controller_list, - const std::vector controllers_to_deactivate); + const std::vector & controllers_to_deactivate); /** * Switch chained mode for all the controllers with respect to the following cases: @@ -282,22 +283,7 @@ class ControllerManager : public rclcpp::Node */ void activate_controllers( const std::vector & rt_controller_list, - const std::vector controllers_to_activate); - - /// Activate chosen controllers from real-time controller list. - /** - * Activate controllers with names \p controllers_to_activate from list \p rt_controller_list. - * The controller list will be iterated as many times as there are controller names. - * - * *NOTE*: There is currently not difference to `activate_controllers` method. - * Check https://github.com/ros-controls/ros2_control/issues/263 for more information. - * - * \param[in] rt_controller_list controllers in the real-time list. - * \param[in] controllers_to_activate names of the controller that have to be activated. - */ - void activate_controllers_asap( - const std::vector & rt_controller_list, - const std::vector controllers_to_activate); + const std::vector & controllers_to_activate); void list_controllers_srv_cb( const std::shared_ptr request, @@ -661,11 +647,6 @@ class ControllerManager : public rclcpp::Node rclcpp::Service::SharedPtr set_hardware_component_state_service_; - std::vector activate_request_, deactivate_request_; - std::vector to_chained_mode_request_, from_chained_mode_request_; - std::vector activate_command_interface_request_, - deactivate_command_interface_request_; - std::map> controller_chained_reference_interfaces_cache_; std::map> controller_chained_state_interfaces_cache_; @@ -686,17 +667,34 @@ class ControllerManager : public rclcpp::Node activate_asap = false; } - bool do_switch; + std::atomic_bool do_switch; bool started; // Switch options int strictness; - bool activate_asap; + std::atomic_bool activate_asap; std::chrono::nanoseconds timeout; // conditional variable and mutex to wait for the switch to complete std::condition_variable cv; std::mutex mutex; + + bool skip_cycle(const controller_manager::ControllerSpec & spec) const + { + const std::string controller_name = spec.info.name; + return ros2_control::has_item(activate_request, controller_name) || + ros2_control::has_item(deactivate_request, controller_name) || + ros2_control::has_item(to_chained_mode_request, controller_name) || + ros2_control::has_item(from_chained_mode_request, controller_name); + } + + // The controllers list to activate and deactivate + std::vector activate_request; + std::vector deactivate_request; + std::vector to_chained_mode_request; + std::vector from_chained_mode_request; + std::vector activate_command_interface_request; + std::vector deactivate_command_interface_request; }; SwitchParams switch_params_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 18a22caad9..f2f91cea9c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1298,19 +1298,20 @@ controller_interface::return_type ControllerManager::configure_controller( void ControllerManager::clear_requests() { switch_params_.do_switch = false; - deactivate_request_.clear(); - activate_request_.clear(); + switch_params_.activate_asap = false; + switch_params_.deactivate_request.clear(); + switch_params_.activate_request.clear(); // Set these interfaces as unavailable when clearing requests to avoid leaving them in available // state without the controller being in active state - for (const auto & controller_name : to_chained_mode_request_) + for (const auto & controller_name : switch_params_.to_chained_mode_request) { resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } - to_chained_mode_request_.clear(); - from_chained_mode_request_.clear(); - activate_command_interface_request_.clear(); - deactivate_command_interface_request_.clear(); + switch_params_.to_chained_mode_request.clear(); + switch_params_.from_chained_mode_request.clear(); + switch_params_.activate_command_interface_request.clear(); + switch_params_.deactivate_command_interface_request.clear(); } controller_interface::return_type ControllerManager::switch_controller( @@ -1340,7 +1341,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( // reset the switch param internal variables switch_params_.reset(); - if (!deactivate_request_.empty() || !activate_request_.empty()) + if (!switch_params_.deactivate_request.empty() || !switch_params_.activate_request.empty()) { RCLCPP_FATAL( get_logger(), @@ -1349,7 +1350,8 @@ controller_interface::return_type ControllerManager::switch_controller_cb( throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); } if ( - !deactivate_command_interface_request_.empty() || !activate_command_interface_request_.empty()) + !switch_params_.deactivate_command_interface_request.empty() || + !switch_params_.activate_command_interface_request.empty()) { RCLCPP_FATAL( get_logger(), @@ -1357,7 +1359,9 @@ controller_interface::return_type ControllerManager::switch_controller_cb( "switch_controller() call. This should never happen."); throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); } - if (!from_chained_mode_request_.empty() || !to_chained_mode_request_.empty()) + if ( + !switch_params_.from_chained_mode_request.empty() || + !switch_params_.to_chained_mode_request.empty()) { RCLCPP_FATAL( get_logger(), @@ -1473,19 +1477,21 @@ controller_interface::return_type ControllerManager::switch_controller_cb( }; // list all controllers to deactivate (check if all controllers exist) - auto ret = list_controllers(deactivate_controllers, deactivate_request_, "deactivate", message); + auto ret = list_controllers( + deactivate_controllers, switch_params_.deactivate_request, "deactivate", message); if (ret != controller_interface::return_type::OK) { - deactivate_request_.clear(); + switch_params_.deactivate_request.clear(); return ret; } // list all controllers to activate (check if all controllers exist) - ret = list_controllers(activate_controllers, activate_request_, "activate", message); + ret = + list_controllers(activate_controllers, switch_params_.activate_request, "activate", message); if (ret != controller_interface::return_type::OK) { - deactivate_request_.clear(); - activate_request_.clear(); + switch_params_.deactivate_request.clear(); + switch_params_.activate_request.clear(); return ret; } // If it is a best effort switch, we can remove the controllers log that could not be activated @@ -1501,7 +1507,8 @@ controller_interface::return_type ControllerManager::switch_controller_cb( propagate_deactivation_of_chained_mode(controllers); // check if controllers should be switched 'to' chained mode when controllers are activated - for (auto ctrl_it = activate_request_.begin(); ctrl_it != activate_request_.end(); ++ctrl_it) + for (auto ctrl_it = switch_params_.activate_request.begin(); + ctrl_it != switch_params_.activate_request.end(); ++ctrl_it) { auto controller_it = std::find_if( controllers.begin(), controllers.end(), @@ -1523,8 +1530,8 @@ controller_interface::return_type ControllerManager::switch_controller_cb( { if ( std::find( - deactivate_request_.begin(), deactivate_request_.end(), controller_it->info.name) == - deactivate_request_.end()) + switch_params_.deactivate_request.begin(), switch_params_.deactivate_request.end(), + controller_it->info.name) == switch_params_.deactivate_request.end()) { message = fmt::format( FMT_COMPILE("Controller with name '{}' is already active."), controller_it->info.name); @@ -1569,7 +1576,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN); // remove controller that can not be activated from the activation request and step-back // iterator to correctly step to the next element in the list in the loop - activate_request_.erase(ctrl_it); + switch_params_.activate_request.erase(ctrl_it); message.clear(); --ctrl_it; } @@ -1584,7 +1591,8 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } // check if controllers should be deactivated if used in chained mode - for (auto ctrl_it = deactivate_request_.begin(); ctrl_it != deactivate_request_.end(); ++ctrl_it) + for (auto ctrl_it = switch_params_.deactivate_request.begin(); + ctrl_it != switch_params_.deactivate_request.end(); ++ctrl_it) { auto controller_it = std::find_if( controllers.begin(), controllers.end(), @@ -1618,7 +1626,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( { // remove controller that can not be activated from the activation request and step-back // iterator to correctly step to the next element in the list in the loop - deactivate_request_.erase(ctrl_it); + switch_params_.deactivate_request.erase(ctrl_it); message.clear(); --ctrl_it; } @@ -1633,7 +1641,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } // Check after the check if the activate and deactivate list is empty or not - if (activate_request_.empty() && deactivate_request_.empty()) + if (switch_params_.activate_request.empty() && switch_params_.deactivate_request.empty()) { message = "After checking the controllers, no controllers need to be activated or deactivated."; RCLCPP_INFO(get_logger(), "%s", message.c_str()); @@ -1645,16 +1653,21 @@ controller_interface::return_type ControllerManager::switch_controller_cb( for (const auto & controller : controllers) { auto to_chained_mode_list_it = std::find( - to_chained_mode_request_.begin(), to_chained_mode_request_.end(), controller.info.name); - bool in_to_chained_mode_list = to_chained_mode_list_it != to_chained_mode_request_.end(); + switch_params_.to_chained_mode_request.begin(), switch_params_.to_chained_mode_request.end(), + controller.info.name); + bool in_to_chained_mode_list = + to_chained_mode_list_it != switch_params_.to_chained_mode_request.end(); auto from_chained_mode_list_it = std::find( - from_chained_mode_request_.begin(), from_chained_mode_request_.end(), controller.info.name); - bool in_from_chained_mode_list = from_chained_mode_list_it != from_chained_mode_request_.end(); + switch_params_.from_chained_mode_request.begin(), + switch_params_.from_chained_mode_request.end(), controller.info.name); + bool in_from_chained_mode_list = + from_chained_mode_list_it != switch_params_.from_chained_mode_request.end(); - auto deactivate_list_it = - std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name); - bool in_deactivate_list = deactivate_list_it != deactivate_request_.end(); + auto deactivate_list_it = std::find( + switch_params_.deactivate_request.begin(), switch_params_.deactivate_request.end(), + controller.info.name); + bool in_deactivate_list = deactivate_list_it != switch_params_.deactivate_request.end(); const bool is_active = is_controller_active(*controller.c); const bool is_inactive = is_controller_inactive(*controller.c); @@ -1664,19 +1677,21 @@ controller_interface::return_type ControllerManager::switch_controller_cb( { if (is_active && !in_deactivate_list) { - deactivate_request_.push_back(controller.info.name); - activate_request_.push_back(controller.info.name); + switch_params_.deactivate_request.push_back(controller.info.name); + switch_params_.activate_request.push_back(controller.info.name); } } // get pointers to places in deactivate and activate lists ((de)activate lists have changed) - deactivate_list_it = - std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name); - in_deactivate_list = deactivate_list_it != deactivate_request_.end(); + deactivate_list_it = std::find( + switch_params_.deactivate_request.begin(), switch_params_.deactivate_request.end(), + controller.info.name); + in_deactivate_list = deactivate_list_it != switch_params_.deactivate_request.end(); - auto activate_list_it = - std::find(activate_request_.begin(), activate_request_.end(), controller.info.name); - bool in_activate_list = activate_list_it != activate_request_.end(); + auto activate_list_it = std::find( + switch_params_.activate_request.begin(), switch_params_.activate_request.end(), + controller.info.name); + bool in_activate_list = activate_list_it != switch_params_.activate_request.end(); auto handle_conflict = [&](const std::string & msg) { @@ -1684,12 +1699,12 @@ controller_interface::return_type ControllerManager::switch_controller_cb( { message = msg; RCLCPP_ERROR(get_logger(), "%s", msg.c_str()); - deactivate_request_.clear(); - deactivate_command_interface_request_.clear(); - activate_request_.clear(); - activate_command_interface_request_.clear(); - to_chained_mode_request_.clear(); - from_chained_mode_request_.clear(); + switch_params_.deactivate_request.clear(); + switch_params_.deactivate_command_interface_request.clear(); + switch_params_.activate_request.clear(); + switch_params_.activate_command_interface_request.clear(); + switch_params_.to_chained_mode_request.clear(); + switch_params_.from_chained_mode_request.clear(); return controller_interface::return_type::ERROR; } RCLCPP_WARN(get_logger(), "%s", msg.c_str()); @@ -1706,7 +1721,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( return conflict_status; } in_deactivate_list = false; - deactivate_request_.erase(deactivate_list_it); + switch_params_.deactivate_request.erase(deactivate_list_it); } // check for doubled activation @@ -1719,7 +1734,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( return conflict_status; } in_activate_list = false; - activate_request_.erase(activate_list_it); + switch_params_.activate_request.erase(activate_list_it); } // check for illegal activation of an unconfigured/finalized controller @@ -1733,18 +1748,18 @@ controller_interface::return_type ControllerManager::switch_controller_cb( return conflict_status; } in_activate_list = false; - activate_request_.erase(activate_list_it); + switch_params_.activate_request.erase(activate_list_it); } if (in_activate_list) { extract_command_interfaces_for_controller( - controller, resource_manager_, activate_command_interface_request_); + controller, resource_manager_, switch_params_.activate_command_interface_request); } if (in_deactivate_list) { extract_command_interfaces_for_controller( - controller, resource_manager_, deactivate_command_interface_request_); + controller, resource_manager_, switch_params_.deactivate_command_interface_request); } // cache mapping between hardware and controllers for stopping when read/write error happens @@ -1787,7 +1802,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } } - if (activate_request_.empty() && deactivate_request_.empty()) + if (switch_params_.activate_request.empty() && switch_params_.deactivate_request.empty()) { message = "After checking the controllers, no controllers need to be activated or deactivated."; RCLCPP_INFO(get_logger(), "Empty activate and deactivate list, not requesting switch"); @@ -1796,7 +1811,8 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } if ( - check_for_interfaces_availability_to_activate(controllers, activate_request_, message) != + check_for_interfaces_availability_to_activate( + controllers, switch_params_.activate_request, message) != controller_interface::return_type::OK) { clear_requests(); @@ -1804,18 +1820,18 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } RCLCPP_DEBUG(get_logger(), "Request for command interfaces from activating controllers:"); - for (const auto & interface : activate_command_interface_request_) + for (const auto & interface : switch_params_.activate_command_interface_request) { RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); } RCLCPP_DEBUG(get_logger(), "Release of command interfaces from deactivating controllers:"); - for (const auto & interface : deactivate_command_interface_request_) + for (const auto & interface : switch_params_.deactivate_command_interface_request) { RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); } // wait for deactivating async controllers to finish their current cycle - for (const auto & controller : deactivate_request_) + for (const auto & controller : switch_params_.deactivate_request) { auto controller_it = std::find_if( controllers.begin(), controllers.end(), @@ -1827,10 +1843,12 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } if ( - !activate_command_interface_request_.empty() || !deactivate_command_interface_request_.empty()) + !switch_params_.activate_command_interface_request.empty() || + !switch_params_.deactivate_command_interface_request.empty()) { if (!resource_manager_->prepare_command_mode_switch( - activate_command_interface_request_, deactivate_command_interface_request_)) + switch_params_.activate_command_interface_request, + switch_params_.deactivate_command_interface_request)) { message = "Could not switch controllers since prepare command mode switch was rejected."; RCLCPP_ERROR(get_logger(), "%s", message.c_str()); @@ -1854,17 +1872,27 @@ controller_interface::return_type ControllerManager::switch_controller_cb( switch_params_.do_switch = true; // wait until switch is finished - RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); - std::unique_lock switch_params_guard(switch_params_.mutex); - if (!switch_params_.cv.wait_for( - switch_params_guard, switch_params_.timeout, [this] { return !switch_params_.do_switch; })) - { - message = fmt::format( - FMT_COMPILE("Switch controller timed out after {} seconds!"), - static_cast(switch_params_.timeout.count()) / 1e9); - RCLCPP_ERROR(get_logger(), "%s", message.c_str()); - clear_requests(); - return controller_interface::return_type::ERROR; + if (switch_params_.activate_asap) + { + RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); + std::unique_lock switch_params_guard(switch_params_.mutex); + if (!switch_params_.cv.wait_for( + switch_params_guard, switch_params_.timeout, + [this] { return !switch_params_.do_switch; })) + { + message = fmt::format( + FMT_COMPILE("Switch controller timed out after {} seconds!"), + static_cast(switch_params_.timeout.count()) / 1e9); + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); + clear_requests(); + return controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_INFO(get_logger(), "Requested controller switch from non-realtime loop"); + // This should work as the realtime thread operation is read-only operation + manage_switch(); } // copy the controllers spec from the used to the unused list @@ -1873,8 +1901,8 @@ controller_interface::return_type ControllerManager::switch_controller_cb( // update the claimed interface controller info auto switch_result = evaluate_switch_result( - resource_manager_, activate_request_, deactivate_request_, strictness, get_logger(), to, - message); + resource_manager_, switch_params_.activate_request, switch_params_.deactivate_request, + strictness, get_logger(), to, message); // switch lists rt_controllers_wrapper_.switch_updated_list(guard); @@ -1966,7 +1994,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co void ControllerManager::deactivate_controllers( const std::vector & rt_controller_list, - const std::vector controllers_to_deactivate) + const std::vector & controllers_to_deactivate) { // deactivate controllers for (const auto & controller_name : controllers_to_deactivate) @@ -2070,7 +2098,7 @@ void ControllerManager::switch_chained_mode( void ControllerManager::activate_controllers( const std::vector & rt_controller_list, - const std::vector controllers_to_activate) + const std::vector & controllers_to_activate) { for (const auto & controller_name : controllers_to_activate) { @@ -2222,14 +2250,6 @@ void ControllerManager::activate_controllers( } } -void ControllerManager::activate_controllers_asap( - const std::vector & rt_controller_list, - const std::vector controllers_to_activate) -{ - // https://github.com/ros-controls/ros2_control/issues/263 - activate_controllers(rt_controller_list, controllers_to_activate); -} - void ControllerManager::list_controllers_srv_cb( const std::shared_ptr, std::shared_ptr response) @@ -2717,7 +2737,8 @@ 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_)) + switch_params_.activate_command_interface_request, + switch_params_.deactivate_command_interface_request)) { RCLCPP_ERROR(get_logger(), "Error while performing mode switch."); } @@ -2725,21 +2746,13 @@ void ControllerManager::manage_switch() std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); - deactivate_controllers(rt_controller_list, deactivate_request_); + deactivate_controllers(rt_controller_list, switch_params_.deactivate_request); - switch_chained_mode(to_chained_mode_request_, true); - switch_chained_mode(from_chained_mode_request_, false); + switch_chained_mode(switch_params_.to_chained_mode_request, true); + switch_chained_mode(switch_params_.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_); - } + activate_controllers(rt_controller_list, switch_params_.activate_request); // All controllers switched --> switching done switch_params_.do_switch = false; @@ -2778,6 +2791,15 @@ controller_interface::return_type ControllerManager::update( rt_buffer_.deactivate_controllers_list.clear(); for (const auto & loaded_controller : rt_controller_list) { + if ( + switch_params_.do_switch && !switch_params_.activate_asap && + switch_params_.skip_cycle(loaded_controller)) + { + RCLCPP_DEBUG( + get_logger(), "Skipping update for controller '%s' as it is being switched", + loaded_controller.info.name.c_str()); + continue; + } // TODO(v-lopez) we could cache this information // https://github.com/ros-controls/ros2_control/issues/153 if (is_controller_active(*loaded_controller.c)) @@ -2785,8 +2807,8 @@ controller_interface::return_type ControllerManager::update( if ( switch_params_.do_switch && loaded_controller.c->is_async() && std::find( - deactivate_request_.begin(), deactivate_request_.end(), loaded_controller.info.name) != - deactivate_request_.end()) + switch_params_.deactivate_request.begin(), switch_params_.deactivate_request.end(), + loaded_controller.info.name) != switch_params_.deactivate_request.end()) { RCLCPP_DEBUG( get_logger(), "Skipping update for async controller '%s' as it is being deactivated", @@ -2934,7 +2956,7 @@ controller_interface::return_type ControllerManager::update( resource_manager_->enforce_command_limits(period); // there are controllers to (de)activate - if (switch_params_.do_switch) + if (switch_params_.do_switch && switch_params_.activate_asap) { manage_switch(); } @@ -3146,10 +3168,11 @@ void ControllerManager::propagate_deactivation_of_chained_mode( for (const auto & controller : controllers) { // get pointers to places in deactivate and activate lists ((de)activate lists have changed) - auto deactivate_list_it = - std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name); + auto deactivate_list_it = std::find( + switch_params_.deactivate_request.begin(), switch_params_.deactivate_request.end(), + controller.info.name); - if (deactivate_list_it != deactivate_request_.end()) + if (deactivate_list_it != switch_params_.deactivate_request.end()) { // if controller is not active then skip adding following-controllers to "from" chained mode // request @@ -3179,10 +3202,11 @@ void ControllerManager::propagate_deactivation_of_chained_mode( // with matching interface name to "from" chained mode list (if not already in it) if ( std::find( - from_chained_mode_request_.begin(), from_chained_mode_request_.end(), - following_ctrl_it->info.name) == from_chained_mode_request_.end()) + switch_params_.from_chained_mode_request.begin(), + switch_params_.from_chained_mode_request.end(), + following_ctrl_it->info.name) == switch_params_.from_chained_mode_request.end()) { - from_chained_mode_request_.push_back(following_ctrl_it->info.name); + switch_params_.from_chained_mode_request.push_back(following_ctrl_it->info.name); RCLCPP_DEBUG( get_logger(), "Adding controller '%s' in 'from chained mode' request.", following_ctrl_it->info.name.c_str()); @@ -3248,8 +3272,8 @@ controller_interface::return_type ControllerManager::check_following_controllers // will following controller be deactivated? if ( std::find( - deactivate_request_.begin(), deactivate_request_.end(), following_ctrl_it->info.name) != - deactivate_request_.end()) + switch_params_.deactivate_request.begin(), switch_params_.deactivate_request.end(), + following_ctrl_it->info.name) != switch_params_.deactivate_request.end()) { message = fmt::format( FMT_COMPILE( @@ -3262,8 +3286,9 @@ controller_interface::return_type ControllerManager::check_following_controllers } // check if following controller will not be activated else if ( - std::find(activate_request_.begin(), activate_request_.end(), following_ctrl_it->info.name) == - activate_request_.end()) + std::find( + switch_params_.activate_request.begin(), switch_params_.activate_request.end(), + following_ctrl_it->info.name) == switch_params_.activate_request.end()) { message = fmt::format( FMT_COMPILE( @@ -3289,14 +3314,15 @@ controller_interface::return_type ControllerManager::check_following_controllers // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) // { // // insert to the begin of activate request list to be activated before preceding controller - // activate_request_.insert(activate_request_.begin(), following_ctrl_name); + // switch_params_.activate_request.insert(switch_params_.activate_request.begin(), + // following_ctrl_name); // } if (!following_ctrl_it->c->is_in_chained_mode()) { auto found_it = std::find( - to_chained_mode_request_.begin(), to_chained_mode_request_.end(), - following_ctrl_it->info.name); - if (found_it == to_chained_mode_request_.end()) + switch_params_.to_chained_mode_request.begin(), + switch_params_.to_chained_mode_request.end(), following_ctrl_it->info.name); + if (found_it == switch_params_.to_chained_mode_request.end()) { // 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) @@ -3310,7 +3336,7 @@ controller_interface::return_type ControllerManager::check_following_controllers { resource_manager_->make_controller_reference_interfaces_available( following_ctrl_it->info.name); - to_chained_mode_request_.push_back(following_ctrl_it->info.name); + switch_params_.to_chained_mode_request.push_back(following_ctrl_it->info.name); RCLCPP_DEBUG( get_logger(), "Adding controller '%s' in 'to chained mode' request.", following_ctrl_it->info.name.c_str()); @@ -3321,11 +3347,11 @@ controller_interface::return_type ControllerManager::check_following_controllers { // Check if following controller is in 'from' chained mode list and remove it, if so auto found_it = std::find( - from_chained_mode_request_.begin(), from_chained_mode_request_.end(), - following_ctrl_it->info.name); - if (found_it != from_chained_mode_request_.end()) + switch_params_.from_chained_mode_request.begin(), + switch_params_.from_chained_mode_request.end(), following_ctrl_it->info.name); + if (found_it != switch_params_.from_chained_mode_request.end()) { - from_chained_mode_request_.erase(found_it); + switch_params_.from_chained_mode_request.erase(found_it); RCLCPP_DEBUG( get_logger(), "Removing controller '%s' in 'from chained mode' request because it " @@ -3369,8 +3395,9 @@ controller_interface::return_type ControllerManager::check_preceding_controllers { if ( is_controller_inactive(found_it->c) && - std::find(activate_request_.begin(), activate_request_.end(), preceding_controller) != - activate_request_.end()) + std::find( + switch_params_.activate_request.begin(), switch_params_.activate_request.end(), + preceding_controller) != switch_params_.activate_request.end()) { message = fmt::format( FMT_COMPILE( @@ -3382,8 +3409,9 @@ controller_interface::return_type ControllerManager::check_preceding_controllers } if ( is_controller_active(found_it->c) && - std::find(deactivate_request_.begin(), deactivate_request_.end(), preceding_controller) == - deactivate_request_.end()) + std::find( + switch_params_.deactivate_request.begin(), switch_params_.deactivate_request.end(), + preceding_controller) == switch_params_.deactivate_request.end()) { message = fmt::format( FMT_COMPILE( @@ -3403,7 +3431,8 @@ controller_interface::return_type ControllerManager::check_preceding_controllers // { // // insert to the begin of activate request list to be activated before preceding // controller - // activate_request_.insert(activate_request_.begin(), preceding_ctrl_name); + // switch_params_.activate_request.insert(switch_params_.activate_request.begin(), + // preceding_ctrl_name); // } return controller_interface::return_type::OK; diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 654240f5f3..5a6d53da1c 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -149,6 +149,17 @@ class TestControllerManagerSrvs public: TestControllerManagerSrvs() {} + ~TestControllerManagerSrvs() override + { + RCLCPP_DEBUG(cm_->get_logger(), "Stopping controller manager updater thread"); + stop_runner_ = true; + if (cm_rt_thread_.joinable()) + { + cm_rt_thread_.join(); + } + RCLCPP_DEBUG(cm_->get_logger(), "Controller manager updater thread stopped"); + } + void SetUp() override { ControllerManagerFixture::SetUp(); @@ -157,13 +168,32 @@ class TestControllerManagerSrvs void SetUpSrvsCMExecutor() { - update_timer_ = cm_->create_wall_timer( - std::chrono::milliseconds(10), - [&]() + cm_rt_thread_ = std::thread( + [&] { - cm_->read(time_, PERIOD); - cm_->update(time_, PERIOD); - cm_->write(time_, PERIOD); + // for calculating sleep time + auto const period = std::chrono::nanoseconds(1'000'000'000 / cm_->get_update_rate()); + + // for calculating the measured period of the loop + rclcpp::Time previous_time = cm_->get_clock()->now(); + std::this_thread::sleep_for(period); + std::chrono::steady_clock::time_point next_iteration_time{std::chrono::steady_clock::now()}; + + while (rclcpp::ok() && !stop_runner_) + { + auto const current_time = cm_->get_clock()->now(); + auto const measured_period = current_time - previous_time; + previous_time = current_time; + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); + next_iteration_time += period; + if (next_iteration_time < std::chrono::steady_clock::now()) + { + next_iteration_time = std::chrono::steady_clock::now(); + } + std::this_thread::sleep_until(next_iteration_time); + } }); executor_->add_node(cm_); @@ -202,7 +232,8 @@ class TestControllerManagerSrvs } protected: - rclcpp::TimerBase::SharedPtr update_timer_; + std::atomic_bool stop_runner_ = false; + std::thread cm_rt_thread_; std::future executor_spin_future_; }; diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 4737f34be5..31a0c8174a 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -150,6 +150,19 @@ CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*pr return CallbackReturn::SUCCESS; } +CallbackReturn TestController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (activation_processing_time > 0.0) + { + RCLCPP_INFO( + get_node()->get_logger(), "Sleeping for %.3f seconds to simulate activation processing time", + activation_processing_time); + std::this_thread::sleep_for(std::chrono::duration(activation_processing_time)); + } + + return CallbackReturn::SUCCESS; +} + CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) { if (simulate_cleanup_failure) diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index a28dfa420e..aa15796323 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -52,6 +52,8 @@ class TestController : public controller_interface::ControllerInterface CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; @@ -67,6 +69,7 @@ class TestController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr service_; unsigned int internal_counter = 0; + double activation_processing_time = 0.0; bool simulate_cleanup_failure = false; // Variable where we store when shutdown was called, pointer because the controller // is usually destroyed after shutdown diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 54c34bd55d..ae3f138bcc 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -1143,7 +1143,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) /// @todo add controllers in random order /// For now, adding the ordered case to see that current sorting doesn't change order { - ControllerManagerRunner cm_runner(this); cm_->add_controller( test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -1399,7 +1398,6 @@ TEST_F(TestControllerManagerSrvs, list_large_number_of_controllers_with_chains) /// @todo add controllers in random order /// For now, adding the ordered case to see that current sorting doesn't change order { - ControllerManagerRunner cm_runner(this); cm_->add_controller( test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -1465,7 +1463,6 @@ TEST_F(TestControllerManagerSrvs, list_large_number_of_controllers_with_chains) TEST_CONTROLLER_2, TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_7, TEST_CHAINED_CONTROLLER_8}; { - ControllerManagerRunner cm_runner(this); for (const auto & controller : ctrls_order) { cm_->configure_controller(controller); @@ -1674,7 +1671,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_large_chained_controller_tree) std::shuffle(controllers_list.begin(), controllers_list.end(), mers); { - ControllerManagerRunner cm_runner(this); for (auto random_chain_ctrl : random_chainable_controllers_list) { cm_->add_controller( @@ -1697,7 +1693,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_large_chained_controller_tree) // configure controllers { - ControllerManagerRunner cm_runner(this); for (auto random_ctrl : controllers_list) { cm_->configure_controller(random_ctrl); @@ -2216,3 +2211,343 @@ TEST_F(TestControllerManagerSrvs, switch_controller_failure_behaviour_on_unknown result->controller[0].required_state_interfaces, UnorderedElementsAre("joint1/position", "joint1/velocity", "joint2/position")); } + +TEST_F(TestControllerManagerSrvs, switch_controller_test_activate_asap) +{ + // create server client and request + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + // create set of chained controllers + static constexpr char TEST_CHAINED_CONTROLLER_1[] = "test_chainable_controller_name_1"; + static constexpr char TEST_CHAINED_CONTROLLER_2[] = "test_chainable_controller_name_2"; + auto test_chained_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}}; + controller_interface::InterfaceConfiguration chained_state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_chained_controller_1->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_1->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_1->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_2 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_1) + "/joint1/position"}}; + test_chained_controller_2->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_2->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_2->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + // create non-chained controller + auto test_controller = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/velocity", "joint2/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller->set_command_interface_configuration(cmd_cfg); + test_controller->set_state_interface_configuration(state_cfg); + // add controllers + cm_->add_controller( + test_chained_controller_1, TEST_CHAINED_CONTROLLER_1, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + // check chainable controller + ASSERT_EQ(3u, result->controller.size()); + ASSERT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_1); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_2); + // check test controller + ASSERT_EQ(result->controller[1].name, "test_controller_name"); + + // configure controllers + for (const auto & controller : + {TEST_CHAINED_CONTROLLER_1, test_controller::TEST_CONTROLLER_NAME, + TEST_CHAINED_CONTROLLER_2}) + { + cm_->configure_controller(controller); + } + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(3u, result->controller.size()); + + // reordered controllers + ASSERT_EQ(result->controller[0].name, "test_controller_name"); + ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_2); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_1); + + // activate controllers all at once + auto res = cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_2, test_controller::TEST_CONTROLLER_NAME}, + {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + res = cm_->switch_controller( + {}, + {TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_2, test_controller::TEST_CONTROLLER_NAME}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chained_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chained_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + + // Now test with activate_asap to false + + res = cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_2, test_controller::TEST_CONTROLLER_NAME}, + {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, false, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + res = cm_->switch_controller( + {}, + {TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_2, test_controller::TEST_CONTROLLER_NAME}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, false, rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chained_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chained_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); +} + +TEST_F(TestControllerManagerSrvs, switch_controller_controllers_taking_long_time_to_activate) +{ + // create server client and request + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + // create non-chained controllers + auto test_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller_1->set_command_interface_configuration(cmd_cfg); + test_controller_1->set_state_interface_configuration(state_cfg); + + auto test_controller_2 = std::make_shared(); + cmd_cfg = {controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/velocity"}}; + state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller_2->set_command_interface_configuration(cmd_cfg); + test_controller_2->set_state_interface_configuration(state_cfg); + + auto test_controller_3 = std::make_shared(); + cmd_cfg = {controller_interface::interface_configuration_type::INDIVIDUAL, {"joint3/velocity"}}; + state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller_3->set_command_interface_configuration(cmd_cfg); + test_controller_3->set_state_interface_configuration(state_cfg); + // take 2 seconds to activate + test_controller_3->activation_processing_time = 2.0; + + // add controllers + const std::string test_ctrl_1_name = "test_controller_name_1"; + const std::string test_ctrl_2_name = "test_controller_name_2"; + const std::string test_ctrl_3_name = "test_controller_name_3"; + + cm_->add_controller( + test_controller_1, test_ctrl_1_name, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, test_ctrl_2_name, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_3, test_ctrl_3_name, test_controller::TEST_CONTROLLER_CLASS_NAME); + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(3u, result->controller.size()); + ASSERT_EQ(result->controller[0].name, test_ctrl_1_name); + ASSERT_EQ(result->controller[1].name, test_ctrl_2_name); + ASSERT_EQ(result->controller[2].name, test_ctrl_3_name); + + // configure controllers + for (const auto & controller : {test_ctrl_1_name, test_ctrl_2_name, test_ctrl_3_name}) + { + cm_->configure_controller(controller); + } + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(3u, result->controller.size()); + + // reordered controllers + ASSERT_EQ(result->controller[2].name, test_ctrl_1_name); + ASSERT_EQ(result->controller[1].name, test_ctrl_2_name); + ASSERT_EQ(result->controller[0].name, test_ctrl_3_name); + + ASSERT_EQ(0u, test_controller_1->internal_counter); + ASSERT_EQ(0u, test_controller_2->internal_counter); + ASSERT_EQ(0u, test_controller_3->internal_counter); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + ASSERT_EQ(0u, test_controller_1->internal_counter); + ASSERT_EQ(0u, test_controller_2->internal_counter); + ASSERT_EQ(0u, test_controller_3->internal_counter); + + // activate non time taking controllers + { + auto res = cm_->switch_controller( + {test_ctrl_1_name, test_ctrl_2_name}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + } + unsigned int old_counter_1 = test_controller_1->internal_counter; + unsigned int old_counter_2 = test_controller_2->internal_counter; + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + ASSERT_GE(test_controller_1->internal_counter, old_counter_1 + 1); + ASSERT_GE(test_controller_2->internal_counter, old_counter_2 + 1); + EXPECT_EQ(old_counter_1 + 1, test_controller_1->internal_counter); + EXPECT_EQ(old_counter_2 + 1, test_controller_2->internal_counter); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + + // Now let's activate the time taking controller with activate_asap set to true + old_counter_1 = test_controller_1->internal_counter; + old_counter_2 = test_controller_2->internal_counter; + { + auto res = cm_->switch_controller( + {test_ctrl_3_name}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + } + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); + + // In 2 seconds, the others should do much more cycles but as the switch is happening in the RT + // loop, it blocked the update for the long time. + ASSERT_THAT( + test_controller_1->internal_counter, + testing::AllOf(testing::Gt(old_counter_1), testing::Lt((old_counter_1 + 4)))); + ASSERT_THAT( + test_controller_2->internal_counter, + testing::AllOf(testing::Gt(old_counter_2), testing::Lt((old_counter_2 + 4)))); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // Now, let's deactivate and activate again but with activate_asap as false + { + auto res = cm_->switch_controller( + {}, {test_ctrl_3_name}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + } + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + + old_counter_1 = test_controller_1->internal_counter; + old_counter_2 = test_controller_2->internal_counter; + { + auto res = cm_->switch_controller( + {test_ctrl_3_name}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, + false, rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + } + const auto ideal_cycles = + cm_->get_update_rate() * + static_cast(test_controller_3->activation_processing_time); + + ASSERT_GT(test_controller_1->internal_counter, old_counter_1 + ideal_cycles - 20); + ASSERT_GT(test_controller_2->internal_counter, old_counter_2 + ideal_cycles - 20); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); + + old_counter_1 = test_controller_1->internal_counter; + old_counter_2 = test_controller_2->internal_counter; + const auto old_counter_3 = test_controller_3->internal_counter; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(old_counter_1 + 1, test_controller_1->internal_counter); + EXPECT_EQ(old_counter_2 + 1, test_controller_2->internal_counter); + EXPECT_EQ(old_counter_3 + 1, test_controller_3->internal_counter); +} diff --git a/doc/migration.rst b/doc/migration.rst index 4e08aa96ad..89bb5ad158 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -5,6 +5,13 @@ Migration Guides: Jazzy to Kilted This list summarizes important changes between Jazzy (previous) and Kilted (current) releases, where changes to user code might be necessary. +controller_manager +****************** + +* The spawner now supports two new arguments ``--switch-asap`` and ``--no-switch-asap`` to control the behaviour of the spawner when switching controllers to be in realtime loop (or) non-realtime loop. + By default, it is set to ``--no-switch-asap`` because when activating multiple controllers at same time might affect the realtime loop performance (`#2452 `_) + If it is needed to switch controllers in realtime loop, then the argument ``--switch-asap`` need to be parsed to the spawner. + hardware_interface ****************** diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 5ae2ad3480..0194208791 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -9,6 +9,8 @@ controller_manager ****************** * The default strictness of the ``switch_controllers`` can now we be chosen using ROS 2 parameters. The default behaviour is still left to ``BEST_EFFORT`` (`#2168 `_). * Parameter ``shutdown_on_initial_state_failure`` was added to avoid shutting down on hardware initial state failure (`#2230 `_). +* The controller manager now supports switching (activating and deactivating) controllers in both realtime and non-realtime modes. This is controlled by the parameter ``activate_asap`` of the ``switch_controllers`` service (`#2452 `_). +* The spawner now supports two new arguments ``--switch-asap`` and ``--no-switch-asap`` to control the behaviour of the spawner when switching controllers to be in realtime loop (or) non-realtime loop. By default, it is set to ``--no-switch-asap`` because when activating multiple controllers at same time might affect the realtime loop performance (`#2452 `_). hardware_interface ****************** diff --git a/hardware_interface/include/hardware_interface/helpers.hpp b/hardware_interface/include/hardware_interface/helpers.hpp index 605ecb9216..080c833845 100644 --- a/hardware_interface/include/hardware_interface/helpers.hpp +++ b/hardware_interface/include/hardware_interface/helpers.hpp @@ -38,7 +38,7 @@ template { if constexpr (std::is_same_v>) { - return std::find(container.begin(), container.end(), item); + return std::find(container.cbegin(), container.cend(), item); } else if constexpr ( std::is_same_v> || @@ -68,7 +68,7 @@ template template [[nodiscard]] bool has_item(const Container & container, const T & item) { - return get_item_iterator(container, item) != container.end(); + return get_item_iterator(container, item) != container.cend(); } /**