diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index ec17bec402..980b8c74ec 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -143,6 +143,8 @@ class ControllerManager : public rclcpp::Node * \param[in] activate_controllers is a list of controllers to activate. * \param[in] deactivate_controllers is a list of controllers to deactivate. * \param[in] set level of strictness (BEST_EFFORT or STRICT) + * \param[in] activate_asap flag to activate controllers as soon as possible. + * \param[in] timeout to wait for the controllers to be switched. * \see Documentation in controller_manager_msgs/SwitchController.srv */ controller_interface::return_type switch_controller( @@ -151,6 +153,21 @@ class ControllerManager : public rclcpp::Node bool activate_asap = kWaitForAllResources, const rclcpp::Duration & timeout = rclcpp::Duration::from_nanoseconds(kInfiniteTimeout)); + /// switch_controller Deactivates some controllers and activates others. + /** + * \param[in] activate_controllers is a list of controllers to activate. + * \param[in] deactivate_controllers is a list of controllers to deactivate. + * \param[in] set level of strictness (BEST_EFFORT or STRICT) + * \param[in] activate_asap flag to activate controllers as soon as possible. + * \param[in] timeout to wait for the controllers to be switched. + * \param[out] message describing the result of the switch. + * \see Documentation in controller_manager_msgs/SwitchController.srv + */ + controller_interface::return_type switch_controller_cb( + const std::vector & activate_controllers, + const std::vector & deactivate_controllers, int strictness, bool activate_asap, + const rclcpp::Duration & timeout, std::string & message); + /// Read values to state interfaces. /** * Read current values from hardware to state interfaces. @@ -364,6 +381,7 @@ class ControllerManager : public rclcpp::Node * controllers will be automatically added to the activate request list if they are not in the * deactivate request. * \param[in] controller_it iterator to the controller for which the following controllers are + * \param[out] message describing the result of the check. * checked. * * \returns return_type::OK if all following controllers pass the checks, otherwise @@ -371,7 +389,7 @@ class ControllerManager : public rclcpp::Node */ controller_interface::return_type check_following_controllers_for_activate( const std::vector & controllers, int strictness, - const ControllersListIterator controller_it); + const ControllersListIterator controller_it, std::string & message); /// Check if all the preceding controllers will be in inactive state after controllers' switch. /** @@ -388,24 +406,39 @@ class ControllerManager : public rclcpp::Node * controllers will be automatically added to the deactivate request list. * \param[in] controller_it iterator to the controller for which the preceding controllers are * checked. + * \param[out] message describing the result of the check. * * \returns return_type::OK if all preceding controllers pass the checks, otherwise * return_type::ERROR. */ controller_interface::return_type check_preceeding_controllers_for_deactivate( const std::vector & controllers, int strictness, - const ControllersListIterator controller_it); + const ControllersListIterator controller_it, std::string & message); /// Checks if the fallback controllers of the given controllers are in the right /// state, so they can be activated immediately /** * \param[in] controllers is a list of controllers to activate. * \param[in] controller_it is the iterator pointing to the controller to be activated. + * \param[out] message describing the result of the check. * \return return_type::OK if all fallback controllers are in the right state, otherwise * return_type::ERROR. */ controller_interface::return_type check_fallback_controllers_state_pre_activation( - const std::vector & controllers, const ControllersListIterator controller_it); + const std::vector & controllers, const ControllersListIterator controller_it, + std::string & message); + + /** + * Checks that all the interfaces required by the controller are available to activate. + * + * \param[in] controllers list with controllers. + * \param[in] activation_list list with controllers to activate. + * \param[out] message describing the result of the check. + * \return return_type::OK if all interfaces are available, otherwise return_type::ERROR. + */ + controller_interface::return_type check_for_interfaces_availability_to_activate( + const std::vector & controllers, const std::vector activation_list, + std::string & message); /** * @brief Inserts a controller into an ordered list based on dependencies to compute the diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f89fe11a4b..4b784c1f31 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1111,13 +1111,23 @@ controller_interface::return_type ControllerManager::switch_controller( const std::vector & activate_controllers, const std::vector & deactivate_controllers, int strictness, bool activate_asap, const rclcpp::Duration & timeout) +{ + std::string message; + return switch_controller_cb( + activate_controllers, deactivate_controllers, strictness, activate_asap, timeout, message); +} + +controller_interface::return_type ControllerManager::switch_controller_cb( + const std::vector & activate_controllers, + const std::vector & deactivate_controllers, int strictness, bool activate_asap, + const rclcpp::Duration & timeout, std::string & message) { if (!is_resource_manager_initialized()) { - RCLCPP_ERROR( - get_logger(), + message = "Resource Manager is not initialized yet! Please provide robot description on " - "'robot_description' topic before trying to switch controllers."); + "'robot_description' topic before trying to switch controllers."; + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } @@ -1180,10 +1190,10 @@ controller_interface::return_type ControllerManager::switch_controller( get_logger(), !deactivate_list.empty(), "Deactivating controllers: [ %s]", deactivate_list.c_str()); - const auto list_controllers = [this, strictness]( - const std::vector & controller_list, - std::vector & request_list, - const std::string & action) + const auto list_controllers = + [this, strictness]( + const std::vector & controller_list, std::vector & request_list, + const std::string & action, std::string & msg) -> controller_interface::return_type { // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); @@ -1200,16 +1210,17 @@ controller_interface::return_type ControllerManager::switch_controller( if (found_it == updated_controllers.end()) { - RCLCPP_WARN( - get_logger(), - "Could not '%s' controller with name '%s' because no controller with this name exists", - action.c_str(), controller.c_str()); + const std::string error_msg = "Could not " + action + " controller with name '" + + controller + "' because no controller with this name exists"; + msg += error_msg + "\n"; + RCLCPP_WARN(get_logger(), "%s", error_msg.c_str()); // For the BEST_EFFORT switch, if there are more controllers that are in the list, this is // not a critical error result = request_list.empty() ? controller_interface::return_type::ERROR : controller_interface::return_type::OK; if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { + msg = error_msg; RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! ('STRICT' switch)"); return controller_interface::return_type::ERROR; } @@ -1230,7 +1241,7 @@ controller_interface::return_type ControllerManager::switch_controller( }; // list all controllers to deactivate (check if all controllers exist) - auto ret = list_controllers(deactivate_controllers, deactivate_request_, "deactivate"); + auto ret = list_controllers(deactivate_controllers, deactivate_request_, "deactivate", message); if (ret != controller_interface::return_type::OK) { deactivate_request_.clear(); @@ -1238,13 +1249,15 @@ controller_interface::return_type ControllerManager::switch_controller( } // list all controllers to activate (check if all controllers exist) - ret = list_controllers(activate_controllers, activate_request_, "activate"); + ret = list_controllers(activate_controllers, activate_request_, "activate", message); if (ret != controller_interface::return_type::OK) { deactivate_request_.clear(); activate_request_.clear(); return ret; } + // If it is a best effort switch, we can remove the controllers log that could not be activated + message.clear(); // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); @@ -1264,23 +1277,37 @@ controller_interface::return_type ControllerManager::switch_controller( controller_interface::return_type status = controller_interface::return_type::OK; // if controller is not inactive then do not do any following-controllers checks - if (!is_controller_inactive(controller_it->c)) + if (is_controller_unconfigured(*controller_it->c)) { - RCLCPP_WARN( - get_logger(), - "Controller with name '%s' is not inactive so its following " - "controllers do not have to be checked, because it cannot be activated.", - controller_it->info.name.c_str()); + message = "Controller with name '" + controller_it->info.name + + "' is in 'unconfigured' state. The controller needs to be configured to be in " + "'inactive' state before it can be checked and activated."; + RCLCPP_WARN(get_logger(), "%s", message.c_str()); + status = controller_interface::return_type::ERROR; + } + else if (is_controller_active(controller_it->c)) + { + message = "Controller with name '" + controller_it->info.name + "' is already active."; + RCLCPP_WARN(get_logger(), "%s", message.c_str()); + status = controller_interface::return_type::ERROR; + } + else if (!is_controller_inactive(controller_it->c)) + { + message = "Controller with name '" + controller_it->info.name + + "' is not in 'inactive' state. The controller needs to be in 'inactive' state " + "before it can be checked and activated."; + RCLCPP_WARN(get_logger(), "%s", message.c_str()); status = controller_interface::return_type::ERROR; } else { - status = check_following_controllers_for_activate(controllers, strictness, controller_it); + status = + check_following_controllers_for_activate(controllers, strictness, controller_it, message); } if (status == controller_interface::return_type::OK) { - status = check_fallback_controllers_state_pre_activation(controllers, controller_it); + status = check_fallback_controllers_state_pre_activation(controllers, controller_it, message); } if (status != controller_interface::return_type::OK) @@ -1299,6 +1326,7 @@ controller_interface::return_type ControllerManager::switch_controller( // 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); + message.clear(); --ctrl_it; } if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) @@ -1329,7 +1357,8 @@ controller_interface::return_type ControllerManager::switch_controller( } else { - status = check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it); + status = check_preceeding_controllers_for_deactivate( + controllers, strictness, controller_it, message); } if (status != controller_interface::return_type::OK) @@ -1345,6 +1374,7 @@ controller_interface::return_type ControllerManager::switch_controller( // 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); + message.clear(); --ctrl_it; } if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) @@ -1360,10 +1390,12 @@ controller_interface::return_type ControllerManager::switch_controller( // Check after the check if the activate and deactivate list is empty or not if (activate_request_.empty() && deactivate_request_.empty()) { - RCLCPP_INFO(get_logger(), "Empty activate and deactivate list, not requesting switch"); + message = "After checking the controllers, no controllers need to be activated or deactivated."; + RCLCPP_INFO(get_logger(), "%s", message.c_str()); clear_requests(); return controller_interface::return_type::OK; } + message.clear(); for (const auto & controller : controllers) { @@ -1405,6 +1437,7 @@ controller_interface::return_type ControllerManager::switch_controller( { if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { + message = msg; RCLCPP_ERROR(get_logger(), "%s", msg.c_str()); deactivate_request_.clear(); deactivate_command_interface_request_.clear(); @@ -1511,11 +1544,20 @@ controller_interface::return_type ControllerManager::switch_controller( if (activate_request_.empty() && 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"); clear_requests(); return controller_interface::return_type::OK; } + if ( + check_for_interfaces_availability_to_activate(controllers, activate_request_, message) != + controller_interface::return_type::OK) + { + clear_requests(); + return controller_interface::return_type::ERROR; + } + RCLCPP_DEBUG(get_logger(), "Request for command interfaces from activating controllers:"); for (const auto & interface : activate_command_interface_request_) { @@ -1545,9 +1587,8 @@ controller_interface::return_type ControllerManager::switch_controller( if (!resource_manager_->prepare_command_mode_switch( activate_command_interface_request_, deactivate_command_interface_request_)) { - RCLCPP_ERROR( - get_logger(), - "Could not switch controllers since prepare command mode switch was rejected."); + message = "Could not switch controllers since prepare command mode switch was rejected."; + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); clear_requests(); return controller_interface::return_type::ERROR; } @@ -1572,9 +1613,10 @@ controller_interface::return_type ControllerManager::switch_controller( if (!switch_params_.cv.wait_for( switch_params_guard, switch_params_.timeout, [this] { return !switch_params_.do_switch; })) { - RCLCPP_ERROR( - get_logger(), "Switch controller timed out after %f seconds!", - static_cast(switch_params_.timeout.count()) / 1e9); + message = "Switch controller timed out after " + + std::to_string(static_cast(switch_params_.timeout.count()) / 1e9) + + " seconds!"; + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); clear_requests(); return controller_interface::return_type::ERROR; } @@ -1584,7 +1626,10 @@ controller_interface::return_type ControllerManager::switch_controller( to = controllers; // update the claimed interface controller info + message.clear(); auto switch_result = controller_interface::return_type::OK; + std::string unable_to_activate_controllers(""); + std::string unable_to_deactivate_controllers(""); for (auto & controller : to) { if (is_controller_active(controller.c)) @@ -1611,6 +1656,7 @@ controller_interface::return_type ControllerManager::switch_controller( { if (!is_controller_active(controller.c)) { + unable_to_activate_controllers += controller.info.name + " "; RCLCPP_ERROR( get_logger(), "Could not activate controller : '%s'", controller.info.name.c_str()); switch_result = controller_interface::return_type::ERROR; @@ -1626,12 +1672,30 @@ controller_interface::return_type ControllerManager::switch_controller( { if (is_controller_active(controller.c)) { + unable_to_deactivate_controllers += controller.info.name + " "; RCLCPP_ERROR( get_logger(), "Could not deactivate controller : '%s'", controller.info.name.c_str()); switch_result = controller_interface::return_type::ERROR; } } } + if (switch_result != controller_interface::return_type::OK) + { + message = "Failed switching controllers.... "; + if (!unable_to_activate_controllers.empty()) + { + message += "\nUnable to activate controllers: [ " + unable_to_activate_controllers + "]. "; + } + if (!unable_to_deactivate_controllers.empty()) + { + message += + "\nUnable to deactivate controllers: [ " + unable_to_deactivate_controllers + "]. "; + } + } + else + { + message = "Successfully switched controllers!"; + } // switch lists rt_controllers_wrapper_.switch_updated_list(guard); @@ -2263,10 +2327,10 @@ void ControllerManager::switch_controller_service_cb( std::lock_guard guard(services_lock_); RCLCPP_DEBUG(get_logger(), "switching service locked"); - response->ok = - switch_controller( - request->activate_controllers, request->deactivate_controllers, request->strictness, - request->activate_asap, request->timeout) == controller_interface::return_type::OK; + response->ok = switch_controller_cb( + request->activate_controllers, request->deactivate_controllers, + request->strictness, request->activate_asap, request->timeout, + response->message) == controller_interface::return_type::OK; RCLCPP_DEBUG(get_logger(), "switching service finished"); } @@ -2870,7 +2934,7 @@ void ControllerManager::propagate_deactivation_of_chained_mode( controller_interface::return_type ControllerManager::check_following_controllers_for_activate( const std::vector & controllers, int strictness, - const ControllersListIterator controller_it) + const ControllersListIterator controller_it, std::string & message) { // we assume that the controller exists is checked in advance RCLCPP_DEBUG( @@ -2909,11 +2973,11 @@ controller_interface::return_type ControllerManager::check_following_controllers // check if following controller is chainable if (!following_ctrl_it->c->is_chainable()) { - RCLCPP_WARN( - get_logger(), - "No state/reference interface '%s' exist, since the following controller with name " - "'%s' is not chainable.", - ctrl_itf_name.c_str(), following_ctrl_it->info.name.c_str()); + message = "No state/reference interface from controller : '" + ctrl_itf_name + + "' exist, since the following " + "controller with name '" + + following_ctrl_it->info.name + "' is not chainable."; + RCLCPP_WARN(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } @@ -2925,9 +2989,9 @@ controller_interface::return_type ControllerManager::check_following_controllers deactivate_request_.begin(), deactivate_request_.end(), following_ctrl_it->info.name) != deactivate_request_.end()) { - RCLCPP_WARN( - get_logger(), "The following controller with name '%s' will be deactivated.", - following_ctrl_it->info.name.c_str()); + message = "The following controller with name '" + following_ctrl_it->info.name + + "' is currently active but it is requested to be deactivated."; + RCLCPP_WARN(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } } @@ -2936,17 +3000,17 @@ controller_interface::return_type ControllerManager::check_following_controllers std::find(activate_request_.begin(), activate_request_.end(), following_ctrl_it->info.name) == activate_request_.end()) { - RCLCPP_WARN( - get_logger(), - "The following controller with name '%s' is not active and will not be activated.", - following_ctrl_it->info.name.c_str()); + message = "The following controller with name '" + following_ctrl_it->info.name + + "' is currently inactive and it is not requested to be activated."; + RCLCPP_WARN(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } // Trigger recursion to check all the following controllers only if they are OK, add this // controller update chained mode requests if ( - check_following_controllers_for_activate(controllers, strictness, following_ctrl_it) == + check_following_controllers_for_activate( + controllers, strictness, following_ctrl_it, message) == controller_interface::return_type::ERROR) { return controller_interface::return_type::ERROR; @@ -3007,7 +3071,7 @@ controller_interface::return_type ControllerManager::check_following_controllers controller_interface::return_type ControllerManager::check_preceeding_controllers_for_deactivate( const std::vector & controllers, int /*strictness*/, - const ControllersListIterator controller_it) + const ControllersListIterator controller_it, std::string & message) { // if not chainable no need for any checks if (!controller_it->c->is_chainable()) @@ -3040,11 +3104,10 @@ controller_interface::return_type ControllerManager::check_preceeding_controller std::find(activate_request_.begin(), activate_request_.end(), preceeding_controller) != activate_request_.end()) { - RCLCPP_WARN( - get_logger(), - "Could not deactivate controller with name '%s' because " - "preceding controller with name '%s' is inactive and will be activated.", - controller_it->info.name.c_str(), preceeding_controller.c_str()); + message = "Unable to deactivate controller with name '" + controller_it->info.name + + "' because preceding controller with name '" + preceeding_controller + + "' is inactive and will be activated."; + RCLCPP_WARN(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } if ( @@ -3052,11 +3115,10 @@ controller_interface::return_type ControllerManager::check_preceeding_controller std::find(deactivate_request_.begin(), deactivate_request_.end(), preceeding_controller) == deactivate_request_.end()) { - RCLCPP_WARN( - get_logger(), - "Could not deactivate controller with name '%s' because " - "preceding controller with name '%s' is active and will not be deactivated.", - controller_it->info.name.c_str(), preceeding_controller.c_str()); + message = "Unable to deactivate controller with name '" + controller_it->info.name + + "' because preceding controller with name '" + preceeding_controller + + "' is currently active and will not be deactivated."; + RCLCPP_WARN(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } } @@ -3077,7 +3139,8 @@ controller_interface::return_type ControllerManager::check_preceeding_controller controller_interface::return_type ControllerManager::check_fallback_controllers_state_pre_activation( - const std::vector & controllers, const ControllersListIterator controller_it) + const std::vector & controllers, const ControllersListIterator controller_it, + std::string & message) { for (const auto & fb_ctrl : controller_it->info.fallback_controllers_names) { @@ -3086,22 +3149,19 @@ ControllerManager::check_fallback_controllers_state_pre_activation( std::bind(controller_name_compare, std::placeholders::_1, fb_ctrl)); if (fb_ctrl_it == controllers.end()) { - RCLCPP_ERROR( - get_logger(), - "Unable to find the fallback controller : '%s' of the controller : '%s' " - "within the controller list", - fb_ctrl.c_str(), controller_it->info.name.c_str()); + message = "Unable to find the fallback controller : '" + fb_ctrl + "' of the controller : '" + + controller_it->info.name + "' within the controller list"; + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } else { if (!(is_controller_inactive(fb_ctrl_it->c) || is_controller_active(fb_ctrl_it->c))) { - RCLCPP_ERROR( - get_logger(), - "Controller with name '%s' cannot be activated, as it's fallback controller : '%s'" - " need to be configured and be in inactive/active state!", - controller_it->info.name.c_str(), fb_ctrl.c_str()); + message = "Controller with name '" + controller_it->info.name + + "' cannot be activated, as its fallback controller : '" + fb_ctrl + + "' need to be configured and be in inactive/active state!"; + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } for (const auto & fb_cmd_itf : fb_ctrl_it->c->command_interface_configuration().names) @@ -3130,37 +3190,35 @@ ControllerManager::check_fallback_controllers_state_pre_activation( std::find(exported_ref_itfs.begin(), exported_ref_itfs.end(), fb_cmd_itf) == exported_ref_itfs.end()) { - RCLCPP_ERROR( - get_logger(), - "Controller with name '%s' cannot be activated, as the command interface : " - "'%s' required by its fallback controller : '%s' is not exported by the " - "controller : '%s' in the current fallback list!", - controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), - following_ctrl_it->info.name.c_str()); + message = "Controller with name '" + controller_it->info.name + + "' cannot be activated, as the command interface : '" + fb_cmd_itf + + "' required by its fallback controller : '" + fb_ctrl + + "' is not exported by the controller : '" + + following_ctrl_it->info.name + "' in the current fallback list!"; + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } } else { - RCLCPP_ERROR( - get_logger(), - "Controller with name '%s' cannot be activated, as the command interface : " - "'%s' required by its fallback controller : '%s' is not available as the " - "controller is not in active state!. May be consider adding this controller to " - "the fallback list of the controller : '%s' or already have it activated.", - controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), - following_ctrl_it->info.name.c_str()); + message = + "Controller with name '" + controller_it->info.name + + "' cannot be activated, as the command interface : '" + fb_cmd_itf + + "' required by its fallback controller : '" + fb_ctrl + + "' is not available as the controller is not in active state!. May be " + "consider adding this controller to the fallback list of the controller : '" + + following_ctrl_it->info.name + "' or already have it activated."; + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } } } else { - RCLCPP_ERROR( - get_logger(), - "Controller with name '%s' cannot be activated, as not all of its fallback " - "controller's : '%s' command interfaces are currently available!", - controller_it->info.name.c_str(), fb_ctrl.c_str()); + message = "Controller with name '" + controller_it->info.name + + "' cannot be activated, as not all of its fallback controller's : '" + + fb_ctrl + "' command interfaces are currently available!"; + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } } @@ -3191,37 +3249,35 @@ ControllerManager::check_fallback_controllers_state_pre_activation( std::find(exported_state_itfs.begin(), exported_state_itfs.end(), fb_state_itf) == exported_state_itfs.end()) { - RCLCPP_ERROR( - get_logger(), - "Controller with name '%s' cannot be activated, as the state interface : " - "'%s' required by its fallback controller : '%s' is not exported by the " - "controller : '%s' in the current fallback list!", - controller_it->info.name.c_str(), fb_state_itf.c_str(), fb_ctrl.c_str(), - following_ctrl_it->info.name.c_str()); + message = "Controller with name '" + controller_it->info.name + + "' cannot be activated, as the state interface : '" + fb_state_itf + + "' required by its fallback controller : '" + fb_ctrl + + "' is not exported by the controller : '" + + following_ctrl_it->info.name + "' in the current fallback list!"; + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } } else { - RCLCPP_ERROR( - get_logger(), - "Controller with name '%s' cannot be activated, as the state interface : " - "'%s' required by its fallback controller : '%s' is not available as the " - "controller is not in active state!. May be consider adding this controller to " - "the fallback list of the controller : '%s' or already have it activated.", - controller_it->info.name.c_str(), fb_state_itf.c_str(), fb_ctrl.c_str(), - following_ctrl_it->info.name.c_str()); + message = + "Controller with name '" + controller_it->info.name + + "' cannot be activated, as the state interface : '" + fb_state_itf + + "' required by its fallback controller : '" + fb_ctrl + + "' is not available as the controller is not in active state!. May be " + "consider adding this controller to the fallback list of the controller : '" + + following_ctrl_it->info.name + "' or already have it activated."; + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } } } else { - RCLCPP_ERROR( - get_logger(), - "Controller with name '%s' cannot be activated, as not all of its fallback " - "controller's : '%s' state interfaces are currently available!", - controller_it->info.name.c_str(), fb_ctrl.c_str()); + message = "Controller with name '" + controller_it->info.name + + "' cannot be activated, as not all of its fallback controller's : '" + + fb_ctrl + "' state interfaces are currently available!"; + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } } @@ -3231,6 +3287,52 @@ ControllerManager::check_fallback_controllers_state_pre_activation( return controller_interface::return_type::OK; } +controller_interface::return_type ControllerManager::check_for_interfaces_availability_to_activate( + const std::vector & controllers, const std::vector activation_list, + std::string & message) +{ + for (const auto & controller_name : activation_list) + { + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (controller_it == controllers.end()) + { + message = + "Unable to find the controller : '" + controller_name + "' within the controller list"; + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); + return controller_interface::return_type::ERROR; + } + const auto controller_cmd_interfaces = + controller_it->c->command_interface_configuration().names; + const auto controller_state_interfaces = + controller_it->c->state_interface_configuration().names; + + // check if the interfaces are available in the first place + for (const auto & cmd_itf : controller_cmd_interfaces) + { + if (!resource_manager_->command_interface_is_available(cmd_itf)) + { + message = "Unable to activate controller '" + controller_it->info.name + + "' since the command interface '" + cmd_itf + "' is not available."; + RCLCPP_WARN(get_logger(), "%s", message.c_str()); + return controller_interface::return_type::ERROR; + } + } + for (const auto & state_itf : controller_state_interfaces) + { + if (!resource_manager_->state_interface_is_available(state_itf)) + { + message = "Unable to activate controller '" + controller_it->info.name + + "' since the state interface '" + state_itf + "' is not available."; + RCLCPP_WARN(get_logger(), "%s", message.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + return controller_interface::return_type::OK; +} + void ControllerManager::controller_activity_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { diff --git a/controller_manager_msgs/srv/SwitchController.srv b/controller_manager_msgs/srv/SwitchController.srv index 840433d9a8..f000aaf61a 100644 --- a/controller_manager_msgs/srv/SwitchController.srv +++ b/controller_manager_msgs/srv/SwitchController.srv @@ -17,6 +17,7 @@ # The return value "ok" indicates if the controllers were switched # successfully or not. The meaning of success depends on the # specified strictness. +# The return value "message" provides some human-readable information string[] activate_controllers @@ -28,3 +29,4 @@ bool activate_asap builtin_interfaces/Duration timeout --- bool ok +string message diff --git a/ros2controlcli/ros2controlcli/verb/switch_controllers.py b/ros2controlcli/ros2controlcli/verb/switch_controllers.py index 60ceb000c4..0d4d742038 100644 --- a/ros2controlcli/ros2controlcli/verb/switch_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/switch_controllers.py @@ -63,12 +63,8 @@ def main(self, *, args): args.switch_timeout, ) if not response.ok: - print( - bcolors.FAIL - + "Error switching controllers, check controller_manager logs" - + bcolors.ENDC - ) + print(bcolors.FAIL + response.message + bcolors.ENDC) return 1 - print(bcolors.OKBLUE + "Successfully switched controllers" + bcolors.ENDC) + print(bcolors.OKBLUE + response.message + bcolors.ENDC) return 0