diff --git a/crates/hiroz/src/action/driver.rs b/crates/hiroz/src/action/driver.rs index c6cc3a4f6..a5bed390c 100644 --- a/crates/hiroz/src/action/driver.rs +++ b/crates/hiroz/src/action/driver.rs @@ -103,14 +103,23 @@ pub(crate) async fn run_driver_loop( }); } - // 5. Cancel Requests + // 5. Cancel Requests — must stay responsive while get_result waiters run. query = inner.cancel_server.queue().recv_async() => { - handle_cancel_request(&inner, query).await; + let inner = inner.clone(); + goal_tasks.spawn(async move { + handle_cancel_request(&inner, query).await; + }); } - // 6. Result Requests + // 6. Result Requests — MUST NOT block the driver loop. ros2 clients + // call get_result immediately after accept and hold it open for the + // whole goal. Awaiting that here starves cancel_goal + further + // send_goals (queries arrive at the service layer but sit in queue). query = inner.result_server.queue().recv_async() => { - handle_result_request(&inner, query).await; + let inner = inner.clone(); + goal_tasks.spawn(async move { + handle_result_request(&inner, query).await; + }); } } } @@ -147,14 +156,23 @@ async fn handle_goal_request( let requested = GoalHandle { goal: request.goal, info: GoalInfo::new(request.goal_id), - server, - query: Some(query), + server: server.clone(), cancel_flag: None, cancel_rx: None, + cleanup: crate::action::server::GoalCleanup::new(request.goal_id, server, Some(query)), _state: PhantomData::, }; - let accepted = requested.accept(); + // accept() does a blocking zenoh reply wait. Run it off the async worker + // so a stuck reply (dead client / uplink blip) cannot starve the driver + // loop from processing cancel_goal / get_result / further send_goals. + let accepted = match tokio::task::spawn_blocking(move || requested.accept()).await { + Ok(accepted) => accepted, + Err(e) => { + tracing::error!("send_goal accept task failed: {e}"); + return; + } + }; let executing = accepted.execute(); // Execute the user's handler @@ -178,35 +196,50 @@ async fn handle_cancel_request( } }; - // Mark goal as canceling using the atomic flag - let cancelled = inner.goal_manager.read(|manager| { - if let Some(ServerGoalState::Executing { cancel_flag, .. }) = - manager.goals.get(&request.goal_info.goal_id) - { - cancel_flag.store(true, std::sync::atomic::Ordering::Relaxed); - true - } else { - false + let server = ZActionServer::from_inner(Arc::clone(inner)); + + // Zero UUID = cancel all (ROS 2 / `ros2 action cancel` convention). + // Specific UUID = cancel that executing/canceling goal only. + // `request_cancel` also transitions Executing → Canceling for status. + let goals_canceling = if !request.goal_info.goal_id.is_valid() { + let ids: Vec = inner + .goal_manager + .read(|manager| manager.goals.keys().copied().collect()); + let mut out = Vec::new(); + for goal_id in ids { + if server.request_cancel(goal_id) { + out.push(GoalInfo::new(goal_id)); + } } - }); + out + } else if server.request_cancel(request.goal_info.goal_id) { + vec![request.goal_info.clone()] + } else { + vec![] + }; - // Send response + tracing::info!( + count = goals_canceling.len(), + specific = request.goal_info.goal_id.is_valid(), + "action cancel request processed" + ); + + // ERROR_NONE (0) if at least one goal is canceling; ERROR_REJECTED (1) otherwise. let response = CancelGoalServiceResponse { - return_code: if cancelled { 0 } else { 1 }, - goals_canceling: if cancelled { - vec![request.goal_info] - } else { - vec![] - }, + return_code: if goals_canceling.is_empty() { 1 } else { 0 }, + goals_canceling, }; let response_bytes = ::serialize(&response); let attachment: Attachment = query.attachment().unwrap().try_into().unwrap(); - // FIXME: address the result - let _ = query - .reply(query.key_expr().clone(), response_bytes) - .attachment(attachment) - .wait(); + // Blocking zenoh wait off the async worker (same rationale as send_goal accept). + let _ = tokio::task::spawn_blocking(move || { + let _ = query + .reply(query.key_expr().clone(), response_bytes) + .attachment(attachment) + .wait(); + }) + .await; tracing::debug!("Sent cancel response"); } @@ -276,14 +309,20 @@ async fn handle_result_request( (r, s) } Err(_) => { - tracing::warn!("Result future cancelled for goal {:?}", request.goal_id); - return; // Don't send response + tracing::warn!( + "Result future cancelled for goal {:?}; replying Aborted", + request.goal_id + ); + (A::Result::default(), super::GoalStatus::Aborted) } } } ResultState::NotFound => { - tracing::warn!("Goal {:?} not found", request.goal_id); - return; // Don't send response + tracing::warn!( + "Goal {:?} not found; replying Unknown", + request.goal_id + ); + (A::Result::default(), super::GoalStatus::Unknown) } }; @@ -294,9 +333,12 @@ async fn handle_result_request( }; let response_bytes = as ZMessage>::serialize(&response); let attachment: Attachment = query.attachment().unwrap().try_into().unwrap(); - let _ = query - .reply(query.key_expr().clone(), response_bytes) - .attachment(attachment) - .wait(); + let _ = tokio::task::spawn_blocking(move || { + let _ = query + .reply(query.key_expr().clone(), response_bytes) + .attachment(attachment) + .wait(); + }) + .await; tracing::debug!("Sent result response"); } diff --git a/crates/hiroz/src/action/mod.rs b/crates/hiroz/src/action/mod.rs index 318628e13..6f4fdf533 100644 --- a/crates/hiroz/src/action/mod.rs +++ b/crates/hiroz/src/action/mod.rs @@ -32,6 +32,7 @@ pub trait ZAction: Send + Sync + 'static { type Goal: ZMessage + Clone + Send + Sync + serde::Serialize + for<'de> serde::Deserialize<'de>; type Result: ZMessage + Clone + + Default + Send + Sync + serde::Serialize diff --git a/crates/hiroz/src/action/server.rs b/crates/hiroz/src/action/server.rs index 7e9781f79..32b2a18ad 100644 --- a/crates/hiroz/src/action/server.rs +++ b/crates/hiroz/src/action/server.rs @@ -27,12 +27,20 @@ use crate::{ topic_name::qualify_topic_name, }; +/// Per-goal cancel signal routed by [`CancelDispatcher`]. +pub(crate) enum RoutedCancel { + /// Specific cancel — receiver owns the query and must reply. + Query(zenoh::query::Query), + /// Cancel-all fan-out — dispatcher already replied; just mark canceling. + All, +} + /// Routes cancel requests from the shared cancel service queue to per-goal channels. /// /// Follows zenoh-python's per-entity queue pattern: each executing goal registers /// a dedicated channel. `drain()` reads the shared queue and routes by goal ID. pub(crate) struct CancelDispatcher { - routes: parking_lot::Mutex>>, + routes: parking_lot::Mutex>>, } impl CancelDispatcher { @@ -43,7 +51,7 @@ impl CancelDispatcher { } /// Register a goal; returns the per-goal receiver. - pub(crate) fn register(&self, goal_id: GoalId) -> flume::Receiver { + pub(crate) fn register(&self, goal_id: GoalId) -> flume::Receiver { let (tx, rx) = flume::bounded(4); self.routes.lock().insert(goal_id, tx); rx @@ -54,8 +62,22 @@ impl CancelDispatcher { self.routes.lock().remove(&goal_id); } + fn reply_cancel(query: zenoh::query::Query, response: CancelGoalServiceResponse) { + let response_bytes = ::serialize(&response); + if let Some(raw_attachment) = query.attachment() + && let Ok(attachment) = Attachment::try_from(raw_attachment) + { + let _ = query + .reply(query.key_expr().clone(), response_bytes) + .attachment(attachment) + .wait(); + } + } + /// Drain the shared cancel queue, routing each request to the appropriate per-goal channel. - /// Messages for goals with no registered handle are logged and dropped. + /// + /// Zero UUID = cancel-all: notify every registered handle and reply once. + /// Unknown specific UUID: reply `ERROR_REJECTED` so the client does not hang. pub(crate) fn drain(&self, queue: &Arc>) { while let Some(query) = queue.try_recv() { let Some(payload) = query.payload() else { @@ -70,19 +92,46 @@ impl CancelDispatcher { continue; } }; + + if !goal_id.is_valid() { + let routes = self.routes.lock(); + let goals_canceling: Vec = + routes.keys().map(|id| GoalInfo::new(*id)).collect(); + for tx in routes.values() { + let _ = tx.try_send(RoutedCancel::All); + } + drop(routes); + Self::reply_cancel( + query, + CancelGoalServiceResponse { + return_code: if goals_canceling.is_empty() { 1 } else { 0 }, + goals_canceling, + }, + ); + continue; + } + let routes = self.routes.lock(); if let Some(tx) = routes.get(&goal_id) { - if tx.try_send(query).is_err() { + if tx.try_send(RoutedCancel::Query(query)).is_err() { tracing::warn!( "CancelDispatcher: per-goal channel full for goal {:?}", goal_id ); } } else { + drop(routes); tracing::warn!( "CancelDispatcher: no handle registered for goal {:?}", goal_id ); + Self::reply_cancel( + query, + CancelGoalServiceResponse { + return_code: 1, // ERROR_REJECTED + goals_canceling: vec![], + }, + ); } } } @@ -270,17 +319,20 @@ async fn handle_result_requests_legacy_inner( let goal_id = request.goal_id; - // Either extract the result immediately (goal already terminated) or register - // a oneshot channel so `ExecutingGoal::terminate` can notify us later. + // Either extract the result immediately (goal already terminated / unknown) or + // register a oneshot channel so `ExecutingGoal::terminate` can notify us later. let (result_data, maybe_rx) = inner.goal_manager.modify(|manager| { if let Some(ServerGoalState::Terminated { result, status, .. }) = manager.goals.get(&goal_id) { (Some((result.clone(), *status)), None) - } else { + } else if manager.goals.contains_key(&goal_id) { let (tx, rx) = tokio::sync::oneshot::channel(); manager.result_futures.entry(goal_id).or_default().push(tx); (None, Some(rx)) + } else { + // Unknown goal — reply STATUS_UNKNOWN so the client does not hang. + (Some((A::Result::default(), GoalStatus::Unknown)), None) } }); @@ -300,7 +352,11 @@ async fn handle_result_requests_legacy_inner( reply_result::(query, result, status); } Err(_) => { - tracing::warn!("Result future dropped for goal {:?}", goal_id); + tracing::warn!( + "Result future dropped for goal {:?}; replying Aborted", + goal_id + ); + reply_result::(query, A::Result::default(), GoalStatus::Aborted); } } }); @@ -582,9 +638,9 @@ impl ZActionServer { goal: request.goal, info: GoalInfo::new(request.goal_id), server: self.clone(), - query: Some(query), cancel_flag: None, cancel_rx: None, + cleanup: GoalCleanup::new(request.goal_id, self.clone(), Some(query)), _state: PhantomData, }) } @@ -601,19 +657,52 @@ impl ZActionServer { !self.cancel_server().queue().is_empty() } - /// Marks a goal as canceling by setting its atomic cancel flag. - /// This is a lock-free operation that can be called from any thread. + /// Marks a goal as canceling by setting its atomic cancel flag and transitioning + /// to [`ServerGoalState::Canceling`] so status subscribers see `CANCELING`. + /// + /// Accepted goals (pre-`execute`) are cancelable — the same flag is reused when + /// execution starts so a cancel that races accept→execute is not lost. pub fn request_cancel(&self, goal_id: GoalId) -> bool { - self.goal_manager().read(|manager| { - if let Some(ServerGoalState::Executing { cancel_flag, .. }) = - manager.goals.get(&goal_id) - { - cancel_flag.store(true, Ordering::Relaxed); - true - } else { - false + let changed = self.goal_manager().modify(|manager| { + match manager.goals.remove(&goal_id) { + Some(ServerGoalState::Accepted { + goal, + cancel_flag, + expires_at, + .. + }) + | Some(ServerGoalState::Executing { + goal, + cancel_flag, + expires_at, + }) => { + cancel_flag.store(true, Ordering::Relaxed); + manager.goals.insert( + goal_id, + ServerGoalState::Canceling { + goal, + cancel_flag, + expires_at, + }, + ); + true + } + Some(state @ ServerGoalState::Canceling { .. }) => { + // Already canceling — still a successful cancel target. + manager.goals.insert(goal_id, state); + true + } + Some(other) => { + manager.goals.insert(goal_id, other); + false + } + None => false, } - }) + }); + if changed { + self.publish_status(); + } + changed } pub async fn recv_result_request(&self) -> Result<(GoalId, zenoh::query::Query)> { @@ -742,32 +831,47 @@ impl ZActionServer { /// println!("Expired {} goals", expired.len()); /// ``` pub fn expire_goals(&self) -> Vec { - let expired = self.goal_manager().modify(|manager| { + let (expired, waiters) = self.goal_manager().modify(|manager| { let now = Instant::now(); let mut expired = Vec::new(); + let mut waiters = Vec::new(); - // Find goals that have passed their expiration time - manager.goals.retain(|goal_id, state| { - let should_expire = match state { - ServerGoalState::Accepted { expires_at, .. } - | ServerGoalState::Executing { expires_at, .. } - | ServerGoalState::Terminated { expires_at, .. } => { - expires_at.is_some_and(|exp| now >= exp) - } - ServerGoalState::Canceling { .. } => false, - }; + let to_expire: Vec = manager + .goals + .iter() + .filter_map(|(goal_id, state)| { + let should_expire = match state { + ServerGoalState::Accepted { expires_at, .. } + | ServerGoalState::Executing { expires_at, .. } + | ServerGoalState::Canceling { expires_at, .. } + | ServerGoalState::Terminated { expires_at, .. } => { + expires_at.is_some_and(|exp| now >= exp) + } + }; + should_expire.then_some(*goal_id) + }) + .collect(); - if should_expire { - expired.push(*goal_id); - false // Remove this goal - } else { - true // Keep this goal + for goal_id in to_expire { + manager.goals.remove(&goal_id); + if let Some(txs) = manager.result_futures.remove(&goal_id) { + waiters.extend(txs); } - }); + expired.push(goal_id); + } - expired + (expired, waiters) }); // Lock released here + for goal_id in &expired { + self.cancel_dispatcher().deregister(*goal_id); + } + + // Wake any get_result waiters so clients do not hang after expiration. + for tx in waiters { + let _ = tx.send((A::Result::default(), GoalStatus::Aborted)); + } + // Publish updated status if any goals were expired if !expired.is_empty() { self.publish_status(); @@ -863,13 +967,111 @@ pub struct GoalHandle { /// The goal metadata. pub info: GoalInfo, pub(crate) server: ZActionServer, - pub(crate) query: Option, pub(crate) cancel_flag: Option>, /// Per-goal cancel channel registered with the CancelDispatcher (Some only in Executing state). - pub(crate) cancel_rx: Option>, + pub(crate) cancel_rx: Option>, + /// Drop cleanup; disarmed on `accept`/`execute` so type-state moves do not abort. + pub(crate) cleanup: GoalCleanup, pub(crate) _state: PhantomData, } +/// Aborts non-terminal goals (and rejects unanswered send_goal) if the handle is +/// dropped without an explicit terminal transition. Disarmed when ownership is +/// transferred via `accept` / `execute`. +pub(crate) struct GoalCleanup { + pub(crate) goal_id: GoalId, + pub(crate) server: ZActionServer, + pub(crate) query: Option, + pub(crate) armed: bool, +} + +impl GoalCleanup { + pub(crate) fn new( + goal_id: GoalId, + server: ZActionServer, + query: Option, + ) -> Self { + Self { + goal_id, + server, + query, + armed: true, + } + } + + pub(crate) fn disarm(&mut self) { + self.armed = false; + self.query = None; + } +} + +impl Drop for GoalCleanup { + fn drop(&mut self) { + if !self.armed { + return; + } + + if let Some(query) = self.query.take() { + let response = GoalResponse { + accepted: false, + stamp_sec: 0, + stamp_nanosec: 0, + }; + let response_bytes = ::serialize(&response); + if let Some(raw) = query.attachment() + && let Ok(attachment) = Attachment::try_from(raw) + { + let _ = query + .reply(query.key_expr().clone(), response_bytes) + .attachment(attachment) + .wait(); + } + } + + self.server.cancel_dispatcher().deregister(self.goal_id); + + let aborted = self.server.goal_manager().modify(|manager| { + match manager.goals.get(&self.goal_id) { + Some( + ServerGoalState::Accepted { .. } + | ServerGoalState::Executing { .. } + | ServerGoalState::Canceling { .. }, + ) => { + let now = Instant::now(); + let expires_at = Some(now + manager.result_timeout); + manager.goals.insert( + self.goal_id, + ServerGoalState::Terminated { + result: A::Result::default(), + status: GoalStatus::Aborted, + timestamp: now, + expires_at, + }, + ); + Some( + manager + .result_futures + .remove(&self.goal_id) + .unwrap_or_default(), + ) + } + _ => None, + } + }); + + if let Some(waiters) = aborted { + tracing::warn!( + goal_id = %self.goal_id, + "GoalHandle dropped without succeed/abort/canceled; aborting" + ); + for tx in waiters { + let _ = tx.send((A::Result::default(), GoalStatus::Aborted)); + } + self.server.publish_status(); + } + } +} + // --- State-specific implementations --- /// Methods available only for goals in the "Requested" state. @@ -889,12 +1091,15 @@ impl GoalHandle { /// This sends an acceptance response to the client and updates the server state. pub fn accept(mut self) -> GoalHandle { // Insert before replying — client may fire get_result before we'd register the goal. + // Create cancel_flag early so cancel can race accept→execute without being lost. + let cancel_flag = Arc::new(AtomicBool::new(false)); self.server.goal_manager().modify(|manager| { let expires_at = manager.goal_timeout.map(|timeout| Instant::now() + timeout); manager.goals.insert( self.info.goal_id, ServerGoalState::Accepted { goal: self.goal.clone(), + cancel_flag: cancel_flag.clone(), timestamp: Instant::now(), expires_at, }, @@ -910,7 +1115,7 @@ impl GoalHandle { }; let response_bytes = ::serialize(&response); - if let Some(query) = self.query.take() { + if let Some(query) = self.cleanup.query.take() { let attachment: Attachment = query.attachment().unwrap().try_into().unwrap(); // FIXME: address the result let _ = query @@ -922,13 +1127,18 @@ impl GoalHandle { // Publish status update self.server.publish_status(); + // Disarm so Drop of this Requested handle does not abort the Accepted goal. + let goal_id = self.info.goal_id; + let server = self.server.clone(); + self.cleanup.disarm(); + GoalHandle { goal: self.goal, info: self.info, - server: self.server, - query: None, - cancel_flag: None, + server: server.clone(), + cancel_flag: Some(cancel_flag), cancel_rx: None, + cleanup: GoalCleanup::new(goal_id, server, None), _state: PhantomData, } } @@ -945,7 +1155,7 @@ impl GoalHandle { }; let response_bytes = ::serialize(&response); - if let Some(query) = self.query.take() { + if let Some(query) = self.cleanup.query.take() { // FIXME: Address the unwrap usage let attachment: Attachment = query.attachment().unwrap().try_into().unwrap(); let _ = query @@ -953,6 +1163,7 @@ impl GoalHandle { .attachment(attachment) .wait(); } + self.cleanup.disarm(); Ok(()) } } @@ -972,35 +1183,71 @@ impl GoalHandle { /// Begin executing this goal and transition to the "Executing" state. /// /// This updates the server state to executing and publishes a status update. - pub fn execute(self) -> GoalHandle { - // Create cancel flag - let cancel_flag = Arc::new(AtomicBool::new(false)); + pub fn execute(mut self) -> GoalHandle { + // Reuse cancel_flag from Accepted (or Canceling if cancel already raced in). + let (cancel_flag, expires_at) = self.server.goal_manager().modify(|manager| { + match manager.goals.remove(&self.info.goal_id) { + Some(ServerGoalState::Accepted { + cancel_flag, + expires_at, + .. + }) + | Some(ServerGoalState::Canceling { + cancel_flag, + expires_at, + .. + }) => (cancel_flag, expires_at), + other => { + if let Some(state) = other { + manager.goals.insert(self.info.goal_id, state); + } + ( + Arc::new(AtomicBool::new(false)), + manager.goal_timeout.map(|timeout| Instant::now() + timeout), + ) + } + } + }); // Register with the cancel dispatcher to get a dedicated per-goal channel let cancel_rx = self.server.cancel_dispatcher().register(self.info.goal_id); - // Transition to EXECUTING + // If already cancel-requested while Accepted, stay in Canceling; otherwise Executing. self.server.goal_manager().modify(|manager| { - let expires_at = manager.goal_timeout.map(|timeout| Instant::now() + timeout); - manager.goals.insert( - self.info.goal_id, - ServerGoalState::Executing { - goal: self.goal.clone(), - cancel_flag: cancel_flag.clone(), - expires_at, - }, - ); + if cancel_flag.load(Ordering::Relaxed) { + manager.goals.insert( + self.info.goal_id, + ServerGoalState::Canceling { + goal: self.goal.clone(), + cancel_flag: cancel_flag.clone(), + expires_at, + }, + ); + } else { + manager.goals.insert( + self.info.goal_id, + ServerGoalState::Executing { + goal: self.goal.clone(), + cancel_flag: cancel_flag.clone(), + expires_at, + }, + ); + } }); self.server.publish_status(); + let goal_id = self.info.goal_id; + let server = self.server.clone(); + self.cleanup.disarm(); + GoalHandle { goal: self.goal, info: self.info, - server: self.server, - query: None, + server: server.clone(), cancel_flag: Some(cancel_flag), cancel_rx: Some(cancel_rx), + cleanup: GoalCleanup::new(goal_id, server, None), _state: PhantomData, } } @@ -1084,35 +1331,42 @@ impl GoalHandle { let Some(cancel_rx) = &self.cancel_rx else { return false; }; - if let Ok(query) = cancel_rx.try_recv() { - let payload = match query.payload() { - Some(p) => p.to_bytes(), - None => return false, - }; - let request = match ::deserialize(&payload) { - Ok(r) => r, - Err(e) => { - tracing::error!("try_process_cancel: deserialize error: {}", e); - return false; + match cancel_rx.try_recv() { + Ok(RoutedCancel::All) => { + self.server.request_cancel(self.info.goal_id); + true + } + Ok(RoutedCancel::Query(query)) => { + let payload = match query.payload() { + Some(p) => p.to_bytes(), + None => return false, + }; + let request = match ::deserialize(&payload) { + Ok(r) => r, + Err(e) => { + tracing::error!("try_process_cancel: deserialize error: {}", e); + return false; + } + }; + self.server.request_cancel(self.info.goal_id); + // ERROR_NONE (0) — cancel accepted for this goal. + let response = CancelGoalServiceResponse { + return_code: 0, + goals_canceling: vec![request.goal_info], + }; + let response_bytes = ::serialize(&response); + if let Some(raw_attachment) = query.attachment() + && let Ok(attachment) = Attachment::try_from(raw_attachment) + { + let _ = query + .reply(query.key_expr().clone(), response_bytes) + .attachment(attachment) + .wait(); } - }; - self.server.request_cancel(self.info.goal_id); - let response = CancelGoalServiceResponse { - return_code: 1, - goals_canceling: vec![request.goal_info], - }; - let response_bytes = ::serialize(&response); - if let Some(raw_attachment) = query.attachment() - && let Ok(attachment) = Attachment::try_from(raw_attachment) - { - let _ = query - .reply(query.key_expr().clone(), response_bytes) - .attachment(attachment) - .wait(); + true } - return true; + Err(_) => false, } - false } /// Mark this goal as succeeded with the given result. @@ -1136,7 +1390,10 @@ impl GoalHandle { self.terminate(result, GoalStatus::Canceled) } - fn terminate(self, result: A::Result, status: GoalStatus) -> Result<()> { + fn terminate(mut self, result: A::Result, status: GoalStatus) -> Result<()> { + // Ownership of termination; disarm so Drop does not double-abort. + self.cleanup.disarm(); + // Deregister from the cancel dispatcher so no more cancel messages are routed here self.server .cancel_dispatcher() diff --git a/crates/hiroz/src/action/state.rs b/crates/hiroz/src/action/state.rs index 93e21bd62..5de3fd338 100644 --- a/crates/hiroz/src/action/state.rs +++ b/crates/hiroz/src/action/state.rs @@ -79,6 +79,7 @@ pub struct GoalManagerInternal { pub enum ServerGoalState { Accepted { goal: A::Goal, + cancel_flag: Arc, timestamp: Instant, expires_at: Option, }, @@ -89,6 +90,8 @@ pub enum ServerGoalState { }, Canceling { goal: A::Goal, + cancel_flag: Arc, + expires_at: Option, }, Terminated { result: A::Result, diff --git a/crates/hiroz/src/ffi/action.rs b/crates/hiroz/src/ffi/action.rs index bc89d31c4..bfb2dce50 100644 --- a/crates/hiroz/src/ffi/action.rs +++ b/crates/hiroz/src/ffi/action.rs @@ -283,10 +283,19 @@ pub unsafe extern "C" fn hiroz_action_client_cancel_goal(goal_handle: *mut CGoal let gh = &(*goal_handle); let client = &(*gh.client); + // CDR-encoded action_msgs/srv/CancelGoal Request (GoalInfo = UUID + Time). + let request = crate::action::messages::CancelGoalServiceRequest { + goal_info: crate::action::GoalInfo::new(crate::action::GoalId::from_bytes(gh.goal_id)), + }; + let request_bytes = + ::serialize( + &request, + ); + match client .inner .cancel_goal_client - .call_raw(&gh.goal_id, Duration::from_secs(10)) + .call_raw(&request_bytes, Duration::from_secs(10)) { Ok(_) => ErrorCode::Success as i32, Err(e) => { @@ -431,16 +440,49 @@ pub unsafe extern "C" fn hiroz_action_server_create( None => continue, }; - // Cancel request payload: 16-byte raw goal_id (Go client sends raw UUID). - if payload.len() >= 16 { - let mut goal_id = [0u8; 16]; - goal_id.copy_from_slice(&payload[..16]); - cancel_flags_clone.lock().unwrap().insert(goal_id, true); - // Store the query and reply with empty response. - let mut server = server_mutex_cancel.lock().unwrap(); - server.cancel_goal_server.map.insert(key.clone(), query); - let _ = server.cancel_goal_server.send_response_raw(&key, &[]); - } + // Prefer CDR CancelGoal request; fall back to legacy raw 16-byte UUID + // for older Go clients that did not wrap GoalInfo. + let goal_id = match ::deserialize( + &payload, + ) { + Ok(req) => *req.goal_info.goal_id.as_bytes(), + Err(_) if payload.len() >= 16 => { + let mut id = [0u8; 16]; + // Skip optional CDR header if present. + let start = if payload.len() >= 20 + && payload[0] == 0x00 + && payload[1] == 0x01 + { + 4 + } else { + 0 + }; + id.copy_from_slice(&payload[start..start + 16]); + id + } + Err(e) => { + tracing::warn!("hiroz: Failed to parse cancel request: {}", e); + continue; + } + }; + + cancel_flags_clone.lock().unwrap().insert(goal_id, true); + + let response = crate::action::messages::CancelGoalServiceResponse { + return_code: 0, // ERROR_NONE + goals_canceling: vec![crate::action::GoalInfo::new( + crate::action::GoalId::from_bytes(goal_id), + )], + }; + let response_bytes = ::serialize( + &response, + ); + + let mut server = server_mutex_cancel.lock().unwrap(); + server.cancel_goal_server.map.insert(key.clone(), query); + let _ = server + .cancel_goal_server + .send_response_raw(&key, &response_bytes); } } }); diff --git a/crates/hiroz/tests/action/client.rs b/crates/hiroz/tests/action/client.rs index 219bb5f23..a8a68ef8a 100644 --- a/crates/hiroz/tests/action/client.rs +++ b/crates/hiroz/tests/action/client.rs @@ -11,7 +11,7 @@ pub struct TestGoal { pub order: i32, } -#[derive(Debug, Clone, Serialize, Deserialize)] +#[derive(Debug, Clone, Default, Serialize, Deserialize)] pub struct TestResult { pub value: i32, } diff --git a/crates/hiroz/tests/action/communication.rs b/crates/hiroz/tests/action/communication.rs index 00ad11ea7..327143de0 100644 --- a/crates/hiroz/tests/action/communication.rs +++ b/crates/hiroz/tests/action/communication.rs @@ -15,7 +15,7 @@ pub struct TestGoal { pub order: i32, } -#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +#[derive(Debug, Clone, PartialEq, Default, Serialize, Deserialize)] pub struct TestResult { pub value: i32, } @@ -443,7 +443,7 @@ mod tests { handle2.canceled(TestResult { value: 2 })?; let (cancel_response, _) = client_task.await.expect("client task panicked")?; - assert_eq!(cancel_response.return_code, 1); + assert_eq!(cancel_response.return_code, 0); let _ = timeout(Duration::from_secs(5), goal_handle1.result()) .await diff --git a/crates/hiroz/tests/action/expiration.rs b/crates/hiroz/tests/action/expiration.rs index 728bea991..609bb1b4a 100644 --- a/crates/hiroz/tests/action/expiration.rs +++ b/crates/hiroz/tests/action/expiration.rs @@ -23,7 +23,7 @@ struct TestGoal { order: i32, } -#[derive(Debug, Clone, Serialize, Deserialize)] +#[derive(Debug, Clone, Default, Serialize, Deserialize)] struct TestResult { sequence: Vec, } @@ -160,6 +160,7 @@ async fn test_accepted_goal_expiration_with_timeout() -> Result<()> { goal_id, ServerGoalState::Accepted { goal: TestGoal { order: 5 }, + cancel_flag: Arc::new(AtomicBool::new(false)), timestamp: now, expires_at: Some(now + Duration::from_secs(1)), }, @@ -273,6 +274,7 @@ async fn test_multiple_goals_expiration() -> Result<()> { goal_id3, ServerGoalState::Accepted { goal: TestGoal { order: 2 }, + cancel_flag: Arc::new(AtomicBool::new(false)), timestamp: now, expires_at: Some(expires), }, diff --git a/crates/hiroz/tests/action/goal_handle.rs b/crates/hiroz/tests/action/goal_handle.rs index fff119a42..d5252706a 100644 --- a/crates/hiroz/tests/action/goal_handle.rs +++ b/crates/hiroz/tests/action/goal_handle.rs @@ -7,7 +7,7 @@ pub struct TestGoal { pub order: i32, } -#[derive(Debug, Clone, Serialize, Deserialize)] +#[derive(Debug, Clone, Default, Serialize, Deserialize)] pub struct TestResult { pub value: i32, } diff --git a/crates/hiroz/tests/action/graph.rs b/crates/hiroz/tests/action/graph.rs index db52ceba9..cfd2e53a6 100644 --- a/crates/hiroz/tests/action/graph.rs +++ b/crates/hiroz/tests/action/graph.rs @@ -9,7 +9,7 @@ pub struct TestGoal { pub order: i32, } -#[derive(Debug, Clone, Serialize, Deserialize)] +#[derive(Debug, Clone, Default, Serialize, Deserialize)] pub struct TestResult { pub value: i32, } diff --git a/crates/hiroz/tests/action/interaction.rs b/crates/hiroz/tests/action/interaction.rs index 9c8203408..024c43853 100644 --- a/crates/hiroz/tests/action/interaction.rs +++ b/crates/hiroz/tests/action/interaction.rs @@ -7,7 +7,7 @@ pub struct TestGoal { pub order: i32, } -#[derive(Debug, Clone, Serialize, Deserialize)] +#[derive(Debug, Clone, Default, Serialize, Deserialize)] pub struct TestResult { pub value: i32, } diff --git a/crates/hiroz/tests/action/remapping.rs b/crates/hiroz/tests/action/remapping.rs index 8b9a89bd2..22cd13171 100644 --- a/crates/hiroz/tests/action/remapping.rs +++ b/crates/hiroz/tests/action/remapping.rs @@ -9,7 +9,7 @@ pub struct TestGoal { pub order: i32, } -#[derive(Debug, Clone, Serialize, Deserialize)] +#[derive(Debug, Clone, Default, Serialize, Deserialize)] pub struct TestResult { pub value: i32, } diff --git a/crates/hiroz/tests/action/server.rs b/crates/hiroz/tests/action/server.rs index ced877bd3..b2cc792b6 100644 --- a/crates/hiroz/tests/action/server.rs +++ b/crates/hiroz/tests/action/server.rs @@ -14,7 +14,7 @@ pub struct TestGoal { pub order: i32, } -#[derive(Debug, Clone, Serialize, Deserialize)] +#[derive(Debug, Clone, Default, Serialize, Deserialize)] pub struct TestResult { pub value: i32, } diff --git a/crates/hiroz/tests/action/wait.rs b/crates/hiroz/tests/action/wait.rs index 56de97ca6..2398aecfd 100644 --- a/crates/hiroz/tests/action/wait.rs +++ b/crates/hiroz/tests/action/wait.rs @@ -11,7 +11,7 @@ pub struct TestGoal { pub order: i32, } -#[derive(Debug, Clone, Serialize, Deserialize)] +#[derive(Debug, Clone, Default, Serialize, Deserialize)] pub struct TestResult { pub value: i32, }