Changes for the starting thread upon activation

ros-controls · Apr 22, 2024 · c4daa3b · c4daa3b
1 parent 2f7bbc2
commit c4daa3b
Show file tree

Hide file tree

Showing 3 changed files with 39 additions and 17 deletions.
diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp
@@ -99,7 +99,15 @@ class AsyncFunctionHandler
   std::pair<bool, T> trigger_async_update(
     const rclcpp::Time & time, const rclcpp::Duration & period)
   {
-    initialize_async_update_thread();
+    if (!is_initialized())
+    {
+      throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!");
+    }
+    if (!is_running())
+    {
+      throw std::runtime_error(
+        "AsyncFunctionHandler: need to start the async update thread first before triggering!");
+    }
     std::unique_lock<std::mutex> lock(async_mtx_, std::try_to_lock);
     bool trigger_status = false;
     if (lock.owns_lock() && !trigger_in_progress_)
@@ -185,14 +193,13 @@ class AsyncFunctionHandler
     }
   }
 
-private:
   /// Initialize the async update thread
   /**
    * If the async update thread is not running, it will start the async update thread.
    * If the async update thread is already configured and running, does nothing and return
    * immediately.
    */
-  void initialize_async_update_thread()
+  void start_async_update_thread()
   {
     if (!is_initialized())
     {
@@ -209,7 +216,9 @@ class AsyncFunctionHandler
           // \note There might be an concurrency issue with the get_state() call here. This mightn't
           // be critical here as the state of the controller is not expected to change during the
           // update cycle
-          while (get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE &&
+          while ((get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE ||
+                  get_state_function_().id() ==
+                    lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING) &&
                  !async_update_stop_)
           {
             {
@@ -227,10 +236,13 @@ class AsyncFunctionHandler
             }
             cycle_end_condition_.notify_one();
           }
+          trigger_in_progress_ = false;
+          cycle_end_condition_.notify_one();
         });
     }
   }
 
+private:
   rclcpp::Time current_update_time_;
   rclcpp::Duration current_update_period_{0, 0};
 

diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp
@@ -58,8 +58,15 @@ return_type ControllerInterfaceBase::init(
   node_->register_on_cleanup(
     std::bind(&ControllerInterfaceBase::on_cleanup, this, std::placeholders::_1));
 
-  node_->register_on_activate(
-    std::bind(&ControllerInterfaceBase::on_activate, this, std::placeholders::_1));
+  auto activate_callback = [this](const rclcpp_lifecycle::State & state)
+  {
+    if (is_async() && async_handler_)
+    {
+      async_handler_->start_async_update_thread();
+    }
+    return on_activate(state);
+  };
+  node_->register_on_activate(activate_callback);
 
   node_->register_on_deactivate(
     std::bind(&ControllerInterfaceBase::on_deactivate, this, std::placeholders::_1));

diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp
@@ -87,13 +87,12 @@ TEST_F(AsyncFunctionHandlerTest, check_initialization)
   ASSERT_FALSE(async_class.get_handler().is_stopped());
 
   // Once initialized, it should not be possible to initialize again
+  async_class.get_handler().start_async_update_thread();
   ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger());
   ASSERT_TRUE(async_class.get_handler().is_initialized());
   ASSERT_TRUE(async_class.get_handler().is_running());
   ASSERT_FALSE(async_class.get_handler().is_stopped());
   ASSERT_THROW(async_class.initialize(), std::runtime_error);
-  // The preempt_async_update is already called with the destructor
-  // async_class.get_handler().preempt_async_update();
 }
 
 TEST_F(AsyncFunctionHandlerTest, check_triggering)
@@ -110,6 +109,9 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering)
   ASSERT_TRUE(async_class.get_handler().is_initialized());
   ASSERT_FALSE(async_class.get_handler().is_running());
   ASSERT_FALSE(async_class.get_handler().is_stopped());
+  // It shouldn't be possible to trigger without starting the thread
+  ASSERT_THROW(async_class.trigger(), std::runtime_error);
+  async_class.get_handler().start_async_update_thread();
 
   EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
   ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger());
@@ -143,6 +145,7 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles)
   ASSERT_TRUE(async_class.get_handler().is_initialized());
   ASSERT_FALSE(async_class.get_handler().is_running());
   ASSERT_FALSE(async_class.get_handler().is_stopped());
+  async_class.get_handler().start_async_update_thread();
 
   EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
   for (int i = 1; i < 1e4; i++)
@@ -172,20 +175,19 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles)
   ASSERT_TRUE(async_class.get_handler().is_initialized());
   ASSERT_FALSE(async_class.get_handler().is_running());
   ASSERT_TRUE(async_class.get_handler().is_stopped());
+  async_class.get_handler().start_async_update_thread();
 
   // The thread will start and end immediately when invoked in inactive state
-  for (size_t i = 0; i < 3; i++)
-  {
-    EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
-    ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger());
-    async_class.get_handler().join_async_update_thread();
-    ASSERT_TRUE(async_class.get_handler().is_initialized());
-    ASSERT_FALSE(async_class.get_handler().is_running());
-    ASSERT_TRUE(async_class.get_handler().is_stopped());
-  }
+  async_class.get_handler().join_async_update_thread();
+  EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+  ASSERT_THROW(async_class.trigger(), std::runtime_error);
+  ASSERT_TRUE(async_class.get_handler().is_initialized());
+  ASSERT_FALSE(async_class.get_handler().is_running());
+  ASSERT_TRUE(async_class.get_handler().is_stopped());
 
   // Now activate it and launch again
   async_class.activate();
+  async_class.get_handler().start_async_update_thread();
   EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
   for (int i = 1; i < 100; i++)
   {
@@ -211,6 +213,7 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles)
   // Now let's test the case of activating it and then deactivating it when the thread is waiting
   // for a trigger to start new update cycle execution
   async_class.activate();
+  async_class.get_handler().start_async_update_thread();
 
   EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
   ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger());