refactor (de)activate request check

Author: TakashiSato

controller_manager/include/controller_manager/controller_manager.hpp

@@ -329,6 +329,13 @@ class ControllerManager : public rclcpp::Node
   std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;
 
 private:
+  enum class CheckDeActivateRequestResult
+  {
+    OK,
+    ERROR,
+    RETRY
+  };
+
   std::vector<std::string> get_controller_names();
   std::pair<std::string, std::string> split_command_interface(
     const std::string & command_interface);
@@ -339,6 +346,7 @@ class ControllerManager : public rclcpp::Node
    * and "control loop" threads.
    */
   void clear_requests();
+  void clear_chained_mode_requests();
 
   /**
    * If a controller is deactivated all following controllers (if any exist) should be switched
@@ -377,6 +385,23 @@ class ControllerManager : public rclcpp::Node
     const std::vector<ControllerSpec> & controllers, int strictness,
     const ControllersListIterator controller_it);
 
+  /**
+   * Check if all activate requests are valid.
+   * Perform a detailed check internally by calling check_following_controllers_for_activate.
+   *
+   * \param[in] controllers list with controllers.
+   * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all following
+   * controllers will be automatically added to the activate request list if they are not in the
+   * deactivate request.
+   *
+   * \returns If all activate requests pass the check, return CheckDeActivateRequestResult::OK. If
+   * an error occurs during the check, the processing will differ based on the value of strictness:
+   * if BEST_EFFORT, erase the target controller from the activate_request and return
+   * CheckDeActivateRequestResult::RETRY; if STRICT, return CheckDeActivateRequestResult::ERROR.
+   */
+  CheckDeActivateRequestResult check_activate_requests(
+    const std::vector<ControllerSpec> & controllers, int strictness);
+
   /// Check if all the preceding controllers will be in inactive state after controllers' switch.
   /**
    * Check that all preceding controllers of the @controller_it
@@ -400,6 +425,23 @@ class ControllerManager : public rclcpp::Node
     const std::vector<ControllerSpec> & controllers, int strictness,
     const ControllersListIterator controller_it);
 
+  /**
+   * Check if all deactivate requests are valid.
+   * Perform a detailed check internally by calling check_preceeding_controllers_for_deactivate.
+   *
+   * \param[in] controllers list with controllers.
+   * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all following
+   * controllers will be automatically added to the activate request list if they are not in the
+   * deactivate request.
+   *
+   * \returns If all deactivate requests pass the check, return CheckDeActivateRequestResult::OK. If
+   * an error occurs during the check, the processing will differ based on the value of strictness:
+   * if BEST_EFFORT, erase the target controller from the deactivate_request and return
+   * CheckDeActivateRequestResult::RETRY; if STRICT, return CheckDeActivateRequestResult::ERROR.
+   */
+  CheckDeActivateRequestResult check_deactivate_requests(
+    const std::vector<ControllerSpec> & controllers, int strictness);
+
   /**
    * @brief Inserts a controller into an ordered list based on dependencies to compute the
    * controller chain.

controller_manager/src/controller_manager.cpp

@@ -831,16 +831,21 @@ void ControllerManager::clear_requests()
 {
   deactivate_request_.clear();
   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
+  deactivate_command_interface_request_.clear();
+}
+
+void ControllerManager::clear_chained_mode_requests()
+{
+  // 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_)
   {
     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();
 }
 
 controller_interface::return_type ControllerManager::switch_controller(
@@ -963,136 +968,25 @@ controller_interface::return_type ControllerManager::switch_controller(
 
   const std::vector<ControllerSpec> & controllers = rt_controllers_wrapper_.get_updated_list(guard);
 
-  enum class CreateRequestResult
-  {
-    OK,
-    ERROR,
-    RETRY
-  };
-
   const auto check_de_activate_request_and_create_chained_mode_request =
-    [this, &strictness, &controllers]() -> CreateRequestResult
-  {
-    const auto clear_chained_mode_requests = [this]()
   {
-      // 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_)
-      {
-        resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
-      }
-      to_chained_mode_request_.clear();
-      from_chained_mode_request_.clear();
-    };
-
     // if a preceding controller is deactivated, all first-level controllers should be switched
     // 'from' chained mode
     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)
-    {
-      auto controller_it = std::find_if(
-        controllers.begin(), controllers.end(),
-        std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it));
-      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))
-      {
-        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());
-        status = controller_interface::return_type::ERROR;
-      }
-      else
-      {
-        status = check_following_controllers_for_activate(controllers, strictness, controller_it);
-      }
-
-      if (status != controller_interface::return_type::OK)
-      {
-        RCLCPP_WARN(
-          get_logger(),
-          "Could not activate controller with name '%s'. Check above warnings for more details. "
-          "Check the state of the controllers and their required interfaces using "
-          "`ros2 control list_controllers -v` CLI to get more information.",
-          (*ctrl_it).c_str());
-        if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT)
-        {
-          // TODO(destogl): automatic manipulation of the chain:
-          // || strictness ==
-          //  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);
-          // reset chained mode request lists and will retry the creation of the request
-          clear_chained_mode_requests();
-          return CreateRequestResult::RETRY;
-        }
-        if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
-        {
-          RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
-          // reset all lists
-          clear_requests();
-          return CreateRequestResult::ERROR;
-        }
-      }
-    }
-
-    // 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)
-    {
-      auto controller_it = std::find_if(
-        controllers.begin(), controllers.end(),
-        std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it));
-      controller_interface::return_type status = controller_interface::return_type::OK;
-
-      // if controller is not active then skip preceding-controllers checks
-      if (!is_controller_active(controller_it->c))
     {
-        RCLCPP_WARN(
-          get_logger(), "Controller with name '%s' can not be deactivated since it is not active.",
-          controller_it->info.name.c_str());
-        status = controller_interface::return_type::ERROR;
     }
-      else
+        result != CheckDeActivateRequestResult::OK)
     {
-        status =
-          check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it);
     }
 
-      if (status != controller_interface::return_type::OK)
-      {
-        RCLCPP_WARN(
-          get_logger(),
-          "Could not deactivate controller with name '%s'. Check above warnings for more details. "
-          "Check the state of the controllers and their required interfaces using "
-          "`ros2 control list_controllers -v` CLI to get more information.",
-          (*ctrl_it).c_str());
-        if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT)
-        {
-          // 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);
-          // reset chained mode request lists and will retry the creation of the request
-          clear_chained_mode_requests();
-          return CreateRequestResult::RETRY;
-        }
-        if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
-        {
-          RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
-          // reset all lists
-          clear_requests();
-          return CreateRequestResult::ERROR;
-        }
-      }
-    }
-
-    return CreateRequestResult::OK;
   };
 
   // Validate the (de)activate request and create from/to chained mode requests as needed.
@@ -1104,15 +998,15 @@ controller_interface::return_type ControllerManager::switch_controller(
   while (true)
   {
     const auto result = check_de_activate_request_and_create_chained_mode_request();
-    if (result == CreateRequestResult::RETRY)
     {
       continue;
     }
-    if (result == CreateRequestResult::ERROR)
     {
       return controller_interface::return_type::ERROR;
     }
-    // if result == CreateRequestResult::OK -> break the loop
     break;
   }
 
@@ -2550,6 +2444,68 @@ controller_interface::return_type ControllerManager::check_following_controllers
   return controller_interface::return_type::OK;
 };
 
+ControllerManager::CheckDeActivateRequestResult ControllerManager::check_activate_requests(
+  const std::vector<ControllerSpec> & controllers, int strictness)
+{
+  // 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)
+  {
+    auto controller_it = std::find_if(
+      controllers.begin(), controllers.end(),
+      std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it));
+    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) &&
+      std::find(deactivate_request_.begin(), deactivate_request_.end(), *ctrl_it) ==
+        deactivate_request_.end())
+    {
+      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());
+      status = controller_interface::return_type::ERROR;
+    }
+    else
+    {
+      status = check_following_controllers_for_activate(controllers, strictness, controller_it);
+    }
+
+    if (status != controller_interface::return_type::OK)
+    {
+      RCLCPP_WARN(
+        get_logger(),
+        "Could not activate controller with name '%s'. Check above warnings for more details. "
+        "Check the state of the controllers and their required interfaces using "
+        "`ros2 control list_controllers -v` CLI to get more information.",
+        (*ctrl_it).c_str());
+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT)
+      {
+        // TODO(destogl): automatic manipulation of the chain:
+        // || strictness ==
+        //  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);
+        // reset chained mode request lists and will retry the creation of the request
+        clear_chained_mode_requests();
+        return CheckDeActivateRequestResult::RETRY;
+      }
+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
+      {
+        RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
+        // reset all lists
+        clear_requests();
+        return CheckDeActivateRequestResult::ERROR;
+      }
+    }
+  }
+
+  return CheckDeActivateRequestResult::OK;
+}
+
 controller_interface::return_type ControllerManager::check_preceeding_controllers_for_deactivate(
   const std::vector<ControllerSpec> & controllers, int /*strictness*/,
   const ControllersListIterator controller_it)
