From 0d8fdc0edc5a380cdeb95843c4b560c4b60f45e1 Mon Sep 17 00:00:00 2001
From: Nathan Wiebe Neufeldt <wn.nathan@gmail.com>
Date: Fri, 9 Aug 2024 17:55:57 -0400
Subject: [PATCH] Implement goal expiration in action server

---
 rclrs/src/action/server.rs | 27 ++++++++++++++++++++++++++-
 1 file changed, 26 insertions(+), 1 deletion(-)

diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs
index d86edc07..ece4515b 100644
--- a/rclrs/src/action/server.rs
+++ b/rclrs/src/action/server.rs
@@ -298,7 +298,32 @@ where
     }
 
     fn execute_goal_expired(&self) -> Result<(), RclrsError> {
-        todo!()
+        // We assume here that only one goal expires at a time. If not, the only consequence is
+        // that we'll call rcl_action_expire_goals() more than necessary.
+
+        // SAFETY: No preconditions
+        let mut expired_goal = unsafe { rcl_action_get_zero_initialized_goal_info() };
+        let mut num_expired = 1;
+
+        loop {
+            unsafe {
+                // SAFETY: The action server is locked through the handle. The `expired_goal`
+                // argument points to an array of one rcl_action_goal_info_t and num_expired points
+                // to a `size_t`.
+                rcl_action_expire_goals(&*self.handle.lock(), &mut expired_goal, 1, &mut num_expired)
+            }
+            .ok()?;
+
+            if num_expired > 0 {
+                // Clean up the expired goal.
+                let uuid = GoalUuid(expired_goal.goal_id.uuid);
+                self.goal_handles.lock().unwrap().remove(&uuid);
+            } else {
+                break
+            }
+        }
+
+        Ok(())
     }
 
     fn publish_status(&self) -> Result<(), RclrsError> {