nros_node/executor/
action.rs

1//! Action server and client registration on the executor and handle types.
2
3use core::marker::PhantomData;
4
5use nros_core::RosAction;
6use nros_rmw::{ActionInfo, QosSettings, ServiceInfo, Session, TopicInfo};
7
8#[allow(unused_imports)]
9use crate::cyclonedds_register::{MessageForRmw, register_type};
10
11use super::{
12    action_core::{ActionClientCore, ActionServerCore, RawActiveGoal},
13    arena::{
14        ActionClientCallbackEntry, ActionClientRawArenaEntry, ActionServerArenaEntry,
15        ActionServerRawArenaEntry, BufferStrategy, CallbackMeta, EntryKind,
16        action_client_callback_try_process, action_client_raw_try_process,
17        action_server_raw_try_process, action_server_try_process, always_ready,
18        as_active_goal_count, as_complete_goal, as_for_each_active_goal, as_publish_feedback,
19        as_raw_active_goal_count, as_raw_complete_goal, as_raw_for_each_active_goal,
20        as_raw_publish_feedback, as_raw_set_goal_status, as_set_goal_status, buffered_region_size,
21        drop_entry, no_pre_sample,
22    },
23    handles::{ActionServer, ActiveGoal},
24    spin::Executor,
25    spsc_ring::SpscRing,
26    triple_buffer::TripleBuffer,
27    types::{
28        HandleId, InvocationMode, NodeError, RawAcceptedCallback, RawCancelCallback,
29        RawFeedbackCallback, RawGoalCallback, RawGoalResponseCallback, RawResultCallback,
30    },
31};
32
33// ============================================================================
34// Raw action registration specs
35// ============================================================================
36
37/// Inputs for raw (untyped) action-server registration.
38///
39/// Collapses the runtime arguments shared by the
40/// `register_action_server_raw*` family. Buffer sizes / max-goals stay
41/// as const-generic turbofish parameters on the registration methods.
42pub struct RawActionServerSpec<'a> {
43    /// `None` registers on the executor's own node; `Some(id)` routes
44    /// the server's 5 underlying handles (send_goal / cancel_goal /
45    /// get_result servers + feedback / status publishers) through the
46    /// named Node's session.
47    pub node_id: Option<super::node_record::NodeId>,
48    pub action_name: &'a str,
49    pub type_name: &'a str,
50    pub type_hash: &'a str,
51    /// QoS for the action's three underlying service servers (send_goal
52    /// / cancel_goal / get_result; Phase 193.4b). The feedback + status
53    /// publishers keep their own profiles. Use
54    /// [`QosSettings::services_default`] for the rclc-compatible default.
55    pub qos: QosSettings,
56    pub goal_callback: RawGoalCallback,
57    pub cancel_callback: RawCancelCallback,
58    pub accepted_callback: Option<RawAcceptedCallback>,
59    pub context: *mut core::ffi::c_void,
60}
61
62/// Inputs for raw (untyped) action-client registration.
63///
64/// Collapses the runtime arguments shared by the
65/// `register_action_client_raw*` family. Buffer sizes stay as
66/// const-generic turbofish parameters on the registration methods.
67pub struct RawActionClientSpec<'a> {
68    /// `None` registers on the executor's own node; `Some(id)` routes
69    /// the client's 4 underlying handles (send_goal / cancel_goal /
70    /// get_result service clients + feedback subscriber) through the
71    /// named Node's session.
72    pub node_id: Option<super::node_record::NodeId>,
73    pub action_name: &'a str,
74    pub type_name: &'a str,
75    pub type_hash: &'a str,
76    pub goal_response_callback: Option<RawGoalResponseCallback>,
77    pub feedback_callback: Option<RawFeedbackCallback>,
78    pub result_callback: Option<RawResultCallback>,
79    pub context: *mut core::ffi::c_void,
80}
81
82// ============================================================================
83// Action server registration
84// ============================================================================
85
86impl Executor {
87    /// Register an action server with goal/cancel callbacks.
88    ///
89    /// The executor automatically dispatches:
90    /// - Goal acceptance via `goal_callback`
91    /// - Cancel requests via `cancel_callback`
92    /// - Result serving for completed goals
93    ///
94    /// Use the returned [`ActionServerHandle`] to publish feedback and complete goals.
95    ///
96    /// Uses default buffer sizes and max 4 concurrent goals.
97    pub fn register_action_server<A, GoalF, CancelF>(
98        &mut self,
99        action_name: &str,
100        goal_callback: GoalF,
101        cancel_callback: CancelF,
102    ) -> Result<ActionServerHandle<A>, NodeError>
103    where
104        A: RosAction + 'static,
105        A::Goal: Clone + MessageForRmw,
106        A::Result: Clone + Default + MessageForRmw,
107        A::Feedback: MessageForRmw,
108        A::SendGoalRequest: MessageForRmw,
109        A::SendGoalResponse: MessageForRmw,
110        A::GetResultRequest: MessageForRmw,
111        A::GetResultResponse: MessageForRmw,
112        A::FeedbackMessage: MessageForRmw,
113        GoalF: FnMut(&nros_core::GoalId, &A::Goal) -> nros_core::GoalResponse + 'static,
114        CancelF:
115            FnMut(&nros_core::GoalId, nros_core::GoalStatus) -> nros_core::CancelResponse + 'static,
116    {
117        self.register_action_server_sized::<A, GoalF, CancelF, { crate::config::DEFAULT_RX_BUF_SIZE }, { crate::config::DEFAULT_RX_BUF_SIZE }, { crate::config::DEFAULT_RX_BUF_SIZE }, 4>(
118            action_name,
119            goal_callback,
120            cancel_callback,
121        )
122    }
123
124    /// Register an action server with custom buffer sizes.
125    pub fn register_action_server_sized<
126        A,
127        GoalF,
128        CancelF,
129        const GOAL_BUF: usize,
130        const RESULT_BUF: usize,
131        const FEEDBACK_BUF: usize,
132        const MAX_GOALS: usize,
133    >(
134        &mut self,
135        action_name: &str,
136        goal_callback: GoalF,
137        cancel_callback: CancelF,
138    ) -> Result<ActionServerHandle<A>, NodeError>
139    where
140        A: RosAction + 'static,
141        A::Goal: Clone + MessageForRmw,
142        A::Result: Clone + Default + MessageForRmw,
143        A::Feedback: MessageForRmw,
144        A::SendGoalRequest: MessageForRmw,
145        A::SendGoalResponse: MessageForRmw,
146        A::GetResultRequest: MessageForRmw,
147        A::GetResultResponse: MessageForRmw,
148        A::FeedbackMessage: MessageForRmw,
149        GoalF: FnMut(&nros_core::GoalId, &A::Goal) -> nros_core::GoalResponse + 'static,
150        CancelF:
151            FnMut(&nros_core::GoalId, nros_core::GoalStatus) -> nros_core::CancelResponse + 'static,
152    {
153        // Phase 212.K.7.6.b + K.7.7.c — under `rmw-cyclonedds`, register
154        // the user-facing message types AND the five action-protocol
155        // envelope types with the cyclonedds runtime registry before
156        // creating the underlying service / topic entities. No-op for
157        // other RMWs. See `Node::create_action_server_sized` for the
158        // detailed rationale.
159        register_type::<A::Goal>()?;
160        register_type::<A::Result>()?;
161        register_type::<A::Feedback>()?;
162        register_type::<A::SendGoalRequest>()?;
163        register_type::<A::SendGoalResponse>()?;
164        register_type::<A::GetResultRequest>()?;
165        register_type::<A::GetResultResponse>()?;
166        register_type::<A::FeedbackMessage>()?;
167        // Phase 244 E3 (RFC-0044) — register the fixed `action_msgs` protocol
168        // types (CancelGoal_{Request,Response}, GoalStatusArray) the cancel /
169        // status plumbing serializes. The generated `impl RosAction` overrides
170        // this (default = no-op); previously every example hand-registered these
171        // three under `#[cfg(feature = "rmw-cyclonedds")]`.
172        A::register_protocol_types().map_err(|()| NodeError::ActionCreationFailed)?;
173        type Entry<
174            A,
175            GoalF,
176            CancelF,
177            const GB: usize,
178            const RB: usize,
179            const FB: usize,
180            const MG: usize,
181        > = ActionServerArenaEntry<A, GoalF, CancelF, GB, RB, FB, MG>;
182
183        let slot = self.next_entry_slot()?;
184
185        // Create the action server entities (same logic as Node::create_action_server_sized)
186        let action_info = ActionInfo::new(action_name, A::ACTION_NAME, A::ACTION_HASH);
187
188        // ROS 2 matches the action's send_goal / get_result services by their
189        // per-channel service types (`<Action>_SendGoal` / `<Action>_GetResult`)
190        // and the feedback topic by `<Action>_FeedbackMessage` — not the bare
191        // action type. Pass those so a real `rcl_action` peer discovers us.
192        let send_goal_type = super::action_core::action_service_base_type(
193            <A::SendGoalRequest as nros_core::RosMessage>::TYPE_NAME,
194            A::ACTION_NAME,
195        );
196        let get_result_type = super::action_core::action_service_base_type(
197            <A::GetResultRequest as nros_core::RosMessage>::TYPE_NAME,
198            A::ACTION_NAME,
199        );
200        let feedback_type = <A::FeedbackMessage as nros_core::RosMessage>::TYPE_NAME;
201
202        let send_goal_keyexpr: heapless::String<256> = action_info.send_goal_key();
203        let send_goal_info =
204            ServiceInfo::new(&send_goal_keyexpr, send_goal_type, A::ACTION_HASH).with_domain(0);
205        let send_goal_server = self
206            .session
207            .create_service_server(&send_goal_info, QosSettings::services_default())
208            .map_err(|_| NodeError::ActionCreationFailed)?;
209
210        let cancel_goal_keyexpr: heapless::String<256> = action_info.cancel_goal_key();
211        let cancel_goal_info = ServiceInfo::new(
212            &cancel_goal_keyexpr,
213            "action_msgs::srv::dds_::CancelGoal_",
214            A::ACTION_HASH,
215        )
216        .with_domain(0);
217        let cancel_goal_server = self
218            .session
219            .create_service_server(&cancel_goal_info, QosSettings::services_default())
220            .map_err(|_| NodeError::ActionCreationFailed)?;
221
222        let get_result_keyexpr: heapless::String<256> = action_info.get_result_key();
223        let get_result_info =
224            ServiceInfo::new(&get_result_keyexpr, get_result_type, A::ACTION_HASH).with_domain(0);
225        let get_result_server = self
226            .session
227            .create_service_server(&get_result_info, QosSettings::services_default())
228            .map_err(|_| NodeError::ActionCreationFailed)?;
229
230        let feedback_keyexpr: heapless::String<256> = action_info.feedback_key();
231        let feedback_topic =
232            TopicInfo::new(&feedback_keyexpr, feedback_type, A::ACTION_HASH).with_domain(0);
233        let feedback_publisher = self
234            .session
235            .create_publisher(&feedback_topic, QosSettings::QOS_PROFILE_DEFAULT)
236            .map_err(|_| NodeError::ActionCreationFailed)?;
237
238        let status_keyexpr: heapless::String<256> = action_info.status_key();
239        let status_topic = TopicInfo::new(
240            &status_keyexpr,
241            "action_msgs::msg::dds_::GoalStatusArray_",
242            A::ACTION_HASH,
243        )
244        .with_domain(0);
245        let status_publisher = self
246            .session
247            .create_publisher(
248                &status_topic,
249                QosSettings::QOS_PROFILE_ACTION_STATUS_DEFAULT,
250            )
251            .map_err(|_| NodeError::ActionCreationFailed)?;
252
253        let server = ActionServer {
254            core: super::action_core::ActionServerCore {
255                send_goal_server,
256                cancel_goal_server,
257                get_result_server,
258                feedback_publisher,
259                status_publisher,
260                active_goals: heapless::Vec::new(),
261                completed_results: heapless::Vec::new(),
262                pending_get_results: heapless::Vec::new(),
263                result_slab: [0u8; RESULT_BUF],
264                result_slab_used: 0,
265                goal_buffer: [0u8; GOAL_BUF],
266                feedback_buffer: [0u8; FEEDBACK_BUF],
267                cancel_buffer: [0u8; 256],
268            },
269            typed_goals: heapless::Vec::new(),
270            completed_goals: heapless::Vec::new(),
271        };
272
273        let offset = self
274            .arena_alloc::<Entry<A, GoalF, CancelF, GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>>(
275            )?;
276
277        unsafe {
278            let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
279            let entry_ptr = arena_ptr.add(offset)
280                as *mut Entry<A, GoalF, CancelF, GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>;
281            core::ptr::write(
282                entry_ptr,
283                Entry {
284                    server,
285                    goal_callback,
286                    cancel_callback,
287                },
288            );
289        }
290
291        self.entries[slot] = Some(CallbackMeta {
292            offset,
293            kind: EntryKind::ActionServer,
294            has_data: always_ready,
295            pre_sample: no_pre_sample,
296            invocation: InvocationMode::Always,
297            try_process: action_server_try_process::<
298                A,
299                GoalF,
300                CancelF,
301                GOAL_BUF,
302                RESULT_BUF,
303                FEEDBACK_BUF,
304                MAX_GOALS,
305            >,
306            drop_fn: drop_entry::<
307                Entry<A, GoalF, CancelF, GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>,
308            >,
309        });
310
311        Ok(ActionServerHandle {
312            entry_index: slot,
313            publish_feedback_fn: as_publish_feedback::<
314                A,
315                GoalF,
316                CancelF,
317                GOAL_BUF,
318                RESULT_BUF,
319                FEEDBACK_BUF,
320                MAX_GOALS,
321            >,
322            complete_goal_fn: as_complete_goal::<
323                A,
324                GoalF,
325                CancelF,
326                GOAL_BUF,
327                RESULT_BUF,
328                FEEDBACK_BUF,
329                MAX_GOALS,
330            >,
331            set_goal_status_fn: as_set_goal_status::<
332                A,
333                GoalF,
334                CancelF,
335                GOAL_BUF,
336                RESULT_BUF,
337                FEEDBACK_BUF,
338                MAX_GOALS,
339            >,
340            active_goal_count_fn: as_active_goal_count::<
341                A,
342                GoalF,
343                CancelF,
344                GOAL_BUF,
345                RESULT_BUF,
346                FEEDBACK_BUF,
347                MAX_GOALS,
348            >,
349            for_each_active_goal_fn: as_for_each_active_goal::<
350                A,
351                GoalF,
352                CancelF,
353                GOAL_BUF,
354                RESULT_BUF,
355                FEEDBACK_BUF,
356                MAX_GOALS,
357            >,
358            _phantom: PhantomData,
359        })
360    }
361}
362
363// ============================================================================
364// Handle types for arena-registered action server
365// ============================================================================
366
367/// Handle to an action server registered in the executor's arena.
368///
369/// Returned by [`Executor::register_action_server()`]. Provides methods
370/// to interact with the server (publish feedback, complete goals) while the
371/// executor automatically handles goal acceptance, cancel requests, and
372/// result serving during [`spin_once()`](Executor::spin_once).
373#[allow(clippy::type_complexity)]
374pub struct ActionServerHandle<A: RosAction> {
375    pub(crate) entry_index: usize,
376    publish_feedback_fn:
377        unsafe fn(*mut u8, &nros_core::GoalId, &A::Feedback) -> Result<(), NodeError>,
378    complete_goal_fn: unsafe fn(*mut u8, &nros_core::GoalId, nros_core::GoalStatus, A::Result),
379    set_goal_status_fn: unsafe fn(*mut u8, &nros_core::GoalId, nros_core::GoalStatus),
380    active_goal_count_fn: unsafe fn(*const u8) -> usize,
381    for_each_active_goal_fn: unsafe fn(*const u8, &mut dyn FnMut(&ActiveGoal<A>)),
382    _phantom: PhantomData<A>,
383}
384
385impl<A: RosAction> Clone for ActionServerHandle<A> {
386    fn clone(&self) -> Self {
387        *self
388    }
389}
390
391impl<A: RosAction> Copy for ActionServerHandle<A> {}
392
393impl<A: RosAction> ActionServerHandle<A> {
394    /// Get the [`HandleId`] for this action server.
395    ///
396    /// Used with `Trigger::One` or `HandleSet` for trigger configuration.
397    pub fn handle_id(&self) -> HandleId {
398        HandleId(self.entry_index)
399    }
400
401    /// Publish feedback for an active goal.
402    ///
403    /// Serialises the feedback message and sends it to all clients
404    /// monitoring this goal. Returns an error if the handle slot has
405    /// been removed from the executor.
406    pub fn publish_feedback(
407        &self,
408        executor: &mut Executor,
409        goal_id: &nros_core::GoalId,
410        feedback: &A::Feedback,
411    ) -> Result<(), NodeError> {
412        let meta = executor.entries[self.entry_index]
413            .as_ref()
414            .ok_or(NodeError::BufferTooSmall)?;
415        let arena_ptr = executor.arena.as_mut_ptr() as *mut u8;
416        unsafe {
417            let data_ptr = arena_ptr.add(meta.offset);
418            (self.publish_feedback_fn)(data_ptr, goal_id, feedback)
419        }
420    }
421
422    /// Complete a goal with a terminal status and result payload.
423    ///
424    /// The goal is moved from the active set to the completed-results
425    /// slab. Clients waiting on a result will receive the response.
426    /// `status` should be one of `Succeeded`, `Aborted`, or `Canceled`.
427    pub fn complete_goal(
428        &self,
429        executor: &mut Executor,
430        goal_id: &nros_core::GoalId,
431        status: nros_core::GoalStatus,
432        result: A::Result,
433    ) {
434        if let Some(meta) = executor.entries[self.entry_index].as_ref() {
435            let arena_ptr = executor.arena.as_mut_ptr() as *mut u8;
436            unsafe {
437                let data_ptr = arena_ptr.add(meta.offset);
438                (self.complete_goal_fn)(data_ptr, goal_id, status, result);
439            }
440        }
441    }
442
443    /// Update a goal's status without completing it.
444    ///
445    /// Use this to transition a goal to `Executing` or `Canceling`
446    /// while it is still active. To finish a goal, use [`complete_goal`](Self::complete_goal).
447    pub fn set_goal_status(
448        &self,
449        executor: &mut Executor,
450        goal_id: &nros_core::GoalId,
451        status: nros_core::GoalStatus,
452    ) {
453        if let Some(meta) = executor.entries[self.entry_index].as_ref() {
454            let arena_ptr = executor.arena.as_mut_ptr() as *mut u8;
455            unsafe {
456                let data_ptr = arena_ptr.add(meta.offset);
457                (self.set_goal_status_fn)(data_ptr, goal_id, status);
458            }
459        }
460    }
461
462    /// Get the number of currently active goals.
463    ///
464    /// Returns 0 if the action server handle has been removed from the executor.
465    pub fn active_goal_count(&self, executor: &Executor) -> usize {
466        match executor.entries[self.entry_index].as_ref() {
467            Some(meta) => {
468                let arena_ptr = executor.arena.as_ptr() as *const u8;
469                unsafe {
470                    let data_ptr = arena_ptr.add(meta.offset);
471                    (self.active_goal_count_fn)(data_ptr)
472                }
473            }
474            None => 0,
475        }
476    }
477
478    /// Iterate over all currently active goals.
479    ///
480    /// Calls `f` for each goal that has been accepted but not yet
481    /// completed. Useful for monitoring progress or canceling stale goals.
482    pub fn for_each_active_goal(&self, executor: &Executor, mut f: impl FnMut(&ActiveGoal<A>)) {
483        if let Some(meta) = executor.entries[self.entry_index].as_ref() {
484            let arena_ptr = executor.arena.as_ptr() as *const u8;
485            unsafe {
486                let data_ptr = arena_ptr.add(meta.offset);
487                (self.for_each_active_goal_fn)(data_ptr, &mut f);
488            }
489        }
490    }
491}
492
493// ============================================================================
494// Raw (untyped) action server registration
495// ============================================================================
496
497impl Executor {
498    /// Register a raw action server with raw-bytes callbacks.
499    ///
500    /// Unlike [`register_action_server()`](Executor::register_action_server), this does
501    /// not require `RosAction` — the goal/cancel callbacks receive raw CDR
502    /// bytes. This is used by the C API thin wrapper.
503    ///
504    /// `type_name` and `type_hash` identify the action type for key expression
505    /// construction and liveliness tokens.
506    #[allow(clippy::too_many_arguments)]
507    pub fn register_action_server_raw(
508        &mut self,
509        spec: RawActionServerSpec<'_>,
510    ) -> Result<ActionServerRawHandle, NodeError> {
511        self.register_action_server_raw_sized::<{ crate::config::DEFAULT_RX_BUF_SIZE }, { crate::config::DEFAULT_RX_BUF_SIZE }, { crate::config::DEFAULT_RX_BUF_SIZE }, 4>(
512            spec,
513        )
514    }
515
516    /// Register a raw action server with custom buffer sizes.
517    ///
518    /// `spec.node_id` selects the target: `None` registers on the
519    /// executor's own node, `Some(id)` routes the server's 5 underlying
520    /// handles through the named Node's session (Phase 104.C.3.3.a).
521    /// `spec.qos` applies to the action's three underlying service
522    /// servers (send_goal / cancel_goal / get_result; Phase 193.4b); the
523    /// feedback + status publishers keep their own profiles.
524    pub fn register_action_server_raw_sized<
525        const GOAL_BUF: usize,
526        const RESULT_BUF: usize,
527        const FEEDBACK_BUF: usize,
528        const MAX_GOALS: usize,
529    >(
530        &mut self,
531        spec: RawActionServerSpec<'_>,
532    ) -> Result<ActionServerRawHandle, NodeError> {
533        let RawActionServerSpec {
534            node_id,
535            action_name,
536            type_name,
537            type_hash,
538            qos,
539            goal_callback,
540            cancel_callback,
541            accepted_callback,
542            context,
543        } = spec;
544
545        type Entry<const GB: usize, const RB: usize, const FB: usize, const MG: usize> =
546            ActionServerRawArenaEntry<GB, RB, FB, MG>;
547
548        let slot = self.next_entry_slot()?;
549
550        let action_info = ActionInfo::new(action_name, type_name, type_hash);
551        let (node_name, ns, session_idx) = match node_id {
552            Some(id) => {
553                let r = self
554                    .nodes
555                    .get(id.index())
556                    .ok_or(NodeError::InvalidSchedContextBinding)?;
557                (r.name.clone(), r.namespace.clone(), r.session_idx)
558            }
559            None => (self.node_name.clone(), self.namespace.clone(), 0u8),
560        };
561
562        // Thread node identity through each underlying ServiceInfo /
563        // TopicInfo so the Zenoh shim declares a liveliness token for
564        // each entity. Without `with_node_name`,
565        // `declare_entity_liveliness` short-circuits and
566        // `wait_for_action_server` has nothing to find — same fix as
567        // `Node::create_action_server_sized` (commit ea5e80b4).
568        // All 5 session-create calls grouped into one scope so the
569        // mutable session borrow drops before arena alloc below.
570        let (
571            send_goal_server,
572            cancel_goal_server,
573            get_result_server,
574            feedback_publisher,
575            status_publisher,
576        ) = {
577            let send_goal_keyexpr: heapless::String<256> = action_info.send_goal_key();
578            let mut send_goal_info =
579                ServiceInfo::new(&send_goal_keyexpr, type_name, type_hash).with_namespace(&ns);
580            if !node_name.is_empty() {
581                send_goal_info = send_goal_info.with_node_name(&node_name);
582            }
583
584            let cancel_goal_keyexpr: heapless::String<256> = action_info.cancel_goal_key();
585            let mut cancel_goal_info = ServiceInfo::new(
586                &cancel_goal_keyexpr,
587                "action_msgs::srv::dds_::CancelGoal_",
588                type_hash,
589            )
590            .with_namespace(&ns);
591            if !node_name.is_empty() {
592                cancel_goal_info = cancel_goal_info.with_node_name(&node_name);
593            }
594
595            let get_result_keyexpr: heapless::String<256> = action_info.get_result_key();
596            let mut get_result_info =
597                ServiceInfo::new(&get_result_keyexpr, type_name, type_hash).with_namespace(&ns);
598            if !node_name.is_empty() {
599                get_result_info = get_result_info.with_node_name(&node_name);
600            }
601
602            let feedback_keyexpr: heapless::String<256> = action_info.feedback_key();
603            let mut feedback_topic =
604                TopicInfo::new(&feedback_keyexpr, type_name, type_hash).with_namespace(&ns);
605            if !node_name.is_empty() {
606                feedback_topic = feedback_topic.with_node_name(&node_name);
607            }
608
609            let status_keyexpr: heapless::String<256> = action_info.status_key();
610            let mut status_topic = TopicInfo::new(
611                &status_keyexpr,
612                "action_msgs::msg::dds_::GoalStatusArray_",
613                type_hash,
614            )
615            .with_namespace(&ns);
616            if !node_name.is_empty() {
617                status_topic = status_topic.with_node_name(&node_name);
618            }
619
620            let session = self
621                .session_at_mut(session_idx)
622                .ok_or(NodeError::BackendMismatch)?;
623            (
624                session
625                    .create_service_server(&send_goal_info, qos)
626                    .map_err(|_| NodeError::ActionCreationFailed)?,
627                session
628                    .create_service_server(&cancel_goal_info, qos)
629                    .map_err(|_| NodeError::ActionCreationFailed)?,
630                session
631                    .create_service_server(&get_result_info, qos)
632                    .map_err(|_| NodeError::ActionCreationFailed)?,
633                session
634                    .create_publisher(&feedback_topic, QosSettings::QOS_PROFILE_DEFAULT)
635                    .map_err(|_| NodeError::ActionCreationFailed)?,
636                session
637                    .create_publisher(
638                        &status_topic,
639                        QosSettings::QOS_PROFILE_ACTION_STATUS_DEFAULT,
640                    )
641                    .map_err(|_| NodeError::ActionCreationFailed)?,
642            )
643        };
644
645        let core = ActionServerCore {
646            send_goal_server,
647            cancel_goal_server,
648            get_result_server,
649            feedback_publisher,
650            status_publisher,
651            active_goals: heapless::Vec::new(),
652            completed_results: heapless::Vec::new(),
653            pending_get_results: heapless::Vec::new(),
654            result_slab: [0u8; RESULT_BUF],
655            result_slab_used: 0,
656            goal_buffer: [0u8; GOAL_BUF],
657            feedback_buffer: [0u8; FEEDBACK_BUF],
658            cancel_buffer: [0u8; 256],
659        };
660
661        let offset = self.arena_alloc::<Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>>()?;
662
663        unsafe {
664            let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
665            let entry_ptr =
666                arena_ptr.add(offset) as *mut Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>;
667            core::ptr::write(
668                entry_ptr,
669                Entry {
670                    core,
671                    goal_callback,
672                    cancel_callback,
673                    accepted_callback,
674                    context,
675                },
676            );
677        }
678
679        self.entries[slot] = Some(CallbackMeta {
680            offset,
681            kind: EntryKind::ActionServer,
682            has_data: always_ready,
683            pre_sample: no_pre_sample,
684            invocation: InvocationMode::Always,
685            try_process: action_server_raw_try_process::<
686                GOAL_BUF,
687                RESULT_BUF,
688                FEEDBACK_BUF,
689                MAX_GOALS,
690            >,
691            drop_fn: drop_entry::<Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>>,
692        });
693        self.apply_node_default_sched(slot, node_id);
694
695        Ok(ActionServerRawHandle {
696            entry_index: slot,
697            publish_feedback_fn: as_raw_publish_feedback::<
698                GOAL_BUF,
699                RESULT_BUF,
700                FEEDBACK_BUF,
701                MAX_GOALS,
702            >,
703            complete_goal_fn: as_raw_complete_goal::<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>,
704            set_goal_status_fn: as_raw_set_goal_status::<
705                GOAL_BUF,
706                RESULT_BUF,
707                FEEDBACK_BUF,
708                MAX_GOALS,
709            >,
710            active_goal_count_fn: as_raw_active_goal_count::<
711                GOAL_BUF,
712                RESULT_BUF,
713                FEEDBACK_BUF,
714                MAX_GOALS,
715            >,
716            for_each_active_goal_fn: as_raw_for_each_active_goal::<
717                GOAL_BUF,
718                RESULT_BUF,
719                FEEDBACK_BUF,
720                MAX_GOALS,
721            >,
722        })
723    }
724}
725
726// ============================================================================
727// Raw action server handle
728// ============================================================================
729
730/// Handle to a raw (untyped) action server registered in the executor's arena.
731///
732/// Returned by [`Executor::register_action_server_raw()`]. Provides methods
733/// to interact with the server using raw CDR bytes.
734#[repr(C)]
735#[allow(clippy::type_complexity)]
736pub struct ActionServerRawHandle {
737    pub(crate) entry_index: usize,
738    publish_feedback_fn:
739        unsafe fn(*mut u8, &nros_core::GoalId, *const u8, usize) -> Result<(), NodeError>,
740    complete_goal_fn:
741        unsafe fn(*mut u8, &nros_core::GoalId, nros_core::GoalStatus, *const u8, usize),
742    set_goal_status_fn: unsafe fn(*mut u8, &nros_core::GoalId, nros_core::GoalStatus),
743    active_goal_count_fn: unsafe fn(*const u8) -> usize,
744    for_each_active_goal_fn: unsafe fn(*const u8, &mut dyn FnMut(&RawActiveGoal)),
745}
746
747impl Clone for ActionServerRawHandle {
748    fn clone(&self) -> Self {
749        *self
750    }
751}
752
753impl Copy for ActionServerRawHandle {}
754
755/// Sentinel value indicating an `ActionServerRawHandle` is not bound to an
756/// arena entry yet. Used by Phase 87.5 to replace `Option<...>` with a
757/// `#[repr(C)]`-compatible inline field.
758///
759/// Function pointers are populated with `unreachable_*` stubs that panic
760/// if anyone is reckless enough to dispatch through an unbound handle —
761/// callers must check `entry_index == INVALID_ENTRY_INDEX` first.
762pub const INVALID_ENTRY_INDEX: usize = usize::MAX;
763
764impl ActionServerRawHandle {
765    /// Construct a sentinel handle representing "not registered yet".
766    ///
767    /// All function pointers are unreachable stubs; only valid use is
768    /// to populate `#[repr(C)]` storage that is later overwritten by a
769    /// real handle (or queried via `is_invalid()` to skip operations).
770    pub const fn invalid() -> Self {
771        unsafe fn unreachable_publish_feedback(
772            _: *mut u8,
773            _: &nros_core::GoalId,
774            _: *const u8,
775            _: usize,
776        ) -> Result<(), NodeError> {
777            unreachable!("ActionServerRawHandle::publish_feedback called on invalid handle")
778        }
779        unsafe fn unreachable_complete_goal(
780            _: *mut u8,
781            _: &nros_core::GoalId,
782            _: nros_core::GoalStatus,
783            _: *const u8,
784            _: usize,
785        ) {
786            unreachable!("ActionServerRawHandle::complete_goal called on invalid handle")
787        }
788        unsafe fn unreachable_set_goal_status(
789            _: *mut u8,
790            _: &nros_core::GoalId,
791            _: nros_core::GoalStatus,
792        ) {
793            unreachable!("ActionServerRawHandle::set_goal_status called on invalid handle")
794        }
795        unsafe fn unreachable_active_goal_count(_: *const u8) -> usize {
796            unreachable!("ActionServerRawHandle::active_goal_count called on invalid handle")
797        }
798        unsafe fn unreachable_for_each_active_goal(
799            _: *const u8,
800            _: &mut dyn FnMut(&RawActiveGoal),
801        ) {
802            unreachable!("ActionServerRawHandle::for_each_active_goal called on invalid handle")
803        }
804        Self {
805            entry_index: INVALID_ENTRY_INDEX,
806            publish_feedback_fn: unreachable_publish_feedback,
807            complete_goal_fn: unreachable_complete_goal,
808            set_goal_status_fn: unreachable_set_goal_status,
809            active_goal_count_fn: unreachable_active_goal_count,
810            for_each_active_goal_fn: unreachable_for_each_active_goal,
811        }
812    }
813
814    /// `true` if this handle is the sentinel returned by `Self::invalid()`.
815    pub const fn is_invalid(&self) -> bool {
816        self.entry_index == INVALID_ENTRY_INDEX
817    }
818}
819
820impl Default for ActionServerRawHandle {
821    fn default() -> Self {
822        Self::invalid()
823    }
824}
825
826impl ActionServerRawHandle {
827    /// Get the [`HandleId`] for this action server.
828    pub fn handle_id(&self) -> HandleId {
829        HandleId(self.entry_index)
830    }
831
832    /// Publish feedback with raw CDR bytes (untyped variant).
833    ///
834    /// Used by the C API when feedback is already serialised.
835    pub fn publish_feedback_raw(
836        &self,
837        executor: &mut Executor,
838        goal_id: &nros_core::GoalId,
839        feedback_data: &[u8],
840    ) -> Result<(), NodeError> {
841        let meta = executor.entries[self.entry_index]
842            .as_ref()
843            .ok_or(NodeError::BufferTooSmall)?;
844        let arena_ptr = executor.arena.as_mut_ptr() as *mut u8;
845        unsafe {
846            let data_ptr = arena_ptr.add(meta.offset);
847            (self.publish_feedback_fn)(
848                data_ptr,
849                goal_id,
850                feedback_data.as_ptr(),
851                feedback_data.len(),
852            )
853        }
854    }
855
856    /// Complete a goal with raw CDR result bytes (untyped variant).
857    ///
858    /// Moves the goal from the active set to the completed-results slab.
859    pub fn complete_goal_raw(
860        &self,
861        executor: &mut Executor,
862        goal_id: &nros_core::GoalId,
863        status: nros_core::GoalStatus,
864        result_data: &[u8],
865    ) {
866        if let Some(meta) = executor.entries[self.entry_index].as_ref() {
867            let arena_ptr = executor.arena.as_mut_ptr() as *mut u8;
868            unsafe {
869                let data_ptr = arena_ptr.add(meta.offset);
870                (self.complete_goal_fn)(
871                    data_ptr,
872                    goal_id,
873                    status,
874                    result_data.as_ptr(),
875                    result_data.len(),
876                );
877            }
878        }
879    }
880
881    /// Update a goal's status without completing it.
882    ///
883    /// Use this to transition a goal to `Executing` or `Canceling`
884    /// while it is still active. To finish a goal, use [`complete_goal_raw`](Self::complete_goal_raw).
885    pub fn set_goal_status(
886        &self,
887        executor: &mut Executor,
888        goal_id: &nros_core::GoalId,
889        status: nros_core::GoalStatus,
890    ) {
891        if let Some(meta) = executor.entries[self.entry_index].as_ref() {
892            let arena_ptr = executor.arena.as_mut_ptr() as *mut u8;
893            unsafe {
894                let data_ptr = arena_ptr.add(meta.offset);
895                (self.set_goal_status_fn)(data_ptr, goal_id, status);
896            }
897        }
898    }
899
900    /// Get the number of currently active goals.
901    ///
902    /// Returns 0 if the action server handle has been removed from the executor.
903    pub fn active_goal_count(&self, executor: &Executor) -> usize {
904        match executor.entries[self.entry_index].as_ref() {
905            Some(meta) => {
906                let arena_ptr = executor.arena.as_ptr() as *const u8;
907                unsafe {
908                    let data_ptr = arena_ptr.add(meta.offset);
909                    (self.active_goal_count_fn)(data_ptr)
910                }
911            }
912            None => 0,
913        }
914    }
915
916    /// Iterate over all currently active goals (raw/untyped variant).
917    ///
918    /// Calls `f` for each goal that has been accepted but not yet completed.
919    pub fn for_each_active_goal(&self, executor: &Executor, mut f: impl FnMut(&RawActiveGoal)) {
920        if let Some(meta) = executor.entries[self.entry_index].as_ref() {
921            let arena_ptr = executor.arena.as_ptr() as *const u8;
922            unsafe {
923                let data_ptr = arena_ptr.add(meta.offset);
924                (self.for_each_active_goal_fn)(data_ptr, &mut f);
925            }
926        }
927    }
928
929    /// Look up the status of a single active goal by UUID.
930    ///
931    /// Returns `Some(status)` while the goal is still in the arena's
932    /// `active_goals` vector. Returns `None` once the goal has been
933    /// retired (completed + result delivered, or cancelled + acknowledged).
934    ///
935    /// This is the authoritative source of goal status — the C/C++ FFI
936    /// layers call this from `nros_action_get_goal_status` rather than
937    /// reading a cached field on their own handle structs.
938    pub fn goal_status(
939        &self,
940        executor: &Executor,
941        goal_id: &nros_core::GoalId,
942    ) -> Option<nros_core::GoalStatus> {
943        let mut found = None;
944        self.for_each_active_goal(executor, |g| {
945            if g.goal_id.uuid == goal_id.uuid && found.is_none() {
946                found = Some(g.status);
947            }
948        });
949        found
950    }
951}
952
953// ============================================================================
954// Action client registration
955// ============================================================================
956
957impl Executor {
958    /// Register a raw action client with the executor.
959    ///
960    /// Creates service clients for send_goal, cancel_goal, get_result, and a
961    /// feedback subscriber. The executor polls these during `spin_once` and
962    /// invokes the provided callbacks when responses/feedback arrive.
963    ///
964    /// # Arguments
965    /// * `action_name` — action name (e.g., "/fibonacci")
966    /// * `type_name` — action type (e.g., "example_interfaces::action::dds_::Fibonacci_")
967    /// * `type_hash` — type hash (e.g., "TypeHashNotSupported")
968    /// * `goal_response_callback` — called when goal is accepted/rejected
969    /// * `feedback_callback` — called when feedback is received
970    /// * `result_callback` — called when result is received
971    /// * `context` — opaque pointer passed to all callbacks
972    #[allow(clippy::too_many_arguments)]
973    pub fn register_action_client_raw(
974        &mut self,
975        spec: RawActionClientSpec<'_>,
976    ) -> Result<ActionClientRawHandle, NodeError> {
977        self.register_action_client_raw_sized::<
978            { crate::config::DEFAULT_RX_BUF_SIZE },
979            { crate::config::DEFAULT_RX_BUF_SIZE },
980            { crate::config::DEFAULT_RX_BUF_SIZE },
981        >(spec)
982    }
983
984    /// Register a raw action client with explicit buffer sizes.
985    ///
986    /// `spec.node_id` selects the target: `None` registers on the
987    /// executor's own node, `Some(id)` routes the client's 4 underlying
988    /// handles through the named Node's session (Phase 104.C.3.3.a).
989    pub fn register_action_client_raw_sized<
990        const GOAL_BUF: usize,
991        const RESULT_BUF: usize,
992        const FEEDBACK_BUF: usize,
993    >(
994        &mut self,
995        spec: RawActionClientSpec<'_>,
996    ) -> Result<ActionClientRawHandle, NodeError> {
997        let RawActionClientSpec {
998            node_id,
999            action_name,
1000            type_name,
1001            type_hash,
1002            goal_response_callback,
1003            feedback_callback,
1004            result_callback,
1005            context,
1006        } = spec;
1007
1008        type Entry<const GB: usize, const RB: usize, const FB: usize> =
1009            ActionClientRawArenaEntry<GB, RB, FB>;
1010
1011        let slot = self.next_entry_slot()?;
1012
1013        let action_info = ActionInfo::new(action_name, type_name, type_hash);
1014        let (node_name, ns, session_idx) = match node_id {
1015            Some(id) => {
1016                let r = self
1017                    .nodes
1018                    .get(id.index())
1019                    .ok_or(NodeError::InvalidSchedContextBinding)?;
1020                (r.name.clone(), r.namespace.clone(), r.session_idx)
1021            }
1022            None => (self.node_name.clone(), self.namespace.clone(), 0u8),
1023        };
1024
1025        let (send_goal_client, cancel_goal_client, get_result_client, feedback_sub) = {
1026            let send_goal_keyexpr: heapless::String<256> = action_info.send_goal_key();
1027            let mut send_goal_info =
1028                ServiceInfo::new(&send_goal_keyexpr, type_name, type_hash).with_namespace(&ns);
1029            if !node_name.is_empty() {
1030                send_goal_info = send_goal_info.with_node_name(&node_name);
1031            }
1032
1033            let cancel_goal_keyexpr: heapless::String<256> = action_info.cancel_goal_key();
1034            let mut cancel_goal_info = ServiceInfo::new(
1035                &cancel_goal_keyexpr,
1036                "action_msgs::srv::dds_::CancelGoal_",
1037                type_hash,
1038            )
1039            .with_namespace(&ns);
1040            if !node_name.is_empty() {
1041                cancel_goal_info = cancel_goal_info.with_node_name(&node_name);
1042            }
1043
1044            let get_result_keyexpr: heapless::String<256> = action_info.get_result_key();
1045            let mut get_result_info =
1046                ServiceInfo::new(&get_result_keyexpr, type_name, type_hash).with_namespace(&ns);
1047            if !node_name.is_empty() {
1048                get_result_info = get_result_info.with_node_name(&node_name);
1049            }
1050
1051            let feedback_keyexpr: heapless::String<256> = action_info.feedback_key();
1052            let mut feedback_topic =
1053                TopicInfo::new(&feedback_keyexpr, type_name, type_hash).with_namespace(&ns);
1054            if !node_name.is_empty() {
1055                feedback_topic = feedback_topic.with_node_name(&node_name);
1056            }
1057
1058            let session = self
1059                .session_at_mut(session_idx)
1060                .ok_or(NodeError::BackendMismatch)?;
1061            (
1062                session
1063                    .create_service_client(&send_goal_info, QosSettings::services_default())
1064                    .map_err(|_| NodeError::ActionCreationFailed)?,
1065                session
1066                    .create_service_client(&cancel_goal_info, QosSettings::services_default())
1067                    .map_err(|_| NodeError::ActionCreationFailed)?,
1068                session
1069                    .create_service_client(&get_result_info, QosSettings::services_default())
1070                    .map_err(|_| NodeError::ActionCreationFailed)?,
1071                session
1072                    .create_subscriber(&feedback_topic, QosSettings::BEST_EFFORT)
1073                    .map_err(|_| NodeError::ActionCreationFailed)?,
1074            )
1075        };
1076
1077        let core = ActionClientCore::new(
1078            send_goal_client,
1079            cancel_goal_client,
1080            get_result_client,
1081            feedback_sub,
1082        );
1083
1084        let offset = self.arena_alloc::<Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>>()?;
1085
1086        unsafe {
1087            let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
1088            let entry_ptr = arena_ptr.add(offset) as *mut Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>;
1089            core::ptr::write(
1090                entry_ptr,
1091                Entry {
1092                    core,
1093                    goal_response_callback,
1094                    feedback_callback,
1095                    result_callback,
1096                    context,
1097                },
1098            );
1099        }
1100
1101        self.entries[slot] = Some(CallbackMeta {
1102            offset,
1103            kind: EntryKind::ActionClient,
1104            has_data: always_ready,
1105            pre_sample: no_pre_sample,
1106            invocation: InvocationMode::Always,
1107            try_process: action_client_raw_try_process::<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>,
1108            drop_fn: drop_entry::<Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>>,
1109        });
1110        self.apply_node_default_sched(slot, node_id);
1111
1112        Ok(ActionClientRawHandle { entry_index: slot })
1113    }
1114
1115    /// RFC-0041 / Phase 239.2 — register a **typed callback** action client.
1116    /// Goal-response / feedback / result are eager-drained at `spin_once` and
1117    /// dispatched as deserialized `A::Feedback` / `A::Result` to the typed
1118    /// closures. Returns the scheduling [`HandleId`] and a `*mut` to the arena
1119    /// entry's core (used to build the typed
1120    /// [`ActionClientCallback`](super::handles::ActionClientCallback)).
1121    #[allow(clippy::too_many_arguments, clippy::type_complexity)]
1122    pub(crate) fn register_action_client_callback<
1123        A,
1124        GRespF,
1125        FbF,
1126        ResF,
1127        const GOAL_BUF: usize,
1128        const RESULT_BUF: usize,
1129        const FEEDBACK_BUF: usize,
1130    >(
1131        &mut self,
1132        node_id: Option<super::node_record::NodeId>,
1133        action_name: &str,
1134        type_name: &str,
1135        type_hash: &str,
1136        feedback_depth: u16,
1137        on_goal_response: GRespF,
1138        on_feedback: FbF,
1139        on_result: ResF,
1140    ) -> Result<
1141        (
1142            HandleId,
1143            *mut ActionClientCore<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>,
1144        ),
1145        NodeError,
1146    >
1147    where
1148        A: nros_core::RosAction + 'static,
1149        GRespF: FnMut(&nros_core::GoalId, bool) + 'static,
1150        FbF: FnMut(&nros_core::GoalId, &A::Feedback) + 'static,
1151        ResF: FnMut(&nros_core::GoalId, nros_core::GoalStatus, &A::Result) + 'static,
1152    {
1153        type Entry<A, G, Fb, R, const GB: usize, const RB: usize, const FB: usize> =
1154            ActionClientCallbackEntry<A, G, Fb, R, GB, RB, FB>;
1155
1156        let slot = self.next_entry_slot()?;
1157        let action_info = ActionInfo::new(action_name, type_name, type_hash);
1158        // Phase 244 E3 (RFC-0044) — register the `action_msgs` protocol types
1159        // (CancelGoal_{Request,Response}, GoalStatusArray) the client's cancel
1160        // service + status subscription serialize, before creating those
1161        // entities. Generated `impl RosAction` overrides this (default no-op);
1162        // replaces the example's hand-rolled `#[cfg(rmw-cyclonedds)]` block.
1163        A::register_protocol_types().map_err(|()| NodeError::ActionCreationFailed)?;
1164        let (node_name, ns, session_idx) = match node_id {
1165            Some(id) => {
1166                let r = self
1167                    .nodes
1168                    .get(id.index())
1169                    .ok_or(NodeError::InvalidSchedContextBinding)?;
1170                (r.name.clone(), r.namespace.clone(), r.session_idx)
1171            }
1172            None => (self.node_name.clone(), self.namespace.clone(), 0u8),
1173        };
1174
1175        let (send_goal_client, cancel_goal_client, get_result_client, feedback_sub) = {
1176            let send_goal_keyexpr: heapless::String<256> = action_info.send_goal_key();
1177            let mut send_goal_info =
1178                ServiceInfo::new(&send_goal_keyexpr, type_name, type_hash).with_namespace(&ns);
1179            if !node_name.is_empty() {
1180                send_goal_info = send_goal_info.with_node_name(&node_name);
1181            }
1182            let cancel_goal_keyexpr: heapless::String<256> = action_info.cancel_goal_key();
1183            let mut cancel_goal_info = ServiceInfo::new(
1184                &cancel_goal_keyexpr,
1185                "action_msgs::srv::dds_::CancelGoal_",
1186                type_hash,
1187            )
1188            .with_namespace(&ns);
1189            if !node_name.is_empty() {
1190                cancel_goal_info = cancel_goal_info.with_node_name(&node_name);
1191            }
1192            let get_result_keyexpr: heapless::String<256> = action_info.get_result_key();
1193            let mut get_result_info =
1194                ServiceInfo::new(&get_result_keyexpr, type_name, type_hash).with_namespace(&ns);
1195            if !node_name.is_empty() {
1196                get_result_info = get_result_info.with_node_name(&node_name);
1197            }
1198            let feedback_keyexpr: heapless::String<256> = action_info.feedback_key();
1199            let mut feedback_topic =
1200                TopicInfo::new(&feedback_keyexpr, type_name, type_hash).with_namespace(&ns);
1201            if !node_name.is_empty() {
1202                feedback_topic = feedback_topic.with_node_name(&node_name);
1203            }
1204            let session = self
1205                .session_at_mut(session_idx)
1206                .ok_or(NodeError::BackendMismatch)?;
1207            (
1208                session
1209                    .create_service_client(&send_goal_info, QosSettings::services_default())
1210                    .map_err(|_| NodeError::ActionCreationFailed)?,
1211                session
1212                    .create_service_client(&cancel_goal_info, QosSettings::services_default())
1213                    .map_err(|_| NodeError::ActionCreationFailed)?,
1214                session
1215                    .create_service_client(&get_result_info, QosSettings::services_default())
1216                    .map_err(|_| NodeError::ActionCreationFailed)?,
1217                session
1218                    .create_subscriber(&feedback_topic, QosSettings::BEST_EFFORT)
1219                    .map_err(|_| NodeError::ActionCreationFailed)?,
1220            )
1221        };
1222
1223        let core = ActionClientCore::new(
1224            send_goal_client,
1225            cancel_goal_client,
1226            get_result_client,
1227            feedback_sub,
1228        );
1229
1230        // Phase 239.5 — trailing-allocate the feedback QoS-depth buffer alongside
1231        // the entry (ring for depth > 1, triple for depth ≤ 1), then drain
1232        // `core.feedback_subscriber` into it in the dispatcher.
1233        let (_slot_count, trailing_bytes) =
1234            buffered_region_size(feedback_depth as u32, FEEDBACK_BUF);
1235        let (offset, trailing_offset) = self.arena_alloc_with_trailing::<Entry<
1236            A,
1237            GRespF,
1238            FbF,
1239            ResF,
1240            GOAL_BUF,
1241            RESULT_BUF,
1242            FEEDBACK_BUF,
1243        >>(trailing_bytes)?;
1244        let buf_ptr = unsafe { (self.arena.as_mut_ptr() as *mut u8).add(trailing_offset) };
1245        let feedback_buffer = if feedback_depth <= 1 {
1246            BufferStrategy::Triple(unsafe { TripleBuffer::init(buf_ptr, FEEDBACK_BUF) })
1247        } else {
1248            BufferStrategy::Ring(unsafe {
1249                SpscRing::init(buf_ptr, FEEDBACK_BUF, feedback_depth as usize)
1250            })
1251        };
1252        let core_ptr = unsafe {
1253            let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
1254            let entry_ptr = arena_ptr.add(offset)
1255                as *mut Entry<A, GRespF, FbF, ResF, GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>;
1256            core::ptr::write(
1257                entry_ptr,
1258                ActionClientCallbackEntry {
1259                    core,
1260                    feedback_buffer,
1261                    on_goal_response,
1262                    on_feedback,
1263                    on_result,
1264                    _phantom: core::marker::PhantomData,
1265                },
1266            );
1267            &mut (*entry_ptr).core as *mut ActionClientCore<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>
1268        };
1269
1270        self.entries[slot] = Some(CallbackMeta {
1271            offset,
1272            kind: EntryKind::ActionClient,
1273            has_data: always_ready,
1274            pre_sample: no_pre_sample,
1275            invocation: InvocationMode::Always,
1276            try_process: action_client_callback_try_process::<
1277                A,
1278                GRespF,
1279                FbF,
1280                ResF,
1281                GOAL_BUF,
1282                RESULT_BUF,
1283                FEEDBACK_BUF,
1284            >,
1285            drop_fn: drop_entry::<Entry<A, GRespF, FbF, ResF, GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>>,
1286        });
1287        self.apply_node_default_sched(slot, node_id);
1288        Ok((HandleId(slot), core_ptr))
1289    }
1290}
1291
1292impl Executor {
1293    /// Register an existing `ActionClientCore` with the executor for async polling.
1294    ///
1295    /// Unlike `register_action_client_raw` (which creates new transport handles),
1296    /// this takes ownership of an existing core. Use this when the core was
1297    /// already created by the C/C++ action client init.
1298    pub fn register_action_client_core<
1299        const GOAL_BUF: usize,
1300        const RESULT_BUF: usize,
1301        const FEEDBACK_BUF: usize,
1302    >(
1303        &mut self,
1304        core: ActionClientCore<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>,
1305        goal_response_callback: Option<RawGoalResponseCallback>,
1306        feedback_callback: Option<RawFeedbackCallback>,
1307        result_callback: Option<RawResultCallback>,
1308        context: *mut core::ffi::c_void,
1309    ) -> Result<ActionClientRawHandle, NodeError> {
1310        type Entry<const GB: usize, const RB: usize, const FB: usize> =
1311            ActionClientRawArenaEntry<GB, RB, FB>;
1312
1313        let slot = self.next_entry_slot()?;
1314        let offset = self.arena_alloc::<Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>>()?;
1315
1316        unsafe {
1317            let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
1318            let entry_ptr = arena_ptr.add(offset) as *mut Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>;
1319            core::ptr::write(
1320                entry_ptr,
1321                Entry {
1322                    core,
1323                    goal_response_callback,
1324                    feedback_callback,
1325                    result_callback,
1326                    context,
1327                },
1328            );
1329        }
1330
1331        self.entries[slot] = Some(CallbackMeta {
1332            offset,
1333            kind: EntryKind::ActionClient,
1334            has_data: always_ready,
1335            pre_sample: no_pre_sample,
1336            invocation: InvocationMode::Always,
1337            try_process: action_client_raw_try_process::<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>,
1338            drop_fn: drop_entry::<Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>>,
1339        });
1340
1341        Ok(ActionClientRawHandle { entry_index: slot })
1342    }
1343}
1344
1345/// Handle returned by [`Executor::register_action_client_raw()`].
1346///
1347/// Provides methods to send goals, request results, and cancel goals
1348/// via the executor's non-blocking path.
1349pub struct ActionClientRawHandle {
1350    entry_index: usize,
1351}
1352
1353impl ActionClientRawHandle {
1354    /// Get the entry index for this action client.
1355    pub fn entry_index(&self) -> usize {
1356        self.entry_index
1357    }
1358}
nros_node/executor/action.rs

nros_node/executor/
action.rs
