Bug fix for switch_controller when using controller chaining

controller_manager/src/controller_manager.cpp

@@ -1013,105 +1013,165 @@ controller_interface::return_type ControllerManager::switch_controller(
 
   const std::vector<ControllerSpec> & controllers = rt_controllers_wrapper_.get_updated_list(guard);
 
-  // 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)
+  enum class CreateRequestResult
   {
-    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;
+    OK,
+    ERROR,
+    RETRY
+  };
 
-    // if controller is not inactive then do not do any following-controllers checks
-    if (!is_controller_inactive(controller_it->c))
+  const auto check_de_activate_request_and_create_chained_mode_request =
+    [this, &strictness, &controllers]() -> CreateRequestResult
+  {
+    const auto clear_chained_mode_requests = [this]()
     {
-      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
+      // 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)
     {
-      status = check_following_controllers_for_activate(controllers, strictness, controller_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;
+        }
+      }
     }
 
-    if (status != controller_interface::return_type::OK)
+    // 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)
     {
-      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)
+      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))
       {
-        // 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);
-        --ctrl_it;
+        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;
       }
-      if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
+      else
       {
-        RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)");
-        // reset all lists
-        clear_requests();
-        return controller_interface::return_type::ERROR;
+        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;
+        }
       }
     }
-  }
 
-  // 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;
+    return CreateRequestResult::OK;
+  };
 
-    // if controller is not active then skip preceding-controllers checks
-    if (!is_controller_active(controller_it->c))
+  // Validate the (de)activate request and create from/to chained mode requests as needed.
+  // If the strictness value is STRICT, return an error for any invalid request.
+  // If the strictness value is BEST_EFFORT, remove any controllers that cannot be
+  // (de)activated from the request and proceed. However, any changes to the
+  // (de)activate request will affect the outcome of the check and creation process,
+  // so retry from the beginning.
+  while (true)
+  {
+    const auto result = check_de_activate_request_and_create_chained_mode_request();
+    if (result == CreateRequestResult::RETRY)
     {
-      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;
+      continue;
     }
-    else
+    if (result == CreateRequestResult::ERROR)
     {
-      status = check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it);
+      return controller_interface::return_type::ERROR;
     }
+    // if result == CreateRequestResult::OK -> break the loop
+    break;
+  }
 
-    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);
-        --ctrl_it;
-      }
-      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 controller_interface::return_type::ERROR;
-      }
-    }
+  // 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");
+    clear_requests();
+    return controller_interface::return_type::OK;
   }
 
   // Check after the check if the activate and deactivate list is empty or not