Add UUID->GoalHandle hash-map to action server

This requires a mutex to enable simultaneous goal acceptance in a
multithreaded executor. We also make ServerGoalHandles implement Send
and Sync, since they will be accessed by multiple threads during
callbacks. Due to containing a raw pointer field, the type doesn't
automatically implement Send/Sync; however, we guarantee that the uses
of this pointer is safe and synchronized, allowing us to safely
implement the traits.

Author: nwn

rclrs/src/action/server.rs

@@ -7,6 +7,7 @@ use crate::{
 };
 use rosidl_runtime_rs::{Action, ActionImpl, Message, Service};
 use std::{
+    collections::HashMap,
     ffi::CString,
     sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard},
 };
@@ -77,6 +78,7 @@ where
     goal_callback: Box<GoalCallback<ActionT>>,
     cancel_callback: Box<CancelCallback<ActionT>>,
     accepted_callback: Box<AcceptedCallback<ActionT>>,
+    goal_handles: Mutex<HashMap<GoalUuid, Arc<ServerGoalHandle<ActionT>>>>,
 }
 
 impl<T> ActionServer<T>
@@ -157,6 +159,7 @@ where
             goal_callback: Box::new(goal_callback),
             cancel_callback: Box::new(cancel_callback),
             accepted_callback: Box::new(accepted_callback),
+            goal_handles: Mutex::new(HashMap::new()),
         })
     }
 
@@ -243,13 +246,13 @@ where
             return self.send_goal_response(request_id, false);
         }
 
-        // SAFETY: No preconditions
-        let mut goal_info = unsafe { rcl_action_get_zero_initialized_goal_info() };
-        // Populate the goal UUID; the other fields will be populated by rcl_action later on.
-        // TODO(nwn): Check this claim.
-        goal_info.goal_id.uuid = uuid.0;
-
         let goal_handle = {
+            // SAFETY: No preconditions
+            let mut goal_info = unsafe { rcl_action_get_zero_initialized_goal_info() };
+            // Only populate the goal UUID; the timestamp will be set internally by
+            // rcl_action_accept_new_goal().
+            goal_info.goal_id.uuid = uuid.0;
+
             let server_handle = &mut *self.handle.lock();
             let goal_handle_ptr = unsafe {
                 // SAFETY: The action server handle is locked and so synchronized with other
@@ -262,17 +265,17 @@ where
                 // Other than rcl_get_error_string(), there's no indication what happened.
                 panic!("Failed to accept goal");
             } else {
-                ServerGoalHandle::<T>::new(
+                Arc::new(ServerGoalHandle::<T>::new(
                     goal_handle_ptr,
-                    todo!(""),
-                    GoalUuid(goal_info.goal_id.uuid),
-                )
+                    todo!("Create an Arc holding the goal message"),
+                    uuid,
+                ))
             }
         };
 
         self.send_goal_response(request_id, true)?;
 
-        // TODO: Add a UUID->goal_handle entry to a server goal map.
+        self.goal_handles.lock().unwrap().insert(uuid, Arc::clone(&goal_handle));
 
         if response == GoalResponse::AcceptAndExecute {
             goal_handle.execute()?;

rclrs/src/action/server_goal_handle.rs

@@ -1,12 +1,6 @@
 use crate::{rcl_bindings::*, GoalUuid, RclrsError, ToResult};
 use std::sync::{Arc, Mutex};
 
-// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
-// they are running in. Therefore, this type can be safely sent to another thread.
-unsafe impl Send for rcl_action_goal_handle_t {}
-
-unsafe impl Sync for rcl_action_goal_handle_t {}
-
 // Values defined by `action_msgs/msg/GoalStatus`
 #[repr(i8)]
 #[derive(Debug, Clone, Hash, PartialEq, Eq)]
@@ -35,6 +29,14 @@ where
     uuid: GoalUuid,
 }
 
+// SAFETY: Send/Sync are not automatically implemented due to the contained raw pointer
+// (specifically, `*mut rcl_action_goal_handle_t`). However, the `rcl_handle` field is wrapped in a
+// mutex, guaranteeing that the underlying data is never simultaneously accessed on the rclrs side
+// by multiple threads. Moreover, the rcl_action functions taking these handles are able to be run
+// from any thread.
+unsafe impl<ActionT> Send for ServerGoalHandle<ActionT> where ActionT: rosidl_runtime_rs::Action {}
+unsafe impl<ActionT> Sync for ServerGoalHandle<ActionT> where ActionT: rosidl_runtime_rs::Action {}
+
 impl<ActionT> ServerGoalHandle<ActionT>
 where
     ActionT: rosidl_runtime_rs::Action,