Sketch out action server construction and destruction

This follows generally the same pattern as the service server. It
required adding a typesupport function to the Action trait and pulling
in some more rcl_action bindings.

Author: nwn

rclrs/package.xml

@@ -16,10 +16,11 @@
   <build_depend>libclang-dev</build_depend>
   <build_depend>rosidl_runtime_rs</build_depend>
   <depend>rcl</depend>
+  <depend>rcl_action</depend>
   <depend>builtin_interfaces</depend>
   <depend>rcl_interfaces</depend>
   <depend>rosgraph_msgs</depend>
-  
+
   <test_depend>test_msgs</test_depend>
 
   <export>

rclrs/src/action.rs

@@ -1,5 +1,8 @@
-use crate::{rcl_bindings::*, NodeHandle, RclrsError};
-use std::sync::{Arc, Mutex, MutexGuard};
+use crate::{error::ToResult, rcl_bindings::*, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX};
+use std::{
+    ffi::CString,
+    sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard},
+};
 
 // 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.
@@ -22,6 +25,46 @@ pub enum CancelResponse {
     Accept = 2,
 }
 
+/// Manage the lifecycle of an `rcl_action_server_t`, including managing its dependencies
+/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are
+/// [dropped after][1] the `rcl_action_server_t`.
+///
+/// [1]: <https://doc.rust-lang.org/reference/destructors.html>
+pub struct ActionServerHandle {
+    rcl_action_server: Mutex<rcl_action_server_t>,
+    node_handle: Arc<NodeHandle>,
+    pub(crate) in_use_by_wait_set: Arc<AtomicBool>,
+}
+
+impl ActionServerHandle {
+    pub(crate) fn lock(&self) -> MutexGuard<rcl_action_server_t> {
+        self.rcl_action_server.lock().unwrap()
+    }
+}
+
+impl Drop for ActionServerHandle {
+    fn drop(&mut self) {
+        let rcl_action_server = self.rcl_action_server.get_mut().unwrap();
+        let mut rcl_node = self.node_handle.rcl_node.lock().unwrap();
+        let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap();
+        // SAFETY: The entity lifecycle mutex is locked to protect against the risk of
+        // global variables in the rmw implementation being unsafely modified during cleanup.
+        unsafe {
+            rcl_action_server_fini(rcl_action_server, &mut *rcl_node);
+        }
+    }
+}
+
+/// Trait to be implemented by concrete ActionServer structs.
+///
+/// See [`ActionServer<T>`] for an example
+pub trait ActionServerBase: Send + Sync {
+    /// Internal function to get a reference to the `rcl` handle.
+    fn handle(&self) -> &ActionServerHandle;
+    // /// Tries to take a new request and run the callback with it.
+    // fn execute(&self) -> Result<(), RclrsError>;
+}
+
 pub struct ActionClient<T>
 where
     T: rosidl_runtime_rs::Action,
@@ -49,6 +92,10 @@ where
     T: rosidl_runtime_rs::Action,
 {
     _marker: PhantomData<T>,
+    pub(crate) handle: Arc<ActionServerHandle>,
+    // goal_callback: (),
+    // cancel_callback: (),
+    // accepted_callback: (),
 }
 
 impl<T> ActionServer<T>
@@ -60,8 +107,50 @@ where
     where
         T: rosidl_runtime_rs::Action,
     {
+        // SAFETY: Getting a zero-initialized value is always safe.
+        let mut rcl_action_server = unsafe { rcl_action_get_zero_initialized_server() };
+        let type_support = <T as rosidl_runtime_rs::Action>::get_type_support()
+            as *const rosidl_action_type_support_t;
+        let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul {
+            err,
+            s: topic.into(),
+        })?;
+
+        // SAFETY: No preconditions for this function.
+        let action_server_options = unsafe { rcl_action_server_get_default_options() };
+
+        {
+            let mut rcl_node = node_handle.rcl_node.lock().unwrap();
+            let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap();
+            unsafe {
+                // SAFETY:
+                // * The rcl_action_server is zero-initialized as mandated by this function.
+                // * The rcl_node is kept alive by the NodeHandle it is a dependency of the action server.
+                // * The topic name and the options are copied by this function, so they can be dropped
+                //   afterwards.
+                // * The entity lifecycle mutex is locked to protect against the risk of global
+                //   variables in the rmw implementation being unsafely modified during initialization.
+                rcl_action_server_init(
+                    &mut rcl_action_server,
+                    &mut *rcl_node,
+                    todo!(),
+                    type_support,
+                    topic_c_string.as_ptr(),
+                    &action_server_options as *const _,
+                )
+                .ok()?;
+            }
+        }
+
+        let handle = Arc::new(ActionServerHandle {
+            rcl_action_server: Mutex::new(rcl_action_server),
+            node_handle,
+            in_use_by_wait_set: Arc::new(AtomicBool::new(false)),
+        });
+
         Ok(Self {
             _marker: Default::default(),
+            handle,
         })
     }
 }

rclrs/src/rcl_wrapper.h

@@ -1,12 +1,11 @@
 #include <rcl/graph.h>
 #include <rcl/rcl.h>
-#include <rcl_action/types.h>
-#include <rcl_action/goal_handle.h>
+#include <rcl_action/rcl_action.h>
 #include <rcl_yaml_param_parser/parser.h>
 #include <rcutils/error_handling.h>
 #include <rmw/types.h>
 #include <rosidl_typesupport_introspection_c/field_types.h>
 #include <rosidl_typesupport_introspection_c/message_introspection.h>
 
 const size_t rmw_gid_storage_size_constant = RMW_GID_STORAGE_SIZE;
-const size_t rcl_action_uuid_size_constant = UUID_SIZE;
+const size_t rcl_action_uuid_size_constant = UUID_SIZE;

rosidl_generator_rs/resource/action.rs.em

@@ -51,6 +51,11 @@ type_name = action_spec.namespaced_type.name
     type Goal = crate::@(subfolder)::rmw::@(type_name)_Goal;
     type Result = crate::@(subfolder)::rmw::@(type_name)_Result;
     type Feedback = crate::@(subfolder)::rmw::@(type_name)_Feedback;
+
+    fn get_type_support() -> *const std::os::raw::c_void {
+      // SAFETY: No preconditions for this function.
+      unsafe { rosidl_typesupport_c__get_action_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() }
+    }
   }
 
 @[end for]

rosidl_runtime_rs/src/traits.rs

@@ -172,4 +172,7 @@ pub trait Action: 'static {
 
     /// The feedback message associated with this service.
     type Feedback: Message;
+
+    /// Get a pointer to the correct `rosidl_action_type_support_t` structure.
+    fn get_type_support() -> *const std::os::raw::c_void;
 }