Async controller lifecycle

controller_interface/src/controller_interface_base.cpp

@@ -129,5 +129,6 @@ std::shared_ptr<rclcpp_lifecycle::LifecycleNode> ControllerInterfaceBase::get_no
 }
 
 unsigned int ControllerInterfaceBase::get_update_rate() const { return update_rate_; }
+bool ControllerInterfaceBase::is_async() const { return is_async_; }
 
 }  // namespace controller_interface

controller_manager/include/controller_manager/controller_manager.hpp

@@ -510,6 +510,64 @@ class ControllerManager : public rclcpp::Node
   };
 
   SwitchParams switch_params_;
+
+  class ControllerThreadWrapper
+  {
+  public:
+    ControllerThreadWrapper(
+      std::shared_ptr<controller_interface::ControllerInterfaceBase> & controller,
+      int cm_update_rate)
+    : terminated_(false), controller_(controller), thread_{}, cm_update_rate_(cm_update_rate)
+    {
+    }
+    ControllerThreadWrapper(const ControllerThreadWrapper & t) = delete;
+    ControllerThreadWrapper(ControllerThreadWrapper && t) = default;
+    ~ControllerThreadWrapper()
+    {
+      terminated_.store(true, std::memory_order_seq_cst);
+      if (thread_.joinable())
+      {
+        thread_.join();
+      }
+    }
+
+    void start() { thread_ = std::thread(&ControllerThreadWrapper::call_controller_update, this); }
+
+    void call_controller_update()
+    {
+      rclcpp::Time previous_time = controller_->get_node()->now();
+
+      while (!terminated_.load(std::memory_order_relaxed))
+      {
+        auto const period = std::chrono::nanoseconds(1'000'000'000 / cm_update_rate_);
+        std::chrono::system_clock::time_point next_iteration_time =
+          std::chrono::system_clock::time_point(
+            std::chrono::nanoseconds(controller_->get_node()->now().nanoseconds()));
+
+        if (controller_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+        {
+          // critical section, not implemented yet
+        }
+
+        next_iteration_time += period;
+        std::this_thread::sleep_until(next_iteration_time);
+      }
+    }
+
+    std::shared_ptr<controller_interface::ControllerInterfaceBase> get_controller()
+    {
+      return controller_;
+    }
+
+  private:
+    std::atomic<bool> terminated_;
+    std::shared_ptr<controller_interface::ControllerInterfaceBase> controller_;
+    std::thread thread_;
+    unsigned int cm_update_rate_;
+  };
+
+  std::unordered_map<std::string, std::unique_ptr<ControllerThreadWrapper>>
+    async_controller_threads_;
 };
 
 }  // namespace controller_manager

controller_manager/src/controller_manager.cpp

@@ -388,6 +388,13 @@ controller_interface::return_type ControllerManager::unload_controller(
       controller_name.c_str());
     return controller_interface::return_type::ERROR;
   }
+  if (controller.c->is_async())
+  {
+    RCLCPP_DEBUG(
+      get_logger(), "Removing controller '%s' from the list of async controllers",
+      controller_name.c_str());
+    async_controller_threads_.erase(controller_name);
+  }
 
   RCLCPP_DEBUG(get_logger(), "Cleanup controller");
   // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
@@ -405,6 +412,7 @@ controller_interface::return_type ControllerManager::unload_controller(
   RCLCPP_DEBUG(get_logger(), "Destruct controller finished");
 
   RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str());
+
   return controller_interface::return_type::OK;
 }
 
@@ -472,6 +480,13 @@ controller_interface::return_type ControllerManager::configure_controller(
     return controller_interface::return_type::ERROR;
   }
 
+  // ASYNCHRONOUS CONTROLLERS: Start background thread for update
+  if (controller->is_async())
+  {
+    async_controller_threads_.emplace(
+      controller_name, std::make_unique<ControllerThreadWrapper>(controller, update_rate_));
+  }
+
   // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers
   if (controller->is_chainable())
   {
@@ -1163,6 +1178,7 @@ void ControllerManager::activate_controllers(
       continue;
     }
     auto controller = found_it->c;
+    auto controller_name = found_it->info.name;
 
     bool assignment_successful = true;
     // assign command interfaces to the controller
@@ -1251,6 +1267,11 @@ void ControllerManager::activate_controllers(
         get_logger(), "After activating, controller '%s' is in state '%s', expected Active",
         controller->get_node()->get_name(), new_state.label().c_str());
     }
+
+    if (controller->is_async())
+    {
+      async_controller_threads_.at(controller_name)->start();
+    }
   }
   // All controllers activated, switching done
   switch_params_.do_switch = false;
@@ -1761,7 +1782,7 @@ controller_interface::return_type ControllerManager::update(
   {
     // TODO(v-lopez) we could cache this information
     // https://github.com/ros-controls/ros2_control/issues/153
-    if (is_controller_active(*loaded_controller.c))
+    if (!loaded_controller.c->is_async() && is_controller_active(*loaded_controller.c))
     {
       const auto controller_update_rate = loaded_controller.c->get_update_rate();