nros_node/executor/

action_core.rs

//! Type-agnostic action protocol core types.
//!
//! [`ActionServerCore`] and [`ActionClientCore`] handle the raw-bytes
//! action protocol (GoalId framing, status publishing, result slab)
//! without requiring `RosAction` type parameters. The typed
//! [`ActionServer`](super::handles::ActionServer) and
//! [`ActionClient`](super::handles::ActionClient) wrap these cores
//! and add serialization/deserialization at the boundary.

use nros_core::{CdrReader, CdrWriter, GoalId, GoalInfo, GoalStatus, GoalStatusStamped, Serialize};
use nros_rmw::{Publisher, ServiceClientTrait, ServiceServerTrait, Subscriber, TransportError};

use super::types::NodeError;
use crate::session;

/// Scratch buffer for a decoded CancelGoal request. Cancel payloads are
/// tiny (a `GoalId` + a `builtin_interfaces/Time` stamp), so a fixed
/// 256-byte buffer covers them without a const-generic parameter like the
/// goal/result/feedback slabs.
pub(crate) const CANCEL_BUF: usize = 256;

/// DDS type name of an action's `send_goal` / `get_result` service.
///
/// ROS 2 names the action's two services by their per-channel service types —
/// `<Action>_SendGoal` / `<Action>_GetResult` — **not** the bare action type
/// (`<Action>`). A real `rcl_action` peer matches on those, so advertising the
/// bare action type leaves our send_goal/get_result services undiscovered and
/// every goal times out.
///
/// The codegen emits each channel's *request* envelope type name in DDS form,
/// e.g. `example_interfaces::action::dds_::Fibonacci_SendGoal_Request_`. The
/// service layer (`xrce_dds_request_type` / the Zenoh shim) extends a base type
/// ending in `_` with `Request_` / `Response_`, so the base we must pass is the
/// request type name with its trailing `Request_` stripped:
/// `…Fibonacci_SendGoal_Request_` → `…Fibonacci_SendGoal_`. Falls back to the
/// bare action type if the request name has an unexpected shape.
pub(crate) fn action_service_base_type<'a>(
    request_type_name: &'a str,
    fallback_action_type: &'a str,
) -> &'a str {
    request_type_name
        .strip_suffix("Request_")
        .unwrap_or(fallback_action_type)
}

/// Scratch buffer for serializing a `GoalStatusArray` before publishing it
/// on the status topic. 512 bytes holds the CDR header plus a status entry
/// (`GoalInfo` + status enum) for every concurrently-tracked goal.
const STATUS_ARRAY_BUF: usize = 512;

// ============================================================================
// Supporting types
// ============================================================================

/// Goal tracked by the core — only GoalId + status, no typed data.
#[derive(Clone, Copy)]
pub struct RawActiveGoal {
    /// Goal ID.
    pub goal_id: GoalId,
    /// Current status.
    pub status: GoalStatus,
}

/// A `get_result` request held until its goal terminates (Phase 237).
///
/// `sequence_number` is the service-backend reply-correlation token; the backend
/// must be able to `send_reply(sequence_number, …)` after the handler returned
/// (Cyclone native; XRCE/Zenoh via the Phase 237 seq-keyed reply tables).
#[derive(Clone, Copy)]
pub struct PendingGetResult {
    /// Goal whose terminal result the requester is waiting for.
    pub goal_id: GoalId,
    /// Backend reply-correlation token for the deferred `send_reply`.
    pub sequence_number: i64,
}

/// Completed goal result metadata — indexes into the result slab.
#[derive(Clone, Copy)]
pub struct CompletedResultEntry {
    /// Unique identifier for the completed goal.
    pub goal_id: GoalId,
    /// Terminal status of the goal.
    pub status: GoalStatus,
    /// Byte offset into the result slab.
    pub offset: usize,
    /// Length of the serialised result in bytes.
    pub len: usize,
}

/// Phase 122.3.c.6.d — information about a peeked cancel-goal
/// request. Returned by
/// [`ActionServerCore::try_recv_cancel_request`].
pub struct PendingCancelRequest {
    /// The goal_id named in the cancel request.
    pub goal_id: GoalId,
    /// Service sequence number — pass back to
    /// [`ActionServerCore::send_cancel_reply`].
    pub sequence_number: i64,
    /// Snapshot of the goal's current status at peek time
    /// (`GoalStatus::Unknown` if no matching active goal).
    pub current_status: GoalStatus,
}

/// Information about a received goal request.
pub struct RawGoalRequest {
    /// The parsed goal ID.
    pub goal_id: GoalId,
    /// Sequence number for the service reply.
    pub sequence_number: i64,
    /// Offset into the goal buffer where the CDR payload begins.
    /// Backends may prepend a sequence-number header (DDS) or place
    /// the payload at offset 0 (zenoh).
    pub data_offset: usize,
    /// Length of valid CDR data starting at `data_offset`.
    pub data_len: usize,
}

// ============================================================================
// GoalId CDR helpers
// ============================================================================

/// Read a GoalId from a CDR reader as a fixed `uint8[16]` array.
///
/// ROS 2 actions carry the goal id as `unique_identifier_msgs/UUID`, whose
/// single field is a **fixed-size** `uint8[16]` array — CDR fixed arrays have
/// **no** length prefix. We must read exactly 16 bytes with no leading count,
/// matching `unique_identifier_msgs::msg::UUID::deserialize`. (The pre-233.6
/// framing wrote a `u32(16)` sequence prefix, which self-matched nano-ros peers
/// but added 4 bytes a real `rcl_action` peer rejects.)
fn read_goal_id(reader: &mut CdrReader<'_>) -> Result<GoalId, NodeError> {
    let mut goal_id = GoalId::default();
    for byte in &mut goal_id.uuid {
        *byte = reader
            .read_u8()
            .map_err(|_| NodeError::Transport(TransportError::DeserializationError))?;
    }
    Ok(goal_id)
}

