Fix async controllers deactivation regime

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -306,6 +306,19 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    */
   void wait_for_trigger_update_to_finish();
 
+  /**
+   * Method to prepare the controller for deactivation. This method is called by the controller
+   * manager before deactivating the controller. The method is used to prepare the controller for
+   * deactivation, e.g., to stop triggering the update cycles further. This method is especially
+   * needed for controllers running in async mode and different frequency than the control manager.
+   *
+   * \note **The method is not real-time safe and shouldn't be called in the RT control loop.**
+   *
+   * If the controller is running in async mode, the method will stop the async update cycles. If
+   * the controller is not running in async mode, the method will do nothing.
+   */
+  void prepare_for_deactivation();
+
   std::string get_name() const;
 
   /// Enable or disable introspection of the controller.
@@ -319,11 +332,18 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
 
 private:
+  /**
+   * Method to stop the async handler thread. This method is called before the controller cleanup,
+   * error and shutdown lifecycle transitions.
+   */
+  void stop_async_handler_thread();
+
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
   std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;
   unsigned int update_rate_ = 0;
   bool is_async_ = false;
   std::string urdf_ = "";
+  std::atomic_bool skip_async_triggers_ = false;
   ControllerUpdateStats trigger_stats_;
 
 protected:

controller_interface/src/controller_interface_base.cpp

@@ -86,16 +86,14 @@ return_type ControllerInterfaceBase::init(
       // make sure introspection is disabled on controller cleanup as users may manually enable
       // it in `on_configure` and `on_deactivate` - see the docs for details
       enable_introspection(false);
-      if (is_async() && async_handler_ && async_handler_->is_running())
-      {
-        async_handler_->stop_thread();
-      }
+      this->stop_async_handler_thread();
       return on_cleanup(previous_state);
     });
 
   node_->register_on_activate(
     [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
     {
+      skip_async_triggers_.store(false);
       enable_introspection(true);
       if (is_async() && async_handler_ && async_handler_->is_running())
       {
@@ -113,10 +111,22 @@ return_type ControllerInterfaceBase::init(
     });
 
   node_->register_on_shutdown(
-    std::bind(&ControllerInterfaceBase::on_shutdown, this, std::placeholders::_1));
+    [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
+    {
+      this->stop_async_handler_thread();
+      auto transition_state_status = on_shutdown(previous_state);
+      this->release_interfaces();
+      return transition_state_status;
+    });
 
   node_->register_on_error(
-    std::bind(&ControllerInterfaceBase::on_error, this, std::placeholders::_1));
+    [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
+    {
+      this->stop_async_handler_thread();
+      auto transition_state_status = on_error(previous_state);
+      this->release_interfaces();
+      return transition_state_status;
+    });
 
   return return_type::OK;
 }
@@ -204,6 +214,13 @@ ControllerUpdateStatus ControllerInterfaceBase::trigger_update(
   trigger_stats_.total_triggers++;
   if (is_async())
   {
+    if (skip_async_triggers_.load())
+    {
+      // Skip further async triggers if the controller is being deactivated
+      status.successful = false;
+      status.result = return_type::OK;
+      return status;
+    }
     const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time();
     const auto result = async_handler_->trigger_async_callback(time, period);
     if (!result.first)
@@ -270,6 +287,20 @@ void ControllerInterfaceBase::wait_for_trigger_update_to_finish()
   }
 }
 
+void ControllerInterfaceBase::prepare_for_deactivation()
+{
+  skip_async_triggers_.store(true);
+  this->wait_for_trigger_update_to_finish();
+}
+
+void ControllerInterfaceBase::stop_async_handler_thread()
+{
+  if (is_async() && async_handler_ && async_handler_->is_running())
+  {
+    async_handler_->stop_thread();
+  }
+}
+
 std::string ControllerInterfaceBase::get_name() const { return get_node()->get_name(); }
 
 void ControllerInterfaceBase::enable_introspection(bool enable)

controller_manager/src/controller_manager.cpp

@@ -1752,7 +1752,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb(
       std::bind(controller_name_compare, std::placeholders::_1, controller));
     if (controller_it != controllers.end())
     {
-      controller_it->c->wait_for_trigger_update_to_finish();
+      controller_it->c->prepare_for_deactivation();
     }
   }
 

controller_manager/test/test_controller_manager.cpp

@@ -490,6 +490,17 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle)
     testing::AllOf(testing::Gt(0.6 * exp_period), testing::Lt((1.4 * exp_period))));
   size_t last_internal_counter = test_controller->internal_counter;
 
