Partial implementation of ActionServer::publish_status()

Author: nwn

rclrs/src/action/server.rs

@@ -236,7 +236,9 @@ where
 
             // TODO: Add a UUID->goal_handle entry to a server goal map.
 
-            // TODO: If accept_and_execute, update goal state
+            if response == GoalResponse::AcceptAndExecute {
+                goal_handle.execute()?;
+            }
 
             // TODO: Call publish_status()
 
@@ -282,6 +284,46 @@ where
     fn execute_goal_expired(&self) -> Result<(), RclrsError> {
         todo!()
     }
+
+    fn publish_status(&self) -> Result<(), RclrsError> {
+        // We need to hold the lock across this entire method because
+        // rcl_action_server_get_goal_handles() returns an internal pointer to the
+        // goal data.
+        let handle = &*self.handle.lock();
+
+        let mut goal_handles = std::ptr::null_mut::<*mut rcl_action_goal_handle_t>();
+        let mut num_goal_handles = 0;
+        unsafe {
+            // SAFETY: The action server is locked for this entire function, ensuring that the
+            // goal_handles array remains valid, unless rcl_shutdown is called. However, that is
+            // outside our control.
+            rcl_action_server_get_goal_handles(handle, &mut goal_handles, &mut num_goal_handles)
+        }.ok()?;
+
+        let mut goal_statuses = unsafe {
+            // SAFETY: No preconditions
+            rcl_action_get_zero_initialized_goal_status_array()
+        };
+        unsafe {
+            // SAFETY: The action server is locked through the handle and goal_statuses is
+            // zero-initialized.
+            rcl_action_get_goal_status_array(handle, &mut goal_statuses)
+        }.ok()?;
+        // TODO(nwn): Ensure that rcl_action_goal_status_array_fini() is always called on exit.
+
+        let goal_status_slice = unsafe {
+            // SAFETY: rcl_action_get_goal_status_array initializes goal_statues.msg.status_list as
+            // an array of goal statuses with the indicated size. The memory backing this array is
+            // not modified by anything else during the lifetime of the slice.
+            std::slice::from_raw_parts(goal_statuses.msg.status_list.data, goal_statuses.msg.status_list.size)
+        };
+        for goal_status in goal_status_slice {
+            // Copy into the correct message type to pass to rcl_action_publish_status().
+        }
+        // Call rcl_action_publish_status().
+
+        todo!()
+    }
 }
 
 impl<T> ActionServerBase for ActionServer<T>

rclrs/src/action/server_goal_handle.rs

@@ -134,7 +134,7 @@ where
     /// called on a goal handle after this is called.
     ///
     /// Returns an error if the goal is in any state other than pending.
-    pub fn execute(&self, result: &ActionT::Result) -> Result<(), RclrsError> {
+    pub fn execute(&self) -> Result<(), RclrsError> {
         self.update_state(rcl_action_goal_event_t::GOAL_EVENT_EXECUTE)?;
 
         // TODO: Invoke on_executing callback