Fix expiration of action goals when EventsExecutors are used

rclcpp_action/include/rclcpp_action/server.hpp

@@ -205,6 +205,10 @@ class ServerBase : public rclcpp::Waitable
     const rclcpp::QoS & qos_service_event_pub,
     rcl_service_introspection_state_t introspection_state);
 
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_goal_handles();
+
 protected:
   RCLCPP_ACTION_PUBLIC
   ServerBase(

rclcpp_action/src/server.cpp

@@ -157,9 +157,17 @@ ServerBase::ServerBase(
 
   rcl_node_t * rcl_node = node_base->get_rcl_node_handle();
 
-  // This timer callback will never be called, we are only interested in
-  // weather the timer itself becomes ready or not.
-  std::function<void()> timer_callback = [] () {};
+  std::function<void()> timer_callback = [this] () {
+      try {
+        execute_check_expired_goals();
+      } catch (const rclcpp::exceptions::RCLError & ex) {
+        RCLCPP_ERROR(
+            rclcpp::get_logger("rclcpp_action"),
+            "Failed to check for expired goals: %s", ex.what()
+        );
+      }
+      ;
+    };
   pimpl_->expire_timer_ = std::make_shared<rclcpp::GenericTimer<decltype (timer_callback)>>(
       node_clock->get_clock(), std::chrono::nanoseconds(options.result_timeout.nanoseconds),
       std::move(timer_callback), node_base->get_context(), false);
@@ -187,6 +195,7 @@ ServerBase::ServerBase(
 
 ServerBase::~ServerBase()
 {
+  pimpl_->expire_timer_->cancel();
 }
 
 size_t
@@ -639,6 +648,13 @@ ServerBase::execute_result_request_received(
   }
 }
 
+size_t
+ServerBase::get_number_of_goal_handles()
+{
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->unordered_map_mutex_);
+  return pimpl_->goal_handles_.size();
+}
+
 void
 ServerBase::execute_check_expired_goals()
 {

rclcpp_action/test/test_server.cpp

@@ -49,7 +49,9 @@ class TestServer : public ::testing::Test
   std::shared_ptr<Fibonacci::Impl::SendGoalService::Request>
   send_goal_request(
     rclcpp::Node::SharedPtr node, GoalUUID uuid,
-    std::chrono::milliseconds timeout = std::chrono::milliseconds(-1))
+    rclcpp::Executor & executor,
+    std::chrono::milliseconds timeout = std::chrono::milliseconds(-1),
+    bool executor_owns_node = false)
   {
     auto client = node->create_client<Fibonacci::Impl::SendGoalService>(
       "fibonacci/_action/send_goal");
@@ -59,7 +61,10 @@ class TestServer : public ::testing::Test
     auto request = std::make_shared<Fibonacci::Impl::SendGoalService::Request>();
     request->goal_id.uuid = uuid;
     auto future = client->async_send_request(request);
-    auto return_code = rclcpp::spin_until_future_complete(node, future, timeout);
+    auto return_code = (executor_owns_node) ?
+      executor.spin_until_future_complete(future, timeout) :
+      rclcpp::executors::spin_node_until_future_complete(executor,
+      node->get_node_base_interface(), future, timeout);
     if (rclcpp::FutureReturnCode::SUCCESS == return_code) {
       return request;
     } else if (rclcpp::FutureReturnCode::TIMEOUT == return_code) {
@@ -69,10 +74,24 @@ class TestServer : public ::testing::Test
     }
   }
 
+  std::shared_ptr<Fibonacci::Impl::SendGoalService::Request>
+  send_goal_request(
+    rclcpp::Node::SharedPtr node, GoalUUID uuid,
+    std::chrono::milliseconds timeout = std::chrono::milliseconds(-1))
+  {
+    rclcpp::ExecutorOptions options;
+    options.context = node->get_node_base_interface()->get_context();
+    rclcpp::executors::SingleThreadedExecutor executor(options);
+    auto ret = send_goal_request(node, uuid, executor, timeout);
+    return ret;
+  }
+
   CancelResponse::SharedPtr
   send_cancel_request(
     rclcpp::Node::SharedPtr node, GoalUUID uuid,
-    std::chrono::milliseconds timeout = std::chrono::milliseconds(-1))
+    rclcpp::Executor & executor,
+    std::chrono::milliseconds timeout = std::chrono::milliseconds(-1),
+    bool executor_owns_node = false)
   {
     auto cancel_client = node->create_client<Fibonacci::Impl::CancelGoalService>(
       "fibonacci/_action/cancel_goal");
@@ -82,7 +101,10 @@ class TestServer : public ::testing::Test
     auto request = std::make_shared<Fibonacci::Impl::CancelGoalService::Request>();
     request->goal_info.goal_id.uuid = uuid;
     auto future = cancel_client->async_send_request(request);
-    auto return_code = rclcpp::spin_until_future_complete(node, future, timeout);
+    auto return_code = (executor_owns_node) ?
+      executor.spin_until_future_complete(future, timeout) :
+      rclcpp::executors::spin_node_until_future_complete(executor,
+      node->get_node_base_interface(), future, timeout);
     if (rclcpp::FutureReturnCode::SUCCESS == return_code) {
       return future.get();
     } else if (rclcpp::FutureReturnCode::TIMEOUT == return_code) {
@@ -91,6 +113,18 @@ class TestServer : public ::testing::Test
       throw std::runtime_error("cancel request future didn't complete succesfully");
     }
   }