/// Write a GoalId into a CDR writer as a fixed `uint8[16]` array (no length
/// prefix) — see [`read_goal_id`] for why the prefix must be absent.
fn write_goal_id(writer: &mut CdrWriter<'_>, goal_id: &GoalId) -> Result<(), NodeError> {
    for b in &goal_id.uuid {
        writer.write_u8(*b).map_err(|_| NodeError::Serialization)?;
    }
    Ok(())
}

// ============================================================================
// ActionServerCore
// ============================================================================

/// Type-agnostic action server core handling the raw-bytes protocol.
///
/// Manages active goal tracking (GoalId + status), completed result storage
/// in a fixed-size slab, and all CDR framing for the action protocol.
///
/// The typed [`ActionServer`](super::handles::ActionServer) wraps this
/// and adds `A::Goal` / `A::Feedback` / `A::Result` (de)serialization.
pub struct ActionServerCore<
    const GOAL_BUF: usize = { crate::config::DEFAULT_RX_BUF_SIZE },
    const RESULT_BUF: usize = { crate::config::DEFAULT_RX_BUF_SIZE },
    const FEEDBACK_BUF: usize = { crate::config::DEFAULT_RX_BUF_SIZE },
    const MAX_GOALS: usize = 4,
> {
    pub(crate) send_goal_server: session::RmwServiceServer,
    pub(crate) cancel_goal_server: session::RmwServiceServer,
    pub(crate) get_result_server: session::RmwServiceServer,
    pub(crate) feedback_publisher: session::RmwPublisher,
    pub(crate) status_publisher: session::RmwPublisher,
    pub(crate) active_goals: heapless::Vec<RawActiveGoal, MAX_GOALS>,
    pub(crate) completed_results: heapless::Vec<CompletedResultEntry, MAX_GOALS>,
    /// `get_result` requests that arrived while their goal was still active
    /// (Phase 237). `rclcpp_action` sends `get_result` immediately after
    /// acceptance and expects the reply only once the goal terminates, so we
    /// hold the request's correlation token (`sequence_number`) here and flush
    /// it in [`Self::complete_goal_raw`]. Deferral relies on the service
    /// backend honoring `send_reply(seq)` after the handler returns — the
    /// seq-keyed reply contract (Cyclone native; XRCE/Zenoh per Phase 237).
    pub(crate) pending_get_results: heapless::Vec<PendingGetResult, MAX_GOALS>,
    /// Slab storage for completed result CDR bytes.
    pub(crate) result_slab: [u8; RESULT_BUF],
    pub(crate) result_slab_used: usize,
    pub(crate) goal_buffer: [u8; GOAL_BUF],
    pub(crate) feedback_buffer: [u8; FEEDBACK_BUF],
    pub(crate) cancel_buffer: [u8; CANCEL_BUF],
}

impl<
    const GOAL_BUF: usize,
    const RESULT_BUF: usize,
    const FEEDBACK_BUF: usize,
    const MAX_GOALS: usize,
