diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index 3a5b8a3d..dfb4e965 100644 --- a/rclrs/src/action/server.rs +++ b/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: ::RmwMsg) -> Result<(), RclrsError> { + let response_rmw = ::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: <<::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 ActionServerBase for ActionServer diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs index ab93a5c7..3ee47d08 100644 --- a/rclrs/src/action/server_goal_handle.rs +++ b/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 = ::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 = ::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 = ::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 = ::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();