@@ -71,14 +71,18 @@ pub type AcceptedCallback<ActionT> = dyn Fn(ServerGoalHandle<ActionT>) + 'static
71
71
72
72
pub struct ActionServer < ActionT >
73
73
where
74
- ActionT : rosidl_runtime_rs:: Action ,
74
+ ActionT : rosidl_runtime_rs:: Action + rosidl_runtime_rs :: ActionImpl ,
75
75
{
76
76
pub ( crate ) handle : Arc < ActionServerHandle > ,
77
77
num_entities : WaitableNumEntities ,
78
78
goal_callback : Box < GoalCallback < ActionT > > ,
79
79
cancel_callback : Box < CancelCallback < ActionT > > ,
80
80
accepted_callback : Box < AcceptedCallback < ActionT > > ,
81
+ // TODO(nwn): Audit these three mutexes to ensure there's no deadlocks or broken invariants. We
82
+ // may want to join them behind a shared mutex, at least for the `goal_results` and `result_requests`.
81
83
goal_handles : Mutex < HashMap < GoalUuid , Arc < ServerGoalHandle < ActionT > > > > ,
84
+ goal_results : Mutex < HashMap < GoalUuid , <<ActionT :: GetResultService as Service >:: Response as Message >:: RmwMsg > > ,
85
+ result_requests : Mutex < HashMap < GoalUuid , Vec < rmw_request_id_t > > > ,
82
86
}
83
87
84
88
impl < T > ActionServer < T >
@@ -160,6 +164,8 @@ where
160
164
cancel_callback : Box :: new ( cancel_callback) ,
161
165
accepted_callback : Box :: new ( accepted_callback) ,
162
166
goal_handles : Mutex :: new ( HashMap :: new ( ) ) ,
167
+ goal_results : Mutex :: new ( HashMap :: new ( ) ) ,
168
+ result_requests : Mutex :: new ( HashMap :: new ( ) ) ,
163
169
} )
164
170
}
165
171
@@ -172,7 +178,9 @@ where
172
178
let mut request_rmw = RmwRequest :: < T > :: default ( ) ;
173
179
let handle = & * self . handle . lock ( ) ;
174
180
unsafe {
175
- // SAFETY: The three pointers are valid/initialized
181
+ // SAFETY: The action server is locked by the handle. The request_id is a
182
+ // zero-initialized rmw_request_id_t, and the request_rmw is a default-initialized
183
+ // SendGoalService request message.
176
184
rcl_action_take_goal_request (
177
185
handle,
178
186
& mut request_id,
@@ -445,8 +453,104 @@ where
445
453
Ok ( ( ) )
446
454
}
447
455
456
+ fn take_result_request ( & self ) -> Result < ( <<T :: GetResultService as Service >:: Request as Message >:: RmwMsg , rmw_request_id_t ) , RclrsError > {
457
+ let mut request_id = rmw_request_id_t {
458
+ writer_guid : [ 0 ; 16 ] ,
459
+ sequence_number : 0 ,
460
+ } ;
461
+ type RmwRequest < T > = <<<T as ActionImpl >:: GetResultService as Service >:: Request as Message >:: RmwMsg ;
462
+ let mut request_rmw = RmwRequest :: < T > :: default ( ) ;
463
+ let handle = & * self . handle . lock ( ) ;
464
+ unsafe {
465
+ // SAFETY: The action server is locked by the handle. The request_id is a
466
+ // zero-initialized rmw_request_id_t, and the request_rmw is a default-initialized
467
+ // GetResultService request message.
468
+ rcl_action_take_result_request (
469
+ handle,
470
+ & mut request_id,
471
+ & mut request_rmw as * mut RmwRequest < T > as * mut _ ,
472
+ )
473
+ }
474
+ . ok ( ) ?;
475
+
476
+ Ok ( ( request_rmw, request_id) )
477
+ }
478
+
479
+ fn send_result_response (
480
+ & self ,
481
+ mut request_id : rmw_request_id_t ,
482
+ response_rmw : & mut <<<T as ActionImpl >:: GetResultService as rosidl_runtime_rs:: Service >:: Response as Message >:: RmwMsg ,
483
+ ) -> Result < ( ) , RclrsError > {
484
+ let handle = & * self . handle . lock ( ) ;
485
+ let result = unsafe {
486
+ // SAFETY: The action server handle is locked and so synchronized with other functions.
487
+ // The request_id and response are both uniquely owned or borrowed, and so neither will
488
+ // mutate during this function call.
489
+ rcl_action_send_result_response (
490
+ handle,
491
+ & mut request_id,
492
+ response_rmw as * mut _ as * mut _ ,
493
+ )
494
+ }
495
+ . ok ( ) ;
496
+ match result {
497
+ Ok ( ( ) ) => Ok ( ( ) ) ,
498
+ Err ( RclrsError :: RclError {
499
+ code : RclReturnCode :: Timeout ,
500
+ ..
501
+ } ) => {
502
+ // TODO(nwn): Log an error and continue.
503
+ // (See https://github.com/ros2/rclcpp/pull/2215 for reasoning.)
504
+ Ok ( ( ) )
505
+ }
506
+ _ => result,
507
+ }
508
+ }
509
+
448
510
fn execute_result_request ( & self ) -> Result < ( ) , RclrsError > {
449
- todo ! ( )
511
+ let ( request, request_id) = match self . take_result_request ( ) {
512
+ Ok ( res) => res,
513
+ Err ( RclrsError :: RclError {
514
+ code : RclReturnCode :: ServiceTakeFailed ,
515
+ ..
516
+ } ) => {
517
+ // Spurious wakeup – this may happen even when a waitset indicated that this
518
+ // action was ready, so it shouldn't be an error.
519
+ return Ok ( ( ) ) ;
520
+ }
521
+ Err ( err) => return Err ( err) ,
522
+ } ;
523
+
524
+ let uuid = GoalUuid ( <T as ActionImpl >:: get_result_request_uuid ( & request) ) ;
525
+
526
+ let goal_exists = unsafe {
527
+ // SAFETY: No preconditions
528
+ let mut goal_info = rcl_action_get_zero_initialized_goal_info ( ) ;
529
+ goal_info. goal_id . uuid = uuid. 0 ;
530
+
531
+ // SAFETY: The action server is locked through the handle. The `goal_info`
532
+ // argument points to a rcl_action_goal_info_t with the desired UUID.
533
+ rcl_action_server_goal_exists ( & * self . handle . lock ( ) , & goal_info)
534
+ } ;
535
+
536
+ if goal_exists {
537
+ if let Some ( result) = self . goal_results . lock ( ) . unwrap ( ) . get_mut ( & uuid) {
538
+ // Respond immediately if the goal already has a response.
539
+ self . send_result_response ( request_id, result) ?;
540
+ } else {
541
+ // Queue up the request for a response once the goal terminates.
542
+ self . result_requests . lock ( ) . unwrap ( ) . entry ( uuid) . or_insert ( vec ! [ ] ) . push ( request_id) ;
543
+ }
544
+ } else {
545
+ type RmwResponse < T > = <<<T as ActionImpl >:: GetResultService as rosidl_runtime_rs:: Service >:: Response as Message >:: RmwMsg ;
546
+ let mut response_rmw = RmwResponse :: < T > :: default ( ) ;
547
+ // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_UNKNOWN in the rcl
548
+ // bindings.
549
+ <T as ActionImpl >:: set_result_response_status ( & mut response_rmw, 0 ) ;
550
+ self . send_result_response ( request_id, & mut response_rmw) ?;
551
+ }
552
+
553
+ Ok ( ( ) )
450
554
}
451
555
452
556
fn execute_goal_expired ( & self ) -> Result < ( ) , RclrsError > {
0 commit comments