r2r/
action_servers.rs

Help
1use futures::{
2    channel::{mpsc, oneshot},
3    future::{join_all, FutureExt, JoinAll},
4    stream::Stream,
5};
6use std::{
7    collections::HashMap,
8    ffi::CString,
9    mem::MaybeUninit,
10    sync::{Arc, Mutex, Weak},
11};
12
13use crate::{
14    error::*,
15    msg_types::{
16        generated_msgs::{action_msgs, builtin_interfaces, unique_identifier_msgs},
17        *,
18    },
19};
20use r2r_actions::*;
21use r2r_rcl::*;
22
23pub trait ActionServer_ {
24    fn handle(&self) -> &rcl_action_server_t;
25    fn handle_mut(&mut self) -> &mut rcl_action_server_t;
26    fn handle_goal_request(&mut self, server: Arc<Mutex<dyn ActionServer_>>);
27    fn send_completed_cancel_requests(&mut self);
28    fn handle_cancel_request(&mut self);
29    fn handle_result_request(&mut self);
30    fn handle_goal_expired(&mut self);
31    fn publish_status(&self);
32    fn set_goal_state(
33        &mut self, uuid: &uuid::Uuid, new_state: rcl_action_goal_event_t,
34    ) -> Result<()>;
35    fn add_result(&mut self, uuid: uuid::Uuid, msg: Box<dyn VoidPtr>);
36    fn cancel_goal(&mut self, uuid: &uuid::Uuid);
37    fn is_cancelling(&self, uuid: &uuid::Uuid) -> Result<bool>;
38    fn add_goal_handle(&mut self, uuid: uuid::Uuid, goal_handle: *mut rcl_action_goal_handle_t);
39    fn destroy(&mut self, node: &mut rcl_node_t);
40}
41
42/// Request to cancel an active goal.
43pub struct ActionServerCancelRequest {
44    pub uuid: uuid::Uuid,
45    response_sender: oneshot::Sender<(uuid::Uuid, bool)>,
46}
47
48impl ActionServerCancelRequest {
49    /// Accepts the cancel request. The action server should now cancel the corresponding goal.
50    pub fn accept(self) {
51        if self.response_sender.send((self.uuid, true)).is_err() {
52            log::error!("warning: could not send goal canellation accept msg")
53        }
54    }
55    /// Rejects the cancel request.
56    pub fn reject(self) {
57        if self.response_sender.send((self.uuid, false)).is_err() {
58            log::error!("warning: could not send goal cancellation rejection")
59        }
60    }
61}
62
63/// Request to the action server to accept a new `Goal`.
64pub struct ActionServerGoalRequest<T>
65where
66    T: WrappedActionTypeSupport,
67{
68    pub uuid: uuid::Uuid,
69    pub goal: T::Goal,
70    cancel_requests: mpsc::Receiver<ActionServerCancelRequest>,
71    server: Weak<Mutex<dyn ActionServer_>>,
72    request_id: rmw_request_id_t,
73}
74
75unsafe impl<T> Send for ActionServerGoalRequest<T> where T: WrappedActionTypeSupport {}
76
77impl<T: 'static> ActionServerGoalRequest<T>
78where
79    T: WrappedActionTypeSupport,
80{
81    /// Accept the goal request and become a ServerGoal.
82    /// Returns a handle to the goal and a stream on which cancel requests can be received.
83    pub fn accept(
84        mut self,
85    ) -> Result<(ActionServerGoal<T>, impl Stream<Item = ActionServerCancelRequest> + Unpin)> {
86        let uuid_msg = unique_identifier_msgs::msg::UUID {
87            uuid: self.uuid.as_bytes().to_vec(),
88        };
89        let time = builtin_interfaces::msg::Time::default();
90        let goal_info = action_msgs::msg::GoalInfo {
91            goal_id: uuid_msg,
92            stamp: time.clone(),
93        };
94        let native_goal_info = WrappedNativeMsg::<action_msgs::msg::GoalInfo>::from(&goal_info);
95
96        let server = self.server.upgrade().unwrap(); // todo fixme
97        let mut server = server.lock().unwrap();
98
99        let goal_handle: *mut rcl_action_goal_handle_t =
100            unsafe { rcl_action_accept_new_goal(server.handle_mut(), &*native_goal_info) };
101
102        // send response
103        let response_msg = T::make_goal_response_msg(true, time);
104        let mut response_msg = WrappedNativeMsg::<
105            <<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Response,
106        >::from(&response_msg);
107
108        let ret = unsafe {
109            rcl_action_send_goal_response(
110                server.handle_mut(),
111                &mut self.request_id,
112                response_msg.void_ptr_mut(),
113            )
114        };
115        if ret != RCL_RET_OK as i32 {
116            return Err(Error::from_rcl_error(ret));
117        }
118
119        unsafe {
120            rcl_action_update_goal_state(goal_handle, rcl_action_goal_event_t::GOAL_EVENT_EXECUTE);
121        }
122
123        server.publish_status();
124
125        let g = ActionServerGoal {
126            uuid: self.uuid,
127            goal: self.goal,
128            server: self.server,
129        };
130
131        // server.goals.insert(g.uuid.clone(), goal_handle);
132        server.add_goal_handle(g.uuid, goal_handle);
133
134        Ok((g, self.cancel_requests))
135    }
136
137    /// reject the goal request and be consumed in the process
138    pub fn reject(mut self) -> Result<()> {
139        let time = builtin_interfaces::msg::Time::default();
140        let server = self.server.upgrade().unwrap(); // todo fixme
141        let mut server = server.lock().unwrap();
142
143        let response_msg = T::make_goal_response_msg(false, time);
144        let mut response_msg = WrappedNativeMsg::<
145            <<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Response,
146        >::from(&response_msg);
147
148        let ret = unsafe {
149            rcl_action_send_goal_response(
150                server.handle_mut(),
151                &mut self.request_id,
152                response_msg.void_ptr_mut(),
153            )
154        };
155        if ret != RCL_RET_OK as i32 {
156            return Err(Error::from_rcl_error(ret));
157        }
158
159        Ok(())
160    }
161}
162pub type ActiveCancelRequest = (
163    rmw_request_id_t,
164    action_msgs::srv::CancelGoal::Response,
165    JoinAll<oneshot::Receiver<(uuid::Uuid, bool)>>,
166);
167
168pub struct WrappedActionServer<T>
169where
170    T: WrappedActionTypeSupport,
171{
172    pub rcl_handle: rcl_action_server_t,
173    pub clock_handle: Box<rcl_clock_t>,
174    pub goal_request_sender: mpsc::Sender<ActionServerGoalRequest<T>>,
175    pub cancel_senders: HashMap<uuid::Uuid, mpsc::Sender<ActionServerCancelRequest>>,
176    pub active_cancel_requests: Vec<ActiveCancelRequest>,
177    pub goals: HashMap<uuid::Uuid, *mut rcl_action_goal_handle_t>,
178    pub result_msgs: HashMap<uuid::Uuid, Box<dyn VoidPtr>>,
179    pub result_requests: HashMap<uuid::Uuid, Vec<rmw_request_id_t>>,
180}
181
182impl<T: 'static> ActionServer_ for WrappedActionServer<T>
183where
184    T: WrappedActionTypeSupport,
185{
186    fn handle(&self) -> &rcl_action_server_t {
187        &self.rcl_handle
188    }
189
190    fn handle_mut(&mut self) -> &mut rcl_action_server_t {
191        &mut self.rcl_handle
192    }
193
194    fn is_cancelling(&self, uuid: &uuid::Uuid) -> Result<bool> {
195        if let Some(handle) = self.goals.get(uuid) {
196            let mut state = action_msgs::msg::GoalStatus::STATUS_UNKNOWN as u8;
197            let ret = unsafe { rcl_action_goal_handle_get_status(*handle, &mut state) };
198
199            if ret != RCL_RET_OK as i32 {
200                log::debug!("action server: Failed to get goal handle state: {}", ret);
201                return Err(Error::from_rcl_error(ret));
202            }
203            return Ok(state == action_msgs::msg::GoalStatus::STATUS_CANCELING as u8);
204        }
205        Err(Error::RCL_RET_ACTION_GOAL_HANDLE_INVALID)
206    }
207
208    fn cancel_goal(&mut self, uuid: &uuid::Uuid) {
209        if let Some(handle) = self.goals.remove(uuid) {
210            let ret = unsafe {
211                rcl_action_update_goal_state(
212                    handle,
213                    rcl_action_goal_event_t::GOAL_EVENT_CANCEL_GOAL,
214                )
215            };
216
217            if ret != RCL_RET_OK as i32 {
218                log::debug!("action server: could not cancel goal: {}", Error::from_rcl_error(ret));
219            }
220        }
221    }
222
223    fn set_goal_state(
224        &mut self, uuid: &uuid::Uuid, new_state: rcl_action_goal_event_t,
225    ) -> Result<()> {
226        let goal_info = action_msgs::msg::GoalInfo {
227            goal_id: unique_identifier_msgs::msg::UUID {
228                uuid: uuid.as_bytes().to_vec(),
229            },
230            ..action_msgs::msg::GoalInfo::default()
231        };
232        let goal_info_native = WrappedNativeMsg::<action_msgs::msg::GoalInfo>::from(&goal_info);
233
234        // does this goal exist?
235        let goal_exists =
236            unsafe { rcl_action_server_goal_exists(self.handle(), &*goal_info_native) };
237
238        if !goal_exists {
239            log::debug!("tried to publish result without a goal");
240            return Err(Error::RCL_RET_ACTION_GOAL_HANDLE_INVALID);
241        }
242
243        if let Some(handle) = self.goals.get(uuid) {
244            // todo: error handling
245            unsafe {
246                rcl_action_update_goal_state(*handle, new_state);
247            }
248
249            // todo: error handling
250            unsafe {
251                rcl_action_notify_goal_done(self.handle());
252            }
253
254            // send out updated statues
255            self.publish_status();
256
257            Ok(())
258        } else {
259            Err(Error::RCL_RET_ACTION_GOAL_HANDLE_INVALID)
260        }
261    }
262
263    fn send_completed_cancel_requests(&mut self) {
264        let mut canceled = vec![];
265        let mut responses = vec![];
266        self.active_cancel_requests
267            .retain_mut(|(request_id, msg, fut)| {
268                let boxed = fut.boxed();
269                if let Some(results) = boxed.now_or_never() {
270                    let mut response_msg = msg.clone();
271                    let requested_cancels = response_msg.goals_canceling.len();
272                    for r in results {
273                        match r {
274                            Ok((uuid, do_cancel)) => {
275                                // cancel goal and filter response msg.
276                                if do_cancel {
277                                    canceled.push(uuid);
278                                }
279
280                                response_msg.goals_canceling.retain(|goal_info| {
281                                    let msg_uuid = uuid_msg_to_uuid(&goal_info.goal_id);
282                                    do_cancel && msg_uuid == uuid
283                                });
284                            }
285                            Err(oneshot::Canceled) => {
286                                log::error!("Warning, cancel request not handled!");
287                                return false; // skip this request.
288                            }
289                        }
290                    }
291
292                    // check if all cancels were rejected.
293                    if requested_cancels >= 1 && response_msg.goals_canceling.is_empty() {
294                        response_msg.return_code =
295                            action_msgs::srv::CancelGoal::Response::ERROR_REJECTED as i8;
296                    }
297
298                    responses.push((*request_id, response_msg));
299
300                    false
301                } else {
302                    true
303                }
304            });
305
306        canceled.iter().for_each(|uuid| self.cancel_goal(uuid));
307        if !canceled.is_empty() {
308            // at least one goal state changed, publish a new status message
309            self.publish_status();
310        }
311
312        // send out responses
313        for (mut request_id, response_msg) in responses {
314            // send out response msg.
315            let mut native_msg =
316                WrappedNativeMsg::<action_msgs::srv::CancelGoal::Response>::from(&response_msg);
317            let ret = unsafe {
318                rcl_action_send_cancel_response(
319                    &self.rcl_handle,
320                    &mut request_id,
321                    native_msg.void_ptr_mut(),
322                )
323            };
324
325            if ret != RCL_RET_OK as i32 {
326                log::debug!("action server: could send cancel response. {}", ret);
327            }
328        }
329    }
330
331    fn handle_goal_request(&mut self, server: Arc<Mutex<dyn ActionServer_>>) {
332        let mut request_id = MaybeUninit::<rmw_request_id_t>::uninit();
333        let mut request_msg = WrappedNativeMsg::<
334            <<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Request,
335        >::new();
336        let ret = unsafe {
337            rcl_action_take_goal_request(
338                &self.rcl_handle,
339                request_id.as_mut_ptr(),
340                request_msg.void_ptr_mut(),
341            )
342        };
343
344        if ret != RCL_RET_OK as i32 {
345            // this seems normal if client dies.
346            return;
347        }
348        let msg = <<<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Request>::from_native(&request_msg);
349        let (uuid_msg, goal) = T::destructure_goal_request_msg(msg);
350        let uuid = uuid_msg_to_uuid(&uuid_msg);
351
352        let (cancel_sender, cancel_receiver) = mpsc::channel::<ActionServerCancelRequest>(10);
353        self.cancel_senders.insert(uuid, cancel_sender);
354
355        let gr: ActionServerGoalRequest<T> = ActionServerGoalRequest {
356            uuid,
357            goal,
358            cancel_requests: cancel_receiver,
359            server: Arc::downgrade(&server),
360            request_id: unsafe { request_id.assume_init() },
361        };
362
363        // send out request.
364        if let Err(e) = self.goal_request_sender.try_send(gr) {
365            log::error!("warning: could not send service request ({})", e)
366        }
367    }
368
369    fn handle_cancel_request(&mut self) {
370        let mut request_id = MaybeUninit::<rmw_request_id_t>::uninit();
371        let mut request_msg = WrappedNativeMsg::<action_msgs::srv::CancelGoal::Request>::new();
372        let ret = unsafe {
373            rcl_action_take_cancel_request(
374                &self.rcl_handle,
375                request_id.as_mut_ptr(),
376                request_msg.void_ptr_mut(),
377            )
378        };
379
380        let request_id = unsafe { request_id.assume_init() };
381
382        if ret != RCL_RET_OK as i32 {
383            // this seems normal if client dies.
384            return;
385        }
386
387        let mut cancel_response = unsafe { rcl_action_get_zero_initialized_cancel_response() };
388        let ret = unsafe {
389            rcl_action_process_cancel_request(&self.rcl_handle, &*request_msg, &mut cancel_response)
390        };
391
392        if ret != RCL_RET_OK as i32 {
393            log::debug!("action server: could not process cancel request. {}", ret);
394            return;
395        }
396
397        let response_msg =
398            action_msgs::srv::CancelGoal::Response::from_native(&cancel_response.msg);
399
400        let return_channels = response_msg
401            .goals_canceling
402            .iter()
403            .flat_map(|goal_info| {
404                let uuid = uuid_msg_to_uuid(&goal_info.goal_id);
405                self.cancel_senders
406                    .get_mut(&uuid)
407                    .and_then(|cancel_sender| {
408                        let (s, r) = oneshot::channel::<(uuid::Uuid, bool)>();
409                        let cr = ActionServerCancelRequest {
410                            uuid,
411                            response_sender: s,
412                        };
413                        match cancel_sender.try_send(cr) {
414                            Err(_) => {
415                                log::error!("warning: could not send goal cancellation request");
416                                None
417                            }
418                            _ => Some(r),
419                        }
420                    })
421            })
422            .collect::<Vec<_>>();
423
424        // because we want to reply to the caller when all goals have been either accepted or rejected,
425        // join the channels into one future that we can poll during spin.
426        self.active_cancel_requests
427            .push((request_id, response_msg, join_all(return_channels)));
428    }
429
430    fn handle_goal_expired(&mut self) {
431        let mut goal_info = WrappedNativeMsg::<action_msgs::msg::GoalInfo>::new();
432        let mut num_expired = 1;
433
434        while num_expired > 1 {
435            let ret = unsafe {
436                rcl_action_expire_goals(&self.rcl_handle, &mut *goal_info, 1, &mut num_expired)
437            };
438            if ret != RCL_RET_OK as i32 {
439                log::debug!("action server: could not expire goal.");
440                return;
441            }
442            let gi = action_msgs::msg::GoalInfo::from_native(&goal_info);
443            let uuid = uuid_msg_to_uuid(&gi.goal_id);
444            log::debug!("goal expired: {} - {}", uuid, num_expired);
445            // todo
446            // self.goals.remove(&uuid);
447            self.result_msgs.remove(&uuid);
448            self.result_requests.remove(&uuid);
449        }
450    }
451
452    fn publish_status(&self) {
453        unsafe {
454            let mut status = rcl_action_get_zero_initialized_goal_status_array();
455            let ret = rcl_action_get_goal_status_array(&self.rcl_handle, &mut status);
456            if ret != RCL_RET_OK as i32 {
457                log::debug!(
458                    "action server: failed to get goal status array: {}",
459                    Error::from_rcl_error(ret)
460                );
461                return;
462            }
463            let ret = rcl_action_publish_status(
464                &self.rcl_handle,
465                &status as *const _ as *const std::os::raw::c_void,
466            );
467            if ret != RCL_RET_OK as i32 {
468                log::debug!(
469                    "action server: failed to publish status: {}",
470                    Error::from_rcl_error(ret)
471                );
472                return;
473            }
474            rcl_action_goal_status_array_fini(&mut status);
475        }
476    }
477
478    fn add_goal_handle(&mut self, uuid: uuid::Uuid, goal_handle: *mut rcl_action_goal_handle_t) {
479        self.goals.insert(uuid, goal_handle);
480    }
481
482    // bit of a hack...
483    fn add_result(&mut self, uuid: uuid::Uuid, mut msg: Box<dyn VoidPtr>) {
484        // if there are already requests for this goal, send the result immediately.
485        if let Some(rr) = self.result_requests.remove(&uuid) {
486            for mut req in rr {
487                let ret = unsafe {
488                    rcl_action_send_result_response(&self.rcl_handle, &mut req, msg.void_ptr_mut())
489                };
490                if ret != RCL_RET_OK as i32 {
491                    log::debug!(
492                        "action server: could send result request response. {}",
493                        Error::from_rcl_error(ret)
494                    );
495                }
496            }
497        }
498        self.result_msgs.insert(uuid, msg);
499    }
500
501    fn handle_result_request(&mut self) {
502        let mut request_id = MaybeUninit::<rmw_request_id_t>::uninit();
503        let mut request_msg = WrappedNativeMsg::<
504            <<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Request,
505        >::new();
506        let ret = unsafe {
507            rcl_action_take_result_request(
508                &self.rcl_handle,
509                request_id.as_mut_ptr(),
510                request_msg.void_ptr_mut(),
511            )
512        };
513
514        if ret != RCL_RET_OK as i32 {
515            // this seems normal if client dies.
516            return;
517        }
518
519        let msg = <<<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Request>::from_native(&request_msg);
520        let goal_info = action_msgs::msg::GoalInfo {
521            goal_id: T::destructure_result_request_msg(msg),
522            ..action_msgs::msg::GoalInfo::default()
523        };
524        let goal_info_native = WrappedNativeMsg::<action_msgs::msg::GoalInfo>::from(&goal_info);
525
526        // does this goal exist?
527        let goal_exists =
528            unsafe { rcl_action_server_goal_exists(&self.rcl_handle, &*goal_info_native) };
529
530        let uuid = uuid_msg_to_uuid(&goal_info.goal_id);
531
532        let response_msg = if !goal_exists {
533            // Goal does not exists
534            let msg = T::make_result_response_msg(
535                action_msgs::msg::GoalStatus::STATUS_UNKNOWN as i8,
536                T::Result::default(),
537            );
538            let mut response_msg = WrappedNativeMsg::<
539                <<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response,
540            >::from(&msg);
541            Some(response_msg.void_ptr_mut())
542        } else {
543            self.result_msgs
544                .get_mut(&uuid)
545                .map(|msg| msg.void_ptr_mut())
546        };
547
548        let mut request_id = unsafe { request_id.assume_init() };
549        if let Some(response_msg) = response_msg {
550            let ret = unsafe {
551                rcl_action_send_result_response(&self.rcl_handle, &mut request_id, response_msg)
552            };
553
554            if ret != RCL_RET_OK as i32 {
555                log::debug!(
556                    "action server: could send result request response. {}",
557                    Error::from_rcl_error(ret)
558                );
559            }
560        } else {
561            // keep request for later when result comes in
562            // todo: add logic that replies to the requests
563            self.result_requests
564                .entry(uuid)
565                .or_default()
566                .push(request_id);
567        }
568    }
569
570    fn destroy(&mut self, node: &mut rcl_node_t) {
571        unsafe {
572            rcl_action_server_fini(&mut self.rcl_handle, node);
573            rcl_ros_clock_fini(self.clock_handle.as_mut());
574        }
575    }
576}
577
578/// A handle to an active `Goal`
579#[derive(Clone)]
580pub struct ActionServerGoal<T>
581where
582    T: WrappedActionTypeSupport,
583{
584    pub uuid: uuid::Uuid,
585    pub goal: T::Goal,
586    server: Weak<Mutex<dyn ActionServer_>>,
587}
588
589unsafe impl<T> Send for ActionServerGoal<T> where T: WrappedActionTypeSupport {}
590
591impl<T: 'static> ActionServerGoal<T>
592where
593    T: WrappedActionTypeSupport,
594{
595    pub fn is_cancelling(&self) -> Result<bool> {
596        let action_server = self
597            .server
598            .upgrade()
599            .ok_or(Error::RCL_RET_ACTION_SERVER_INVALID)?;
600
601        let action_server = action_server.lock().unwrap();
602        action_server.is_cancelling(&self.uuid)
603    }
604
605    pub fn publish_feedback(&self, msg: T::Feedback) -> Result<()>
606    where
607        T: WrappedActionTypeSupport,
608    {
609        // upgrade to actual ref. if still alive
610        let action_server = self
611            .server
612            .upgrade()
613            .ok_or(Error::RCL_RET_ACTION_SERVER_INVALID)?;
614
615        let uuid_msg = unique_identifier_msgs::msg::UUID {
616            uuid: self.uuid.as_bytes().to_vec(),
617        };
618        let feedback_msg = T::make_feedback_msg(uuid_msg, msg);
619        let mut native_msg = WrappedNativeMsg::<T::FeedbackMessage>::from(&feedback_msg);
620        let ret = unsafe {
621            rcl_action_publish_feedback(
622                action_server.lock().unwrap().handle(),
623                native_msg.void_ptr_mut(),
624            )
625        };
626
627        if ret != RCL_RET_OK as i32 {
628            log::error!("could not publish {}", Error::from_rcl_error(ret));
629        }
630        Ok(()) // todo: error codes
631    }
632
633    pub fn cancel(&mut self, msg: T::Result) -> Result<()> {
634        // upgrade to actual ref. if still alive
635        let action_server = self
636            .server
637            .upgrade()
638            .ok_or(Error::RCL_RET_ACTION_SERVER_INVALID)?;
639        let mut action_server = action_server.lock().unwrap();
640
641        // todo: check that the goal exists
642        let goal_info = action_msgs::msg::GoalInfo {
643            goal_id: unique_identifier_msgs::msg::UUID {
644                uuid: self.uuid.as_bytes().to_vec(),
645            },
646            ..action_msgs::msg::GoalInfo::default()
647        };
648        let goal_info_native = WrappedNativeMsg::<action_msgs::msg::GoalInfo>::from(&goal_info);
649
650        // does this goal exist?
651        let goal_exists =
652            unsafe { rcl_action_server_goal_exists(action_server.handle(), &*goal_info_native) };
653
654        if !goal_exists {
655            log::debug!("tried to publish result without a goal");
656            return Err(Error::RCL_RET_ACTION_GOAL_HANDLE_INVALID);
657        }
658
659        // todo: error handling
660        unsafe {
661            rcl_action_notify_goal_done(action_server.handle());
662        }
663
664        // send out updated statues
665        action_server.publish_status();
666
667        // create result message
668        let result_msg =
669            T::make_result_response_msg(action_msgs::msg::GoalStatus::STATUS_CANCELED as i8, msg);
670        let native_msg = WrappedNativeMsg::<
671            <<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response,
672        >::from(&result_msg);
673        action_server.add_result(self.uuid, Box::new(native_msg));
674
675        Ok(())
676    }
677
678    pub fn abort(&mut self, msg: T::Result) -> Result<()> {
679        // upgrade to actual ref. if still alive
680        let action_server = self
681            .server
682            .upgrade()
683            .ok_or(Error::RCL_RET_ACTION_SERVER_INVALID)?;
684        let mut action_server = action_server.lock().unwrap();
685
686        action_server.set_goal_state(&self.uuid, rcl_action_goal_event_t::GOAL_EVENT_ABORT)?;
687
688        // create result message
689        let result_msg =
690            T::make_result_response_msg(action_msgs::msg::GoalStatus::STATUS_ABORTED as i8, msg);
691        let native_msg = WrappedNativeMsg::<
692            <<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response,
693        >::from(&result_msg);
694        action_server.add_result(self.uuid, Box::new(native_msg));
695
696        Ok(())
697    }
698
699    pub fn succeed(&mut self, msg: T::Result) -> Result<()>
700    where
701        T: WrappedActionTypeSupport,
702    {
703        // upgrade to actual ref. if still alive
704        let action_server = self
705            .server
706            .upgrade()
707            .ok_or(Error::RCL_RET_ACTION_SERVER_INVALID)?;
708        let mut action_server = action_server.lock().unwrap();
709
710        action_server.set_goal_state(&self.uuid, rcl_action_goal_event_t::GOAL_EVENT_SUCCEED)?;
711
712        // create result message
713        let result_msg =
714            T::make_result_response_msg(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED as i8, msg);
715        let native_msg = WrappedNativeMsg::<
716            <<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response,
717        >::from(&result_msg);
718        action_server.add_result(self.uuid, Box::new(native_msg));
719
720        Ok(())
721    }
722}
723
724pub fn create_action_server_helper(
725    node: &mut rcl_node_t, action_name: &str, clock_handle: *mut rcl_clock_t,
726    action_ts: *const rosidl_action_type_support_t,
727) -> Result<rcl_action_server_t> {
728    let mut server_handle = unsafe { rcl_action_get_zero_initialized_server() };
729    let action_name_c_string =
730        CString::new(action_name).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?;
731
732    let result = unsafe {
733        let server_options = rcl_action_server_get_default_options();
734
735        rcl_action_server_init(
736            &mut server_handle,
737            node,
738            clock_handle,
739            action_ts,
740            action_name_c_string.as_ptr(),
741            &server_options,
742        )
743    };
744    if result == RCL_RET_OK as i32 {
745        Ok(server_handle)
746    } else {
747        Err(Error::from_rcl_error(result))
748    }
749}
750
751pub fn action_server_get_num_waits(
752    rcl_handle: &rcl_action_server_t, num_subs: &mut usize, num_gc: &mut usize,
753    num_timers: &mut usize, num_clients: &mut usize, num_services: &mut usize,
754) -> Result<()> {
755    unsafe {
756        let result = rcl_action_server_wait_set_get_num_entities(
757            rcl_handle,
758            num_subs,
759            num_gc,
760            num_timers,
761            num_clients,
762            num_services,
763        );
764        if result == RCL_RET_OK as i32 {
765            Ok(())
766        } else {
767            Err(Error::from_rcl_error(result))
768        }
769    }
770}
r2r/action_servers.rs

r2r/
action_servers.rs
