From a34b58fe625239d7455e130b520fcd6185ed6272 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 27 Feb 2025 00:42:30 +0100 Subject: [PATCH 01/14] Add message field to the SwitchController service --- controller_manager_msgs/srv/SwitchController.srv | 2 ++ 1 file changed, 2 insertions(+) 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 From d62c1709a97dd648da39f3323c752c8d97c20694 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 27 Feb 2025 01:01:06 +0100 Subject: [PATCH 02/14] [WIP] Add message string to fill in from code --- .../controller_manager/controller_manager.hpp | 6 ++- controller_manager/src/controller_manager.cpp | 48 ++++++++++--------- 2 files changed, 30 insertions(+), 24 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index ec17bec402..18c3b5be87 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -143,13 +143,17 @@ 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. + * \param[out] message describing the result of the switch. * \see Documentation in controller_manager_msgs/SwitchController.srv */ controller_interface::return_type switch_controller( const std::vector & activate_controllers, const std::vector & deactivate_controllers, int strictness, bool activate_asap = kWaitForAllResources, - const rclcpp::Duration & timeout = rclcpp::Duration::from_nanoseconds(kInfiniteTimeout)); + const rclcpp::Duration & timeout = rclcpp::Duration::from_nanoseconds(kInfiniteTimeout), + std::string & message); /// Read values to state interfaces. /** diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f89fe11a4b..70e090ada9 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1110,14 +1110,14 @@ void ControllerManager::clear_requests() 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) + 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 +1180,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 +1200,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 +1231,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 +1239,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_); @@ -1266,11 +1269,10 @@ controller_interface::return_type ControllerManager::switch_controller( // if controller is not inactive then do not do any following-controllers checks if (!is_controller_inactive(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 not inactive so its following controllers do not have to be checked, because " + "it cannot be activated."; + RCLCPP_WARN(get_logger(), "%s", message.c_str()); status = controller_interface::return_type::ERROR; } else @@ -2263,10 +2265,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( + 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"); } From 461be6b49a6f02c64688cf7eb97969814265ec2b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 2 Mar 2025 21:37:31 +0100 Subject: [PATCH 03/14] Add message propagation to `check_following_controllers_for_activate` method --- .../controller_manager/controller_manager.hpp | 3 +- controller_manager/src/controller_manager.cpp | 31 ++++++++++--------- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 18c3b5be87..42c00d9b63 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -368,6 +368,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 @@ -375,7 +376,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. /** diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 70e090ada9..8c931e8f3f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1277,7 +1277,8 @@ controller_interface::return_type ControllerManager::switch_controller( } 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) @@ -2872,7 +2873,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( @@ -2911,11 +2912,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; } @@ -2927,9 +2928,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; } } @@ -2938,17 +2939,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; From e4bd3e60f65efa92a071161d641658b73b9449df Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 2 Mar 2025 21:37:49 +0100 Subject: [PATCH 04/14] Add `switch_controller_cb` new method --- .../controller_manager/controller_manager.hpp | 19 ++++++++++++++++--- controller_manager/src/controller_manager.cpp | 12 +++++++++++- 2 files changed, 27 insertions(+), 4 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 42c00d9b63..90cbc87110 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -145,15 +145,28 @@ class ControllerManager : public rclcpp::Node * \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( const std::vector & activate_controllers, const std::vector & deactivate_controllers, int strictness, bool activate_asap = kWaitForAllResources, - const rclcpp::Duration & timeout = rclcpp::Duration::from_nanoseconds(kInfiniteTimeout), - std::string & message); + 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. /** diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8c931e8f3f..01973771c7 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1108,6 +1108,16 @@ void ControllerManager::clear_requests() } 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) @@ -2266,7 +2276,7 @@ void ControllerManager::switch_controller_service_cb( std::lock_guard guard(services_lock_); RCLCPP_DEBUG(get_logger(), "switching service locked"); - response->ok = switch_controller( + 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; From 9e076341dd2bdbe7af0712b4424c7ede8747ff7b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 2 Mar 2025 22:02:24 +0100 Subject: [PATCH 05/14] propagate message arg to `check_fallback_controllers_state_pre_activation` --- .../controller_manager/controller_manager.hpp | 4 +- controller_manager/src/controller_manager.cpp | 98 +++++++++---------- 2 files changed, 49 insertions(+), 53 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 90cbc87110..a9be32dfa7 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -419,11 +419,13 @@ class ControllerManager : public rclcpp::Node /** * \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); /** * @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 01973771c7..9b9ff2b906 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1293,7 +1293,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( 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) @@ -3090,7 +3090,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) { @@ -3099,22 +3100,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) @@ -3143,37 +3141,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; } } @@ -3204,37 +3200,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; } } From 3e6b10cc304e6c9b2c28a7414f7420dcb01ae8d8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 2 Mar 2025 22:18:38 +0100 Subject: [PATCH 06/14] propagate message arg to `check_preceeding_controllers_for_deactivate` --- .../controller_manager/controller_manager.hpp | 3 ++- controller_manager/src/controller_manager.cpp | 25 ++++++++++--------- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index a9be32dfa7..e88c03671b 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -406,13 +406,14 @@ 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 diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9b9ff2b906..436a3a0def 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1312,6 +1312,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 activate_request_.erase(ctrl_it); + message.clear(); --ctrl_it; } if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) @@ -1342,7 +1343,8 @@ controller_interface::return_type ControllerManager::switch_controller_cb( } 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) @@ -1358,6 +1360,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); + message.clear(); --ctrl_it; } if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) @@ -3020,7 +3023,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()) @@ -3053,11 +3056,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 ( @@ -3065,11 +3067,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; } } From 740c782017e5ba57f61244be807c3b98946f425e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 2 Mar 2025 22:43:30 +0100 Subject: [PATCH 07/14] Add more message introspection message logging --- controller_manager/src/controller_manager.cpp | 40 +++++++++++++++---- 1 file changed, 33 insertions(+), 7 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 436a3a0def..88adc8fbd2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1376,10 +1376,12 @@ 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()) { + 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; } + message.clear(); for (const auto & controller : controllers) { @@ -1417,10 +1419,11 @@ controller_interface::return_type ControllerManager::switch_controller_cb( std::find(activate_request_.begin(), activate_request_.end(), controller.info.name); bool in_activate_list = activate_list_it != activate_request_.end(); - auto handle_conflict = [&](const std::string & msg) + auto handle_conflict = [&, &message](const std::string & msg) { 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(); @@ -1527,6 +1530,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( 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; @@ -1561,9 +1565,8 @@ controller_interface::return_type ControllerManager::switch_controller_cb( 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; } @@ -1588,9 +1591,10 @@ controller_interface::return_type ControllerManager::switch_controller_cb( 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; } @@ -1600,7 +1604,10 @@ controller_interface::return_type ControllerManager::switch_controller_cb( 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)) @@ -1627,6 +1634,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( { 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; @@ -1642,12 +1650,30 @@ controller_interface::return_type ControllerManager::switch_controller_cb( { 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); From a2e264b122e84a98caf5ca5aaa5b1cf636e00a2b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 2 Mar 2025 22:47:20 +0100 Subject: [PATCH 08/14] Propagate the swtich controllers message response to CLI --- ros2controlcli/ros2controlcli/verb/switch_controllers.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) 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 From 0f9517173bb7f8739abcdf3843b9ae12599f1760 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 2 Mar 2025 23:40:03 +0100 Subject: [PATCH 09/14] check the controllers that activate also have their interfaces available --- controller_manager/src/controller_manager.cpp | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 88adc8fbd2..24480141cd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2921,6 +2921,29 @@ controller_interface::return_type ControllerManager::check_following_controllers 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_ERROR(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_ERROR(get_logger(), "%s", message.c_str()); + return controller_interface::return_type::ERROR; + } + } + // get all interfaces of the controller auto controller_interfaces = controller_cmd_interfaces; controller_interfaces.insert( From be1903025d135051d1ebe46c2eaa8c1ea0223c08 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 2 Mar 2025 23:43:38 +0100 Subject: [PATCH 10/14] Revert "check the controllers that activate also have their interfaces available" This reverts commit ca8e2821245b40660a1356571f837def495777c2. --- controller_manager/src/controller_manager.cpp | 23 ------------------- 1 file changed, 23 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 24480141cd..88adc8fbd2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2921,29 +2921,6 @@ controller_interface::return_type ControllerManager::check_following_controllers 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_ERROR(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_ERROR(get_logger(), "%s", message.c_str()); - return controller_interface::return_type::ERROR; - } - } - // get all interfaces of the controller auto controller_interfaces = controller_cmd_interfaces; controller_interfaces.insert( From d70af77cff4b02dc73d6a962746adb3bc540a621 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 2 Mar 2025 23:56:22 +0100 Subject: [PATCH 11/14] remove message variable reference --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 88adc8fbd2..7b85a04a14 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1419,7 +1419,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( std::find(activate_request_.begin(), activate_request_.end(), controller.info.name); bool in_activate_list = activate_list_it != activate_request_.end(); - auto handle_conflict = [&, &message](const std::string & msg) + auto handle_conflict = [&](const std::string & msg) { if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { From a47fd6e3e9131e16f7b0ac4b220b029bccd2592a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 3 Mar 2025 18:55:17 +0100 Subject: [PATCH 12/14] add method --- .../controller_manager/controller_manager.hpp | 12 +++++ controller_manager/src/controller_manager.cpp | 54 +++++++++++++++++++ 2 files changed, 66 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index e88c03671b..980b8c74ec 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -428,6 +428,18 @@ class ControllerManager : public rclcpp::Node 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 * controller chain. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 7b85a04a14..4e5ff1ca0c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1536,6 +1536,14 @@ controller_interface::return_type ControllerManager::switch_controller_cb( 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_) { @@ -3265,6 +3273,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) { From 650c818e28701aebc6e65bdcd7a8eed5a52ee22c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 4 Mar 2025 17:09:20 +0100 Subject: [PATCH 13/14] Add more informative message if the requested controller to activate is not in the desired state --- controller_manager/src/controller_manager.cpp | 20 ++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 4e5ff1ca0c..989906210d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1277,11 +1277,25 @@ controller_interface::return_type ControllerManager::switch_controller_cb( 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)) { message = "Controller with name '" + controller_it->info.name + - "' is not inactive so its following controllers do not have to be checked, because " - "it cannot be activated."; + "' 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; } From 4d16c0b378d327ae037369bacd12e34657dd6ffa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 4 Mar 2025 19:06:33 +0100 Subject: [PATCH 14/14] Update info log msg Co-authored-by: Sai Kishor Kothakota --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 989906210d..4b784c1f31 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1391,7 +1391,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( 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"); + RCLCPP_INFO(get_logger(), "%s", message.c_str()); clear_requests(); return controller_interface::return_type::OK; }