+  {
+    ControllerManagerRunner cm_runner(this);
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  }
+  ASSERT_THAT(
+    test_controller->internal_counter,
+    testing::AllOf(
+      testing::Ge(last_internal_counter + 18), testing::Le(last_internal_counter + 21)))
+    << "As the sleep is 1 sec and the controller rate is 20Hz, we should have approx. 20 updates";
+
+  last_internal_counter = test_controller->internal_counter;
   // Stop controller, will take effect at the end of the update function
   start_controllers = {};
   stop_controllers = {test_controller::TEST_CONTROLLER_NAME};
@@ -533,6 +544,215 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle)
   EXPECT_EQ(1, test_controller.use_count());
 }
 
+TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle_at_cm_rate)
+{
+  const auto test_param = GetParam();
+  auto test_controller = std::make_shared<test_controller::TestController>();
+  auto test_controller2 = std::make_shared<test_controller::TestController>();
+  constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name";
+  cm_->add_controller(
+    test_controller, test_controller::TEST_CONTROLLER_NAME,
+    test_controller::TEST_CONTROLLER_CLASS_NAME);
+  cm_->add_controller(
+    test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
+  EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
+  EXPECT_EQ(2, test_controller.use_count());
+
+  // setup interface to claim from controllers
+  controller_interface::InterfaceConfiguration cmd_itfs_cfg;
+  cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
+  {
+    cmd_itfs_cfg.names.push_back(interface);
+  }
+  test_controller->set_command_interface_configuration(cmd_itfs_cfg);
+
+  controller_interface::InterfaceConfiguration state_itfs_cfg;
+  state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
+  {
+    state_itfs_cfg.names.push_back(interface);
+  }
+  for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
+  {
+    state_itfs_cfg.names.push_back(interface);
+  }
+  test_controller->set_state_interface_configuration(state_itfs_cfg);
+
+  controller_interface::InterfaceConfiguration cmd_itfs_cfg2;
+  cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES)
+  {
+    cmd_itfs_cfg2.names.push_back(interface);
+  }
+  test_controller2->set_command_interface_configuration(cmd_itfs_cfg2);
+
+  controller_interface::InterfaceConfiguration state_itfs_cfg2;
+  state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL;
+  test_controller2->set_state_interface_configuration(state_itfs_cfg2);
+
+  // Check if namespace is set correctly
+  RCLCPP_INFO(
+    rclcpp::get_logger("test_controller_manager"), "Controller Manager namespace is '%s'",
+    cm_->get_namespace());
+  EXPECT_STREQ(cm_->get_namespace(), "/");
+  RCLCPP_INFO(
+    rclcpp::get_logger("test_controller_manager"), "Controller 1 namespace is '%s'",
+    test_controller->get_node()->get_namespace());
+  EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/");
+  RCLCPP_INFO(
+    rclcpp::get_logger("test_controller_manager"), "Controller 2 namespace is '%s'",
+    test_controller2->get_node()->get_namespace());
+  EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/");
+
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(0u, test_controller->internal_counter)
+    << "Update should not reach an unconfigured controller";
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    test_controller->get_lifecycle_state().id());
+
+  // configure controller
+  rclcpp::Parameter is_async_parameter("is_async", rclcpp::ParameterValue(true));
+  test_controller->get_node()->set_parameter(is_async_parameter);
+  {
+    ControllerManagerRunner cm_runner(this);
+    cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+    cm_->configure_controller(TEST_CONTROLLER2_NAME);
+  }
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
+  EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started";
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_controller->get_lifecycle_state().id());
+
+  // Start controller, will take effect at the end of the update function
+  std::vector<std::string> start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME};
+  std::vector<std::string> stop_controllers = {};
+  auto switch_future = std::async(
+    std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+    start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0));
+
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update";
+  {
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(test_param.expected_return, switch_future.get());
+  }
+
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter);
+
+  // Start the real test controller, will take effect at the end of the update function
+  start_controllers = {test_controller::TEST_CONTROLLER_NAME};
+  stop_controllers = {};
+  switch_future = std::async(
+    std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+    start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0));
+
+  ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
+    << "switch_controller should be blocking until next update cycle";
+
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update";
+  {
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+  }
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
+
+  time_ = cm_->get_clock()->now();
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_GE(test_controller->internal_counter, 1u);
+  auto current_counter = test_controller->internal_counter;
+  std::this_thread::sleep_for(
+    std::chrono::milliseconds(1000 / (test_controller->get_update_rate())));
+  EXPECT_EQ(test_controller->internal_counter, current_counter + 1u);
+  EXPECT_NEAR(test_controller->update_period_.seconds(), 0.01, 0.008);
+
+  const double exp_period = (cm_->get_clock()->now() - time_).seconds();
+  time_ = cm_->get_clock()->now();
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(test_controller->internal_counter, current_counter + 1u);
+  std::this_thread::sleep_for(
+    std::chrono::milliseconds(1000 / (test_controller->get_update_rate())));
+  EXPECT_EQ(test_controller->internal_counter, current_counter + 2u);
+  EXPECT_THAT(
+    test_controller->update_period_.seconds(),
+    testing::AllOf(testing::Gt(0.6 * exp_period), testing::Lt((1.4 * exp_period))));
+  size_t last_internal_counter = test_controller->internal_counter;
+
+  {
+    ControllerManagerRunner cm_runner(this);
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  }
+  ASSERT_THAT(
+    test_controller->internal_counter,
+    testing::AllOf(
+      testing::Ge(last_internal_counter + cm_->get_update_rate() - 3),
+      testing::Le(last_internal_counter + cm_->get_update_rate() + 1)))
+    << "As the sleep is 1 sec and the controller rate is 100Hz, we should have approx. 100 updates";
+
+  // Sleep for 2 cycles to allow for any changes
+  std::this_thread::sleep_for(std::chrono::milliseconds(20));
+  last_internal_counter = test_controller->internal_counter;
+  {
+    ControllerManagerRunner cm_runner(this);
+    // Stop controller, will take effect at the end of the update function
+    start_controllers = {};
+    stop_controllers = {test_controller::TEST_CONTROLLER_NAME};
+    switch_future = std::async(
+      std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+      start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0));
+
+    ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
+      << "switch_controller should be blocking until next update cycle";
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+    EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+    EXPECT_EQ(last_internal_counter + 1, test_controller->internal_counter)
+      << "Controller is stopped at the end of update, it should finish it's active cycle";
+  }
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_controller->get_lifecycle_state().id());
+  auto unload_future = std::async(
+    std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_,
+    test_controller::TEST_CONTROLLER_NAME);
+
+  ASSERT_EQ(std::future_status::timeout, unload_future.wait_for(std::chrono::milliseconds(100)))
+    << "unload_controller should be blocking until next update cycle";
+
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(controller_interface::return_type::OK, unload_future.get());
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
+    test_controller->get_lifecycle_state().id());
+  EXPECT_EQ(1, test_controller.use_count());
+}
+
 TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
 {
   auto strictness = GetParam().strictness;