1use core::marker::PhantomData;
4
5use nros_core::RosAction;
6use nros_rmw::{ActionEndpointHashes, 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_expire_goals, as_for_each_active_goal,
19 as_goal_exists, as_publish_feedback, as_raw_active_goal_count, as_raw_complete_goal,
20 as_raw_expire_goals, as_raw_for_each_active_goal, as_raw_goal_exists,
21 as_raw_publish_feedback, as_raw_set_goal_status, as_set_goal_status, buffered_region_size,
22 drop_entry, no_pre_sample,
23 },
24 handles::{ActionServer, ActiveGoal},
25 spin::Executor,
26 spsc_ring::SpscRing,
27 triple_buffer::TripleBuffer,
28 types::{
29 HandleId, InvocationMode, NodeError, RawAcceptedCallback, RawCancelCallback,
30 RawCancelResponseCallback, RawFeedbackCallback, RawGoalCallback, RawGoalResponseCallback,
31 RawResultCallback,
32 },
33};
34
35pub const ACTION_SERVER_QUERYABLES: usize = 3;
49
50pub const ACTION_SERVER_PUBLISHERS: usize = 2;
60
61pub const ACTION_CLIENT_SUBSCRIPTIONS: usize = 1;
70
71pub const ACTION_CLIENT_SERVICE_CLIENTS: usize = 3;
81
82pub struct RawActionServerSpec<'a> {
92 pub node_id: Option<super::node_record::NodeId>,
97 pub action_name: &'a str,
98 pub type_name: &'a str,
99 pub type_hash: &'a str,
102 pub hashes: ActionEndpointHashes<'a>,
104 pub qos: QoSProfile,
109 pub goal_callback: RawGoalCallback,
110 pub cancel_callback: RawCancelCallback,
111 pub accepted_callback: Option<RawAcceptedCallback>,
112 pub context: *mut core::ffi::c_void,
113}
114
115pub struct RawActionClientSpec<'a> {
121 pub node_id: Option<super::node_record::NodeId>,
126 pub action_name: &'a str,
127 pub type_name: &'a str,
128 pub type_hash: &'a str,
131 pub hashes: ActionEndpointHashes<'a>,
133 pub goal_response_callback: Option<RawGoalResponseCallback>,
134 pub feedback_callback: Option<RawFeedbackCallback>,
135 pub result_callback: Option<RawResultCallback>,
136 pub cancel_response_callback: Option<RawCancelResponseCallback>,
140 pub context: *mut core::ffi::c_void,
141}
142
143impl<'s> Executor<'s> {
148 pub fn register_action_server<A, GoalF, CancelF>(
159 &mut self,
160 action_name: &str,
161 goal_callback: GoalF,
162 cancel_callback: CancelF,
163 ) -> Result<ActionServerHandle<A>, NodeError>
164 where
165 A: RosAction + 'static,
166 A::Goal: Clone + MessageForRmw,
167 A::Result: Clone + Default + MessageForRmw,
168 A::Feedback: MessageForRmw,
169 A::SendGoalRequest: MessageForRmw,
170 A::SendGoalResponse: MessageForRmw,
171 A::GetResultRequest: MessageForRmw,
172 A::GetResultResponse: MessageForRmw,
173 A::FeedbackMessage: MessageForRmw,
174 GoalF: FnMut(&nros_core::GoalUuid, &A::Goal) -> nros_core::GoalResponse + 'static,
175 CancelF: FnMut(&nros_core::GoalUuid, nros_core::GoalStatusCode) -> nros_core::CancelResponse
176 + 'static,
177 {
178 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>(
179 action_name,
180 goal_callback,
181 cancel_callback,
182 )
183 }
184
185 pub fn register_action_server_sized<
187 A,
188 GoalF,
189 CancelF,
190 const GOAL_BUF: usize,
191 const RESULT_BUF: usize,
192 const FEEDBACK_BUF: usize,
193 const MAX_GOALS: usize,
194 >(
195 &mut self,
196 action_name: &str,
197 goal_callback: GoalF,
198 cancel_callback: CancelF,
199 ) -> Result<ActionServerHandle<A>, NodeError>
200 where
201 A: RosAction + 'static,
202 A::Goal: Clone + MessageForRmw,
203 A::Result: Clone + Default + MessageForRmw,
204 A::Feedback: MessageForRmw,
205 A::SendGoalRequest: MessageForRmw,
206 A::SendGoalResponse: MessageForRmw,
207 A::GetResultRequest: MessageForRmw,
208 A::GetResultResponse: MessageForRmw,
209 A::FeedbackMessage: MessageForRmw,
210 GoalF: FnMut(&nros_core::GoalUuid, &A::Goal) -> nros_core::GoalResponse + 'static,
211 CancelF: FnMut(&nros_core::GoalUuid, nros_core::GoalStatusCode) -> nros_core::CancelResponse
212 + 'static,
213 {
214 register_type::<A::Goal>()?;
221 register_type::<A::Result>()?;
222 register_type::<A::Feedback>()?;
223 register_type::<A::SendGoalRequest>()?;
224 register_type::<A::SendGoalResponse>()?;
225 register_type::<A::GetResultRequest>()?;
226 register_type::<A::GetResultResponse>()?;
227 register_type::<A::FeedbackMessage>()?;
228 A::register_protocol_types().map_err(|()| NodeError::ActionCreationFailed)?;
234 type Entry<
235 A,
236 GoalF,
237 CancelF,
238 const GB: usize,
239 const RB: usize,
240 const FB: usize,
241 const MG: usize,
242 > = ActionServerArenaEntry<A, GoalF, CancelF, GB, RB, FB, MG>;
243
244 let slot = self.next_entry_slot()?;
245
246 let action_info = ActionInfo::new(action_name, A::ACTION_NAME, A::ACTION_HASH);
248
249 let send_goal_type = super::action_core::action_service_base_type(
254 <A::SendGoalRequest as nros_core::RosMessage>::TYPE_NAME,
255 A::ACTION_NAME,
256 );
257 let get_result_type = super::action_core::action_service_base_type(
258 <A::GetResultRequest as nros_core::RosMessage>::TYPE_NAME,
259 A::ACTION_NAME,
260 );
261 let feedback_type = <A::FeedbackMessage as nros_core::RosMessage>::TYPE_NAME;
262
263 let send_goal_keyexpr: heapless::String<256> = action_info.send_goal_key();
264 let send_goal_info = ServiceInfo::new(
265 &send_goal_keyexpr,
266 send_goal_type,
267 A::SEND_GOAL_SERVICE_HASH,
268 )
269 .with_domain(self.domain_id);
270 let send_goal_server = self
271 .session
272 .create_service(&send_goal_info, QoSProfile::services_default())
273 .map_err(NodeError::Transport)?;
274
275 let cancel_goal_keyexpr: heapless::String<256> = action_info.cancel_goal_key();
276 let cancel_goal_info = ServiceInfo::new(
277 &cancel_goal_keyexpr,
278 "action_msgs::srv::dds_::CancelGoal_",
279 A::CANCEL_GOAL_SERVICE_HASH,
280 )
281 .with_domain(self.domain_id);
282 let cancel_goal_server = self
283 .session
284 .create_service(&cancel_goal_info, QoSProfile::services_default())
285 .map_err(NodeError::Transport)?;
286
287 let get_result_keyexpr: heapless::String<256> = action_info.get_result_key();
288 let get_result_info = ServiceInfo::new(
289 &get_result_keyexpr,
290 get_result_type,
291 A::GET_RESULT_SERVICE_HASH,
292 )
293 .with_domain(self.domain_id);
294 let get_result_server = self
295 .session
296 .create_service(&get_result_info, QoSProfile::services_default())
297 .map_err(NodeError::Transport)?;
298
299 let feedback_keyexpr: heapless::String<256> = action_info.feedback_key();
300 let feedback_topic = TopicInfo::new(
301 &feedback_keyexpr,
302 feedback_type,
303 <A::FeedbackMessage as nros_core::RosMessage>::TYPE_HASH,
304 )
305 .with_domain(self.domain_id);
306 let feedback_publisher = self
307 .session
308 .create_publisher(&feedback_topic, QoSProfile::QOS_PROFILE_DEFAULT)
309 .map_err(NodeError::Transport)?;
310
311 let status_keyexpr: heapless::String<256> = action_info.status_key();
312 let status_topic = TopicInfo::new(
313 &status_keyexpr,
314 "action_msgs::msg::dds_::GoalStatusArray_",
315 A::STATUS_MESSAGE_HASH,
316 )
317 .with_domain(self.domain_id);
318 let status_publisher = self
319 .session
320 .create_publisher(&status_topic, QoSProfile::QOS_PROFILE_ACTION_STATUS_DEFAULT)
321 .map_err(NodeError::Transport)?;
322
323 let server = ActionServer {
324 core: super::action_core::ActionServerCore {
325 send_goal_server,
326 cancel_goal_server,
327 get_result_server,
328 feedback_publisher,
329 status_publisher,
330 active_goals: heapless::Vec::new(),
331 completed_results: heapless::Vec::new(),
332 pending_get_results: heapless::Vec::new(),
333 result_slab: [0u8; RESULT_BUF],
334 result_slab_used: 0,
335 goal_buffer: [0u8; GOAL_BUF],
336 feedback_buffer: [0u8; FEEDBACK_BUF],
337 cancel_buffer: [0u8; 256],
338 },
339 typed_goals: heapless::Vec::new(),
340 completed_goals: heapless::Vec::new(),
341 };
342
343 let offset = self
344 .arena_alloc::<Entry<A, GoalF, CancelF, GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>>(
345 )?;
346
347 unsafe {
348 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
349 let entry_ptr = arena_ptr.add(offset)
350 as *mut Entry<A, GoalF, CancelF, GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>;
351 core::ptr::write(
352 entry_ptr,
353 Entry {
354 server,
355 goal_callback,
356 cancel_callback,
357 },
358 );
359 }
360
361 let meta = CallbackMeta {
362 offset,
363 kind: EntryKind::ActionServer,
364 arena_len: super::arena::ArenaLen::UNRECORDED,
365 has_data: always_ready,
366 pre_sample: no_pre_sample,
367 invocation: InvocationMode::Always,
368 try_process: action_server_try_process::<
369 A,
370 GoalF,
371 CancelF,
372 GOAL_BUF,
373 RESULT_BUF,
374 FEEDBACK_BUF,
375 MAX_GOALS,
376 >,
377 drop_fn: drop_entry::<
378 Entry<A, GoalF, CancelF, GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>,
379 >,
380 };
381 self.emplace_entry(slot, meta, TraceName::Text(action_name));
382
383 Ok(ActionServerHandle {
384 entry_index: slot,
385 publish_feedback_fn: as_publish_feedback::<
386 A,
387 GoalF,
388 CancelF,
389 GOAL_BUF,
390 RESULT_BUF,
391 FEEDBACK_BUF,
392 MAX_GOALS,
393 >,
394 complete_goal_fn: as_complete_goal::<
395 A,
396 GoalF,
397 CancelF,
398 GOAL_BUF,
399 RESULT_BUF,
400 FEEDBACK_BUF,
401 MAX_GOALS,
402 >,
403 set_goal_status_fn: as_set_goal_status::<
404 A,
405 GoalF,
406 CancelF,
407 GOAL_BUF,
408 RESULT_BUF,
409 FEEDBACK_BUF,
410 MAX_GOALS,
411 >,
412 active_goal_count_fn: as_active_goal_count::<
413 A,
414 GoalF,
415 CancelF,
416 GOAL_BUF,
417 RESULT_BUF,
418 FEEDBACK_BUF,
419 MAX_GOALS,
420 >,
421 goal_exists_fn: as_goal_exists::<
422 A,
423 GoalF,
424 CancelF,
425 GOAL_BUF,
426 RESULT_BUF,
427 FEEDBACK_BUF,
428 MAX_GOALS,
429 >,
430 expire_goals_fn: as_expire_goals::<
431 A,
432 GoalF,
433 CancelF,
434 GOAL_BUF,
435 RESULT_BUF,
436 FEEDBACK_BUF,
437 MAX_GOALS,
438 >,
439 for_each_active_goal_fn: as_for_each_active_goal::<
440 A,
441 GoalF,
442 CancelF,
443 GOAL_BUF,
444 RESULT_BUF,
445 FEEDBACK_BUF,
446 MAX_GOALS,
447 >,
448 _phantom: PhantomData,
449 })
450 }
451}
452
453#[allow(clippy::type_complexity)]
464pub struct ActionServerHandle<A: RosAction> {
465 pub(crate) entry_index: usize,
466 publish_feedback_fn:
467 unsafe fn(*mut u8, &nros_core::GoalUuid, &A::Feedback) -> Result<(), NodeError>,
468 complete_goal_fn: unsafe fn(
469 *mut u8,
470 &nros_core::GoalUuid,
471 nros_core::GoalStatusCode,
472 A::Result,
473 ) -> Result<(), NodeError>,
474 set_goal_status_fn: unsafe fn(*mut u8, &nros_core::GoalUuid, nros_core::GoalStatusCode),
475 active_goal_count_fn: unsafe fn(*const u8) -> usize,
476 goal_exists_fn: unsafe fn(*const u8, &nros_core::GoalUuid) -> bool,
477 expire_goals_fn: unsafe fn(*mut u8) -> usize,
478 for_each_active_goal_fn: unsafe fn(*const u8, &mut dyn FnMut(&ActiveGoal<A>)),
479 _phantom: PhantomData<A>,
480}
481
482impl<A: RosAction> Clone for ActionServerHandle<A> {
483 fn clone(&self) -> Self {
484 *self
485 }
486}
487
488impl<A: RosAction> Copy for ActionServerHandle<A> {}
489
490impl<A: RosAction> ActionServerHandle<A> {
491 pub fn handle_id(&self) -> HandleId {
495 HandleId::for_owned_slot(self.entry_index)
496 }
497
498 pub fn publish_feedback(
504 &self,
505 executor: &mut Executor,
506 goal_id: &nros_core::GoalUuid,
507 feedback: &A::Feedback,
508 ) -> Result<(), NodeError> {
509 let meta = executor.entries[self.entry_index]
510 .as_ref()
511 .ok_or(NodeError::BufferTooSmall)?;
512 let arena_ptr = executor.arena.as_mut_ptr() as *mut u8;
513 unsafe {
514 let data_ptr = arena_ptr.add(meta.offset);
515 (self.publish_feedback_fn)(data_ptr, goal_id, feedback)
516 }
517 }
518
519 pub fn succeed(
546 &self,
547 executor: &mut Executor,
548 goal_id: &nros_core::GoalUuid,
549 result: A::Result,
550 ) -> Result<(), NodeError> {
551 self.complete_goal(
552 executor,
553 goal_id,
554 nros_core::GoalStatusCode::Succeeded,
555 result,
556 )
557 }
558
559 pub fn abort(
561 &self,
562 executor: &mut Executor,
563 goal_id: &nros_core::GoalUuid,
564 result: A::Result,
565 ) -> Result<(), NodeError> {
566 self.complete_goal(
567 executor,
568 goal_id,
569 nros_core::GoalStatusCode::Aborted,
570 result,
571 )
572 }
573
574 pub fn canceled(
585 &self,
586 executor: &mut Executor,
587 goal_id: &nros_core::GoalUuid,
588 result: A::Result,
589 ) -> Result<(), NodeError> {
590 self.complete_goal(
591 executor,
592 goal_id,
593 nros_core::GoalStatusCode::Canceled,
594 result,
595 )
596 }
597
598 pub fn complete_goal(
599 &self,
600 executor: &mut Executor,
601 goal_id: &nros_core::GoalUuid,
602 status: nros_core::GoalStatusCode,
603 result: A::Result,
604 ) -> Result<(), NodeError> {
605 let meta = executor.entries[self.entry_index]
606 .as_ref()
607 .ok_or(NodeError::BufferTooSmall)?;
608 let arena_ptr = executor.arena.as_mut_ptr() as *mut u8;
609 unsafe {
610 let data_ptr = arena_ptr.add(meta.offset);
611 (self.complete_goal_fn)(data_ptr, goal_id, status, result)
612 }
613 }
614
615 pub fn set_goal_status(
620 &self,
621 executor: &mut Executor,
622 goal_id: &nros_core::GoalUuid,
623 status: nros_core::GoalStatusCode,
624 ) {
625 if let Some(meta) = executor.entries[self.entry_index].as_ref() {
626 let arena_ptr = executor.arena.as_mut_ptr() as *mut u8;
627 unsafe {
628 let data_ptr = arena_ptr.add(meta.offset);
629 (self.set_goal_status_fn)(data_ptr, goal_id, status);
630 }
631 }
632 }
633
634 pub fn active_goal_count(&self, executor: &Executor) -> usize {
638 match executor.entries[self.entry_index].as_ref() {
639 Some(meta) => {
640 let arena_ptr = executor.arena.as_ptr() as *const u8;
641 unsafe {
642 let data_ptr = arena_ptr.add(meta.offset);
643 (self.active_goal_count_fn)(data_ptr)
644 }
645 }
646 None => 0,
647 }
648 }
649
650 pub fn goal_exists(&self, executor: &Executor, goal_id: &nros_core::GoalUuid) -> bool {
662 match executor.entries[self.entry_index].as_ref() {
663 Some(meta) => {
664 let arena_ptr = executor.arena.as_ptr() as *const u8;
665 unsafe {
666 let data_ptr = arena_ptr.add(meta.offset);
667 (self.goal_exists_fn)(data_ptr, goal_id)
668 }
669 }
670 None => false,
671 }
672 }
673
674 pub fn expire_goals(&self, executor: &mut Executor) -> usize {
684 match executor.entries[self.entry_index].as_ref() {
685 Some(meta) => {
686 let arena_ptr = executor.arena.as_mut_ptr() as *mut u8;
687 unsafe {
688 let data_ptr = arena_ptr.add(meta.offset);
689 (self.expire_goals_fn)(data_ptr)
690 }
691 }
692 None => 0,
693 }
694 }
695
696 pub fn for_each_active_goal(&self, executor: &Executor, mut f: impl FnMut(&ActiveGoal<A>)) {
701 if let Some(meta) = executor.entries[self.entry_index].as_ref() {
702 let arena_ptr = executor.arena.as_ptr() as *const u8;
703 unsafe {
704 let data_ptr = arena_ptr.add(meta.offset);
705 (self.for_each_active_goal_fn)(data_ptr, &mut f);
706 }
707 }
708 }
709}
710
711impl<'s> Executor<'s> {
716 #[doc(hidden)]
725 #[allow(clippy::too_many_arguments)]
726 pub(crate) fn register_action_server_raw(
727 &mut self,
728 spec: RawActionServerSpec<'_>,
729 ) -> Result<ActionServerRawHandle, NodeError> {
730 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>(
731 spec,
732 )
733 }
734
735 #[doc(hidden)]
744 pub fn register_action_server_raw_sized<
745 const GOAL_BUF: usize,
746 const RESULT_BUF: usize,
747 const FEEDBACK_BUF: usize,
748 const MAX_GOALS: usize,
749 >(
750 &mut self,
751 spec: RawActionServerSpec<'_>,
752 ) -> Result<ActionServerRawHandle, NodeError> {
753 let RawActionServerSpec {
754 node_id,
755 action_name,
756 type_name,
757 type_hash,
758 hashes,
759 qos,
760 goal_callback,
761 cancel_callback,
762 accepted_callback,
763 context,
764 } = spec;
765
766 type Entry<const GB: usize, const RB: usize, const FB: usize, const MG: usize> =
767 ActionServerRawArenaEntry<GB, RB, FB, MG>;
768
769 let slot = self.next_entry_slot()?;
770
771 let action_info = ActionInfo::new(action_name, type_name, type_hash);
772 let domain_id = self.domain_id;
776 let (node_name, ns, session_idx) = match node_id {
777 Some(id) => {
778 let r = self
779 .nodes
780 .get(id.index())
781 .ok_or(NodeError::InvalidSchedContextBinding)?;
782 (r.name.clone(), r.namespace.clone(), r.session_idx)
783 }
784 None => (self.node_name.clone(), self.namespace.clone(), 0u8),
785 };
786
787 let (
796 send_goal_server,
797 cancel_goal_server,
798 get_result_server,
799 feedback_publisher,
800 status_publisher,
801 ) = {
802 let send_goal_type: heapless::String<256> =
808 super::action_core::action_channel_type(type_name, "SendGoal");
809 let get_result_type: heapless::String<256> =
810 super::action_core::action_channel_type(type_name, "GetResult");
811 let feedback_type: heapless::String<256> =
812 super::action_core::action_channel_type(type_name, "FeedbackMessage");
813
814 let send_goal_keyexpr: heapless::String<256> = action_info.send_goal_key();
815 let mut send_goal_info =
816 ServiceInfo::new(&send_goal_keyexpr, &send_goal_type, hashes.send_goal)
817 .with_namespace(&ns)
818 .with_domain(domain_id);
819 if !node_name.is_empty() {
820 send_goal_info = send_goal_info.with_node_name(&node_name);
821 }
822
823 let cancel_goal_keyexpr: heapless::String<256> = action_info.cancel_goal_key();
824 let mut cancel_goal_info = ServiceInfo::new(
825 &cancel_goal_keyexpr,
826 "action_msgs::srv::dds_::CancelGoal_",
827 hashes.cancel_goal,
828 )
829 .with_namespace(&ns)
830 .with_domain(domain_id);
831 if !node_name.is_empty() {
832 cancel_goal_info = cancel_goal_info.with_node_name(&node_name);
833 }
834
835 let get_result_keyexpr: heapless::String<256> = action_info.get_result_key();
836 let mut get_result_info =
837 ServiceInfo::new(&get_result_keyexpr, &get_result_type, hashes.get_result)
838 .with_namespace(&ns)
839 .with_domain(domain_id);
840 if !node_name.is_empty() {
841 get_result_info = get_result_info.with_node_name(&node_name);
842 }
843
844 let feedback_keyexpr: heapless::String<256> = action_info.feedback_key();
845 let mut feedback_topic =
846 TopicInfo::new(&feedback_keyexpr, &feedback_type, hashes.feedback)
847 .with_namespace(&ns)
848 .with_domain(domain_id);
849 if !node_name.is_empty() {
850 feedback_topic = feedback_topic.with_node_name(&node_name);
851 }
852
853 let status_keyexpr: heapless::String<256> = action_info.status_key();
854 let mut status_topic = TopicInfo::new(
855 &status_keyexpr,
856 "action_msgs::msg::dds_::GoalStatusArray_",
857 hashes.status,
858 )
859 .with_namespace(&ns)
860 .with_domain(domain_id);
861 if !node_name.is_empty() {
862 status_topic = status_topic.with_node_name(&node_name);
863 }
864
865 let session = self
866 .session_at_mut(session_idx)
867 .ok_or(NodeError::BackendMismatch)?;
868 (
869 session
870 .create_service(&send_goal_info, qos)
871 .map_err(NodeError::Transport)?,
872 session
873 .create_service(&cancel_goal_info, qos)
874 .map_err(NodeError::Transport)?,
875 session
876 .create_service(&get_result_info, qos)
877 .map_err(NodeError::Transport)?,
878 session
879 .create_publisher(&feedback_topic, QoSProfile::QOS_PROFILE_DEFAULT)
880 .map_err(NodeError::Transport)?,
881 session
882 .create_publisher(&status_topic, QoSProfile::QOS_PROFILE_ACTION_STATUS_DEFAULT)
883 .map_err(NodeError::Transport)?,
884 )
885 };
886
887 let core = ActionServerCore {
888 send_goal_server,
889 cancel_goal_server,
890 get_result_server,
891 feedback_publisher,
892 status_publisher,
893 active_goals: heapless::Vec::new(),
894 completed_results: heapless::Vec::new(),
895 pending_get_results: heapless::Vec::new(),
896 result_slab: [0u8; RESULT_BUF],
897 result_slab_used: 0,
898 goal_buffer: [0u8; GOAL_BUF],
899 feedback_buffer: [0u8; FEEDBACK_BUF],
900 cancel_buffer: [0u8; 256],
901 };
902
903 let offset = self.arena_alloc::<Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>>()?;
904
905 unsafe {
906 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
907 let entry_ptr =
908 arena_ptr.add(offset) as *mut Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>;
909 core::ptr::write(
910 entry_ptr,
911 Entry {
912 core,
913 goal_callback,
914 cancel_callback,
915 accepted_callback,
916 context,
917 },
918 );
919 }
920
921 let meta = CallbackMeta {
922 offset,
923 kind: EntryKind::ActionServer,
924 arena_len: super::arena::ArenaLen::UNRECORDED,
925 has_data: always_ready,
926 pre_sample: no_pre_sample,
927 invocation: InvocationMode::Always,
928 try_process: action_server_raw_try_process::<
929 GOAL_BUF,
930 RESULT_BUF,
931 FEEDBACK_BUF,
932 MAX_GOALS,
933 >,
934 drop_fn: drop_entry::<Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>>,
935 };
936 self.emplace_entry(slot, meta, TraceName::Text(spec.action_name));
937 self.apply_node_default_sched(slot, node_id, None);
938
939 Ok(ActionServerRawHandle {
940 entry_index: slot,
941 publish_feedback_fn: as_raw_publish_feedback::<
942 GOAL_BUF,
943 RESULT_BUF,
944 FEEDBACK_BUF,
945 MAX_GOALS,
946 >,
947 complete_goal_fn: as_raw_complete_goal::<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>,
948 set_goal_status_fn: as_raw_set_goal_status::<
949 GOAL_BUF,
950 RESULT_BUF,
951 FEEDBACK_BUF,
952 MAX_GOALS,
953 >,
954 active_goal_count_fn: as_raw_active_goal_count::<
955 GOAL_BUF,
956 RESULT_BUF,
957 FEEDBACK_BUF,
958 MAX_GOALS,
959 >,
960 goal_exists_fn: as_raw_goal_exists::<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>,
961 expire_goals_fn: as_raw_expire_goals::<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>,
962 for_each_active_goal_fn: as_raw_for_each_active_goal::<
963 GOAL_BUF,
964 RESULT_BUF,
965 FEEDBACK_BUF,
966 MAX_GOALS,
967 >,
968 })
969 }
970}
971
972unsafe extern "C" fn detached_goal_callback(
991 _goal_id: *const nros_core::GoalUuid,
992 _goal_data: *const u8,
993 _goal_len: usize,
994 _context: *mut core::ffi::c_void,
995) -> nros_core::GoalResponse {
996 nros_core::GoalResponse::Reject
997}
998
999unsafe extern "C" fn detached_cancel_callback(
1010 _goal_id: *const nros_core::GoalUuid,
1011 _status: nros_core::GoalStatusCode,
1012 _context: *mut core::ffi::c_void,
1013) -> nros_core::CancelResponse {
1014 nros_core::CancelResponse::Accept
1015}
1016
1017impl<'s> Executor<'s> {
1018 pub(crate) unsafe fn detach_action_server_raw_sized<
1050 const GOAL_BUF: usize,
1051 const RESULT_BUF: usize,
1052 const FEEDBACK_BUF: usize,
1053 const MAX_GOALS: usize,
1054 >(
1055 &mut self,
1056 entry_index: usize,
1057 ) -> bool {
1058 let Some(meta) = self.entries.get(entry_index).and_then(|e| e.as_ref()) else {
1059 return false;
1060 };
1061 if !matches!(meta.kind, EntryKind::ActionServer) {
1062 return false;
1063 }
1064 let offset = meta.offset;
1065 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
1066 unsafe {
1067 let entry = &mut *(arena_ptr.add(offset)
1068 as *mut ActionServerRawArenaEntry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF, MAX_GOALS>);
1069 entry.goal_callback = detached_goal_callback;
1070 entry.cancel_callback = detached_cancel_callback;
1071 entry.accepted_callback = None;
1072 entry.context = core::ptr::null_mut();
1073 }
1074 true
1075 }
1076
1077 pub(crate) unsafe fn detach_action_server_raw(
1091 &mut self,
1092 handle: &ActionServerRawHandle,
1093 ) -> bool {
1094 unsafe {
1095 self.detach_action_server_raw_sized::<
1096 { crate::config::DEFAULT_RX_BUF_SIZE },
1097 { crate::config::DEFAULT_RX_BUF_SIZE },
1098 { crate::config::DEFAULT_RX_BUF_SIZE },
1099 4,
1100 >(handle.entry_index)
1101 }
1102 }
1103
1104 pub(crate) unsafe fn retarget_action_server_raw(
1119 &mut self,
1120 handle: &ActionServerRawHandle,
1121 context: *mut core::ffi::c_void,
1122 ) -> bool {
1123 let Some(meta) = self
1124 .entries
1125 .get(handle.entry_index)
1126 .and_then(|e| e.as_ref())
1127 else {
1128 return false;
1129 };
1130 if !matches!(meta.kind, EntryKind::ActionServer) {
1131 return false;
1132 }
1133 let offset = meta.offset;
1134 unsafe {
1135 let entry = &mut *((self.arena.as_mut_ptr() as *mut u8).add(offset)
1136 as *mut ActionServerRawArenaEntry<
1137 { crate::config::DEFAULT_RX_BUF_SIZE },
1138 { crate::config::DEFAULT_RX_BUF_SIZE },
1139 { crate::config::DEFAULT_RX_BUF_SIZE },
1140 4,
1141 >);
1142 entry.context = context;
1143 }
1144 true
1145 }
1146
1147 pub(crate) unsafe fn detach_action_client_raw(&mut self, entry_index: usize) -> bool {
1166 let Some(meta) = self.entries.get(entry_index).and_then(|e| e.as_ref()) else {
1167 return false;
1168 };
1169 if !matches!(meta.kind, EntryKind::ActionClient) {
1170 return false;
1171 }
1172 let offset = meta.offset;
1173 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
1174 unsafe {
1175 let entry = &mut *(arena_ptr.add(offset)
1176 as *mut ActionClientRawArenaEntry<
1177 { crate::config::DEFAULT_RX_BUF_SIZE },
1178 { crate::config::DEFAULT_RX_BUF_SIZE },
1179 { crate::config::DEFAULT_RX_BUF_SIZE },
1180 >);
1181 entry.goal_response_callback = None;
1182 entry.feedback_callback = None;
1183 entry.result_callback = None;
1184 entry.context = core::ptr::null_mut();
1185 }
1186 true
1187 }
1188
1189 pub(crate) unsafe fn retarget_action_client_raw(
1201 &mut self,
1202 entry_index: usize,
1203 context: *mut core::ffi::c_void,
1204 ) -> bool {
1205 let Some(meta) = self.entries.get(entry_index).and_then(|e| e.as_ref()) else {
1206 return false;
1207 };
1208 if !matches!(meta.kind, EntryKind::ActionClient) {
1209 return false;
1210 }
1211 let offset = meta.offset;
1212 unsafe {
1213 let entry = &mut *((self.arena.as_mut_ptr() as *mut u8).add(offset)
1214 as *mut ActionClientRawArenaEntry<
1215 { crate::config::DEFAULT_RX_BUF_SIZE },
1216 { crate::config::DEFAULT_RX_BUF_SIZE },
1217 { crate::config::DEFAULT_RX_BUF_SIZE },
1218 >);
1219 entry.context = context;
1220 }
1221 true
1222 }
1223
1224 pub(crate) unsafe fn release_action_server_raw_sized<
1247 const GOAL_BUF: usize,
1248 const RESULT_BUF: usize,
1249 const FEEDBACK_BUF: usize,
1250 const MAX_GOALS: usize,
1251 >(
1252 &mut self,
1253 entry_index: usize,
1254 ) -> bool {
1255 unsafe { self.release_entry(entry_index, EntryKind::ActionServer) }
1256 }
1257
1258 pub(crate) unsafe fn release_action_server_raw(
1265 &mut self,
1266 handle: &ActionServerRawHandle,
1267 ) -> bool {
1268 unsafe {
1269 self.release_action_server_raw_sized::<
1270 { crate::config::DEFAULT_RX_BUF_SIZE },
1271 { crate::config::DEFAULT_RX_BUF_SIZE },
1272 { crate::config::DEFAULT_RX_BUF_SIZE },
1273 4,
1274 >(handle.entry_index)
1275 }
1276 }
1277
1278 pub(crate) unsafe fn release_action_client_raw(&mut self, entry_index: usize) -> bool {
1288 unsafe {
1289 self.release_action_client_raw_sized::<
1290 { crate::config::DEFAULT_RX_BUF_SIZE },
1291 { crate::config::DEFAULT_RX_BUF_SIZE },
1292 { crate::config::DEFAULT_RX_BUF_SIZE },
1293 >(entry_index)
1294 }
1295 }
1296
1297 pub(crate) unsafe fn release_action_client_raw_sized<
1305 const GOAL_BUF: usize,
1306 const RESULT_BUF: usize,
1307 const FEEDBACK_BUF: usize,
1308 >(
1309 &mut self,
1310 entry_index: usize,
1311 ) -> bool {
1312 unsafe { self.release_entry(entry_index, EntryKind::ActionClient) }
1313 }
1314}
1315
1316#[repr(C)]
1325#[allow(clippy::type_complexity)]
1326pub struct ActionServerRawHandle {
1327 pub(crate) entry_index: usize,
1328 publish_feedback_fn:
1329 unsafe fn(*mut u8, &nros_core::GoalUuid, *const u8, usize) -> Result<(), NodeError>,
1330 complete_goal_fn: unsafe fn(
1331 *mut u8,
1332 &nros_core::GoalUuid,
1333 nros_core::GoalStatusCode,
1334 *const u8,
1335 usize,
1336 ) -> Result<(), NodeError>,
1337 set_goal_status_fn: unsafe fn(*mut u8, &nros_core::GoalUuid, nros_core::GoalStatusCode),
1338 active_goal_count_fn: unsafe fn(*const u8) -> usize,
1339 goal_exists_fn: unsafe fn(*const u8, &nros_core::GoalUuid) -> bool,
1340 expire_goals_fn: unsafe fn(*mut u8) -> usize,
1341 for_each_active_goal_fn: unsafe fn(*const u8, &mut dyn FnMut(&RawActiveGoal)),
1342}
1343
1344impl Clone for ActionServerRawHandle {
1345 fn clone(&self) -> Self {
1346 *self
1347 }
1348}
1349
1350impl Copy for ActionServerRawHandle {}
1351
1352pub const INVALID_ENTRY_INDEX: usize = usize::MAX;
1360
1361impl ActionServerRawHandle {
1362 pub(crate) const fn invalid() -> Self {
1368 unsafe fn unreachable_publish_feedback(
1369 _: *mut u8,
1370 _: &nros_core::GoalUuid,
1371 _: *const u8,
1372 _: usize,
1373 ) -> Result<(), NodeError> {
1374 unreachable!("ActionServerRawHandle::publish_feedback called on invalid handle")
1375 }
1376 unsafe fn unreachable_complete_goal(
1377 _: *mut u8,
1378 _: &nros_core::GoalUuid,
1379 _: nros_core::GoalStatusCode,
1380 _: *const u8,
1381 _: usize,
1382 ) -> Result<(), NodeError> {
1383 unreachable!("ActionServerRawHandle::complete_goal called on invalid handle")
1384 }
1385 unsafe fn unreachable_set_goal_status(
1386 _: *mut u8,
1387 _: &nros_core::GoalUuid,
1388 _: nros_core::GoalStatusCode,
1389 ) {
1390 unreachable!("ActionServerRawHandle::set_goal_status called on invalid handle")
1391 }
1392 unsafe fn unreachable_active_goal_count(_: *const u8) -> usize {
1393 unreachable!("ActionServerRawHandle::active_goal_count called on invalid handle")
1394 }
1395 unsafe fn unreachable_goal_exists(_: *const u8, _: &nros_core::GoalUuid) -> bool {
1396 unreachable!("ActionServerRawHandle::goal_exists called on invalid handle")
1397 }
1398 unsafe fn unreachable_expire_goals(_: *mut u8) -> usize {
1399 unreachable!("ActionServerRawHandle::expire_goals called on invalid handle")
1400 }
1401 unsafe fn unreachable_for_each_active_goal(
1402 _: *const u8,
1403 _: &mut dyn FnMut(&RawActiveGoal),
1404 ) {
1405 unreachable!("ActionServerRawHandle::for_each_active_goal called on invalid handle")
1406 }
1407 Self {
1408 entry_index: INVALID_ENTRY_INDEX,
1409 publish_feedback_fn: unreachable_publish_feedback,
1410 complete_goal_fn: unreachable_complete_goal,
1411 set_goal_status_fn: unreachable_set_goal_status,
1412 active_goal_count_fn: unreachable_active_goal_count,
1413 goal_exists_fn: unreachable_goal_exists,
1414 expire_goals_fn: unreachable_expire_goals,
1415 for_each_active_goal_fn: unreachable_for_each_active_goal,
1416 }
1417 }
1418
1419 pub(crate) const fn is_invalid(&self) -> bool {
1421 self.entry_index == INVALID_ENTRY_INDEX
1422 }
1423}
1424
1425impl Default for ActionServerRawHandle {
1426 fn default() -> Self {
1427 Self::invalid()
1428 }
1429}
1430
1431impl ActionServerRawHandle {
1432 pub(crate) fn handle_id(&self) -> HandleId {
1434 HandleId::for_owned_slot(self.entry_index)
1435 }
1436
1437 pub(crate) fn publish_feedback_raw(
1441 &self,
1442 executor: &mut Executor,
1443 goal_id: &nros_core::GoalUuid,
1444 feedback_data: &[u8],
1445 ) -> Result<(), NodeError> {
1446 let meta = executor.entries[self.entry_index]
1447 .as_ref()
1448 .ok_or(NodeError::BufferTooSmall)?;
1449 let arena_ptr = executor.arena.as_mut_ptr() as *mut u8;
1450 unsafe {
1451 let data_ptr = arena_ptr.add(meta.offset);
1452 (self.publish_feedback_fn)(
1453 data_ptr,
1454 goal_id,
1455 feedback_data.as_ptr(),
1456 feedback_data.len(),
1457 )
1458 }
1459 }
1460
1461 pub(crate) fn complete_goal_raw(
1472 &self,
1473 executor: &mut Executor,
1474 goal_id: &nros_core::GoalUuid,
1475 status: nros_core::GoalStatusCode,
1476 result_data: &[u8],
1477 ) -> Result<(), NodeError> {
1478 let meta = executor.entries[self.entry_index]
1479 .as_ref()
1480 .ok_or(NodeError::BufferTooSmall)?;
1481 let arena_ptr = executor.arena.as_mut_ptr() as *mut u8;
1482 unsafe {
1483 let data_ptr = arena_ptr.add(meta.offset);
1484 (self.complete_goal_fn)(
1485 data_ptr,
1486 goal_id,
1487 status,
1488 result_data.as_ptr(),
1489 result_data.len(),
1490 )
1491 }
1492 }
1493
1494 pub(crate) fn set_goal_status(
1499 &self,
1500 executor: &mut Executor,
1501 goal_id: &nros_core::GoalUuid,
1502 status: nros_core::GoalStatusCode,
1503 ) {
1504 if let Some(meta) = executor.entries[self.entry_index].as_ref() {
1505 let arena_ptr = executor.arena.as_mut_ptr() as *mut u8;
1506 unsafe {
1507 let data_ptr = arena_ptr.add(meta.offset);
1508 (self.set_goal_status_fn)(data_ptr, goal_id, status);
1509 }
1510 }
1511 }
1512
1513 pub(crate) fn active_goal_count(&self, executor: &Executor) -> usize {
1517 match executor.entries[self.entry_index].as_ref() {
1518 Some(meta) => {
1519 let arena_ptr = executor.arena.as_ptr() as *const u8;
1520 unsafe {
1521 let data_ptr = arena_ptr.add(meta.offset);
1522 (self.active_goal_count_fn)(data_ptr)
1523 }
1524 }
1525 None => 0,
1526 }
1527 }
1528
1529 pub(crate) fn goal_exists(&self, executor: &Executor, goal_id: &nros_core::GoalUuid) -> bool {
1536 match executor.entries[self.entry_index].as_ref() {
1537 Some(meta) => {
1538 let arena_ptr = executor.arena.as_ptr() as *const u8;
1539 unsafe {
1540 let data_ptr = arena_ptr.add(meta.offset);
1541 (self.goal_exists_fn)(data_ptr, goal_id)
1542 }
1543 }
1544 None => false,
1545 }
1546 }
1547
1548 pub(crate) fn expire_goals(&self, executor: &mut Executor) -> usize {
1552 match executor.entries[self.entry_index].as_ref() {
1553 Some(meta) => {
1554 let arena_ptr = executor.arena.as_mut_ptr() as *mut u8;
1555 unsafe {
1556 let data_ptr = arena_ptr.add(meta.offset);
1557 (self.expire_goals_fn)(data_ptr)
1558 }
1559 }
1560 None => 0,
1561 }
1562 }
1563
1564 pub(crate) fn for_each_active_goal(
1568 &self,
1569 executor: &Executor,
1570 mut f: impl FnMut(&RawActiveGoal),
1571 ) {
1572 if let Some(meta) = executor.entries[self.entry_index].as_ref() {
1573 let arena_ptr = executor.arena.as_ptr() as *const u8;
1574 unsafe {
1575 let data_ptr = arena_ptr.add(meta.offset);
1576 (self.for_each_active_goal_fn)(data_ptr, &mut f);
1577 }
1578 }
1579 }
1580
1581 pub(crate) fn goal_status(
1591 &self,
1592 executor: &Executor,
1593 goal_id: &nros_core::GoalUuid,
1594 ) -> Option<nros_core::GoalStatusCode> {
1595 let mut found = None;
1596 self.for_each_active_goal(executor, |g| {
1597 if g.goal_id.uuid == goal_id.uuid && found.is_none() {
1598 found = Some(g.status);
1599 }
1600 });
1601 found
1602 }
1603}
1604
1605impl<'s> Executor<'s> {
1610 #[doc(hidden)]
1625 #[allow(clippy::too_many_arguments)]
1626 pub(crate) fn register_action_client_raw(
1627 &mut self,
1628 spec: RawActionClientSpec<'_>,
1629 ) -> Result<ActionClientRawHandle, NodeError> {
1630 self.register_action_client_raw_sized::<
1631 { crate::config::DEFAULT_RX_BUF_SIZE },
1632 { crate::config::DEFAULT_RX_BUF_SIZE },
1633 { crate::config::DEFAULT_RX_BUF_SIZE },
1634 >(spec)
1635 }
1636
1637 #[doc(hidden)]
1643 pub fn register_action_client_raw_sized<
1644 const GOAL_BUF: usize,
1645 const RESULT_BUF: usize,
1646 const FEEDBACK_BUF: usize,
1647 >(
1648 &mut self,
1649 spec: RawActionClientSpec<'_>,
1650 ) -> Result<ActionClientRawHandle, NodeError> {
1651 let RawActionClientSpec {
1652 node_id,
1653 action_name,
1654 type_name,
1655 type_hash,
1656 hashes,
1657 goal_response_callback,
1658 feedback_callback,
1659 result_callback,
1660 cancel_response_callback,
1661 context,
1662 } = spec;
1663
1664 type Entry<const GB: usize, const RB: usize, const FB: usize> =
1665 ActionClientRawArenaEntry<GB, RB, FB>;
1666
1667 let slot = self.next_entry_slot()?;
1668
1669 let action_info = ActionInfo::new(action_name, type_name, type_hash);
1670 let domain_id = self.domain_id;
1674 let (node_name, ns, session_idx) = match node_id {
1675 Some(id) => {
1676 let r = self
1677 .nodes
1678 .get(id.index())
1679 .ok_or(NodeError::InvalidSchedContextBinding)?;
1680 (r.name.clone(), r.namespace.clone(), r.session_idx)
1681 }
1682 None => (self.node_name.clone(), self.namespace.clone(), 0u8),
1683 };
1684
1685 let (send_goal_client, cancel_goal_client, get_result_client, feedback_sub) = {
1686 let send_goal_type: heapless::String<256> =
1691 super::action_core::action_channel_type(type_name, "SendGoal");
1692 let get_result_type: heapless::String<256> =
1693 super::action_core::action_channel_type(type_name, "GetResult");
1694 let feedback_type: heapless::String<256> =
1695 super::action_core::action_channel_type(type_name, "FeedbackMessage");
1696
1697 let send_goal_keyexpr: heapless::String<256> = action_info.send_goal_key();
1698 let mut send_goal_info =
1699 ServiceInfo::new(&send_goal_keyexpr, &send_goal_type, hashes.send_goal)
1700 .with_namespace(&ns)
1701 .with_domain(domain_id);
1702 if !node_name.is_empty() {
1703 send_goal_info = send_goal_info.with_node_name(&node_name);
1704 }
1705
1706 let cancel_goal_keyexpr: heapless::String<256> = action_info.cancel_goal_key();
1707 let mut cancel_goal_info = ServiceInfo::new(
1708 &cancel_goal_keyexpr,
1709 "action_msgs::srv::dds_::CancelGoal_",
1710 hashes.cancel_goal,
1711 )
1712 .with_namespace(&ns)
1713 .with_domain(domain_id);
1714 if !node_name.is_empty() {
1715 cancel_goal_info = cancel_goal_info.with_node_name(&node_name);
1716 }
1717
1718 let get_result_keyexpr: heapless::String<256> = action_info.get_result_key();
1719 let mut get_result_info =
1720 ServiceInfo::new(&get_result_keyexpr, &get_result_type, hashes.get_result)
1721 .with_namespace(&ns)
1722 .with_domain(domain_id);
1723 if !node_name.is_empty() {
1724 get_result_info = get_result_info.with_node_name(&node_name);
1725 }
1726
1727 let feedback_keyexpr: heapless::String<256> = action_info.feedback_key();
1728 let mut feedback_topic =
1729 TopicInfo::new(&feedback_keyexpr, &feedback_type, hashes.feedback)
1730 .with_namespace(&ns)
1731 .with_domain(domain_id);
1732 if !node_name.is_empty() {
1733 feedback_topic = feedback_topic.with_node_name(&node_name);
1734 }
1735
1736 let session = self
1737 .session_at_mut(session_idx)
1738 .ok_or(NodeError::BackendMismatch)?;
1739 (
1749 session
1750 .create_client(&send_goal_info, QoSProfile::services_default())
1751 .map_err(|e| {
1752 nros_log::log_error!(
1753 nros_log::get_logger("nros_node"),
1754 "action client: send_goal client failed: {:?}",
1755 e
1756 );
1757 NodeError::Transport(e)
1758 })?,
1759 session
1760 .create_client(&cancel_goal_info, QoSProfile::services_default())
1761 .map_err(|e| {
1762 nros_log::log_error!(
1763 nros_log::get_logger("nros_node"),
1764 "action client: cancel_goal client failed: {:?}",
1765 e
1766 );
1767 NodeError::Transport(e)
1768 })?,
1769 session
1770 .create_client(&get_result_info, QoSProfile::services_default())
1771 .map_err(|e| {
1772 nros_log::log_error!(
1773 nros_log::get_logger("nros_node"),
1774 "action client: get_result client failed: {:?}",
1775 e
1776 );
1777 NodeError::Transport(e)
1778 })?,
1779 session
1780 .create_subscription(&feedback_topic, QoSProfile::BEST_EFFORT)
1781 .map_err(|e| {
1782 nros_log::log_error!(
1783 nros_log::get_logger("nros_node"),
1784 "action client: feedback subscription failed: {:?}",
1785 e
1786 );
1787 NodeError::Transport(e)
1788 })?,
1789 )
1790 };
1791
1792 let core = ActionClientCore::new(
1793 send_goal_client,
1794 cancel_goal_client,
1795 get_result_client,
1796 feedback_sub,
1797 );
1798
1799 let offset = self.arena_alloc::<Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>>()?;
1800
1801 unsafe {
1802 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
1803 let entry_ptr = arena_ptr.add(offset) as *mut Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>;
1804 core::ptr::write(
1805 entry_ptr,
1806 Entry {
1807 core,
1808 goal_response_callback,
1809 feedback_callback,
1810 result_callback,
1811 cancel_response_callback,
1812 context,
1813 },
1814 );
1815 }
1816
1817 let meta = CallbackMeta {
1818 offset,
1819 kind: EntryKind::ActionClient,
1820 arena_len: super::arena::ArenaLen::UNRECORDED,
1821 has_data: always_ready,
1822 pre_sample: no_pre_sample,
1823 invocation: InvocationMode::Always,
1824 try_process: action_client_raw_try_process::<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>,
1825 drop_fn: drop_entry::<Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>>,
1826 };
1827 self.emplace_entry(slot, meta, TraceName::Text(spec.action_name));
1828 self.apply_node_default_sched(slot, node_id, None);
1829
1830 Ok(ActionClientRawHandle { entry_index: slot })
1831 }
1832
1833 #[allow(clippy::too_many_arguments, clippy::type_complexity)]
1840 pub(crate) fn register_action_client_callback<
1841 A,
1842 GRespF,
1843 FbF,
1844 ResF,
1845 const GOAL_BUF: usize,
1846 const RESULT_BUF: usize,
1847 const FEEDBACK_BUF: usize,
1848 >(
1849 &mut self,
1850 node_id: Option<super::node_record::NodeId>,
1851 action_name: &str,
1852 type_name: &str,
1853 type_hash: &str,
1854 feedback_depth: u16,
1855 on_goal_response: GRespF,
1856 on_feedback: FbF,
1857 on_result: ResF,
1858 ) -> Result<
1859 (
1860 HandleId,
1861 *mut ActionClientCore<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>,
1862 ),
1863 NodeError,
1864 >
1865 where
1866 A: nros_core::RosAction + 'static,
1867 GRespF: FnMut(&nros_core::GoalUuid, bool) + 'static,
1868 FbF: FnMut(&nros_core::GoalUuid, &A::Feedback) + 'static,
1869 ResF: FnMut(&nros_core::GoalUuid, nros_core::GoalStatusCode, &A::Result) + 'static,
1870 {
1871 type Entry<A, G, Fb, R, const GB: usize, const RB: usize, const FB: usize> =
1872 ActionClientCallbackEntry<A, G, Fb, R, GB, RB, FB>;
1873
1874 let slot = self.next_entry_slot()?;
1875 let action_info = ActionInfo::new(action_name, type_name, type_hash);
1876 let hashes = ActionEndpointHashes::of::<A>();
1878 A::register_protocol_types().map_err(|()| NodeError::ActionCreationFailed)?;
1884 let domain_id = self.domain_id;
1888 let (node_name, ns, session_idx) = match node_id {
1889 Some(id) => {
1890 let r = self
1891 .nodes
1892 .get(id.index())
1893 .ok_or(NodeError::InvalidSchedContextBinding)?;
1894 (r.name.clone(), r.namespace.clone(), r.session_idx)
1895 }
1896 None => (self.node_name.clone(), self.namespace.clone(), 0u8),
1897 };
1898
1899 let (send_goal_client, cancel_goal_client, get_result_client, feedback_sub) = {
1900 let send_goal_type: heapless::String<256> =
1905 super::action_core::action_channel_type(type_name, "SendGoal");
1906 let get_result_type: heapless::String<256> =
1907 super::action_core::action_channel_type(type_name, "GetResult");
1908 let feedback_type: heapless::String<256> =
1909 super::action_core::action_channel_type(type_name, "FeedbackMessage");
1910
1911 let send_goal_keyexpr: heapless::String<256> = action_info.send_goal_key();
1912 let mut send_goal_info =
1913 ServiceInfo::new(&send_goal_keyexpr, &send_goal_type, hashes.send_goal)
1914 .with_namespace(&ns)
1915 .with_domain(domain_id);
1916 if !node_name.is_empty() {
1917 send_goal_info = send_goal_info.with_node_name(&node_name);
1918 }
1919 let cancel_goal_keyexpr: heapless::String<256> = action_info.cancel_goal_key();
1920 let mut cancel_goal_info = ServiceInfo::new(
1921 &cancel_goal_keyexpr,
1922 "action_msgs::srv::dds_::CancelGoal_",
1923 hashes.cancel_goal,
1924 )
1925 .with_namespace(&ns)
1926 .with_domain(domain_id);
1927 if !node_name.is_empty() {
1928 cancel_goal_info = cancel_goal_info.with_node_name(&node_name);
1929 }
1930 let get_result_keyexpr: heapless::String<256> = action_info.get_result_key();
1931 let mut get_result_info =
1932 ServiceInfo::new(&get_result_keyexpr, &get_result_type, hashes.get_result)
1933 .with_namespace(&ns)
1934 .with_domain(domain_id);
1935 if !node_name.is_empty() {
1936 get_result_info = get_result_info.with_node_name(&node_name);
1937 }
1938 let feedback_keyexpr: heapless::String<256> = action_info.feedback_key();
1939 let mut feedback_topic =
1940 TopicInfo::new(&feedback_keyexpr, &feedback_type, hashes.feedback)
1941 .with_namespace(&ns)
1942 .with_domain(domain_id);
1943 if !node_name.is_empty() {
1944 feedback_topic = feedback_topic.with_node_name(&node_name);
1945 }
1946 let session = self
1947 .session_at_mut(session_idx)
1948 .ok_or(NodeError::BackendMismatch)?;
1949 (
1950 session
1951 .create_client(&send_goal_info, QoSProfile::services_default())
1952 .map_err(NodeError::Transport)?,
1953 session
1954 .create_client(&cancel_goal_info, QoSProfile::services_default())
1955 .map_err(NodeError::Transport)?,
1956 session
1957 .create_client(&get_result_info, QoSProfile::services_default())
1958 .map_err(NodeError::Transport)?,
1959 session
1960 .create_subscription(&feedback_topic, QoSProfile::BEST_EFFORT)
1961 .map_err(NodeError::Transport)?,
1962 )
1963 };
1964
1965 let core = ActionClientCore::new(
1966 send_goal_client,
1967 cancel_goal_client,
1968 get_result_client,
1969 feedback_sub,
1970 );
1971
1972 let (_slot_count, trailing_bytes) =
1976 buffered_region_size(feedback_depth as u32, FEEDBACK_BUF);
1977 let (offset, trailing_offset) = self.arena_alloc_with_trailing::<Entry<
1978 A,
1979 GRespF,
1980 FbF,
1981 ResF,
1982 GOAL_BUF,
1983 RESULT_BUF,
1984 FEEDBACK_BUF,
1985 >>(trailing_bytes)?;
1986 let buf_ptr = unsafe { (self.arena.as_mut_ptr() as *mut u8).add(trailing_offset) };
1987 let feedback_buffer = if feedback_depth <= 1 {
1988 BufferStrategy::Triple(unsafe { TripleBuffer::init(buf_ptr, FEEDBACK_BUF) })
1989 } else {
1990 BufferStrategy::Ring(unsafe {
1991 SpscRing::init(buf_ptr, FEEDBACK_BUF, feedback_depth as usize)
1992 })
1993 };
1994 let core_ptr = unsafe {
1995 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
1996 let entry_ptr = arena_ptr.add(offset)
1997 as *mut Entry<A, GRespF, FbF, ResF, GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>;
1998 core::ptr::write(
1999 entry_ptr,
2000 ActionClientCallbackEntry {
2001 core,
2002 feedback_buffer,
2003 on_goal_response,
2004 on_feedback,
2005 on_result,
2006 _phantom: core::marker::PhantomData,
2007 },
2008 );
2009 &mut (*entry_ptr).core as *mut ActionClientCore<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>
2010 };
2011
2012 let meta = CallbackMeta {
2013 offset,
2014 kind: EntryKind::ActionClient,
2015 arena_len: super::arena::ArenaLen::UNRECORDED,
2016 has_data: always_ready,
2017 pre_sample: no_pre_sample,
2018 invocation: InvocationMode::Always,
2019 try_process: action_client_callback_try_process::<
2020 A,
2021 GRespF,
2022 FbF,
2023 ResF,
2024 GOAL_BUF,
2025 RESULT_BUF,
2026 FEEDBACK_BUF,
2027 >,
2028 drop_fn: drop_entry::<Entry<A, GRespF, FbF, ResF, GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>>,
2029 };
2030 self.emplace_entry(slot, meta, TraceName::Text(action_name));
2031 self.apply_node_default_sched(slot, node_id, None);
2032 Ok((self.handle_for(slot), core_ptr))
2033 }
2034}
2035
2036impl<'s> Executor<'s> {
2037 #[doc(hidden)]
2043 pub(crate) fn register_action_client_core<
2044 const GOAL_BUF: usize,
2045 const RESULT_BUF: usize,
2046 const FEEDBACK_BUF: usize,
2047 >(
2048 &mut self,
2049 core: ActionClientCore<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>,
2050 goal_response_callback: Option<RawGoalResponseCallback>,
2051 feedback_callback: Option<RawFeedbackCallback>,
2052 result_callback: Option<RawResultCallback>,
2053 cancel_response_callback: Option<RawCancelResponseCallback>,
2054 context: *mut core::ffi::c_void,
2055 ) -> Result<ActionClientRawHandle, NodeError> {
2056 type Entry<const GB: usize, const RB: usize, const FB: usize> =
2057 ActionClientRawArenaEntry<GB, RB, FB>;
2058
2059 let slot = self.next_entry_slot()?;
2060 let offset = self.arena_alloc::<Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>>()?;
2061
2062 unsafe {
2063 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
2064 let entry_ptr = arena_ptr.add(offset) as *mut Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>;
2065 core::ptr::write(
2066 entry_ptr,
2067 Entry {
2068 core,
2069 goal_response_callback,
2070 feedback_callback,
2071 result_callback,
2072 cancel_response_callback,
2073 context,
2074 },
2075 );
2076 }
2077
2078 let meta = CallbackMeta {
2079 offset,
2080 kind: EntryKind::ActionClient,
2081 arena_len: super::arena::ArenaLen::UNRECORDED,
2082 has_data: always_ready,
2083 pre_sample: no_pre_sample,
2084 invocation: InvocationMode::Always,
2085 try_process: action_client_raw_try_process::<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>,
2086 drop_fn: drop_entry::<Entry<GOAL_BUF, RESULT_BUF, FEEDBACK_BUF>>,
2087 };
2088 self.emplace_entry(slot, meta, TraceName::Slot("action_client", slot));
2089
2090 Ok(ActionClientRawHandle { entry_index: slot })
2091 }
2092}
2093
2094pub struct ActionClientRawHandle {
2099 entry_index: usize,
2100}
2101
2102impl ActionClientRawHandle {
2103 pub(crate) fn entry_index(&self) -> usize {
2105 self.entry_index
2106 }
2107}
2108
2109#[cfg(all(test, feature = "alloc", not(feature = "rmw-cffi")))]
2116mod terminal_verb_tests {
2117 use super::*;
2118 use core::sync::atomic::{AtomicI8, Ordering};
2119 use nros_core::{CdrReader, CdrWriter, DeserError, Deserialize, SerError, Serialize};
2120 use nros_rmw::TransportError;
2121
2122 use crate::mock::MockSession;
2123
2124 static LAST_STATUS: AtomicI8 = AtomicI8::new(-1);
2128
2129 #[derive(Default)]
2130 struct Unit;
2131
2132 impl Serialize for Unit {
2133 fn serialize(&self, _w: &mut CdrWriter) -> Result<(), SerError> {
2134 Ok(())
2135 }
2136 }
2137 impl Deserialize for Unit {
2138 fn deserialize(_r: &mut CdrReader) -> Result<Self, DeserError> {
2139 Ok(Self)
2140 }
2141 }
2142 impl nros_core::RosMessage for Unit {
2143 const TYPE_NAME: &'static str = "test/action/Verbs_Unit";
2144 const TYPE_HASH: &'static str = "test_hash";
2145 }
2146
2147 struct VerbAction;
2148
2149 impl RosAction for VerbAction {
2150 type Goal = Unit;
2151 type Result = Unit;
2152 type Feedback = Unit;
2153 type SendGoalRequest = Unit;
2154 type SendGoalResponse = Unit;
2155 type GetResultRequest = Unit;
2156 type GetResultResponse = Unit;
2157 type FeedbackMessage = Unit;
2158 const ACTION_NAME: &'static str = "test/action/dds_/Verbs_";
2159 const ACTION_HASH: &'static str = "test_hash";
2160 }
2161
2162 unsafe fn record_status(
2163 _data: *mut u8,
2164 _goal_id: &nros_core::GoalUuid,
2165 status: nros_core::GoalStatusCode,
2166 _result: Unit,
2167 ) -> Result<(), NodeError> {
2168 LAST_STATUS.store(status as i8, Ordering::SeqCst);
2169 Ok(())
2170 }
2171
2172 unsafe fn unused_feedback(
2173 _data: *mut u8,
2174 _goal_id: &nros_core::GoalUuid,
2175 _fb: &Unit,
2176 ) -> Result<(), NodeError> {
2177 unreachable!("the terminal-verb test never publishes feedback")
2178 }
2179
2180 unsafe fn unused_set_status(
2181 _data: *mut u8,
2182 _goal_id: &nros_core::GoalUuid,
2183 _status: nros_core::GoalStatusCode,
2184 ) {
2185 unreachable!("the terminal-verb test never sets a non-terminal status")
2186 }
2187
2188 unsafe fn unused_count(_data: *const u8) -> usize {
2189 unreachable!("the terminal-verb test never counts active goals")
2190 }
2191
2192 unsafe fn unused_for_each(_data: *const u8, _f: &mut dyn FnMut(&ActiveGoal<VerbAction>)) {
2193 unreachable!("the terminal-verb test never iterates active goals")
2194 }
2195
2196 unsafe fn unused_goal_exists(_data: *const u8, _goal_id: &nros_core::GoalUuid) -> bool {
2197 unreachable!("the terminal-verb test never asks whether a goal exists")
2198 }
2199
2200 unsafe fn unused_expire_goals(_data: *mut u8) -> usize {
2201 unreachable!("the terminal-verb test never expires goals")
2202 }
2203
2204 unsafe fn never_processes(
2205 _data: *mut u8,
2206 _delta_us: u64,
2207 _slot: u8,
2208 ) -> Result<bool, TransportError> {
2209 Ok(false)
2210 }
2211
2212 unsafe fn drops_nothing(_data: *mut u8) {}
2213
2214 fn handle() -> ActionServerHandle<VerbAction> {
2215 ActionServerHandle {
2216 entry_index: 0,
2217 publish_feedback_fn: unused_feedback,
2218 complete_goal_fn: record_status,
2219 set_goal_status_fn: unused_set_status,
2220 active_goal_count_fn: unused_count,
2221 goal_exists_fn: unused_goal_exists,
2222 expire_goals_fn: unused_expire_goals,
2223 for_each_active_goal_fn: unused_for_each,
2224 _phantom: PhantomData,
2225 }
2226 }
2227
2228 fn meta() -> CallbackMeta {
2229 CallbackMeta {
2230 offset: 0,
2231 kind: EntryKind::ActionServer,
2232 arena_len: crate::executor::arena::ArenaLen::UNRECORDED,
2233 try_process: never_processes,
2234 has_data: always_ready,
2235 pre_sample: no_pre_sample,
2236 invocation: InvocationMode::OnNewData,
2237 drop_fn: drops_nothing,
2238 }
2239 }
2240
2241 #[test]
2250 fn terminal_verbs_carry_their_spec_state_and_cancel_forwards_to_canceled() {
2251 let mut executor = Executor::from_session(MockSession::new());
2252 executor.entries[0] = Some(meta());
2253
2254 let h = handle();
2255 let goal = nros_core::GoalUuid::zero();
2256
2257 LAST_STATUS.store(-1, Ordering::SeqCst);
2258 h.succeed(&mut executor, &goal, Unit).unwrap();
2259 assert_eq!(
2260 LAST_STATUS.load(Ordering::SeqCst),
2261 nros_core::GoalStatusCode::Succeeded as i8,
2262 "succeed() must terminate the goal as SUCCEEDED"
2263 );
2264
2265 LAST_STATUS.store(-1, Ordering::SeqCst);
2266 h.abort(&mut executor, &goal, Unit).unwrap();
2267 assert_eq!(
2268 LAST_STATUS.load(Ordering::SeqCst),
2269 nros_core::GoalStatusCode::Aborted as i8,
2270 "abort() must terminate the goal as ABORTED"
2271 );
2272
2273 LAST_STATUS.store(-1, Ordering::SeqCst);
2277 h.canceled(&mut executor, &goal, Unit).unwrap();
2278 assert_eq!(
2279 LAST_STATUS.load(Ordering::SeqCst),
2280 nros_core::GoalStatusCode::Canceled as i8,
2281 "canceled() must terminate the goal as CANCELED"
2282 );
2283
2284 executor.entries[0] = None;
2287 }
2288}