diff --git a/examples/minimal_action_client/Cargo.toml b/examples/minimal_action_client/Cargo.toml new file mode 100644 index 000000000..96275ba4e --- /dev/null +++ b/examples/minimal_action_client/Cargo.toml @@ -0,0 +1,22 @@ +[package] +name = "examples_rclrs_minimal_action_client" +version = "0.3.1" +# This project is not military-sponsored, Jacob's employment contract just requires him to use this email address +authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] +edition = "2021" + +[[bin]] +name = "minimal_action_client" +path = "src/minimal_action_client.rs" + +[dependencies] +anyhow = {version = "1", features = ["backtrace"]} + +[dependencies.rclrs] +version = "0.4" + +[dependencies.rosidl_runtime_rs] +version = "0.4" + +[dependencies.example_interfaces] +version = "*" diff --git a/examples/minimal_action_client/package.xml b/examples/minimal_action_client/package.xml new file mode 100644 index 000000000..a2576efb1 --- /dev/null +++ b/examples/minimal_action_client/package.xml @@ -0,0 +1,26 @@ + + + + examples_rclrs_minimal_action_client + 0.3.1 + Minimal action client examples for rclrs. + Esteve Fernandez + Nikolai Morin + + Jacob Hassold + Apache License 2.0 + + rclrs + rosidl_runtime_rs + example_interfaces + + rclrs + rosidl_runtime_rs + example_interfaces + + + ament_cargo + + diff --git a/examples/minimal_action_client/src/minimal_action_client.rs b/examples/minimal_action_client/src/minimal_action_client.rs new file mode 100644 index 000000000..e67c46656 --- /dev/null +++ b/examples/minimal_action_client/src/minimal_action_client.rs @@ -0,0 +1,14 @@ +use std::env; + +use anyhow::{Error, Result}; + +fn main() -> Result<(), Error> { + let context = rclrs::Context::new(env::args())?; + + let node = rclrs::create_node(&context, "minimal_client")?; + + let _client = + node.create_action_client::("fibonacci")?; + + Ok(()) +} diff --git a/examples/minimal_action_server/Cargo.toml b/examples/minimal_action_server/Cargo.toml new file mode 100644 index 000000000..3ec27526c --- /dev/null +++ b/examples/minimal_action_server/Cargo.toml @@ -0,0 +1,22 @@ +[package] +name = "examples_rclrs_minimal_action_server" +version = "0.3.1" +# This project is not military-sponsored, Jacob's employment contract just requires him to use this email address +authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] +edition = "2021" + +[[bin]] +name = "minimal_action_server" +path = "src/minimal_action_server.rs" + +[dependencies] +anyhow = {version = "1", features = ["backtrace"]} + +[dependencies.rclrs] +version = "0.4" + +[dependencies.rosidl_runtime_rs] +version = "0.4" + +[dependencies.example_interfaces] +version = "*" diff --git a/examples/minimal_action_server/package.xml b/examples/minimal_action_server/package.xml new file mode 100644 index 000000000..74ae8e40e --- /dev/null +++ b/examples/minimal_action_server/package.xml @@ -0,0 +1,26 @@ + + + + examples_rclrs_minimal_action_server + 0.3.1 + Minimal action server examples for rclrs. + Esteve Fernandez + Nikolai Morin + + Jacob Hassold + Apache License 2.0 + + rclrs + rosidl_runtime_rs + example_interfaces + + rclrs + rosidl_runtime_rs + example_interfaces + + + ament_cargo + + diff --git a/examples/minimal_action_server/src/minimal_action_server.rs b/examples/minimal_action_server/src/minimal_action_server.rs new file mode 100644 index 000000000..a87eb1f9f --- /dev/null +++ b/examples/minimal_action_server/src/minimal_action_server.rs @@ -0,0 +1,79 @@ +use std::{env, sync::Arc, thread}; + +use anyhow::{Error, Result}; + +type Fibonacci = example_interfaces::action::Fibonacci; +type GoalHandleFibonacci = rclrs::ServerGoalHandle; + +fn handle_goal( + _uuid: &rclrs::GoalUUID, + goal: Arc, +) -> rclrs::GoalResponse { + println!("Received goal request with order {}", goal.order); + if goal.order > 9000 { + rclrs::GoalResponse::Reject + } else { + rclrs::GoalResponse::AcceptAndExecute + } +} + +fn handle_cancel(_goal_handle: Arc) -> rclrs::CancelResponse { + println!("Got request to cancel goal"); + rclrs::CancelResponse::Accept +} + +fn execute(goal_handle: Arc) { + println!("Executing goal"); + let feedback = example_interfaces::action::Fibonacci_Feedback { + sequence: [0, 1].to_vec(), + }; + + for i in 1..goal_handle.goal_request.order { + if goal_handle.is_canceling() { + let result = example_interfaces::action::Fibonacci_Result { + sequence: Vec::new(), + }; + + goal_handle.canceled(&result); + println!("Goal canceled"); + return; + } + + // Update sequence sequence + feedback + .sequence + .push(feedback.sequence[i as usize] + feedback.sequence[(i - 1) as usize]); + // Publish feedback + goal_handle.publish_feedback(&feedback); + println!("Publishing feedback"); + thread::sleep(std::time::Duration::from_millis(100)); + } + + let result = example_interfaces::action::Fibonacci_Result { + sequence: Vec::new(), + }; + result.sequence = feedback.sequence.clone(); + goal_handle.succeed(&result); + println!("Goal succeeded"); +} + +fn handle_accepted(goal_handle: Arc) { + thread::spawn(move || { + execute(goal_handle); + }); +} + +fn main() -> Result<(), Error> { + let context = rclrs::Context::new(env::args())?; + + let mut node = rclrs::create_node(&context, "minimal_action_server")?; + + let _action_server = node.create_action_server::( + "fibonacci", + handle_goal, + handle_cancel, + handle_accepted, + ); + + rclrs::spin(node).map_err(|err| err.into()) +} diff --git a/rclrs/package.xml b/rclrs/package.xml index 4c3754f48..d8b0d30ae 100644 --- a/rclrs/package.xml +++ b/rclrs/package.xml @@ -16,10 +16,11 @@ libclang-dev rosidl_runtime_rs rcl + rcl_action builtin_interfaces rcl_interfaces rosgraph_msgs - + test_msgs diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs new file mode 100644 index 000000000..ad91e4f35 --- /dev/null +++ b/rclrs/src/action.rs @@ -0,0 +1,57 @@ +pub(crate) mod client; +pub(crate) mod server; +mod server_goal_handle; + +use crate::rcl_bindings::RCL_ACTION_UUID_SIZE; +use std::fmt; + +pub use client::{ActionClient, ActionClientBase}; +pub use server::{ActionServer, ActionServerBase}; +pub use server_goal_handle::ServerGoalHandle; + +/// A unique identifier for a goal request. +#[derive(Copy, Clone, Debug, Default, PartialEq, Eq, PartialOrd, Ord, Hash)] +pub struct GoalUuid(pub [u8; RCL_ACTION_UUID_SIZE]); + +impl fmt::Display for GoalUuid { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> { + write!(f, "{:02x}{:02x}{:02x}{:02x}-{:02x}{:02x}-{:02x}{:02x}-{:02x}{:02x}-{:02x}{:02x}{:02x}{:02x}{:02x}{:02x}", + self.0[0], + self.0[1], + self.0[2], + self.0[3], + self.0[4], + self.0[5], + self.0[6], + self.0[7], + self.0[8], + self.0[9], + self.0[10], + self.0[11], + self.0[12], + self.0[13], + self.0[14], + self.0[15], + ) + } +} + +/// The response returned by an [`ActionServer`]'s goal callback when a goal request is received. +#[derive(PartialEq, Eq)] +pub enum GoalResponse { + /// The goal is rejected and will not be executed. + Reject = 1, + /// The server accepts the goal and will begin executing it immediately. + AcceptAndExecute = 2, + /// The server accepts the goal and will begin executing it later. + AcceptAndDefer = 3, +} + +/// The response returned by an [`ActionServer`]'s cancel callback when a goal is requested to be cancelled. +#[derive(PartialEq, Eq)] +pub enum CancelResponse { + /// The server will not try to cancel the goal. + Reject = 1, + /// The server will try to cancel the goal. + Accept = 2, +} diff --git a/rclrs/src/action/client.rs b/rclrs/src/action/client.rs new file mode 100644 index 000000000..0c8f62ddf --- /dev/null +++ b/rclrs/src/action/client.rs @@ -0,0 +1,185 @@ +use crate::{ + error::ToResult, rcl_bindings::*, wait::WaitableNumEntities, NodeHandle, RclrsError, + ENTITY_LIFECYCLE_MUTEX, +}; +use std::{ + ffi::CString, + marker::PhantomData, + sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, +}; + +// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread +// they are running in. Therefore, this type can be safely sent to another thread. +unsafe impl Send for rcl_action_client_t {} + +/// Manage the lifecycle of an `rcl_action_client_t`, including managing its dependencies +/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are +/// [dropped after][1] the `rcl_action_client_t`. +/// +/// [1]: +pub struct ActionClientHandle { + rcl_action_client: Mutex, + node_handle: Arc, + pub(crate) in_use_by_wait_set: Arc, +} + +impl ActionClientHandle { + pub(crate) fn lock(&self) -> MutexGuard { + self.rcl_action_client.lock().unwrap() + } +} + +impl Drop for ActionClientHandle { + fn drop(&mut self) { + let rcl_action_client = self.rcl_action_client.get_mut().unwrap(); + let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The entity lifecycle mutex is locked to protect against the risk of + // global variables in the rmw implementation being unsafely modified during cleanup. + unsafe { + rcl_action_client_fini(rcl_action_client, &mut *rcl_node); + } + } +} + +/// Trait to be implemented by concrete ActionClient structs. +/// +/// See [`ActionClient`] for an example +pub trait ActionClientBase: Send + Sync { + /// Internal function to get a reference to the `rcl` handle. + fn handle(&self) -> &ActionClientHandle; + /// Returns the number of underlying entities for the action client. + fn num_entities(&self) -> &WaitableNumEntities; + /// Tries to run the callback for the given readiness mode. + fn execute(&self, mode: ReadyMode) -> Result<(), RclrsError>; +} + +pub(crate) enum ReadyMode { + Feedback, + Status, + GoalResponse, + CancelResponse, + ResultResponse, +} + +pub struct ActionClient +where + ActionT: rosidl_runtime_rs::Action, +{ + _marker: PhantomData ActionT>, + pub(crate) handle: Arc, + num_entities: WaitableNumEntities, +} + +impl ActionClient +where + T: rosidl_runtime_rs::Action, +{ + /// Creates a new action client. + pub(crate) fn new(node_handle: Arc, topic: &str) -> Result + where + T: rosidl_runtime_rs::Action, + { + // SAFETY: Getting a zero-initialized value is always safe. + let mut rcl_action_client = unsafe { rcl_action_get_zero_initialized_client() }; + let type_support = T::get_type_support() as *const rosidl_action_type_support_t; + let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { + err, + s: topic.into(), + })?; + + // SAFETY: No preconditions for this function. + let action_client_options = unsafe { rcl_action_client_get_default_options() }; + + { + let mut rcl_node = node_handle.rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + + // SAFETY: + // * The rcl_action_client was zero-initialized as expected by this function. + // * The rcl_node is kept alive by the NodeHandle because it is a dependency of the action client. + // * The topic name and the options are copied by this function, so they can be dropped + // afterwards. + // * The entity lifecycle mutex is locked to protect against the risk of global + // variables in the rmw implementation being unsafely modified during initialization. + unsafe { + rcl_action_client_init( + &mut rcl_action_client, + &mut *rcl_node, + type_support, + topic_c_string.as_ptr(), + &action_client_options, + ) + .ok()?; + } + } + + let handle = Arc::new(ActionClientHandle { + rcl_action_client: Mutex::new(rcl_action_client), + node_handle, + in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + }); + + let mut num_entities = WaitableNumEntities::default(); + unsafe { + rcl_action_client_wait_set_get_num_entities( + &*handle.lock(), + &mut num_entities.num_subscriptions, + &mut num_entities.num_guard_conditions, + &mut num_entities.num_timers, + &mut num_entities.num_clients, + &mut num_entities.num_services, + ) + .ok()?; + } + + Ok(Self { + _marker: Default::default(), + handle, + num_entities, + }) + } + + fn execute_feedback(&self) -> Result<(), RclrsError> { + todo!() + } + + fn execute_status(&self) -> Result<(), RclrsError> { + todo!() + } + + fn execute_goal_response(&self) -> Result<(), RclrsError> { + todo!() + } + + fn execute_cancel_response(&self) -> Result<(), RclrsError> { + todo!() + } + + fn execute_result_response(&self) -> Result<(), RclrsError> { + todo!() + } +} + +impl ActionClientBase for ActionClient +where + T: rosidl_runtime_rs::Action, +{ + fn handle(&self) -> &ActionClientHandle { + &self.handle + } + + fn num_entities(&self) -> &WaitableNumEntities { + &self.num_entities + } + + fn execute(&self, mode: ReadyMode) -> Result<(), RclrsError> { + match mode { + ReadyMode::Feedback => self.execute_feedback(), + ReadyMode::Status => self.execute_status(), + ReadyMode::GoalResponse => self.execute_goal_response(), + ReadyMode::CancelResponse => self.execute_cancel_response(), + ReadyMode::ResultResponse => self.execute_result_response(), + } + } +} diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs new file mode 100644 index 000000000..0b0357224 --- /dev/null +++ b/rclrs/src/action/server.rs @@ -0,0 +1,707 @@ +use crate::{ + action::{CancelResponse, GoalResponse, GoalUuid, ServerGoalHandle}, + error::{RclReturnCode, ToResult}, + rcl_bindings::*, + wait::WaitableNumEntities, + Clock, DropGuard, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, +}; +use rosidl_runtime_rs::{Action, ActionImpl, Message, Service}; +use std::{ + collections::HashMap, + ffi::CString, + sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, +}; + +// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread +// they are running in. Therefore, this type can be safely sent to another thread. +unsafe impl Send for rcl_action_server_t {} + +/// Manage the lifecycle of an `rcl_action_server_t`, including managing its dependencies +/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are +/// [dropped after][1] the `rcl_action_server_t`. +/// +/// [1]: +pub struct ActionServerHandle { + rcl_action_server: Mutex, + node_handle: Arc, + pub(crate) in_use_by_wait_set: Arc, +} + +impl ActionServerHandle { + pub(crate) fn lock(&self) -> MutexGuard { + self.rcl_action_server.lock().unwrap() + } +} + +impl Drop for ActionServerHandle { + fn drop(&mut self) { + let rcl_action_server = self.rcl_action_server.get_mut().unwrap(); + let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The entity lifecycle mutex is locked to protect against the risk of + // global variables in the rmw implementation being unsafely modified during cleanup. + unsafe { + rcl_action_server_fini(rcl_action_server, &mut *rcl_node); + } + } +} + +/// Trait to be implemented by concrete ActionServer structs. +/// +/// See [`ActionServer`] for an example +pub trait ActionServerBase: Send + Sync { + /// Internal function to get a reference to the `rcl` handle. + fn handle(&self) -> &ActionServerHandle; + /// Returns the number of underlying entities for the action server. + fn num_entities(&self) -> &WaitableNumEntities; + /// Tries to run the callback for the given readiness mode. + fn execute(self: Arc, mode: ReadyMode) -> Result<(), RclrsError>; +} + +pub(crate) enum ReadyMode { + GoalRequest, + CancelRequest, + ResultRequest, + GoalExpired, +} + +pub type GoalCallback = dyn Fn(GoalUuid, ::Goal) -> GoalResponse + 'static + Send + Sync; +pub type CancelCallback = dyn Fn(ServerGoalHandle) -> CancelResponse + 'static + Send + Sync; +pub type AcceptedCallback = dyn Fn(ServerGoalHandle) + 'static + Send + Sync; + +pub struct ActionServer +where + ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, +{ + pub(crate) handle: Arc, + num_entities: WaitableNumEntities, + goal_callback: Box>, + cancel_callback: Box>, + accepted_callback: Box>, + // TODO(nwn): Audit these three mutexes to ensure there's no deadlocks or broken invariants. We + // may want to join them behind a shared mutex, at least for the `goal_results` and `result_requests`. + goal_handles: Mutex>>>, + goal_results: Mutex::Response as Message>::RmwMsg>>, + result_requests: Mutex>>, +} + +impl ActionServer +where + T: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, +{ + /// Creates a new action server. + pub(crate) fn new( + node_handle: Arc, + clock: Clock, + topic: &str, + goal_callback: impl Fn(GoalUuid, T::Goal) -> GoalResponse + 'static + Send + Sync, + cancel_callback: impl Fn(ServerGoalHandle) -> CancelResponse + 'static + Send + Sync, + accepted_callback: impl Fn(ServerGoalHandle) + 'static + Send + Sync, + ) -> Result + where + T: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, + { + // SAFETY: Getting a zero-initialized value is always safe. + let mut rcl_action_server = unsafe { rcl_action_get_zero_initialized_server() }; + let type_support = T::get_type_support() as *const rosidl_action_type_support_t; + let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { + err, + s: topic.into(), + })?; + + // SAFETY: No preconditions for this function. + let action_server_options = unsafe { rcl_action_server_get_default_options() }; + + { + let mut rcl_node = node_handle.rcl_node.lock().unwrap(); + let rcl_clock = clock.rcl_clock(); + let mut rcl_clock = rcl_clock.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + + // SAFETY: + // * The rcl_action_server is zero-initialized as mandated by this function. + // * The rcl_node is kept alive by the NodeHandle because it is a dependency of the action server. + // * The topic name and the options are copied by this function, so they can be dropped + // afterwards. + // * The entity lifecycle mutex is locked to protect against the risk of global + // variables in the rmw implementation being unsafely modified during initialization. + unsafe { + rcl_action_server_init( + &mut rcl_action_server, + &mut *rcl_node, + &mut *rcl_clock, + type_support, + topic_c_string.as_ptr(), + &action_server_options, + ) + .ok()?; + } + } + + let handle = Arc::new(ActionServerHandle { + rcl_action_server: Mutex::new(rcl_action_server), + node_handle, + in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + }); + + let mut num_entities = WaitableNumEntities::default(); + unsafe { + rcl_action_server_wait_set_get_num_entities( + &*handle.lock(), + &mut num_entities.num_subscriptions, + &mut num_entities.num_guard_conditions, + &mut num_entities.num_timers, + &mut num_entities.num_clients, + &mut num_entities.num_services, + ) + .ok()?; + } + + Ok(Self { + handle, + num_entities, + goal_callback: Box::new(goal_callback), + cancel_callback: Box::new(cancel_callback), + accepted_callback: Box::new(accepted_callback), + goal_handles: Mutex::new(HashMap::new()), + goal_results: Mutex::new(HashMap::new()), + result_requests: Mutex::new(HashMap::new()), + }) + } + + fn take_goal_request(&self) -> Result<(<::Request as Message>::RmwMsg, rmw_request_id_t), RclrsError> { + let mut request_id = rmw_request_id_t { + writer_guid: [0; 16], + sequence_number: 0, + }; + type RmwRequest = <<::SendGoalService as Service>::Request as Message>::RmwMsg; + let mut request_rmw = RmwRequest::::default(); + let handle = &*self.handle.lock(); + unsafe { + // SAFETY: The action server is locked by the handle. The request_id is a + // zero-initialized rmw_request_id_t, and the request_rmw is a default-initialized + // SendGoalService request message. + rcl_action_take_goal_request( + handle, + &mut request_id, + &mut request_rmw as *mut RmwRequest as *mut _, + ) + } + .ok()?; + + Ok((request_rmw, request_id)) + } + + fn send_goal_response( + &self, + mut request_id: rmw_request_id_t, + accepted: bool, + ) -> Result<(), RclrsError> { + let mut response_rmw = ::create_goal_response(accepted, (0, 0)); + let handle = &*self.handle.lock(); + let result = unsafe { + // SAFETY: The action server handle is locked and so synchronized with other + // functions. The request_id and response message are uniquely owned, and so will + // not mutate during this function call. + // Also, when appropriate, `rcl_action_accept_new_goal()` has been called beforehand, + // as specified in the `rcl_action` docs. + rcl_action_send_goal_response( + handle, + &mut request_id, + &mut response_rmw as *mut _ as *mut _, + ) + } + .ok(); + match result { + Ok(()) => Ok(()), + Err(RclrsError::RclError { + code: RclReturnCode::Timeout, + .. + }) => { + // TODO(nwn): Log an error and continue. + // (See https://github.com/ros2/rclcpp/pull/2215 for reasoning.) + Ok(()) + } + _ => result, + } + } + + fn execute_goal_request(self: Arc) -> Result<(), RclrsError> { + let (request, request_id) = match self.take_goal_request() { + Ok(res) => res, + Err(RclrsError::RclError { + code: RclReturnCode::ServiceTakeFailed, + .. + }) => { + // Spurious wakeup – this may happen even when a waitset indicated that this + // action was ready, so it shouldn't be an error. + return Ok(()); + } + Err(err) => return Err(err), + }; + + let uuid = GoalUuid(*::get_goal_request_uuid(&request)); + + let response: GoalResponse = { + todo!("Optionally convert request to an idiomatic type for the user's callback."); + todo!("Call self.goal_callback(uuid, request)"); + }; + + // Don't continue if the goal was rejected by the user. + if response == GoalResponse::Reject { + return self.send_goal_response(request_id, false); + } + + let goal_handle = { + // SAFETY: No preconditions + let mut goal_info = unsafe { rcl_action_get_zero_initialized_goal_info() }; + // Only populate the goal UUID; the timestamp will be set internally by + // rcl_action_accept_new_goal(). + goal_info.goal_id.uuid = uuid.0; + + let server_handle = &mut *self.handle.lock(); + let goal_handle_ptr = unsafe { + // SAFETY: The action server handle is locked and so synchronized with other + // functions. The request_id and response message are uniquely owned, and so will + // not mutate during this function call. The returned goal handle pointer should be + // valid unless it is null. + rcl_action_accept_new_goal(server_handle, &goal_info) + }; + if goal_handle_ptr.is_null() { + // Other than rcl_get_error_string(), there's no indication what happened. + panic!("Failed to accept goal"); + } else { + Arc::new(ServerGoalHandle::::new( + goal_handle_ptr, + Arc::downgrade(&self), + todo!("Create an Arc holding the goal message"), + uuid, + )) + } + }; + + self.send_goal_response(request_id, true)?; + + self.goal_handles + .lock() + .unwrap() + .insert(uuid, Arc::clone(&goal_handle)); + + if response == GoalResponse::AcceptAndExecute { + goal_handle.execute()?; + } + + self.publish_status()?; + + // TODO: Call the user's goal_accepted callback. + todo!("Call self.accepted_callback(goal_handle)"); + + Ok(()) + } + + fn take_cancel_request(&self) -> Result<(action_msgs__srv__CancelGoal_Request, rmw_request_id_t), RclrsError> { + let mut request_id = rmw_request_id_t { + writer_guid: [0; 16], + sequence_number: 0, + }; + // SAFETY: No preconditions + let mut request_rmw = unsafe { rcl_action_get_zero_initialized_cancel_request() }; + let handle = &*self.handle.lock(); + unsafe { + // SAFETY: The action server is locked by the handle. The request_id is a + // zero-initialized rmw_request_id_t, and the request_rmw is a zero-initialized + // action_msgs__srv__CancelGoal_Request. + rcl_action_take_cancel_request( + handle, + &mut request_id, + &mut request_rmw as *mut _ as *mut _, + ) + } + .ok()?; + + Ok((request_rmw, request_id)) + } + + fn send_cancel_response( + &self, + mut request_id: rmw_request_id_t, + response_rmw: &mut action_msgs__srv__CancelGoal_Response, + ) -> Result<(), RclrsError> { + let handle = &*self.handle.lock(); + let result = unsafe { + // SAFETY: The action server handle is locked and so synchronized with other functions. + // The request_id and response are both uniquely owned or borrowed, and so neither will + // mutate during this function call. + rcl_action_send_cancel_response( + handle, + &mut request_id, + response_rmw as *mut _ as *mut _, + ) + } + .ok(); + match result { + Ok(()) => Ok(()), + Err(RclrsError::RclError { + code: RclReturnCode::Timeout, + .. + }) => { + // TODO(nwn): Log an error and continue. + // (See https://github.com/ros2/rclcpp/pull/2215 for reasoning.) + Ok(()) + } + _ => result, + } + } + + fn execute_cancel_request(&self) -> Result<(), RclrsError> { + let (request, request_id) = match self.take_cancel_request() { + Ok(res) => res, + Err(RclrsError::RclError { + code: RclReturnCode::ServiceTakeFailed, + .. + }) => { + // Spurious wakeup – this may happen even when a waitset indicated that this + // action was ready, so it shouldn't be an error. + return Ok(()); + } + Err(err) => return Err(err), + }; + + let mut response_rmw = { + // SAFETY: No preconditions + let mut response_rmw = unsafe { rcl_action_get_zero_initialized_cancel_response() }; + unsafe { + // SAFETY: The action server is locked by the handle. The request was initialized + // by rcl_action, and the response is a zero-initialized + // rcl_action_cancel_response_t. + rcl_action_process_cancel_request( + &*self.handle.lock(), + &request, + &mut response_rmw as *mut _, + ) + } + .ok()?; + + DropGuard::new(response_rmw, |mut response_rmw| unsafe { + // SAFETY: The response was initialized by rcl_action_process_cancel_request(). + // Later modifications only truncate the size of the array and shift elements, + // without modifying the data pointer or capacity. + rcl_action_cancel_response_fini(&mut response_rmw); + }) + }; + + let num_candidates = response_rmw.msg.goals_canceling.size; + let mut num_accepted = 0; + for idx in 0..response_rmw.msg.goals_canceling.size { + let goal_info = unsafe { + // SAFETY: The array pointed to by response_rmw.msg.goals_canceling.data is + // guaranteed to contain at least response_rmw.msg.goals_canceling.size members. + &*response_rmw.msg.goals_canceling.data.add(idx) + }; + let goal_uuid = GoalUuid(goal_info.goal_id.uuid); + + let response = { + if let Some(goal_handle) = self.goal_handles.lock().unwrap().get(&goal_uuid) { + let response: CancelResponse = todo!("Call self.cancel_callback(goal_handle)"); + if response == CancelResponse::Accept { + // Still reject the request if the goal is no longer cancellable. + if goal_handle.cancel().is_ok() { + CancelResponse::Accept + } else { + CancelResponse::Reject + } + } else { + CancelResponse::Reject + } + } else { + CancelResponse::Reject + } + }; + + if response == CancelResponse::Accept { + // Shift the accepted entry back to the first rejected slot, if necessary. + if num_accepted < idx { + let goal_info_slot = unsafe { + // SAFETY: The array pointed to by response_rmw.msg.goals_canceling.data is + // guaranteed to contain at least response_rmw.msg.goals_canceling.size + // members. Since `num_accepted` is strictly less than `idx`, it is a + // distinct element of the array, so there is no mutable aliasing. + &mut *response_rmw.msg.goals_canceling.data.add(num_accepted) + }; + } + num_accepted += 1; + } + } + response_rmw.msg.goals_canceling.size = num_accepted; + + // If the user rejects all individual cancel requests, consider the entire request as + // having been rejected. + if num_accepted == 0 && num_candidates > 0 { + // TODO(nwn): Include action_msgs__srv__CancelGoal_Response__ERROR_REJECTED in the rcl + // bindings. + response_rmw.msg.return_code = 1; + } + + // If any goal states changed, publish a status update. + if num_accepted > 0 { + self.publish_status()?; + } + + self.send_cancel_response(request_id, &mut response_rmw.msg)?; + + Ok(()) + } + + fn take_result_request(&self) -> Result<(<::Request as Message>::RmwMsg, rmw_request_id_t), RclrsError> { + let mut request_id = rmw_request_id_t { + writer_guid: [0; 16], + sequence_number: 0, + }; + type RmwRequest = <<::GetResultService as Service>::Request as Message>::RmwMsg; + let mut request_rmw = RmwRequest::::default(); + let handle = &*self.handle.lock(); + unsafe { + // SAFETY: The action server is locked by the handle. The request_id is a + // zero-initialized rmw_request_id_t, and the request_rmw is a default-initialized + // GetResultService request message. + rcl_action_take_result_request( + handle, + &mut request_id, + &mut request_rmw as *mut RmwRequest as *mut _, + ) + } + .ok()?; + + Ok((request_rmw, request_id)) + } + + fn send_result_response( + &self, + mut request_id: rmw_request_id_t, + response_rmw: &mut <<::GetResultService as rosidl_runtime_rs::Service>::Response as Message>::RmwMsg, + ) -> Result<(), RclrsError> { + let handle = &*self.handle.lock(); + let result = unsafe { + // SAFETY: The action server handle is locked and so synchronized with other functions. + // The request_id and response are both uniquely owned or borrowed, and so neither will + // mutate during this function call. + rcl_action_send_result_response( + handle, + &mut request_id, + response_rmw as *mut _ as *mut _, + ) + } + .ok(); + match result { + Ok(()) => Ok(()), + Err(RclrsError::RclError { + code: RclReturnCode::Timeout, + .. + }) => { + // TODO(nwn): Log an error and continue. + // (See https://github.com/ros2/rclcpp/pull/2215 for reasoning.) + Ok(()) + } + _ => result, + } + } + + fn execute_result_request(&self) -> Result<(), RclrsError> { + let (request, request_id) = match self.take_result_request() { + Ok(res) => res, + Err(RclrsError::RclError { + code: RclReturnCode::ServiceTakeFailed, + .. + }) => { + // Spurious wakeup – this may happen even when a waitset indicated that this + // action was ready, so it shouldn't be an error. + return Ok(()); + } + Err(err) => return Err(err), + }; + + let uuid = GoalUuid(*::get_result_request_uuid(&request)); + + let goal_exists = unsafe { + // SAFETY: No preconditions + let mut goal_info = rcl_action_get_zero_initialized_goal_info(); + goal_info.goal_id.uuid = uuid.0; + + // SAFETY: The action server is locked through the handle. The `goal_info` + // argument points to a rcl_action_goal_info_t with the desired UUID. + rcl_action_server_goal_exists(&*self.handle.lock(), &goal_info) + }; + + if goal_exists { + if let Some(result) = self.goal_results.lock().unwrap().get_mut(&uuid) { + // Respond immediately if the goal already has a response. + self.send_result_response(request_id, result)?; + } else { + // Queue up the request for a response once the goal terminates. + self.result_requests.lock().unwrap().entry(uuid).or_insert(vec![]).push(request_id); + } + } else { + // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_UNKNOWN in the rcl + // bindings. + let null_response = ::RmwMsg::default(); + let mut response_rmw = ::create_result_response(0, null_response); + self.send_result_response(request_id, &mut response_rmw)?; + } + + Ok(()) + } + + fn execute_goal_expired(&self) -> Result<(), RclrsError> { + // We assume here that only one goal expires at a time. If not, the only consequence is + // that we'll call rcl_action_expire_goals() more than necessary. + + // SAFETY: No preconditions + let mut expired_goal = unsafe { rcl_action_get_zero_initialized_goal_info() }; + let mut num_expired = 1; + + loop { + unsafe { + // SAFETY: The action server is locked through the handle. The `expired_goal` + // argument points to an array of one rcl_action_goal_info_t and num_expired points + // to a `size_t`. + rcl_action_expire_goals(&*self.handle.lock(), &mut expired_goal, 1, &mut num_expired) + } + .ok()?; + + if num_expired > 0 { + // Clean up the expired goal. + let uuid = GoalUuid(expired_goal.goal_id.uuid); + self.goal_results.lock().unwrap().remove(&uuid); + self.result_requests.lock().unwrap().remove(&uuid); + self.goal_handles.lock().unwrap().remove(&uuid); + } else { + break; + } + } + + Ok(()) + } + + // TODO(nwn): Replace `status` with a "properly typed" action_msgs::msg::GoalStatus enum. + pub(crate) fn terminate_goal(&self, goal_id: &GoalUuid, status: i8, result: ::RmwMsg) -> Result<(), RclrsError> { + let response_rmw = ::create_result_response(status, result); + + // Publish the result to anyone listening. + self.publish_result(goal_id, response_rmw); + + // Publish the state change. + self.publish_status(); + + // Notify rcl that a goal has terminated and to therefore recalculate the expired goal timer. + unsafe { + // SAFETY: The action server is locked and valid. No other preconditions. + rcl_action_notify_goal_done(&*self.handle.lock()) + } + .ok()?; + + // Release ownership of the goal handle. It will persist until the user also drops it. + self.goal_handles.lock().unwrap().remove(&goal_id); + + Ok(()) + } + + pub(crate) fn publish_status(&self) -> Result<(), RclrsError> { + let mut goal_statuses = DropGuard::new( + unsafe { + // SAFETY: No preconditions + rcl_action_get_zero_initialized_goal_status_array() + }, + |mut goal_statuses| unsafe { + // SAFETY: The goal_status array is either zero-initialized and empty or populated by + // `rcl_action_get_goal_status_array`. In either case, it can be safely finalized. + rcl_action_goal_status_array_fini(&mut goal_statuses); + }, + ); + + unsafe { + // SAFETY: The action server is locked through the handle and goal_statuses is + // zero-initialized. + rcl_action_get_goal_status_array(&*self.handle.lock(), &mut *goal_statuses) + } + .ok()?; + + unsafe { + // SAFETY: The action server is locked through the handle and goal_statuses.msg is a + // valid `action_msgs__msg__GoalStatusArray` by construction. + rcl_action_publish_status( + &*self.handle.lock(), + &goal_statuses.msg as *const _ as *const std::ffi::c_void, + ) + } + .ok() + } + + pub(crate) fn publish_feedback(&self, goal_id: &GoalUuid, feedback: &::Feedback) -> Result<(), RclrsError> { + let feedback_rmw = <::Feedback as Message>::into_rmw_message(std::borrow::Cow::Borrowed(feedback)); + let mut feedback_msg = ::create_feedback_message(&goal_id.0, feedback_rmw.into_owned()); + unsafe { + // SAFETY: The action server is locked through the handle, meaning that no other + // non-thread-safe functions can be called on it at the same time. The feedback_msg is + // exclusively owned here, ensuring that it won't be modified during the call. + // rcl_action_publish_feedback() guarantees that it won't modify `feedback_msg`. + rcl_action_publish_feedback( + &*self.handle.lock(), + &mut feedback_msg as *mut _ as *mut std::ffi::c_void, + ) + } + .ok() + } + + fn publish_result(&self, goal_id: &GoalUuid, mut result: <<::GetResultService as Service>::Response as Message>::RmwMsg) -> Result<(), RclrsError> { + let goal_exists = unsafe { + // SAFETY: No preconditions + let mut goal_info = rcl_action_get_zero_initialized_goal_info(); + goal_info.goal_id.uuid = goal_id.0; + + // SAFETY: The action server is locked through the handle. The `goal_info` + // argument points to a rcl_action_goal_info_t with the desired UUID. + rcl_action_server_goal_exists(&*self.handle.lock(), &goal_info) + }; + if !goal_exists { + panic!("Cannot publish result for unknown goal") + } + + // TODO(nwn): Fix synchronization problem between goal_results and result_requests. + // Currently, there is a gap between the request queue being drained and the result being + // stored for future requests. Any requests received during that gap would never receive a + // response. Fixing this means we'll need combined locking over these two hash maps. + + // Respond to all queued requests. + if let Some(result_requests) = self.result_requests.lock().unwrap().remove(&goal_id) { + for mut result_request in result_requests { + self.send_result_response(result_request, &mut result)?; + } + } + + self.goal_results.lock().unwrap().insert(*goal_id, result); + + Ok(()) + } +} + +impl ActionServerBase for ActionServer +where + T: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, +{ + fn handle(&self) -> &ActionServerHandle { + &self.handle + } + + fn num_entities(&self) -> &WaitableNumEntities { + &self.num_entities + } + + fn execute(self: Arc, mode: ReadyMode) -> Result<(), RclrsError> { + match mode { + ReadyMode::GoalRequest => self.execute_goal_request(), + ReadyMode::CancelRequest => self.execute_cancel_request(), + ReadyMode::ResultRequest => self.execute_result_request(), + ReadyMode::GoalExpired => self.execute_goal_expired(), + } + } +} diff --git a/rclrs/src/action/server_goal_handle.rs b/rclrs/src/action/server_goal_handle.rs new file mode 100644 index 000000000..3ee47d086 --- /dev/null +++ b/rclrs/src/action/server_goal_handle.rs @@ -0,0 +1,241 @@ +use crate::{action::ActionServer, rcl_bindings::*, GoalUuid, RclrsError, ToResult}; +use std::sync::{Arc, Mutex, Weak}; + +// Values defined by `action_msgs/msg/GoalStatus` +#[repr(i8)] +#[derive(Debug, Clone, Hash, PartialEq, Eq)] +enum GoalStatus { + Unknown = 0, + Accepted = 1, + Executing = 2, + Canceling = 3, + Succeeded = 4, + Canceled = 5, + Aborted = 6, +} + +/// Handle to interact with goals on a server. +/// +/// Use this to check the status of a goal and to set its result. +/// +/// This type will only be created by an [`ActionServer`] when a goal is accepted and will be +/// passed to the user in the associated `handle_accepted` callback. +pub struct ServerGoalHandle +where + ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, +{ + rcl_handle: Mutex<*mut rcl_action_goal_handle_t>, + action_server: Weak>, + goal_request: Arc, + uuid: GoalUuid, +} + +// SAFETY: Send/Sync are not automatically implemented due to the contained raw pointer +// (specifically, `*mut rcl_action_goal_handle_t`). However, the `rcl_handle` field is wrapped in a +// mutex, guaranteeing that the underlying data is never simultaneously accessed on the rclrs side +// by multiple threads. Moreover, the rcl_action functions taking these handles are able to be run +// from any thread. +unsafe impl Send for ServerGoalHandle where ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl {} +unsafe impl Sync for ServerGoalHandle where ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl {} + +impl ServerGoalHandle +where + ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, +{ + pub(crate) fn new( + rcl_handle: *mut rcl_action_goal_handle_t, + action_server: Weak>, + goal_request: Arc, + uuid: GoalUuid, + ) -> Self { + Self { + rcl_handle: Mutex::new(rcl_handle), + action_server, + goal_request: Arc::clone(&goal_request), + uuid, + } + } + + /// Returns the goal state. + fn get_state(&self) -> Result { + let mut state = GoalStatus::Unknown as rcl_action_goal_state_t; + { + let rcl_handle = self.rcl_handle.lock().unwrap(); + // SAFETY: The provided goal handle is properly initialized by construction. + unsafe { rcl_action_goal_handle_get_status(*rcl_handle, &mut state).ok()? } + } + // SAFETY: state is initialized to a valid GoalStatus value and will only ever by set by + // rcl_action_goal_handle_get_status to a valid GoalStatus value. + Ok(unsafe { std::mem::transmute(state) }) + } + + /// Returns whether the client has requested that this goal be cancelled. + pub fn is_canceling(&self) -> bool { + self.get_state().unwrap() == GoalStatus::Canceling + } + + /// Returns true if the goal is either pending or executing, or false if it has reached a + /// terminal state. + pub fn is_active(&self) -> bool { + let rcl_handle = self.rcl_handle.lock().unwrap(); + // SAFETY: The provided goal handle is properly initialized by construction. + unsafe { rcl_action_goal_handle_is_active(*rcl_handle) } + } + + /// Returns whether the goal is executing. + pub fn is_executing(&self) -> bool { + self.get_state().unwrap() == GoalStatus::Executing + } + + /// Attempt to perform the given goal state transition. + fn update_state(&self, event: rcl_action_goal_event_t) -> Result<(), RclrsError> { + let mut rcl_handle = self.rcl_handle.lock().unwrap(); + // SAFETY: The provided goal handle is properly initialized by construction. + unsafe { rcl_action_update_goal_state(*rcl_handle, event).ok() } + } + + /// Indicate that the goal is being cancelled. + /// + /// This is called when a cancel request for the goal has been accepted. + /// + /// Returns an error if the goal is in any state other than accepted or executing. + pub(crate) fn cancel(&self) -> Result<(), RclrsError> { + self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCEL_GOAL) + } + + /// Indicate that the goal could not be reached and has been aborted. + /// + /// Only call this if the goal is executing but cannot be completed. This is a terminal state, + /// so no more methods may be called on a goal handle after this is called. + /// + /// Returns an error if the goal is in any state other than executing. + pub fn abort(&self, result: &ActionT::Result) -> Result<(), RclrsError> { + self.update_state(rcl_action_goal_event_t::GOAL_EVENT_ABORT)?; + + if let Some(action_server) = self.action_server.upgrade() { + let result_rmw = ::into_rmw_message(std::borrow::Cow::Borrowed(result)).into_owned(); + // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_ABORTED in the rcl + // bindings. + action_server.terminate_goal(&self.uuid, 6, result_rmw)?; + } + Ok(()) + } + + /// Indicate that the goal has succeeded. + /// + /// Only call this if the goal is executing and has reached the desired final state. This is a + /// terminal state, so no more methods may be called on a goal handle after this is called. + /// + /// Returns an error if the goal is in any state other than executing. + pub fn succeed(&self, result: &ActionT::Result) -> Result<(), RclrsError> { + self.update_state(rcl_action_goal_event_t::GOAL_EVENT_SUCCEED)?; + + if let Some(action_server) = self.action_server.upgrade() { + let result_rmw = ::into_rmw_message(std::borrow::Cow::Borrowed(result)).into_owned(); + // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_SUCCEEDED in the rcl + // bindings. + action_server.terminate_goal(&self.uuid, 4, result_rmw)?; + } + Ok(()) + } + + /// Indicate that the goal has been cancelled. + /// + /// Only call this if the goal is executing or pending, but has been cancelled. This is a + /// terminal state, so no more methods may be called on a goal handle after this is called. + /// + /// Returns an error if the goal is in any state other than executing or pending. + pub fn canceled(&self, result: &ActionT::Result) -> Result<(), RclrsError> { + self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCELED)?; + + if let Some(action_server) = self.action_server.upgrade() { + let result_rmw = ::into_rmw_message(std::borrow::Cow::Borrowed(result)).into_owned(); + // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_CANCELED in the rcl + // bindings. + action_server.terminate_goal(&self.uuid, 5, result_rmw)?; + } + Ok(()) + } + + /// Indicate that the server is starting to execute the goal. + /// + /// Only call this if the goal is pending. This is a terminal state, so no more methods may be + /// called on a goal handle after this is called. + /// + /// Returns an error if the goal is in any state other than pending. + pub fn execute(&self) -> Result<(), RclrsError> { + self.update_state(rcl_action_goal_event_t::GOAL_EVENT_EXECUTE)?; + + // Publish the state change. + if let Some(action_server) = self.action_server.upgrade() { + action_server.publish_status()?; + } + Ok(()) + } + + /// Try canceling the goal if possible. + fn try_canceling(&mut self) -> Result { + let rcl_handle = self.rcl_handle.lock().unwrap(); + + // If the goal is in a cancelable state, transition to canceling. + // SAFETY: The provided goal handle is properly initialized by construction. + let is_cancelable = unsafe { rcl_action_goal_handle_is_cancelable(*rcl_handle) }; + if is_cancelable { + self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCEL_GOAL)?; + } + + // If the goal is canceling, transition to canceled. + if self.get_state()? == GoalStatus::Canceling { + self.update_state(rcl_action_goal_event_t::GOAL_EVENT_CANCELED)?; + Ok(true) + } else { + Ok(false) + } + } + + /// Get the unique identifier of the goal. + pub fn goal_id(&self) -> GoalUuid { + self.uuid + } + + /// Get the user-provided message describing the goal. + pub fn goal(&self) -> Arc { + Arc::clone(&self.goal_request) + } + + /// Send an update about the goal's progress. + /// + /// This may only be called when the goal is executing. + /// + /// Returns an error if the goal is in any state other than executing. + pub fn publish_feedback(&self, feedback: Arc) -> Result<(), RclrsError> { + // If the action server no longer exists, simply drop the message. + if let Some(action_server) = self.action_server.upgrade() { + action_server.publish_feedback(&self.uuid, &*feedback)?; + } + Ok(()) + } +} + +impl Drop for ServerGoalHandle +where + ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, +{ + /// Cancel the goal if its handle is dropped without reaching a terminal state. + fn drop(&mut self) { + if self.try_canceling() == Ok(true) { + if let Some(action_server) = self.action_server.upgrade() { + let response_rmw = ::RmwMsg::default(); + // TODO(nwn): Include action_msgs__msg__GoalStatus__STATUS_CANCELED in the rcl + // bindings. + action_server.terminate_goal(&self.uuid, 5, response_rmw); + } + } + { + let rcl_handle = self.rcl_handle.lock().unwrap(); + // SAFETY: The provided goal handle is properly initialized by construction. It will + // not be accessed beyond this point. + unsafe { rcl_action_goal_handle_fini(*rcl_handle); } + } + } +} diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs index f7c085e14..ae7fb0582 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -88,6 +88,11 @@ impl Clock { self.kind } + /// Returns the clock's `rcl_clock_t`. + pub(crate) fn rcl_clock(&self) -> Arc> { + Arc::clone(&self.rcl_clock) + } + /// Returns the current clock's timestamp. pub fn now(&self) -> Time { let mut clock = self.rcl_clock.lock().unwrap(); diff --git a/rclrs/src/drop_guard.rs b/rclrs/src/drop_guard.rs new file mode 100644 index 000000000..f4e47b2df --- /dev/null +++ b/rclrs/src/drop_guard.rs @@ -0,0 +1,48 @@ +use std::{ + mem::ManuallyDrop, + ops::{Deref, DerefMut, Drop, Fn}, +}; + +/// A wrapper providing additional drop-logic for the contained value. +/// +/// When this wrapper is dropped, the contained value will be passed into the given function before +/// being destructed. +pub(crate) struct DropGuard { + value: ManuallyDrop, + drop_fn: F, +} + +impl DropGuard { + /// Create a new `DropGuard` with the given value and drop function. + pub fn new(value: T, drop_fn: F) -> Self { + Self { + value: ManuallyDrop::new(value), + drop_fn, + } + } +} + +impl Deref for DropGuard { + type Target = T; + + fn deref(&self) -> &T { + &*self.value + } +} + +impl DerefMut for DropGuard { + fn deref_mut(&mut self) -> &mut T { + &mut *self.value + } +} + +impl Drop for DropGuard { + fn drop(&mut self) { + // SAFETY: ManuallyDrop::take() leaves `self.value` in an uninitialized state, meaning that + // it must not be accessed further. This is guaranteed since `self` is being dropped and + // cannot be accessed after this function completes. Moreover, the strict ownership of + // `self.value` means that it cannot be accessed by `self.drop_fn`'s drop function either. + let value = unsafe { ManuallyDrop::take(&mut self.value) }; + (self.drop_fn)(value); + } +} diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs index 3eba2549f..67018ae9a 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -174,6 +174,26 @@ pub enum RclReturnCode { EventInvalid = 2000, /// Failed to take an event from the event handle EventTakeFailed = 2001, + // ====== 2XXX: action-specific errors ====== + /// Action name does not pass validation + // TODO(nwn): Consult with upstream about this reused error code. + // ActionNameInvalid = 2000, + /// Action goal accepted + ActionGoalAccepted = 2100, + /// Action goal rejected + ActionGoalRejected = 2101, + /// Action client is invalid + ActionClientInvalid = 2102, + /// Action client failed to take response + ActionClientTakeFailed = 2103, + /// Action server is invalid + ActionServerInvalid = 2200, + /// Action server failed to take request + ActionServerTakeFailed = 2201, + /// Action goal handle invalid + ActionGoalHandleInvalid = 2300, + /// Action invalid event + ActionGoalEventInvalid = 2301, // ====== 30XX: lifecycle-specific errors ====== /// `rcl_lifecycle` state registered LifecycleStateRegistered = 3000, @@ -222,6 +242,15 @@ impl TryFrom for RclReturnCode { x if x == Self::InvalidLogLevelRule as i32 => Self::InvalidLogLevelRule, x if x == Self::EventInvalid as i32 => Self::EventInvalid, x if x == Self::EventTakeFailed as i32 => Self::EventTakeFailed, + // x if x == Self::ActionNameInvalid as i32 => Self::ActionNameInvalid, + x if x == Self::ActionGoalAccepted as i32 => Self::ActionGoalAccepted, + x if x == Self::ActionGoalRejected as i32 => Self::ActionGoalRejected, + x if x == Self::ActionClientInvalid as i32 => Self::ActionClientInvalid, + x if x == Self::ActionClientTakeFailed as i32 => Self::ActionClientTakeFailed, + x if x == Self::ActionServerInvalid as i32 => Self::ActionServerInvalid, + x if x == Self::ActionServerTakeFailed as i32 => Self::ActionServerTakeFailed, + x if x == Self::ActionGoalHandleInvalid as i32 => Self::ActionGoalHandleInvalid, + x if x == Self::ActionGoalEventInvalid as i32 => Self::ActionGoalEventInvalid, x if x == Self::LifecycleStateRegistered as i32 => Self::LifecycleStateRegistered, x if x == Self::LifecycleStateNotRegistered as i32 => Self::LifecycleStateNotRegistered, other => { @@ -303,6 +332,15 @@ impl Display for RclReturnCode { Self::EventTakeFailed => { "Failed to take an event from the event handle (RCL_RET_EVENT_TAKE_FAILED)." } + // Self::ActionNameInvalid => "Action name does not pass validation (RCL_RET_ACTION_NAME_INVALID).", + Self::ActionGoalAccepted => "Action goal accepted (RCL_RET_ACTION_GOAL_ACCEPTED).", + Self::ActionGoalRejected => "Action goal rejected (RCL_RET_ACTION_GOAL_REJECTED).", + Self::ActionClientInvalid => "Action client is invalid (RCL_RET_ACTION_CLIENT_INVALID).", + Self::ActionClientTakeFailed => "Action client failed to take response (RCL_RET_ACTION_CLIENT_TAKE_FAILED).", + Self::ActionServerInvalid => "Action server is invalid (RCL_RET_ACTION_SERVER_INVALID).", + Self::ActionServerTakeFailed => "Action server failed to take request (RCL_RET_ACTION_SERVER_TAKE_FAILED).", + Self::ActionGoalHandleInvalid => "Action goal handle invalid (RCL_RET_ACTION_GOAL_HANDLE_INVALID).", + Self::ActionGoalEventInvalid => "Action invalid event (RCL_RET_ACTION_GOAL_EVENT_INVALID).", Self::LifecycleStateRegistered => { "`rcl_lifecycle` state registered (RCL_RET_LIFECYCLE_STATE_REGISTERED)." } diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 37c43a68e..7bd96c588 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -25,13 +25,15 @@ impl SingleThreadedExecutor { /// Add a node to the executor. pub fn add_node(&self, node: &Arc) -> Result<(), RclrsError> { - { self.nodes_mtx.lock().unwrap() }.push(Arc::downgrade(node)); + self.nodes_mtx.lock().unwrap().push(Arc::downgrade(node)); Ok(()) } /// Remove a node from the executor. pub fn remove_node(&self, node: Arc) -> Result<(), RclrsError> { - { self.nodes_mtx.lock().unwrap() } + self.nodes_mtx + .lock() + .unwrap() .retain(|n| !n.upgrade().map(|n| Arc::ptr_eq(&n, &node)).unwrap_or(false)); Ok(()) } @@ -40,7 +42,10 @@ impl SingleThreadedExecutor { /// /// This function additionally checks that the context is still valid. pub fn spin_once(&self, timeout: Option) -> Result<(), RclrsError> { - for node in { self.nodes_mtx.lock().unwrap() } + for node in self + .nodes_mtx + .lock() + .unwrap() .iter() .filter_map(Weak::upgrade) .filter(|node| unsafe { @@ -61,6 +66,14 @@ impl SingleThreadedExecutor { for ready_service in ready_entities.services { ready_service.execute()?; } + + for (ready_action_client, mode) in ready_entities.action_clients { + ready_action_client.execute(mode)?; + } + + for (ready_action_server, mode) in ready_entities.action_servers { + ready_action_server.execute(mode)?; + } } Ok(()) @@ -68,7 +81,7 @@ impl SingleThreadedExecutor { /// Convenience function for calling [`SingleThreadedExecutor::spin_once`] in a loop. pub fn spin(&self) -> Result<(), RclrsError> { - while !{ self.nodes_mtx.lock().unwrap() }.is_empty() { + while !self.nodes_mtx.lock().unwrap().is_empty() { match self.spin_once(None) { Ok(_) | Err(RclrsError::RclError { diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 4924b36cb..6b689f498 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -5,10 +5,12 @@ //! //! [1]: https://github.com/ros2-rust/ros2_rust/blob/main/README.md +mod action; mod arguments; mod client; mod clock; mod context; +mod drop_guard; mod error; mod executor; mod node; @@ -32,10 +34,12 @@ pub mod dynamic_message; use std::{sync::Arc, time::Duration}; +pub use action::*; pub use arguments::*; pub use client::*; pub use clock::*; pub use context::*; +use drop_guard::DropGuard; pub use error::*; pub use executor::*; pub use node::*; diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 97684d6bc..a77342c65 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -13,10 +13,11 @@ use rosidl_runtime_rs::Message; pub use self::{builder::*, graph::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, - ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, - RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, - TimeSource, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, ActionClient, ActionClientBase, ActionServer, ActionServerBase, + CancelResponse, Client, ClientBase, Clock, Context, ContextHandle, GoalResponse, GoalUuid, + GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, + QoSProfile, RclrsError, ServerGoalHandle, Service, ServiceBase, Subscription, SubscriptionBase, + SubscriptionCallback, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -63,6 +64,8 @@ pub struct Node { pub(crate) guard_conditions_mtx: Mutex>>, pub(crate) services_mtx: Mutex>>, pub(crate) subscriptions_mtx: Mutex>>, + pub(crate) action_servers_mtx: Mutex>>, + pub(crate) action_clients_mtx: Mutex>>, time_source: TimeSource, parameter: ParameterInterface, pub(crate) handle: Arc, @@ -206,10 +209,61 @@ impl Node { T: rosidl_runtime_rs::Service, { let client = Arc::new(Client::::new(Arc::clone(&self.handle), topic)?); - { self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); + self.clients_mtx + .lock() + .unwrap() + .push(Arc::downgrade(&client) as Weak); Ok(client) } + /// Creates an [`ActionClient`][1]. + /// + /// [1]: crate::ActionClient + // TODO: make action client's lifetime depend on node's lifetime + pub fn create_action_client(&self, topic: &str) -> Result>, RclrsError> + where + T: rosidl_runtime_rs::Action, + { + let action_client = Arc::new(ActionClient::::new(Arc::clone(&self.handle), topic)?); + self.action_clients_mtx + .lock() + .unwrap() + .push(Arc::downgrade(&action_client) as Weak); + Ok(action_client) + } + + /// Creates an [`ActionServer`][1]. + /// + /// [1]: crate::ActionServer + // TODO: make action server's lifetime depend on node's lifetime + pub fn create_action_server( + &mut self, + topic: &str, + handle_goal: GoalCallback, + handle_cancel: CancelCallback, + handle_accepted: AcceptedCallback, + ) -> Result>, RclrsError> + where + ActionT: rosidl_runtime_rs::Action + rosidl_runtime_rs::ActionImpl, + GoalCallback: Fn(GoalUuid, ::Goal) -> GoalResponse + 'static + Send + Sync, + CancelCallback: Fn(ServerGoalHandle) -> CancelResponse + 'static + Send + Sync, + AcceptedCallback: Fn(ServerGoalHandle) + 'static + Send + Sync, + { + let action_server = Arc::new(ActionServer::::new( + Arc::clone(&self.handle), + self.get_clock(), + topic, + handle_goal, + handle_cancel, + handle_accepted, + )?); + self.action_servers_mtx + .lock() + .unwrap() + .push(Arc::downgrade(&action_server) as Weak); + Ok(action_server) + } + /// Creates a [`GuardCondition`][1] with no callback. /// /// A weak pointer to the `GuardCondition` is stored within this node. @@ -224,7 +278,9 @@ impl Node { Arc::clone(&self.handle.context_handle), None, )); - { self.guard_conditions_mtx.lock().unwrap() } + self.guard_conditions_mtx + .lock() + .unwrap() .push(Arc::downgrade(&guard_condition) as Weak); guard_condition } @@ -246,7 +302,9 @@ impl Node { Arc::clone(&self.handle.context_handle), Some(Box::new(callback) as Box), )); - { self.guard_conditions_mtx.lock().unwrap() } + self.guard_conditions_mtx + .lock() + .unwrap() .push(Arc::downgrade(&guard_condition) as Weak); guard_condition } @@ -285,7 +343,9 @@ impl Node { topic, callback, )?); - { self.services_mtx.lock().unwrap() } + self.services_mtx + .lock() + .unwrap() .push(Arc::downgrade(&service) as Weak); Ok(service) } @@ -309,7 +369,8 @@ impl Node { qos, callback, )?); - { self.subscriptions_mtx.lock() } + self.subscriptions_mtx + .lock() .unwrap() .push(Arc::downgrade(&subscription) as Weak); Ok(subscription) @@ -317,28 +378,54 @@ impl Node { /// Returns the subscriptions that have not been dropped yet. pub(crate) fn live_subscriptions(&self) -> Vec> { - { self.subscriptions_mtx.lock().unwrap() } + self.subscriptions_mtx + .lock() + .unwrap() .iter() .filter_map(Weak::upgrade) .collect() } pub(crate) fn live_clients(&self) -> Vec> { - { self.clients_mtx.lock().unwrap() } + self.clients_mtx + .lock() + .unwrap() .iter() .filter_map(Weak::upgrade) .collect() } pub(crate) fn live_guard_conditions(&self) -> Vec> { - { self.guard_conditions_mtx.lock().unwrap() } + self.guard_conditions_mtx + .lock() + .unwrap() .iter() .filter_map(Weak::upgrade) .collect() } pub(crate) fn live_services(&self) -> Vec> { - { self.services_mtx.lock().unwrap() } + self.services_mtx + .lock() + .unwrap() + .iter() + .filter_map(Weak::upgrade) + .collect() + } + + pub(crate) fn live_action_clients(&self) -> Vec> { + self.action_clients_mtx + .lock() + .unwrap() + .iter() + .filter_map(Weak::upgrade) + .collect() + } + + pub(crate) fn live_action_servers(&self) -> Vec> { + self.action_servers_mtx + .lock() + .unwrap() .iter() .filter_map(Weak::upgrade) .collect() diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index 011d5d2f3..d1a9a698b 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -314,6 +314,8 @@ impl NodeBuilder { guard_conditions_mtx: Mutex::new(vec![]), services_mtx: Mutex::new(vec![]), subscriptions_mtx: Mutex::new(vec![]), + action_servers_mtx: Mutex::new(vec![]), + action_clients_mtx: Mutex::new(vec![]), time_source: TimeSource::builder(self.clock_type) .clock_qos(self.clock_qos) .build(), diff --git a/rclrs/src/rcl_bindings.rs b/rclrs/src/rcl_bindings.rs index 94491bc91..4eec1acb9 100644 --- a/rclrs/src/rcl_bindings.rs +++ b/rclrs/src/rcl_bindings.rs @@ -138,6 +138,7 @@ cfg_if::cfg_if! { pub struct rosidl_message_type_support_t; pub const RMW_GID_STORAGE_SIZE: usize = 24; + pub const RCL_ACTION_UUID_SIZE: usize = 16; extern "C" { pub fn rcl_context_is_valid(context: *const rcl_context_t) -> bool; @@ -146,5 +147,6 @@ cfg_if::cfg_if! { include!(concat!(env!("OUT_DIR"), "/rcl_bindings_generated.rs")); pub const RMW_GID_STORAGE_SIZE: usize = rmw_gid_storage_size_constant; + pub const RCL_ACTION_UUID_SIZE: usize = rcl_action_uuid_size_constant; } } diff --git a/rclrs/src/rcl_wrapper.h b/rclrs/src/rcl_wrapper.h index fe97cb4e5..33aae1a81 100644 --- a/rclrs/src/rcl_wrapper.h +++ b/rclrs/src/rcl_wrapper.h @@ -1,5 +1,6 @@ #include #include +#include #include #include #include @@ -7,3 +8,4 @@ #include const size_t rmw_gid_storage_size_constant = RMW_GID_STORAGE_SIZE; +const size_t rcl_action_uuid_size_constant = UUID_SIZE; diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 2ef99c026..68c3570fe 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -18,9 +18,13 @@ use std::{sync::Arc, time::Duration, vec::Vec}; use crate::{ + action::{ + client::ReadyMode as ActionClientReadyMode, server::ReadyMode as ActionServerReadyMode, + }, error::{to_rclrs_result, RclReturnCode, RclrsError, ToResult}, rcl_bindings::*, - ClientBase, Context, ContextHandle, Node, ServiceBase, SubscriptionBase, + ActionClientBase, ActionServerBase, ClientBase, Context, ContextHandle, Node, ServiceBase, + SubscriptionBase, }; mod exclusivity_guard; @@ -50,6 +54,8 @@ pub struct WaitSet { // The guard conditions that are currently registered in the wait set. guard_conditions: Vec>>, services: Vec>>, + action_clients: Vec>>, + action_servers: Vec>>, handle: WaitSetHandle, } @@ -63,6 +69,10 @@ pub struct ReadyEntities { pub guard_conditions: Vec>, /// A list of services that have potentially received requests. pub services: Vec>, + /// A list of action clients and the ways in which they are ready. + pub action_clients: Vec<(Arc, ActionClientReadyMode)>, + /// A list of action servers and the ways in which they are ready. + pub action_servers: Vec<(Arc, ActionServerReadyMode)>, } impl Drop for rcl_wait_set_t { @@ -123,6 +133,8 @@ impl WaitSet { guard_conditions: Vec::new(), clients: Vec::new(), services: Vec::new(), + action_clients: Vec::new(), + action_servers: Vec::new(), handle: WaitSetHandle { rcl_wait_set, context_handle: Arc::clone(&context.handle), @@ -138,16 +150,41 @@ impl WaitSet { let live_clients = node.live_clients(); let live_guard_conditions = node.live_guard_conditions(); let live_services = node.live_services(); + let live_action_clients = node.live_action_clients(); + let live_action_servers = node.live_action_servers(); let ctx = Context { handle: Arc::clone(&node.handle.context_handle), }; + + let mut num_subscriptions = live_subscriptions.len(); + let mut num_guard_conditions = live_guard_conditions.len(); + let mut num_timers = 0; + let mut num_clients = live_clients.len(); + let mut num_services = live_services.len(); + let mut num_events = 0; + + let action_client_entities = live_action_clients + .iter() + .map(|client| client.num_entities()); + let action_server_entities = live_action_servers + .iter() + .map(|server| server.num_entities()); + for num_entities in action_client_entities.chain(action_server_entities) { + num_subscriptions += num_entities.num_subscriptions; + num_timers += num_entities.num_timers; + num_guard_conditions += num_entities.num_guard_conditions; + num_clients += num_entities.num_clients; + num_services += num_entities.num_services; + num_events += num_entities.num_events; + } + let mut wait_set = WaitSet::new( - live_subscriptions.len(), - live_guard_conditions.len(), - 0, - live_clients.len(), - live_services.len(), - 0, + num_subscriptions, + num_guard_conditions, + num_timers, + num_clients, + num_services, + num_events, &ctx, )?; @@ -166,6 +203,15 @@ impl WaitSet { for live_service in &live_services { wait_set.add_service(live_service.clone())?; } + + for live_action_client in &live_action_clients { + wait_set.add_action_client(live_action_client.clone())?; + } + + for live_action_server in &live_action_servers { + wait_set.add_action_server(live_action_server.clone())?; + } + Ok(wait_set) } @@ -178,6 +224,8 @@ impl WaitSet { self.guard_conditions.clear(); self.clients.clear(); self.services.clear(); + self.action_clients.clear(); + self.action_servers.clear(); // This cannot fail – the rcl_wait_set_clear function only checks that the input handle is // valid, which it always is in our case. Hence, only debug_assert instead of returning // Result. @@ -311,6 +359,73 @@ impl WaitSet { Ok(()) } + /// Adds an action client to the wait set. + /// + /// # Errors + /// - If the action client was already added to this wait set or another one, + /// [`AlreadyAddedToWaitSet`][1] will be returned + /// - If the number of entities in the wait set would be larger than the + /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned + /// + /// [1]: crate::RclrsError + /// [2]: crate::RclReturnCode + pub fn add_action_client( + &mut self, + action_client: Arc, + ) -> Result<(), RclrsError> { + let exclusive_client = ExclusivityGuard::new( + Arc::clone(&action_client), + Arc::clone(&action_client.handle().in_use_by_wait_set), + )?; + unsafe { + // SAFETY: I'm not sure if it's required, but the action client pointer will remain + // valid for as long as the wait set exists, because it's stored in self.action_clients. + // Passing in a null pointer for the third and fourth arguments is explicitly allowed. + rcl_action_wait_set_add_action_client( + &mut self.handle.rcl_wait_set, + &*action_client.handle().lock(), + core::ptr::null_mut(), + core::ptr::null_mut(), + ) + } + .ok()?; + self.action_clients.push(exclusive_client); + Ok(()) + } + + /// Adds an action server to the wait set. + /// + /// # Errors + /// - If the action server was already added to this wait set or another one, + /// [`AlreadyAddedToWaitSet`][1] will be returned + /// - If the number of entities in the wait set would be larger than the + /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned + /// + /// [1]: crate::RclrsError + /// [2]: crate::RclReturnCode + pub fn add_action_server( + &mut self, + action_server: Arc, + ) -> Result<(), RclrsError> { + let exclusive_server = ExclusivityGuard::new( + Arc::clone(&action_server), + Arc::clone(&action_server.handle().in_use_by_wait_set), + )?; + unsafe { + // SAFETY: I'm not sure if it's required, but the action server pointer will remain + // valid for as long as the wait set exists, because it's stored in self.action_servers. + // Passing in a null pointer for the third argument is explicitly allowed. + rcl_action_wait_set_add_action_server( + &mut self.handle.rcl_wait_set, + &*action_server.handle().lock(), + core::ptr::null_mut(), + ) + } + .ok()?; + self.action_servers.push(exclusive_server); + Ok(()) + } + /// Blocks until the wait set is ready, or until the timeout has been exceeded. /// /// If the timeout is `None` then this function will block indefinitely until @@ -347,8 +462,9 @@ impl WaitSet { }; // SAFETY: The comments in rcl mention "This function cannot operate on the same wait set // in multiple threads, and the wait sets may not share content." - // We cannot currently guarantee that the wait sets may not share content, but it is - // mentioned in the doc comment for `add_subscription`. + // By taking exclusive ownership of `self`, we can guarantee that the wait set is not in + // use from another thread. We guarantee that waits sets may not share content using + // `ExclusivityGuard`s on each entity added. // Also, the rcl_wait_set is obviously valid. match unsafe { rcl_wait(&mut self.handle.rcl_wait_set, timeout_ns) }.ok() { Ok(_) => (), @@ -365,6 +481,8 @@ impl WaitSet { clients: Vec::new(), guard_conditions: Vec::new(), services: Vec::new(), + action_clients: Vec::new(), + action_servers: Vec::new(), }; for (i, subscription) in self.subscriptions.iter().enumerate() { // SAFETY: The `subscriptions` entry is an array of pointers, and this dereferencing is @@ -409,10 +527,117 @@ impl WaitSet { ready_entities.services.push(Arc::clone(&service.waitable)); } } + + for action_client in &self.action_clients { + let mut is_feedback_ready = false; + let mut is_status_ready = false; + let mut is_goal_response_ready = false; + let mut is_cancel_response_ready = false; + let mut is_result_response_ready = false; + // SAFETY: The wait set is exclusively owned by this function, which guarantees thread + // safety. + unsafe { + rcl_action_client_wait_set_get_entities_ready( + &self.handle.rcl_wait_set, + &*action_client.waitable.handle().lock(), + &mut is_feedback_ready, + &mut is_status_ready, + &mut is_goal_response_ready, + &mut is_cancel_response_ready, + &mut is_result_response_ready, + ) + .ok()?; + } + if is_feedback_ready { + ready_entities.action_clients.push(( + Arc::clone(&action_client.waitable), + ActionClientReadyMode::Feedback, + )); + } + if is_status_ready { + ready_entities.action_clients.push(( + Arc::clone(&action_client.waitable), + ActionClientReadyMode::Status, + )); + } + if is_goal_response_ready { + ready_entities.action_clients.push(( + Arc::clone(&action_client.waitable), + ActionClientReadyMode::GoalResponse, + )); + } + if is_cancel_response_ready { + ready_entities.action_clients.push(( + Arc::clone(&action_client.waitable), + ActionClientReadyMode::CancelResponse, + )); + } + if is_result_response_ready { + ready_entities.action_clients.push(( + Arc::clone(&action_client.waitable), + ActionClientReadyMode::ResultResponse, + )); + } + } + + for action_server in &self.action_servers { + let mut is_goal_request_ready = false; + let mut is_cancel_request_ready = false; + let mut is_result_request_ready = false; + let mut is_goal_expired = false; + // SAFETY: The wait set is exclusively owned by this function, which guarantees thread + // safety. + unsafe { + rcl_action_server_wait_set_get_entities_ready( + &self.handle.rcl_wait_set, + &*action_server.waitable.handle().lock(), + &mut is_goal_request_ready, + &mut is_cancel_request_ready, + &mut is_result_request_ready, + &mut is_goal_expired, + ) + .ok()?; + } + if is_goal_request_ready { + ready_entities.action_servers.push(( + Arc::clone(&action_server.waitable), + ActionServerReadyMode::GoalRequest, + )); + } + if is_cancel_request_ready { + ready_entities.action_servers.push(( + Arc::clone(&action_server.waitable), + ActionServerReadyMode::CancelRequest, + )); + } + if is_result_request_ready { + ready_entities.action_servers.push(( + Arc::clone(&action_server.waitable), + ActionServerReadyMode::ResultRequest, + )); + } + if is_goal_expired { + ready_entities.action_servers.push(( + Arc::clone(&action_server.waitable), + ActionServerReadyMode::GoalExpired, + )); + } + } + Ok(ready_entities) } } +#[derive(Default)] +pub struct WaitableNumEntities { + pub(crate) num_subscriptions: usize, + pub(crate) num_guard_conditions: usize, + pub(crate) num_timers: usize, + pub(crate) num_clients: usize, + pub(crate) num_services: usize, + pub(crate) num_events: usize, +} + #[cfg(test)] mod tests { use super::*; diff --git a/rosidl_generator_rs/cmake/rosidl_generator_rs_generate_interfaces.cmake b/rosidl_generator_rs/cmake/rosidl_generator_rs_generate_interfaces.cmake index 9cfdfa579..1dc47514f 100644 --- a/rosidl_generator_rs/cmake/rosidl_generator_rs_generate_interfaces.cmake +++ b/rosidl_generator_rs/cmake/rosidl_generator_rs_generate_interfaces.cmake @@ -37,13 +37,13 @@ foreach(_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES}) if(_parent_folder STREQUAL "msg") set(_has_msg TRUE) - set(_idl_file_without_actions ${_idl_file_without_actions} ${_idl_file}) + set(_idl_files ${_idl_files} ${_idl_file}) elseif(_parent_folder STREQUAL "srv") set(_has_srv TRUE) - set(_idl_file_without_actions ${_idl_file_without_actions} ${_idl_file}) + set(_idl_files ${_idl_files} ${_idl_file}) elseif(_parent_folder STREQUAL "action") set(_has_action TRUE) - message(WARNING "Rust actions generation is not implemented") + set(_idl_files ${_idl_files} ${_idl_file}) else() message(FATAL_ERROR "Interface file with unknown parent folder: ${_idl_file}") endif() @@ -81,12 +81,13 @@ endforeach() set(target_dependencies "${rosidl_generator_rs_BIN}" ${rosidl_generator_rs_GENERATOR_FILES} + "${rosidl_generator_rs_TEMPLATE_DIR}/action.rs.em" "${rosidl_generator_rs_TEMPLATE_DIR}/msg_idiomatic.rs.em" "${rosidl_generator_rs_TEMPLATE_DIR}/msg_rmw.rs.em" "${rosidl_generator_rs_TEMPLATE_DIR}/msg.rs.em" "${rosidl_generator_rs_TEMPLATE_DIR}/srv.rs.em" ${rosidl_generate_interfaces_ABS_IDL_FILES} - ${_idl_file_without_actions} + ${_idl_files} ${_dependency_files}) foreach(dep ${target_dependencies}) if(NOT EXISTS "${dep}") @@ -99,7 +100,7 @@ rosidl_write_generator_arguments( "${generator_arguments_file}" PACKAGE_NAME "${PROJECT_NAME}" IDL_TUPLES "${rosidl_generate_interfaces_IDL_TUPLES}" - ROS_INTERFACE_FILES "${_idl_file_without_actions}" + ROS_INTERFACE_FILES "${_idl_files}" ROS_INTERFACE_DEPENDENCIES "${_dependencies}" OUTPUT_DIR "${_output_path}" TEMPLATE_DIR "${rosidl_generator_rs_TEMPLATE_DIR}" diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em new file mode 100644 index 000000000..4c72b90b8 --- /dev/null +++ b/rosidl_generator_rs/resource/action.rs.em @@ -0,0 +1,175 @@ +@{ +from rosidl_parser.definition import ( + ACTION_FEEDBACK_MESSAGE_SUFFIX, + ACTION_FEEDBACK_SUFFIX, + ACTION_GOAL_SERVICE_SUFFIX, + ACTION_GOAL_SUFFIX, + ACTION_RESULT_SERVICE_SUFFIX, + ACTION_RESULT_SUFFIX, + SERVICE_REQUEST_MESSAGE_SUFFIX, + SERVICE_RESPONSE_MESSAGE_SUFFIX, +) + +action_msg_specs = [] + +for subfolder, action in action_specs: + action_msg_specs.append((subfolder, action.goal)) + action_msg_specs.append((subfolder, action.result)) + action_msg_specs.append((subfolder, action.feedback)) + action_msg_specs.append((subfolder, action.feedback_message)) + +action_srv_specs = [] + +for subfolder, action in action_specs: + action_srv_specs.append((subfolder, action.send_goal_service)) + action_srv_specs.append((subfolder, action.get_result_service)) +}@ + +pub mod rmw { +@{ +TEMPLATE( + 'msg_rmw.rs.em', + package_name=package_name, interface_path=interface_path, + msg_specs=action_msg_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) + +TEMPLATE( + 'srv_rmw.rs.em', + package_name=package_name, interface_path=interface_path, + srv_specs=action_srv_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ +} // mod rmw + +@{ +TEMPLATE( + 'msg_idiomatic.rs.em', + package_name=package_name, interface_path=interface_path, + msg_specs=action_msg_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ + +@{ +TEMPLATE( + 'srv_idiomatic.rs.em', + package_name=package_name, interface_path=interface_path, + srv_specs=action_srv_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ + +@[for subfolder, action_spec in action_specs] + +@{ +type_name = action_spec.namespaced_type.name +}@ + +#[link(name = "@(package_name)__rosidl_typesupport_c")] +extern "C" { + fn rosidl_typesupport_c__get_action_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::os::raw::c_void; +} + +// Corresponds to @(package_name)__@(subfolder)__@(type_name) +pub struct @(type_name); + +impl rosidl_runtime_rs::Action for @(type_name) { + type Goal = crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SUFFIX); + type Result = crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SUFFIX); + type Feedback = crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_SUFFIX); + + fn get_type_support() -> *const std::os::raw::c_void { + // SAFETY: No preconditions for this function. + unsafe { rosidl_typesupport_c__get_action_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } + } +} + +impl rosidl_runtime_rs::ActionImpl for @(type_name) { + type GoalStatusMessage = action_msgs::msg::rmw::GoalStatusArray; + type FeedbackMessage = crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_MESSAGE_SUFFIX); + + type SendGoalService = crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX); + type CancelGoalService = action_msgs::srv::rmw::CancelGoal; + type GetResultService = crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX); + + fn create_goal_request(goal_id: &[u8; 16], goal: crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SUFFIX)) -> crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX) { + crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX) { + goal_id: unique_identifier_msgs::msg::rmw::UUID { uuid: *goal_id }, + goal, + } + } + + fn get_goal_request_uuid(request: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX)) -> &[u8; 16] { + &request.goal_id.uuid + } + + fn create_goal_response(accepted: bool, stamp: (i32, u32)) -> crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX) { + crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX) { + accepted, + stamp: builtin_interfaces::msg::rmw::Time { + sec: stamp.0, + nanosec: stamp.1, + }, + } + } + + fn get_goal_response_accepted(response: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX)) -> bool { + response.accepted + } + + fn get_goal_response_stamp(response: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX)) -> (i32, u32) { + (response.stamp.sec, response.stamp.nanosec) + } + + fn create_feedback_message(goal_id: &[u8; 16], feedback: crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_SUFFIX)) -> crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_MESSAGE_SUFFIX) { + let mut message = crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_MESSAGE_SUFFIX)::default(); + message.goal_id.uuid = *goal_id; + message.feedback = feedback; + message + } + + fn get_feedback_message_uuid(feedback: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_MESSAGE_SUFFIX)) -> &[u8; 16] { + &feedback.goal_id.uuid + } + + fn get_feedback_message_feedback(feedback: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_MESSAGE_SUFFIX)) -> &crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_SUFFIX) { + &feedback.feedback + } + + fn create_result_request(goal_id: &[u8; 16]) -> crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX) { + crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX) { + goal_id: unique_identifier_msgs::msg::rmw::UUID { uuid: *goal_id }, + } + } + + fn get_result_request_uuid(request: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX)) -> &[u8; 16] { + &request.goal_id.uuid + } + + fn create_result_response(status: i8, result: crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SUFFIX)) -> crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX) { + crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX) { + status, + result, + } + } + + fn get_result_response_result(response: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX)) -> &crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SUFFIX) { + &response.result + } + + fn get_result_response_status(response: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX)) -> i8 { + response.status + } +} + +@[end for] diff --git a/rosidl_generator_rs/resource/lib.rs.em b/rosidl_generator_rs/resource/lib.rs.em index 51e4a5ba4..79a0e1def 100644 --- a/rosidl_generator_rs/resource/lib.rs.em +++ b/rosidl_generator_rs/resource/lib.rs.em @@ -7,3 +7,7 @@ pub mod msg; @[if len(srv_specs) > 0]@ pub mod srv; @[end if]@ + +@[if len(action_specs) > 0]@ +pub mod action; +@[end if]@ diff --git a/rosidl_generator_rs/resource/srv.rs.em b/rosidl_generator_rs/resource/srv.rs.em index 369696ff7..dd99e8e76 100644 --- a/rosidl_generator_rs/resource/srv.rs.em +++ b/rosidl_generator_rs/resource/srv.rs.em @@ -1,84 +1,23 @@ -@{ -req_res_specs = [] - -for subfolder, service in srv_specs: - req_res_specs.append((subfolder, service.request_message)) - req_res_specs.append((subfolder, service.response_message)) -}@ - @{ TEMPLATE( - 'msg_idiomatic.rs.em', + 'srv_idiomatic.rs.em', package_name=package_name, interface_path=interface_path, - msg_specs=req_res_specs, + srv_specs=srv_specs, get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, pre_field_serde=pre_field_serde, get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) -}@ - -@[for subfolder, srv_spec in srv_specs] - -@{ -type_name = srv_spec.namespaced_type.name -}@ - -#[link(name = "@(package_name)__rosidl_typesupport_c")] -extern "C" { - fn rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::os::raw::c_void; } -// Corresponds to @(package_name)__@(subfolder)__@(type_name) -pub struct @(type_name); - -impl rosidl_runtime_rs::Service for @(type_name) { - type Request = crate::@(subfolder)::@(type_name)_Request; - type Response = crate::@(subfolder)::@(type_name)_Response; - - fn get_type_support() -> *const std::os::raw::c_void { - // SAFETY: No preconditions for this function. - unsafe { rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } - } -} - -@[end for] - pub mod rmw { @{ TEMPLATE( - 'msg_rmw.rs.em', + 'srv_rmw.rs.em', package_name=package_name, interface_path=interface_path, - msg_specs=req_res_specs, + srv_specs=srv_specs, get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, pre_field_serde=pre_field_serde, get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) }@ - -@[for subfolder, srv_spec in srv_specs] - -@{ -type_name = srv_spec.namespaced_type.name -}@ - - #[link(name = "@(package_name)__rosidl_typesupport_c")] - extern "C" { - fn rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::os::raw::c_void; - } - - // Corresponds to @(package_name)__@(subfolder)__@(type_name) - pub struct @(type_name); - - impl rosidl_runtime_rs::Service for @(type_name) { - type Request = crate::@(subfolder)::rmw::@(type_name)_Request; - type Response = crate::@(subfolder)::rmw::@(type_name)_Response; - - fn get_type_support() -> *const std::os::raw::c_void { - // SAFETY: No preconditions for this function. - unsafe { rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } - } - } - -@[end for] - } // mod rmw diff --git a/rosidl_generator_rs/resource/srv_idiomatic.rs.em b/rosidl_generator_rs/resource/srv_idiomatic.rs.em new file mode 100644 index 000000000..cf202ca01 --- /dev/null +++ b/rosidl_generator_rs/resource/srv_idiomatic.rs.em @@ -0,0 +1,44 @@ +@{ +req_res_specs = [] + +for subfolder, service in srv_specs: + req_res_specs.append((subfolder, service.request_message)) + req_res_specs.append((subfolder, service.response_message)) +}@ + +@{ +TEMPLATE( + 'msg_idiomatic.rs.em', + package_name=package_name, interface_path=interface_path, + msg_specs=req_res_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ + +@[for subfolder, srv_spec in srv_specs] + +@{ +type_name = srv_spec.namespaced_type.name +}@ + +#[link(name = "@(package_name)__rosidl_typesupport_c")] +extern "C" { + fn rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::os::raw::c_void; +} + +// Corresponds to @(package_name)__@(subfolder)__@(type_name) +pub struct @(type_name); + +impl rosidl_runtime_rs::Service for @(type_name) { + type Request = crate::@(subfolder)::@(type_name)_Request; + type Response = crate::@(subfolder)::@(type_name)_Response; + + fn get_type_support() -> *const std::os::raw::c_void { + // SAFETY: No preconditions for this function. + unsafe { rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } + } +} + +@[end for] diff --git a/rosidl_generator_rs/resource/srv_rmw.rs.em b/rosidl_generator_rs/resource/srv_rmw.rs.em new file mode 100644 index 000000000..c7c04c2ad --- /dev/null +++ b/rosidl_generator_rs/resource/srv_rmw.rs.em @@ -0,0 +1,44 @@ +@{ +req_res_specs = [] + +for subfolder, service in srv_specs: + req_res_specs.append((subfolder, service.request_message)) + req_res_specs.append((subfolder, service.response_message)) +}@ + +@{ +TEMPLATE( + 'msg_rmw.rs.em', + package_name=package_name, interface_path=interface_path, + msg_specs=req_res_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ + +@[for subfolder, srv_spec in srv_specs] + +@{ +type_name = srv_spec.namespaced_type.name +}@ + + #[link(name = "@(package_name)__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::os::raw::c_void; + } + + // Corresponds to @(package_name)__@(subfolder)__@(type_name) + pub struct @(type_name); + + impl rosidl_runtime_rs::Service for @(type_name) { + type Request = crate::@(subfolder)::rmw::@(type_name)_Request; + type Response = crate::@(subfolder)::rmw::@(type_name)_Response; + + fn get_type_support() -> *const std::os::raw::c_void { + // SAFETY: No preconditions for this function. + unsafe { rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } + } + } + +@[end for] diff --git a/rosidl_generator_rs/rosidl_generator_rs/__init__.py b/rosidl_generator_rs/rosidl_generator_rs/__init__.py index 502d1d34d..b7850a6d8 100644 --- a/rosidl_generator_rs/rosidl_generator_rs/__init__.py +++ b/rosidl_generator_rs/rosidl_generator_rs/__init__.py @@ -23,6 +23,11 @@ import rosidl_pycommon from rosidl_parser.definition import AbstractGenericString +from rosidl_parser.definition import AbstractNestedType +from rosidl_parser.definition import AbstractSequence +from rosidl_parser.definition import AbstractString +from rosidl_parser.definition import AbstractWString +from rosidl_parser.definition import Action from rosidl_parser.definition import Array from rosidl_parser.definition import BasicType from rosidl_parser.definition import BoundedSequence @@ -86,6 +91,10 @@ def generate_rs(generator_arguments_file, typesupport_impls): os.path.join(template_dir, 'srv.rs.em'): ['rust/src/%s.rs'], } + mapping_actions = { + os.path.join(template_dir, 'action.rs.em'): ['rust/src/%s.rs'], + } + # Ensure the required templates exist for template_file in mapping_msgs.keys(): assert os.path.exists(template_file), \ @@ -93,6 +102,9 @@ def generate_rs(generator_arguments_file, typesupport_impls): for template_file in mapping_srvs.keys(): assert os.path.exists(template_file), \ 'Services template file %s not found' % template_file + for template_file in mapping_actions.keys(): + assert os.path.exists(template_file), \ + 'Actions template file %s not found' % template_file data = { 'pre_field_serde': pre_field_serde, @@ -107,6 +119,7 @@ def generate_rs(generator_arguments_file, typesupport_impls): convert_lower_case_underscore_to_camel_case, 'msg_specs': [], 'srv_specs': [], + 'action_specs': [], 'package_name': args['package_name'], 'typesupport_impls': typesupport_impls, 'interface_path': idl_rel_path, @@ -121,6 +134,9 @@ def generate_rs(generator_arguments_file, typesupport_impls): for service in idl_content.get_elements_of_type(Service): data['srv_specs'].append(('srv', service)) + for action in idl_content.get_elements_of_type(Action): + data['action_specs'].append(('action', action)) + if data['msg_specs']: for template_file, generated_filenames in mapping_msgs.items(): for generated_filename in generated_filenames: @@ -143,6 +159,17 @@ def generate_rs(generator_arguments_file, typesupport_impls): generated_file, minimum_timestamp=latest_target_timestamp) + if data['action_specs']: + for template_file, generated_filenames in mapping_actions.items(): + for generated_filename in generated_filenames: + generated_file = os.path.join(args['output_dir'], + generated_filename % 'action') + rosidl_pycommon.expand_template( + os.path.join(template_dir, template_file), + data.copy(), + generated_file, + minimum_timestamp=latest_target_timestamp) + rosidl_pycommon.expand_template( os.path.join(template_dir, 'lib.rs.em'), data.copy(), diff --git a/rosidl_runtime_rs/src/lib.rs b/rosidl_runtime_rs/src/lib.rs index 93f844192..7c5bad461 100644 --- a/rosidl_runtime_rs/src/lib.rs +++ b/rosidl_runtime_rs/src/lib.rs @@ -9,4 +9,4 @@ mod string; pub use string::{BoundedString, BoundedWString, String, StringExceedsBoundsError, WString}; mod traits; -pub use traits::{Message, RmwMessage, SequenceAlloc, Service}; +pub use traits::{Action, ActionImpl, Message, RmwMessage, SequenceAlloc, Service}; diff --git a/rosidl_runtime_rs/src/traits.rs b/rosidl_runtime_rs/src/traits.rs index 15f206108..d0402dd5f 100644 --- a/rosidl_runtime_rs/src/traits.rs +++ b/rosidl_runtime_rs/src/traits.rs @@ -159,3 +159,79 @@ pub trait Service: 'static { /// Get a pointer to the correct `rosidl_service_type_support_t` structure. fn get_type_support() -> *const std::os::raw::c_void; } + +/// Trait for actions. +/// +/// User code never needs to call this trait's method, much less implement this trait. +pub trait Action: 'static { + /// The goal message associated with this action. + type Goal: Message; + + /// The result message associated with this action. + type Result: Message; + + /// The feedback message associated with this action. + type Feedback: Message; + + /// Get a pointer to the correct `rosidl_action_type_support_t` structure. + fn get_type_support() -> *const std::os::raw::c_void; +} + +/// Trait for action implementation details. +/// +/// User code never needs to implement this trait, nor use its associated types. +pub trait ActionImpl: 'static + Action { + /// The goal_status message associated with this action. + type GoalStatusMessage: Message; + + /// The feedback message associated with this action. + type FeedbackMessage: Message; + + /// The send_goal service associated with this action. + type SendGoalService: Service; + + /// The cancel_goal service associated with this action. + type CancelGoalService: Service; + + /// The get_result service associated with this action. + type GetResultService: Service; + + /// Create a goal request message with the given UUID and goal. + fn create_goal_request(goal_id: &[u8; 16], goal: <::Goal as Message>::RmwMsg) -> <::Request as Message>::RmwMsg; + + /// Get the UUID of a goal request. + fn get_goal_request_uuid(request: &<::Request as Message>::RmwMsg) -> &[u8; 16]; + + /// Create a goal response message with the given acceptance and timestamp. + fn create_goal_response(accepted: bool, stamp: (i32, u32)) -> <::Response as Message>::RmwMsg; + + /// Get the `accepted` field of a goal response. + fn get_goal_response_accepted(response: &<::Response as Message>::RmwMsg) -> bool; + + /// Get the `stamp` field of a goal response. + fn get_goal_response_stamp(response: &<::Response as Message>::RmwMsg) -> (i32, u32); + + /// Create a feedback message with the given goal ID and contents. + fn create_feedback_message(goal_id: &[u8; 16], feedback: <::Feedback as Message>::RmwMsg) -> ::RmwMsg; + + /// Get the UUID of a feedback message. + fn get_feedback_message_uuid(feedback: &::RmwMsg) -> &[u8; 16]; + + /// Get the feedback of a feedback message. + fn get_feedback_message_feedback(feedback: &::RmwMsg) -> &<::Feedback as Message>::RmwMsg; + + /// Create a result request message with the given goal ID. + fn create_result_request(goal_id: &[u8; 16]) -> <::Request as Message>::RmwMsg; + + /// Get the UUID of a result request. + fn get_result_request_uuid(request: &<::Request as Message>::RmwMsg) -> &[u8; 16]; + + /// Create a result response message with the given status and contents. + fn create_result_response(status: i8, result: <::Result as Message>::RmwMsg) -> <::Response as Message>::RmwMsg; + + /// Get the result of a result response. + fn get_result_response_result(response: &<::Response as Message>::RmwMsg) -> &<::Result as Message>::RmwMsg; + + /// Get the status of a result response. + fn get_result_response_status(response: &<::Response as Message>::RmwMsg) -> i8; +}