> ActionServerCore<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>
{
    /// Phase 122.3.c.6.b — construct an `ActionServerCore` from the
    /// 5 already-built transport channels. Caller (typically the C
    /// API's `nros_action_server_init_polling`) owns wiring the
    /// channels via the session's `create_*` methods.
    pub fn from_channels(
        send_goal_server: session::RmwServiceServer,
        cancel_goal_server: session::RmwServiceServer,
        get_result_server: session::RmwServiceServer,
        feedback_publisher: session::RmwPublisher,
        status_publisher: session::RmwPublisher,
    ) -> Self {
        Self {
            send_goal_server,
            cancel_goal_server,
            get_result_server,
            feedback_publisher,
            status_publisher,
            active_goals: heapless::Vec::new(),
            completed_results: heapless::Vec::new(),
            pending_get_results: heapless::Vec::new(),
            result_slab: [0u8; RESULT_BUF],
            result_slab_used: 0,
            goal_buffer: [0u8; GOAL_BUF],
            feedback_buffer: [0u8; FEEDBACK_BUF],
            cancel_buffer: [0u8; CANCEL_BUF],
        }
    }

    /// Try to receive a goal request from the send_goal service.
    ///
    /// Returns the parsed GoalId, sequence number, and data length.
    /// The full CDR data (including GoalId) remains in `goal_buffer`.
    pub fn try_recv_goal_request(&mut self) -> Result<Option<RawGoalRequest>, NodeError> {
        // Capture buf base ptr before borrowing through `try_recv_request`
        // so we can recover the data offset after the borrow ends.
        // DDS-style backends place a sequence-number prefix before the
        // CDR payload; reading the buffer from offset 0 unconditionally
        // would feed the prefix bytes to the deserializer.
        let buf_start = self.goal_buffer.as_ptr() as usize;
        // Phase 120: NoData (no pending request) is the steady-state
        // expected condition — collapse it to `Ok(None)` instead of
        // surfacing as ServiceRequestFailed. Any other transport
        // error remains ServiceRequestFailed.
        let request = match self
            .send_goal_server
            .try_recv_request(&mut self.goal_buffer)
        {
            Ok(opt) => opt,
            Err(TransportError::NoData) => return Ok(None),
            Err(_) => return Err(NodeError::Transport(TransportError::ServiceRequestFailed)),
        };

        let request = match request {
            Some(r) => r,
            None => return Ok(None),
        };

        let data_offset = (request.data.as_ptr() as usize).saturating_sub(buf_start);
        let data_len = request.data.len();
        let sequence_number = request.sequence_number;
        #[allow(clippy::drop_non_drop)]
        drop(request);

        let mut reader =
            CdrReader::new_with_header(&self.goal_buffer[data_offset..data_offset + data_len])
                .map_err(|_| NodeError::Transport(TransportError::DeserializationError))?;

        let goal_id = read_goal_id(&mut reader)?;

        Ok(Some(RawGoalRequest {
            goal_id,
            sequence_number,
            data_offset,
            data_len,
        }))
    }

    /// Get a reference to the goal buffer (valid after `try_recv_goal_request`).
    pub fn goal_buffer(&self) -> &[u8] {
        &self.goal_buffer
    }

    /// Accept a goal: sends the acceptance reply, adds to active goals,
    /// publishes status.
    pub fn accept_goal(&mut self, goal_id: GoalId, seq: i64) -> Result<(), NodeError> {
        // Serialize response: accepted=true + stamp (Time: sec=0, nanosec=0)
        let mut writer = CdrWriter::new_with_header(&mut self.cancel_buffer)
            .map_err(|_| NodeError::BufferTooSmall)?;
        writer.write_u8(1).map_err(|_| NodeError::Serialization)?;
        writer.write_i32(0).map_err(|_| NodeError::Serialization)?;
        writer.write_u32(0).map_err(|_| NodeError::Serialization)?;
        let reply_len = writer.position();

        self.send_goal_server
            .send_reply(seq, &self.cancel_buffer[..reply_len])
            .map_err(|_| NodeError::ServiceReplyFailed)?;

        let _ = self.active_goals.push(RawActiveGoal {
            goal_id,
            status: GoalStatus::Accepted,
        });
        let _ = self.publish_status_array();
        Ok(())
    }

    /// Reject a goal: sends the rejection reply.
    pub fn reject_goal(&mut self, seq: i64) -> Result<(), NodeError> {
        // Serialize response: accepted=false + stamp
        let mut writer = CdrWriter::new_with_header(&mut self.cancel_buffer)
            .map_err(|_| NodeError::BufferTooSmall)?;
        writer.write_u8(0).map_err(|_| NodeError::Serialization)?;
        writer.write_i32(0).map_err(|_| NodeError::Serialization)?;
        writer.write_u32(0).map_err(|_| NodeError::Serialization)?;
        let reply_len = writer.position();

        self.send_goal_server
            .send_reply(seq, &self.cancel_buffer[..reply_len])
            .map_err(|_| NodeError::ServiceReplyFailed)
    }

    /// Publish feedback with raw CDR bytes.
    ///
    /// Writes GoalId framing + raw feedback bytes into the feedback buffer
    /// and publishes.
    pub fn publish_feedback_raw(
        &mut self,
        goal_id: &GoalId,
        feedback_cdr: &[u8],
    ) -> Result<(), NodeError> {
        // GoalId framing (4 + 16 = 20 bytes) + feedback_cdr must fit in FEEDBACK_BUF
        let needed = 4 + 20 + feedback_cdr.len(); // CDR header + GoalId + feedback
        if needed > FEEDBACK_BUF {
            return Err(NodeError::BufferTooSmall);
        }

        let mut writer = CdrWriter::new_with_header(&mut self.feedback_buffer)
            .map_err(|_| NodeError::BufferTooSmall)?;

        write_goal_id(&mut writer, goal_id)?;

        // Copy raw feedback bytes directly after GoalId
        let pos = writer.position();
        if pos + feedback_cdr.len() > FEEDBACK_BUF {
            return Err(NodeError::BufferTooSmall);
        }
        self.feedback_buffer[pos..pos + feedback_cdr.len()].copy_from_slice(feedback_cdr);
        let len = pos + feedback_cdr.len();

        self.feedback_publisher
            .publish_raw(&self.feedback_buffer[..len])
            .map_err(|_| NodeError::Transport(TransportError::PublishFailed))
    }

    /// Set a goal's status and publish the updated GoalStatusArray.
    pub fn set_goal_status(&mut self, goal_id: &GoalId, status: GoalStatus) {
        for goal in &mut self.active_goals {
            if goal.goal_id.uuid == goal_id.uuid {
                goal.status = status;
                break;
            }
        }
        let _ = self.publish_status_array();
    }

    /// Complete a goal: remove from active, store raw result CDR in slab,
    /// publish status.
    pub fn complete_goal_raw(&mut self, goal_id: &GoalId, status: GoalStatus, result_cdr: &[u8]) {
        // Remove from active goals
        if let Some(pos) = self
            .active_goals
            .iter()
            .position(|g| g.goal_id.uuid == goal_id.uuid)
        {
            self.active_goals.swap_remove(pos);
        }

        // Store result CDR in the slab
        let offset = self.result_slab_used;
        let end = offset + result_cdr.len();
        let stored = if end <= RESULT_BUF {
            self.result_slab[offset..end].copy_from_slice(result_cdr);
            self.result_slab_used = end;
            let _ = self.completed_results.push(CompletedResultEntry {
                goal_id: *goal_id,
                status,
                offset,
                len: result_cdr.len(),
            });
            true
        } else {
            false
        };

        // Phase 237 — flush any get_result requests that arrived while this goal
        // was still active. The result is now in the slab; reply to each held
        // requester via its retained `sequence_number`.
        if stored {
            let len = result_cdr.len();
            let mut i = 0;
            while i < self.pending_get_results.len() {
                if self.pending_get_results[i].goal_id.uuid == goal_id.uuid {
                    let seq = self.pending_get_results[i].sequence_number;
                    // swap_remove moves the last entry into slot `i`; re-check `i`.
                    let _ = self.pending_get_results.swap_remove(i);
                    let _ = self.reply_get_result_from_slab(seq, status, offset, len);
                } else {
                    i += 1;
                }
            }
        }

        let _ = self.publish_status_array();
    }

    /// Phase 122.3.c.6.e — register a `Waker` that fires when a new
    /// send_goal request arrives. Event-driven action servers
    /// register here in place of polling `try_recv_goal_request` on
    /// a timer.
    pub fn register_goal_waker(&self, waker: &core::task::Waker) {
        use nros_rmw::ServiceServerTrait;
        self.send_goal_server.register_waker(waker);
    }

    /// Phase 122.3.c.6.e — register a `Waker` that fires when a
    /// cancel-goal request arrives.
    pub fn register_cancel_waker(&self, waker: &core::task::Waker) {
        use nros_rmw::ServiceServerTrait;
        self.cancel_goal_server.register_waker(waker);
    }

    /// Phase 122.3.c.6.e — register a `Waker` that fires when a
    /// get_result query arrives.
    pub fn register_get_result_waker(&self, waker: &core::task::Waker) {
        use nros_rmw::ServiceServerTrait;
        self.get_result_server.register_waker(waker);
    }

    /// Phase 122.3.c.6.d — peek a pending cancel-goal request without
    /// generating a reply. Returns the goal_id named in the request,
    /// the matching service sequence number (use it with
    /// [`send_cancel_reply`](Self::send_cancel_reply)), and the
    /// goal's current status (`GoalStatus::Unknown` if no such
    /// active goal). Returns `Ok(None)` when no cancel request is
    /// pending.
    ///
    /// Used by L1 polling-mode action servers (nros-c / nros-cpp C
    /// FFI) that want to drive cancel-decision policy without
    /// passing a Rust closure across the C ABI. See the matching
    /// [`send_cancel_reply`](Self::send_cancel_reply) for the reply
    /// side. The high-level closure-based
    /// [`try_handle_cancel`](Self::try_handle_cancel) keeps working
    /// and now delegates to this pair.
    pub fn try_recv_cancel_request(&mut self) -> Result<Option<PendingCancelRequest>, NodeError> {
        let buf_start = self.cancel_buffer.as_ptr() as usize;
        let request = match self
            .cancel_goal_server
            .try_recv_request(&mut self.cancel_buffer)
        {
            Ok(Some(r)) => r,
            Ok(None) | Err(TransportError::NoData) => return Ok(None),
            Err(_) => return Err(NodeError::Transport(TransportError::ServiceRequestFailed)),
        };

        let data_offset = (request.data.as_ptr() as usize).saturating_sub(buf_start);
        let data_len = request.data.len();
        let sequence_number = request.sequence_number;
        #[allow(clippy::drop_non_drop)]
        drop(request);

        let mut reader =
            CdrReader::new_with_header(&self.cancel_buffer[data_offset..data_offset + data_len])
                .map_err(|_| NodeError::Transport(TransportError::DeserializationError))?;

        let goal_id = read_goal_id(&mut reader)?;
        let current_status = self.find_goal_status(&goal_id);

        Ok(Some(PendingCancelRequest {
            goal_id,
            sequence_number,
            current_status,
        }))
    }

    /// Phase 122.3.c.6.d — send the reply to a previously-peeked
    /// cancel-goal request. `sequence_number` must match the value
    /// returned by [`try_recv_cancel_request`](Self::try_recv_cancel_request).
    ///
    /// `return_code` is the overall RPC status (`CancelResponse::Ok`
    /// = at least one cancel honoured; other variants = whole-request
    /// failure). `accepted` lists the goals that transition to
    /// `Canceling`; this function flips their stored status before
    /// publishing the status array.
    pub fn send_cancel_reply(
        &mut self,
        sequence_number: i64,
        return_code: nros_core::CancelResponse,
        accepted: &[GoalId],
    ) -> Result<(), NodeError> {
        for id in accepted {
            self.set_goal_status(id, GoalStatus::Canceling);
        }

        let mut writer = CdrWriter::new_with_header(&mut self.goal_buffer)
            .map_err(|_| NodeError::BufferTooSmall)?;
        writer
            .write_i8(return_code as i8)
            .map_err(|_| NodeError::Serialization)?;
        let count = u32::try_from(accepted.len()).unwrap_or(u32::MAX);
        writer
            .write_u32(count)
            .map_err(|_| NodeError::Serialization)?;
        for id in accepted {
            write_goal_id(&mut writer, id)?;
            // GoalInfo.stamp — zero timestamp.
            writer.write_i32(0).map_err(|_| NodeError::Serialization)?;
            writer.write_u32(0).map_err(|_| NodeError::Serialization)?;
        }
        let reply_len = writer.position();

        self.cancel_goal_server
            .send_reply(sequence_number, &self.goal_buffer[..reply_len])
            .map_err(|_| NodeError::ServiceReplyFailed)?;

        if !accepted.is_empty() {
            let _ = self.publish_status_array();
        }

        Ok(())
    }

    /// Try to handle a cancel_goal request (type-agnostic).
    pub fn try_handle_cancel(
        &mut self,
        cancel_handler: impl FnOnce(&GoalId, GoalStatus) -> nros_core::CancelResponse,
    ) -> Result<Option<(GoalId, nros_core::CancelResponse)>, NodeError> {
        let buf_start = self.cancel_buffer.as_ptr() as usize;
        // Phase 120: NoData == steady-state idle; map to Ok(None).
        let request = match self
            .cancel_goal_server
            .try_recv_request(&mut self.cancel_buffer)
        {
            Ok(Some(r)) => r,
            Ok(None) | Err(TransportError::NoData) => return Ok(None),
            Err(_) => return Err(NodeError::Transport(TransportError::ServiceRequestFailed)),
        };

        let data_offset = (request.data.as_ptr() as usize).saturating_sub(buf_start);
        let data_len = request.data.len();
        let sequence_number = request.sequence_number;
        #[allow(clippy::drop_non_drop)]
        drop(request);

        let mut reader =
            CdrReader::new_with_header(&self.cancel_buffer[data_offset..data_offset + data_len])
                .map_err(|_| NodeError::Transport(TransportError::DeserializationError))?;

        let goal_id = read_goal_id(&mut reader)?;

        let current_status = self.find_goal_status(&goal_id);
        let response = cancel_handler(&goal_id, current_status);

        if response == nros_core::CancelResponse::Ok {
            self.set_goal_status(&goal_id, GoalStatus::Canceling);
        }

        // Serialize response: return_code (i8) + goals_canceling (sequence of GoalInfo)
        let mut writer = CdrWriter::new_with_header(&mut self.goal_buffer)
            .map_err(|_| NodeError::BufferTooSmall)?;
        writer
            .write_i8(response as i8)
            .map_err(|_| NodeError::Serialization)?;

        let num_canceling = if response == nros_core::CancelResponse::Ok {
            1u32
        } else {
            0u32
        };
        writer
            .write_u32(num_canceling)
            .map_err(|_| NodeError::Serialization)?;
        if response == nros_core::CancelResponse::Ok {
            write_goal_id(&mut writer, &goal_id)?;
            writer.write_i32(0).map_err(|_| NodeError::Serialization)?;
            writer.write_u32(0).map_err(|_| NodeError::Serialization)?;
        }
        let reply_len = writer.position();

        self.cancel_goal_server
            .send_reply(sequence_number, &self.goal_buffer[..reply_len])
            .map_err(|_| NodeError::ServiceReplyFailed)?;

        Ok(Some((goal_id, response)))
    }

    /// Try to handle a get_result request using raw bytes.
    ///
    /// For completed goals, sends the stored raw result CDR from the slab.
    /// For active/unknown goals, sends the provided `default_result_cdr` bytes.
    ///
    /// `default_result_cdr` should contain serialized result data (without CDR
    /// header or status byte) — typically `A::Result::default()` serialized.
    pub fn try_handle_get_result_raw(
        &mut self,
        default_result_cdr: &[u8],
    ) -> Result<Option<GoalId>, NodeError> {
        let buf_start = self.goal_buffer.as_ptr() as usize;
        // Phase 120: NoData == steady-state idle; map to Ok(None).
        let request = match self
            .get_result_server
            .try_recv_request(&mut self.goal_buffer)
        {
            Ok(Some(r)) => r,
            Ok(None) | Err(TransportError::NoData) => return Ok(None),
            Err(_) => return Err(NodeError::Transport(TransportError::ServiceRequestFailed)),
        };

        let data_offset = (request.data.as_ptr() as usize).saturating_sub(buf_start);
        let data_len = request.data.len();
        let sequence_number = request.sequence_number;
        #[allow(clippy::drop_non_drop)]
        drop(request);

        let mut reader =
            CdrReader::new_with_header(&self.goal_buffer[data_offset..data_offset + data_len])
                .map_err(|_| NodeError::Transport(TransportError::DeserializationError))?;

        let goal_id = read_goal_id(&mut reader)?;

        // Look up in completed results
        let completed = self
            .completed_results
            .iter()
            .find(|c| c.goal_id.uuid == goal_id.uuid);

        if let Some(entry) = completed {
            // Completed: send status + stored result CDR from the slab.
            let (off, len, status) = (entry.offset, entry.len, entry.status);
            self.reply_get_result_from_slab(sequence_number, status, off, len)?;
        } else if self
            .active_goals
            .iter()
            .any(|g| g.goal_id.uuid == goal_id.uuid)
        {
            // Active goal → DEFER (Phase 237). `rclcpp_action` sends get_result
            // right after acceptance and expects the reply only once the goal
            // terminates; replying now with a non-terminal status makes the
            // client treat an unfinished goal as done. Hold the request's
            // correlation token; `complete_goal_raw` flushes it. The backend
            // retains the reply token keyed by `sequence_number`.
            if self
                .pending_get_results
                .push(PendingGetResult {
                    goal_id,
                    sequence_number,
                })
                .is_err()
            {
                // Table full — fail loud rather than silently strand the
                // requester (caller surfaces it; the request is not re-queued).
                return Err(NodeError::BufferTooSmall);
            }
        } else {
            // Unknown goal → reply immediately with UNKNOWN + default result.
            let mut writer = CdrWriter::new_with_header(&mut self.goal_buffer)
                .map_err(|_| NodeError::BufferTooSmall)?;
            writer
                .write_i8(GoalStatus::Unknown as i8)
                .map_err(|_| NodeError::Serialization)?;
            writer.align(4).map_err(|_| NodeError::Serialization)?;
            let pos = writer.position();
            if pos + default_result_cdr.len() > GOAL_BUF {
                return Err(NodeError::BufferTooSmall);
            }
            self.goal_buffer[pos..pos + default_result_cdr.len()]
                .copy_from_slice(default_result_cdr);
            let reply_len = pos + default_result_cdr.len();

            self.get_result_server
                .send_reply(sequence_number, &self.goal_buffer[..reply_len])
                .map_err(|_| NodeError::ServiceReplyFailed)?;
        }

        Ok(Some(goal_id))
    }

    /// Build + send a `get_result` reply — `[status i8][align(4)][result CDR]`
    /// — copying the result bytes from the slab. Shared by the immediate
    /// completed-goal path and the deferred flush in `complete_goal_raw`
    /// (Phase 237). `goal_buffer` and `result_slab` are disjoint fields, so the
    /// header build (into `goal_buffer`) and the slab copy don't alias.
    fn reply_get_result_from_slab(
        &mut self,
        sequence_number: i64,
        status: GoalStatus,
        slab_offset: usize,
        slab_len: usize,
    ) -> Result<(), NodeError> {
        let mut writer = CdrWriter::new_with_header(&mut self.goal_buffer)
            .map_err(|_| NodeError::BufferTooSmall)?;
        writer
            .write_i8(status as i8)
            .map_err(|_| NodeError::Serialization)?;
        // Align to 4 after the status byte: the result CDR starts with a u32
        // sequence length the reader will `align(4)` to.
        writer.align(4).map_err(|_| NodeError::Serialization)?;
        let pos = writer.position();
        if pos + slab_len > GOAL_BUF {
            return Err(NodeError::BufferTooSmall);
        }
        self.goal_buffer[pos..pos + slab_len]
            .copy_from_slice(&self.result_slab[slab_offset..slab_offset + slab_len]);
        let reply_len = pos + slab_len;
        self.get_result_server
            .send_reply(sequence_number, &self.goal_buffer[..reply_len])
            .map_err(|_| NodeError::ServiceReplyFailed)
    }

    /// Get the number of active goals.
    pub fn active_goal_count(&self) -> usize {
        self.active_goals.len()
    }

    /// Get a reference to all active goals.
    pub fn active_goals(&self) -> &[RawActiveGoal] {
        &self.active_goals
    }

    /// Find the status of a goal (active or unknown).
    pub fn find_goal_status(&self, goal_id: &GoalId) -> GoalStatus {
        self.active_goals
            .iter()
            .find(|g| g.goal_id.uuid == goal_id.uuid)
            .map(|g| g.status)
            .unwrap_or(GoalStatus::Unknown)
    }

    /// Publish the current GoalStatusArray on the status topic.
    pub fn publish_status_array(&self) -> Result<(), NodeError> {
        let mut buf = [0u8; STATUS_ARRAY_BUF];
        let mut writer =
            CdrWriter::new_with_header(&mut buf).map_err(|_| NodeError::BufferTooSmall)?;

        writer
            .write_u32(self.active_goals.len() as u32)
            .map_err(|_| NodeError::Serialization)?;

        for goal in &self.active_goals {
            let stamped = GoalStatusStamped::new(GoalInfo::with_id(goal.goal_id), goal.status);
            stamped
                .serialize(&mut writer)
                .map_err(|_| NodeError::Serialization)?;
        }

        let len = writer.position();
        self.status_publisher
            .publish_raw(&buf[..len])
            .map_err(|_| NodeError::Transport(TransportError::PublishFailed))
    }
}