+
+  CancelResponse::SharedPtr
+  send_cancel_request(
+    rclcpp::Node::SharedPtr node, GoalUUID uuid,
+    std::chrono::milliseconds timeout = std::chrono::milliseconds(-1))
+  {
+    rclcpp::ExecutorOptions options;
+    options.context = node->get_node_base_interface()->get_context();
+    rclcpp::executors::SingleThreadedExecutor executor(options);
+    auto ret = send_cancel_request(node, uuid, executor, timeout);
+    return ret;
+  }
 };
 
 TEST_F(TestServer, construction_and_destruction)
@@ -1022,6 +1056,102 @@ TEST_F(TestServer, deferred_execution)
   EXPECT_TRUE(received_handle->is_executing());
 }
 
+TEST_F(TestServer, goals_expired_with_events_executor)
+{
+  // Because timer expiration was typically tied to the waitsets for
+  // the SingleThreadedExecutor and MultiThreadedExecutor,
+  // We specifically want to test with the EventsExecutor here
+  // so we can verify the timer based goal expiration works
+  // and expired goals are properly cleared.
+  auto node = std::make_shared<rclcpp::Node>("expire_goals", "/rclcpp_action/expire_goals");
+  rclcpp::ExecutorOptions opts;
+  opts.context = node->get_node_base_interface()->get_context();
+
+  rclcpp::experimental::executors::EventsExecutor executor(opts);
+  executor.add_node(node);
+  const std::vector<GoalUUID> uuids{
+    {{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}},
+    {{10, 2, 3, 40, 50, 6, 70, 8, 9, 1, 11, 12, 13, 140, 15, 160}},
+    {{12, 23, 34, 45, 50, 6, 70, 8, 9, 100, 11, 120, 13, 140, 15, 170}},
+    {{12, 23, 34, 45, 50, 6, 70, 8, 9, 100, 11, 120, 13, 140, 115, 16}}
+  };
+
+  auto handle_goal = [](
+    const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::ACCEPT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  const std::chrono::milliseconds result_timeout{25};
+
+  rcl_action_server_options_t options = rcl_action_server_get_default_options();
+  options.result_timeout.nanoseconds = RCL_MS_TO_NS(result_timeout.count());
+  auto as = rclcpp_action::create_server<Fibonacci>(
+    node, "fibonacci",
+    handle_goal,
+    handle_cancel,
+    handle_accepted,
+    options);
+
+
+  for (const auto & uuid : uuids) {
+    constexpr bool owns_node {true};
+    send_goal_request(node, uuid, executor, std::chrono::milliseconds(-1), owns_node);
+
+    EXPECT_TRUE(received_handle->is_active());
+    EXPECT_TRUE(received_handle->is_executing());
+
+    // Send result request
+    auto result_client = node->create_client<Fibonacci::Impl::GetResultService>(
+      "fibonacci/_action/get_result");
+    if (!result_client->wait_for_service(std::chrono::seconds(20))) {
+      throw std::runtime_error("get result service didn't become available");
+    }
+    auto request = std::make_shared<Fibonacci::Impl::GetResultService::Request>();
+    request->goal_id.uuid = uuid;
+    auto future = result_client->async_send_request(request);
+
+    // Send a result
+    auto result = std::make_shared<Fibonacci::Result>();
+    result->sequence = {5, 8, 13, 21};
+    received_handle->succeed(result);
+
+    // Wait for the result request to be received
+    ASSERT_EQ(
+      rclcpp::FutureReturnCode::SUCCESS,
+      executor.spin_until_future_complete(future));
+
+    auto response = future.get();
+    EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status);
+    EXPECT_EQ(result->sequence, response->result.sequence);
+
+    auto start = std::chrono::steady_clock::now();
+    while (as->get_number_of_goal_handles() != 0 &&
+      std::chrono::steady_clock::now() - start < std::chrono::seconds(5))
+    {
+      executor.spin_some();
+      rclcpp::sleep_for(std::chrono::milliseconds(10));
+    }
+  }
+  executor.remove_node(node);
+
+  ASSERT_EQ(as->get_number_of_goal_handles(), 0);
+}
+
+
 class TestBasicServer : public TestServer
 {
 public: