[ControllerManager] Add Class for Async Controllers and Lifecycle Man…

…agement (#932) * add `is_async` param to controller manager * async controller lifecycle * use controller's update rate Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>
ros-controls · Mar 21, 2023 · 5dfacd8 · 5dfacd8
1 parent 7ed3786
commit 5dfacd8
Show file tree

Hide file tree

Showing 3 changed files with 85 additions and 1 deletion.
diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp
@@ -130,4 +130,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
diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -514,6 +514,67 @@ 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 activate()
+    {
+      thread_ = std::thread(&ControllerThreadWrapper::call_controller_update, this);
+    }
+
+    void call_controller_update()
+    {
+      using TimePoint = std::chrono::system_clock::time_point;
+      unsigned int used_update_rate =
+        controller_->get_update_rate() == 0
+          ? cm_update_rate_
+          : controller_
+              ->get_update_rate();  // determines if the controller's or CM's update rate is used
+
+      while (!terminated_.load(std::memory_order_relaxed))
+      {
+        auto const period = std::chrono::nanoseconds(1'000'000'000 / used_update_rate);
+        TimePoint next_iteration_time =
+          TimePoint(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);
+      }
+    }
+
+  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

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -403,6 +403,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
@@ -420,6 +427,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;
 }
 
@@ -487,6 +495,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())
   {
@@ -1178,6 +1193,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
@@ -1266,6 +1282,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)->activate();
+    }
   }
   // All controllers activated, switching done
   switch_params_.do_switch = false;
@@ -1740,7 +1761,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();