Add changes to properly use activate_asap in context of switching controllers

Author: saikishor

controller_manager/include/controller_manager/controller_manager.hpp

@@ -40,6 +40,7 @@
 #include "controller_manager_msgs/srv/unload_controller.hpp"
 
 #include "diagnostic_updater/diagnostic_updater.hpp"
+#include "hardware_interface/helpers.hpp"
 #include "hardware_interface/resource_manager.hpp"
 
 #include "pluginlib/class_loader.hpp"
@@ -259,7 +260,7 @@ class ControllerManager : public rclcpp::Node
    */
   void deactivate_controllers(
     const std::vector<ControllerSpec> & rt_controller_list,
-    const std::vector<std::string> controllers_to_deactivate);
+    const std::vector<std::string> & controllers_to_deactivate);
 
   /**
    * Switch chained mode for all the controllers with respect to the following cases:
@@ -282,22 +283,7 @@ class ControllerManager : public rclcpp::Node
    */
   void activate_controllers(
     const std::vector<ControllerSpec> & rt_controller_list,
-    const std::vector<std::string> controllers_to_activate);
-
-  /// Activate chosen controllers from real-time controller list.
-  /**
-   * Activate controllers with names \p controllers_to_activate from list \p rt_controller_list.
-   * The controller list will be iterated as many times as there are controller names.
-   *
-   * *NOTE*: There is currently not difference to `activate_controllers` method.
-   * Check https://github.com/ros-controls/ros2_control/issues/263 for more information.
-   *
-   * \param[in] rt_controller_list controllers in the real-time list.
-   * \param[in] controllers_to_activate names of the controller that have to be activated.
-   */
-  void activate_controllers_asap(
-    const std::vector<ControllerSpec> & rt_controller_list,
-    const std::vector<std::string> controllers_to_activate);
+    const std::vector<std::string> & controllers_to_activate);
 
   void list_controllers_srv_cb(
     const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,
@@ -681,18 +667,27 @@ class ControllerManager : public rclcpp::Node
       activate_asap = false;
     }
 
-    bool do_switch;
+    std::atomic_bool do_switch;
     bool started;
 
     // Switch options
     int strictness;
-    bool activate_asap;
+    std::atomic_bool activate_asap;
     std::chrono::nanoseconds timeout;
 
     // conditional variable and mutex to wait for the switch to complete
     std::condition_variable cv;
     std::mutex mutex;
 
+    bool skip_cycle(const controller_manager::ControllerSpec & spec) const
+    {
+      const std::string controller_name = spec.info.name;
+      return ros2_control::has_item(activate_request, controller_name) ||
+             ros2_control::has_item(deactivate_request, controller_name) ||
+             ros2_control::has_item(to_chained_mode_request, controller_name) ||
+             ros2_control::has_item(from_chained_mode_request, controller_name);
+    }
+
     // The controllers list to activate and deactivate
     std::vector<std::string> activate_request;
     std::vector<std::string> deactivate_request;

controller_manager/src/controller_manager.cpp