// ============================================================================
// ActionClientCore
// ============================================================================

/// Type-agnostic action client core handling the raw-bytes protocol.
///
/// The typed [`ActionClient`](super::handles::ActionClient) wraps this
/// and adds serialization/deserialization at the boundary.
pub struct ActionClientCore<
    const GOAL_BUF: usize = { crate::config::DEFAULT_RX_BUF_SIZE },
    const RESULT_BUF: usize = { crate::config::DEFAULT_RX_BUF_SIZE },
    const FEEDBACK_BUF: usize = { crate::config::DEFAULT_RX_BUF_SIZE },
> {
    pub(crate) send_goal_client: session::RmwServiceClient,
    pub(crate) cancel_goal_client: session::RmwServiceClient,
    pub(crate) get_result_client: session::RmwServiceClient,
    pub(crate) feedback_subscriber: session::RmwSubscriber,
    pub(crate) goal_buffer: [u8; GOAL_BUF],
    pub(crate) result_buffer: [u8; RESULT_BUF],
    pub(crate) feedback_buffer: [u8; FEEDBACK_BUF],
    pub(crate) goal_counter: u64,
    /// Phase 84.D3: per-sub-client in-flight flags. Each of the three
    /// sub-clients (send_goal / cancel / get_result) is an independent
    /// request/reply channel and tracks its own "unconsumed reply"
    /// state. Cleared by `Promise::try_recv` on success.
    pub(crate) in_flight_send_goal: bool,
    pub(crate) in_flight_cancel: bool,
    pub(crate) in_flight_get_result: bool,
}

