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

nros_node/executor/
action.rs