@@ -1298,6 +1298,7 @@ controller_interface::return_type ControllerManager::configure_controller(
 void ControllerManager::clear_requests()
 {
   switch_params_.do_switch = false;
+  switch_params_.activate_asap = false;
   switch_params_.deactivate_request.clear();
   switch_params_.activate_request.clear();
   // Set these interfaces as unavailable when clearing requests to avoid leaving them in available
@@ -1871,17 +1872,27 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
   switch_params_.do_switch = true;
 
   // wait until switch is finished
-  RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop");
-  std::unique_lock<std::mutex> switch_params_guard(switch_params_.mutex);
-  if (!switch_params_.cv.wait_for(
-        switch_params_guard, switch_params_.timeout, [this] { return !switch_params_.do_switch; }))
-  {
-    message = fmt::format(
-      FMT_COMPILE("Switch controller timed out after {} seconds!"),
-      static_cast<double>(switch_params_.timeout.count()) / 1e9);
-    RCLCPP_ERROR(get_logger(), "%s", message.c_str());
-    clear_requests();
-    return controller_interface::return_type::ERROR;
+  if (switch_params_.activate_asap)
+  {
+    RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop");
+    std::unique_lock<std::mutex> switch_params_guard(switch_params_.mutex);
+    if (!switch_params_.cv.wait_for(
+          switch_params_guard, switch_params_.timeout,
+          [this] { return !switch_params_.do_switch; }))
+    {
+      message = fmt::format(
+        FMT_COMPILE("Switch controller timed out after {} seconds!"),
+        static_cast<double>(switch_params_.timeout.count()) / 1e9);
+      RCLCPP_ERROR(get_logger(), "%s", message.c_str());
+      clear_requests();
+      return controller_interface::return_type::ERROR;
+    }
+  }
+  else
+  {
+    RCLCPP_INFO(get_logger(), "Requested controller switch from non-realtime loop");
+    // This should work as the realtime thread operation is read-only operation
+    manage_switch();
   }
 
   // copy the controllers spec from the used to the unused list
@@ -1983,7 +1994,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
 
 void ControllerManager::deactivate_controllers(
   const std::vector<ControllerSpec> & rt_controller_list,
-  const std::vector<std::string> controllers_to_deactivate)
+  const std::vector<std::string> & controllers_to_deactivate)
 {
   // deactivate controllers
   for (const auto & controller_name : controllers_to_deactivate)
@@ -2087,7 +2098,7 @@ void ControllerManager::switch_chained_mode(
 
 void ControllerManager::activate_controllers(
   const std::vector<ControllerSpec> & rt_controller_list,
-  const std::vector<std::string> controllers_to_activate)
+  const std::vector<std::string> & controllers_to_activate)
 {
   for (const auto & controller_name : controllers_to_activate)
   {
@@ -2239,14 +2250,6 @@ void ControllerManager::activate_controllers(
   }
 }
 
-void ControllerManager::activate_controllers_asap(
-  const std::vector<ControllerSpec> & rt_controller_list,
-  const std::vector<std::string> controllers_to_activate)
-{
-  //  https://github.com/ros-controls/ros2_control/issues/263
-  activate_controllers(rt_controller_list, controllers_to_activate);
-}
-
 void ControllerManager::list_controllers_srv_cb(
   const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request>,
   std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response)
@@ -2788,6 +2791,15 @@ controller_interface::return_type ControllerManager::update(
   rt_buffer_.deactivate_controllers_list.clear();
   for (const auto & loaded_controller : rt_controller_list)
   {
+    if (
+      switch_params_.do_switch && !switch_params_.activate_asap &&
+      switch_params_.skip_cycle(loaded_controller))
+    {
+      RCLCPP_DEBUG(
+        get_logger(), "Skipping update for controller '%s' as it is being switched",
+        loaded_controller.info.name.c_str());
+      continue;
+    }
     // TODO(v-lopez) we could cache this information
     // https://github.com/ros-controls/ros2_control/issues/153
     if (is_controller_active(*loaded_controller.c))
@@ -2944,7 +2956,7 @@ controller_interface::return_type ControllerManager::update(
   resource_manager_->enforce_command_limits(period);
 
   // there are controllers to (de)activate
-  if (switch_params_.do_switch)
+  if (switch_params_.do_switch && switch_params_.activate_asap)
   {
     manage_switch();
   }

hardware_interface/include/hardware_interface/helpers.hpp

@@ -38,7 +38,7 @@ template <typename Container, typename T>
 {
   if constexpr (std::is_same_v<Container, std::vector<T>>)
   {
-    return std::find(container.begin(), container.end(), item);
+    return std::find(container.cbegin(), container.cend(), item);
   }
   else if constexpr (
     std::is_same_v<Container, std::map<T, typename Container::mapped_type>> ||
@@ -68,7 +68,7 @@ template <typename Container, typename T>
 template <typename Container, typename T>
 [[nodiscard]] bool has_item(const Container & container, const T & item)
 {
-  return get_item_iterator(container, item) != container.end();
+  return get_item_iterator(container, item) != container.cend();
 }
 
 /**