Hook up goal termination methods for goal handles

These now call into the action server to send a response to prior
(queued) and future goal result requests. There are still some
synchronization tweaks needed for this to be correct.

rclrs/src/action/server.rs

@@ -573,6 +573,8 @@ where
             if num_expired > 0 {
                 // Clean up the expired goal.
                 let uuid = GoalUuid(expired_goal.goal_id.uuid);
+                self.goal_results.lock().unwrap().remove(&uuid);
+                self.result_requests.lock().unwrap().remove(&uuid);
                 self.goal_handles.lock().unwrap().remove(&uuid);
             } else {
                 break;
@@ -582,6 +584,29 @@ where
         Ok(())
     }
 
+    // TODO(nwn): Replace `status` with a "properly typed" action_msgs::msg::GoalStatus enum.
+    pub(crate) fn terminate_goal(&self, goal_id: &GoalUuid, status: i8, result: <T::Result as Message>::RmwMsg) -> Result<(), RclrsError> {
+        let response_rmw = <T as ActionImpl>::create_result_response(status, result);
+
+        // Publish the result to anyone listening.
+        self.publish_result(goal_id, response_rmw);
+
+        // Publish the state change.
+        self.publish_status();
+
+        // Notify rcl that a goal has terminated and to therefore recalculate the expired goal timer.
+        unsafe {
+            // SAFETY: The action server is locked and valid. No other preconditions.
+            rcl_action_notify_goal_done(&*self.handle.lock())
+        }
+        .ok()?;
+
+        // Release ownership of the goal handle. It will persist until the user also drops it.
+        self.goal_handles.lock().unwrap().remove(&goal_id);
+
+        Ok(())
+    }
+
     pub(crate) fn publish_status(&self) -> Result<(), RclrsError> {
         let mut goal_statuses = DropGuard::new(
             unsafe {
@@ -628,6 +653,37 @@ where
         }
         .ok()
     }
+
+    fn publish_result(&self, goal_id: &GoalUuid, mut result: <<<T as ActionImpl>::GetResultService as Service>::Response as Message>::RmwMsg) -> Result<(), RclrsError> {
+        let goal_exists = unsafe {
+            // SAFETY: No preconditions
+            let mut goal_info = rcl_action_get_zero_initialized_goal_info();
+            goal_info.goal_id.uuid = goal_id.0;
+
+            // SAFETY: The action server is locked through the handle. The `goal_info`
+            // argument points to a rcl_action_goal_info_t with the desired UUID.
+            rcl_action_server_goal_exists(&*self.handle.lock(), &goal_info)
+        };
+        if !goal_exists {
+            panic!("Cannot publish result for unknown goal")
+        }
+
+        // TODO(nwn): Fix synchronization problem between goal_results and result_requests.
+        // Currently, there is a gap between the request queue being drained and the result being
+        // stored for future requests. Any requests received during that gap would never receive a
+        // response. Fixing this means we'll need combined locking over these two hash maps.
+
+        // Respond to all queued requests.
+        if let Some(result_requests) = self.result_requests.lock().unwrap().remove(&goal_id) {
+            for mut result_request in result_requests {
+                self.send_result_response(result_request, &mut result)?;
+            }
+        }
+
+        self.goal_results.lock().unwrap().insert(*goal_id, result);
+
+        Ok(())
+    }
 }
 
 impl<T> ActionServerBase for ActionServer<T>

rclrs/src/action/server_goal_handle.rs

@@ -112,7 +112,12 @@ where
     pub fn abort(&self, result: &ActionT::Result) -> Result<(), RclrsError> {
         self.update_state(rcl_action_goal_event_t::GOAL_EVENT_ABORT)?;
 
-        // TODO: Invoke on_terminal_state callback
+        if let Some(action_server) = self.action_server.upgrade() {
+            let result_rmw = <ActionT::Result as rosidl_runtime_rs::Message>::into_rmw_message(std::borrow::Cow::Borrowed(result)).into_owned();
+            // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_ABORTED in the rcl
+            // bindings.
+            action_server.terminate_goal(&self.uuid, 6, result_rmw)?;
+        }
         Ok(())
     }
 
@@ -125,7 +130,12 @@ where
     pub fn succeed(&self, result: &ActionT::Result) -> Result<(), RclrsError> {
         self.update_state(rcl_action_goal_event_t::GOAL_EVENT_SUCCEED)?;
 
-        // TODO: Invoke on_terminal_state callback
+        if let Some(action_server) = self.action_server.upgrade() {
+            let result_rmw = <ActionT::Result as rosidl_runtime_rs::Message>::into_rmw_message(std::borrow::Cow::Borrowed(result)).into_owned();
+            // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_SUCCEEDED in the rcl
+            // bindings.
+            action_server.terminate_goal(&self.uuid, 4, result_rmw)?;
+        }
         Ok(())
     }
 
@@ -138,7 +148,12 @@ where
     pub fn canceled(&self, result: &ActionT::Result) -> Result<(), RclrsError> {
         self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCELED)?;
 
-        // TODO: Invoke on_terminal_state callback
+        if let Some(action_server) = self.action_server.upgrade() {
+            let result_rmw = <ActionT::Result as rosidl_runtime_rs::Message>::into_rmw_message(std::borrow::Cow::Borrowed(result)).into_owned();
+            // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_CANCELED in the rcl
+            // bindings.
+            action_server.terminate_goal(&self.uuid, 5, result_rmw)?;
+        }
         Ok(())
     }
 
@@ -209,7 +224,12 @@ where
     /// Cancel the goal if its handle is dropped without reaching a terminal state.
     fn drop(&mut self) {
         if self.try_canceling() == Ok(true) {
-            // TODO: Invoke on_terminal_state callback
+            if let Some(action_server) = self.action_server.upgrade() {
+                let response_rmw = <ActionT::Result as rosidl_runtime_rs::Message>::RmwMsg::default();
+                // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_CANCELED in the rcl
+                // bindings.
+                action_server.terminate_goal(&self.uuid, 5, response_rmw);
+            }
         }
         {
             let rcl_handle = self.rcl_handle.lock().unwrap();