@@ -2636,6 +2592,60 @@ controller_interface::return_type ControllerManager::check_preceeding_controller
   return controller_interface::return_type::OK;
 }
 
+ControllerManager::CheckDeActivateRequestResult ControllerManager::check_deactivate_requests(
+  const std::vector<ControllerSpec> & controllers, int strictness)
+{
+  // 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)
+  {
+    auto controller_it = std::find_if(
+      controllers.begin(), controllers.end(),
+      std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it));
+    controller_interface::return_type status = controller_interface::return_type::OK;
+
+    // if controller is not active then skip preceding-controllers checks
+    if (!is_controller_active(controller_it->c))
+    {
+      RCLCPP_WARN(
+        get_logger(), "Controller with name '%s' can not be deactivated since it is not active.",
+        controller_it->info.name.c_str());
+      status = controller_interface::return_type::ERROR;
+    }
+    else
+    {
+      status = check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it);
+    }
+
+    if (status != controller_interface::return_type::OK)
+    {
+      RCLCPP_WARN(
+        get_logger(),
+        "Could not deactivate controller with name '%s'. Check above warnings for more details. "
+        "Check the state of the controllers and their required interfaces using "
+        "`ros2 control list_controllers -v` CLI to get more information.",
+        (*ctrl_it).c_str());
+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT)
+      {
+        // 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);
+        // reset chained mode request lists and will retry the creation of the request
+        clear_chained_mode_requests();
+        return CheckDeActivateRequestResult::RETRY;
+      }
+      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
+      {
+        RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
+        // reset all lists
+        clear_requests();
+        return CheckDeActivateRequestResult::ERROR;
+      }
+    }
+  }
+
+  return CheckDeActivateRequestResult::OK;
+}
+
 void ControllerManager::controller_activity_diagnostic_callback(
   diagnostic_updater::DiagnosticStatusWrapper & stat)
 {