impl<const GOAL_BUF: usize, const RESULT_BUF: usize, const FEEDBACK_BUF: usize>
    ActionClientCore<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>
{
    /// Begin a server-discovery probe on the underlying `send_goal`
    /// service client. Used by the C action-client wrapper
    /// (`nros_action_client_wait_for_action_server`) to keep
    /// `send_goal_client` private while still exposing the discovery
    /// surface from `ServiceClientTrait`.
    pub fn start_server_discovery(
        &mut self,
        timeout_ms: u32,
    ) -> Result<(), nros_rmw::TransportError> {
        use nros_rmw::ServiceClientTrait;
        self.send_goal_client.start_server_discovery(timeout_ms)
    }

    /// Poll the in-flight server-discovery probe started by
    /// [`start_server_discovery`](Self::start_server_discovery).
    pub fn poll_server_discovery(&mut self) -> Result<Option<bool>, nros_rmw::TransportError> {
        use nros_rmw::ServiceClientTrait;
        self.send_goal_client.poll_server_discovery()
    }

    /// Latched "is server visible" snapshot. See
    /// `ActionClient::action_server_is_ready` for the semantic.
    pub fn is_server_ready(&self) -> bool {
        use nros_rmw::ServiceClientTrait;
        self.send_goal_client.is_server_ready()
    }

    /// Create a new action client core from the raw transport handles.
    pub fn new(
        send_goal_client: session::RmwServiceClient,
        cancel_goal_client: session::RmwServiceClient,
        get_result_client: session::RmwServiceClient,
        feedback_subscriber: session::RmwSubscriber,
    ) -> Self {
        Self {
            send_goal_client,
            cancel_goal_client,
            get_result_client,
            feedback_subscriber,
            goal_buffer: [0u8; GOAL_BUF],
            result_buffer: [0u8; RESULT_BUF],
            feedback_buffer: [0u8; FEEDBACK_BUF],
            goal_counter: 0,
            in_flight_send_goal: false,
            in_flight_cancel: false,
            in_flight_get_result: false,
        }
    }

    /// Send a goal with raw CDR bytes. Returns the generated GoalId.
    ///
    /// The `goal_cdr` bytes are the serialized goal data (without GoalId framing).
    /// This writes GoalId + goal_cdr into the goal buffer and sends the request.
    ///
    /// After calling, use `send_goal_client` and `result_buffer` to construct
    /// a Promise for the acceptance reply.
    pub fn send_goal_raw(&mut self, goal_cdr: &[u8]) -> Result<GoalId, NodeError> {
        self.goal_counter += 1;
        let mut goal_id = GoalId::default();
        let counter_bytes = self.goal_counter.to_le_bytes();
        goal_id.uuid[..8].copy_from_slice(&counter_bytes);

        let mut writer = CdrWriter::new_with_header(&mut self.goal_buffer)
            .map_err(|_| NodeError::BufferTooSmall)?;

        write_goal_id(&mut writer, &goal_id)?;

        // Copy raw goal CDR bytes after GoalId
        let pos = writer.position();
        if pos + goal_cdr.len() > GOAL_BUF {
            return Err(NodeError::BufferTooSmall);
        }
        self.goal_buffer[pos..pos + goal_cdr.len()].copy_from_slice(goal_cdr);
        let req_len = pos + goal_cdr.len();

        self.send_goal_client
            .send_request_raw(&self.goal_buffer[..req_len])
            .map_err(|_| NodeError::ServiceRequestFailed)?;

        Ok(goal_id)
    }

    /// Send a goal (blocking). Returns the GoalId and whether it was accepted.
    ///
    /// Uses the blocking `call_raw` path (like service calls) so the send_goal
    /// z_get is fully consumed before returning. This avoids leaving a pending
    /// get slot that could interfere with subsequent blocking calls.
    pub fn send_goal_blocking(&mut self, goal_cdr: &[u8]) -> Result<(GoalId, bool), NodeError> {
        self.goal_counter += 1;
        let mut goal_id = GoalId::default();
        let counter_bytes = self.goal_counter.to_le_bytes();
        goal_id.uuid[..8].copy_from_slice(&counter_bytes);

        let mut writer = CdrWriter::new_with_header(&mut self.goal_buffer)
            .map_err(|_| NodeError::BufferTooSmall)?;

        write_goal_id(&mut writer, &goal_id)?;

        let pos = writer.position();
        if pos + goal_cdr.len() > GOAL_BUF {
            return Err(NodeError::BufferTooSmall);
        }
        self.goal_buffer[pos..pos + goal_cdr.len()].copy_from_slice(goal_cdr);
        let req_len = pos + goal_cdr.len();

        #[allow(deprecated)]
        let len = self
            .send_goal_client
            .call_raw(&self.goal_buffer[..req_len], &mut self.result_buffer)
            .map_err(|_| NodeError::ServiceRequestFailed)?;

        // Reply CDR: header(4) + accepted(u8) + stamp(sec i32 + nanosec u32)
        let accepted = len >= 5 && self.result_buffer[4] != 0;

        Ok((goal_id, accepted))
    }

    /// Try to receive feedback (non-blocking, raw bytes).
    ///
    /// Returns the GoalId and total data length. The full CDR data
    /// (including GoalId) is in `feedback_buffer`.
    pub fn try_recv_feedback_raw(&mut self) -> Result<Option<(GoalId, usize)>, NodeError> {
        let data = self
            .feedback_subscriber
            .try_recv_raw(&mut self.feedback_buffer)
            .map_err(|_| NodeError::Transport(TransportError::DeserializationError))?;

        let len = match data {
            Some(len) => len,
            None => return Ok(None),
        };

        let mut reader = CdrReader::new_with_header(&self.feedback_buffer[..len])
            .map_err(|_| NodeError::Transport(TransportError::DeserializationError))?;

        let goal_id = read_goal_id(&mut reader)?;

        Ok(Some((goal_id, len)))
    }

    /// Cancel a goal (non-blocking). Sends the cancel request.
    ///
    /// After calling, use `cancel_goal_client` and `result_buffer` to construct
    /// a Promise for the cancel response.
    pub fn send_cancel_request(&mut self, goal_id: &GoalId) -> Result<(), NodeError> {
        let mut writer = CdrWriter::new_with_header(&mut self.goal_buffer)
            .map_err(|_| NodeError::BufferTooSmall)?;

        write_goal_id(&mut writer, goal_id)?;
        writer.write_i32(0).map_err(|_| NodeError::Serialization)?;
        writer.write_u32(0).map_err(|_| NodeError::Serialization)?;

        let req_len = writer.position();

        self.cancel_goal_client
            .send_request_raw(&self.goal_buffer[..req_len])
            .map_err(|_| NodeError::ServiceRequestFailed)
    }

    /// Send a get_result request.
    ///
    /// After calling, use `get_result_client` and `result_buffer` to construct
    /// a Promise for the result response.
    pub fn send_get_result_request(&mut self, goal_id: &GoalId) -> Result<(), NodeError> {
        let mut writer = CdrWriter::new_with_header(&mut self.goal_buffer)
            .map_err(|_| NodeError::BufferTooSmall)?;

        write_goal_id(&mut writer, goal_id)?;

        let req_len = writer.position();

        self.get_result_client
            .send_request_raw(&self.goal_buffer[..req_len])
            .map_err(|_| NodeError::ServiceRequestFailed)
    }

    /// Phase 122.3.c.6.e — register a `Waker` that fires when the
    /// send_goal RPC reply lands.
    pub fn register_goal_response_waker(&self, waker: &core::task::Waker) {
        use nros_rmw::ServiceClientTrait;
        self.send_goal_client.register_waker(waker);
    }

    /// Phase 122.3.c.6.e — register a `Waker` for cancel-RPC replies.
    pub fn register_cancel_response_waker(&self, waker: &core::task::Waker) {
        use nros_rmw::ServiceClientTrait;
        self.cancel_goal_client.register_waker(waker);
    }

    /// Phase 122.3.c.6.e — register a `Waker` for get_result replies.
    pub fn register_result_waker(&self, waker: &core::task::Waker) {
        use nros_rmw::ServiceClientTrait;
        self.get_result_client.register_waker(waker);
    }

    /// Phase 122.3.c.6.e — register a `Waker` for feedback messages.
    pub fn register_feedback_waker(&self, waker: &core::task::Waker) {
        use nros_rmw::Subscriber;
        self.feedback_subscriber.register_waker(waker);
    }

    /// Phase 122.3.c.6.c — poll for a cancel reply (non-blocking,
    /// raw bytes). Returns `Ok(Some(len))` when a reply landed; the
    /// CDR payload is in `result_buffer_ref()[..len]`. Reply layout
    /// is action_msgs/srv/CancelGoal_Response wire CDR.
    pub fn try_recv_cancel_reply(&mut self) -> Result<Option<usize>, NodeError> {
        match self
            .cancel_goal_client
            .try_recv_reply_raw(&mut self.result_buffer)
        {
            Ok(opt) => Ok(opt),
            Err(TransportError::NoData) => Ok(None),
            Err(_) => Err(NodeError::Transport(TransportError::DeserializationError)),
        }
    }

    /// Poll for a get_result reply (non-blocking, raw bytes).
    ///
    /// Returns `Ok(Some(total_len))` if a reply arrived (data in result buffer),
    /// `Ok(None)` if no reply yet.
    ///
    /// After receiving, use [`result_buffer_ref()`](Self::result_buffer_ref)
    /// to access the raw CDR data. The layout is: CDR header (4) + status
    /// byte (1) + result data.
    pub fn try_recv_get_result_reply(&mut self) -> Result<Option<usize>, NodeError> {
        // Phase 120: NoData == steady-state polling; map to Ok(None).
        match self
            .get_result_client
            .try_recv_reply_raw(&mut self.result_buffer)
        {
            Ok(opt) => Ok(opt),
            Err(TransportError::NoData) => Ok(None),
            Err(_) => Err(NodeError::Transport(TransportError::DeserializationError)),
        }
    }

    /// Poll for the send_goal acceptance reply (non-blocking, raw bytes).
    ///
    /// Returns `Ok(Some(total_len))` if a reply arrived (data in result buffer),
    /// `Ok(None)` if no reply yet.
    ///
    /// The reply CDR contains: header (4) + accepted (u8) + stamp (i32 + u32).
    pub fn try_recv_send_goal_reply(&mut self) -> Result<Option<usize>, NodeError> {
        // Phase 120: NoData == steady-state polling; map to Ok(None).
        match self
            .send_goal_client
            .try_recv_reply_raw(&mut self.result_buffer)
        {
            Ok(opt) => Ok(opt),
            Err(TransportError::NoData) => Ok(None),
            Err(_) => Err(NodeError::Transport(TransportError::DeserializationError)),
        }
    }

    /// Blocking get_result request — sends the request and blocks until
    /// a reply is received (or timeout).
    ///
    /// Returns the total reply length. Data is stored in `result_buffer`.
    /// This uses the blocking `call_raw` path which is more reliable on
    /// platforms with separate I/O tasks (e.g. FreeRTOS).
    pub fn get_result_blocking(&mut self, goal_id: &GoalId) -> Result<usize, NodeError> {
        let mut writer = CdrWriter::new_with_header(&mut self.goal_buffer)
            .map_err(|_| NodeError::BufferTooSmall)?;

        write_goal_id(&mut writer, goal_id)?;

        let req_len = writer.position();

        #[allow(deprecated)]
        let len = self
            .get_result_client
            .call_raw(&self.goal_buffer[..req_len], &mut self.result_buffer)
            .map_err(|_| NodeError::ServiceRequestFailed)?;

        Ok(len)
    }

    /// Read-only access to the result buffer (after polling a reply).
    pub fn result_buffer_ref(&self) -> &[u8] {
        &self.result_buffer
    }

    /// Read-only access to the feedback buffer (after receiving feedback).
    pub fn feedback_buffer_ref(&self) -> &[u8] {
        &self.feedback_buffer
    }

    /// Get the current goal counter (used to reconstruct the last goal ID).
    pub fn goal_counter(&self) -> u64 {
        self.goal_counter
    }
}