diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index 712f98c5..bdcd0715 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -17,12 +17,18 @@ path = "src/lib.rs" # Needed for dynamically finding type support libraries ament_rs = { version = "0.2", optional = true } +# Needed to create the GoalClientStream +async-stream = "*" + # Needed for uploading documentation to docs.rs cfg-if = "1.0.0" # Needed for clients futures = "0.3" +# Needed for racing futures +futures-lite = { version = "2.6", features = ["std", "race"] } + # Needed for the runtime-agnostic timeout feature async-std = "1.13" @@ -36,6 +42,18 @@ rosidl_runtime_rs = "0.4" serde = { version = "1", optional = true, features = ["derive"] } serde-big-array = { version = "0.5.1", optional = true } +# Needed to watch for the cancel signal for actions. Note that this only brings +# in the sync module of tokio, which is a fairly light weight dependency. The +# channels in this module work with any async runtime, so this does not lock us +# into the tokio runtime. +tokio = { version = "1", features = ["sync"] } + +# Needed to combine concurrent streams for easier ergonomics in action clients +tokio-stream = "0.1" + +# Needed by action clients to generate UUID values for their goals +uuid = { version = "1", features = ["v4"] } + [dev-dependencies] # Needed for e.g. writing yaml files in tests tempfile = "3.3.0" diff --git a/rclrs/build.rs b/rclrs/build.rs index 28f38874..f0a3351c 100644 --- a/rclrs/build.rs +++ b/rclrs/build.rs @@ -116,6 +116,7 @@ fn main() { } println!("cargo:rustc-link-lib=dylib=rcl"); + println!("cargo:rustc-link-lib=dylib=rcl_action"); println!("cargo:rustc-link-lib=dylib=rcl_yaml_param_parser"); println!("cargo:rustc-link-lib=dylib=rcutils"); println!("cargo:rustc-link-lib=dylib=rmw"); diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs new file mode 100644 index 00000000..fd826433 --- /dev/null +++ b/rclrs/src/action.rs @@ -0,0 +1,355 @@ +use std::{collections::HashMap, ops::Deref}; + +pub(crate) mod action_client; +pub use action_client::*; + +pub(crate) mod action_goal_receiver; +pub use action_goal_receiver::*; + +pub(crate) mod action_server; +pub use action_server::*; + +use crate::{ + rcl_bindings::*, + vendor::builtin_interfaces::msg::Time, + DropGuard, + log_error, +}; +use std::fmt; + + +/// 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 GoalUuid { + /// A zeroed-out goal ID has a special meaning for cancellation requests + /// which indicates that no specific goal is being requested. + fn zero() -> Self { + Self([0; 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], + ) + } +} + +impl Deref for GoalUuid { + type Target = [u8; RCL_ACTION_UUID_SIZE]; + + fn deref(&self) -> &Self::Target { + &self.0 + } +} + +impl From<[u8; RCL_ACTION_UUID_SIZE]> for GoalUuid { + fn from(value: [u8; RCL_ACTION_UUID_SIZE]) -> Self { + Self(value) + } +} + +impl From<&[u8; RCL_ACTION_UUID_SIZE]> for GoalUuid { + fn from(value: &[u8; RCL_ACTION_UUID_SIZE]) -> Self { + Self(*value) + } +} + +/// The response returned by an [`ActionServer`]'s cancel callback when a goal is requested to be cancelled. +#[repr(i8)] +#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] +pub enum CancelResponseCode { + /// The server will try to cancel the goal. + Accept = 0, + /// The server will not try to cancel the goal. + Reject = 1, + /// The requested goal is unknown. + UnknownGoal = 2, + /// The goal already reached a terminal state. + GoalTerminated = 3, +} + +impl CancelResponseCode { + /// Check if the cancellation was accepted. + pub fn is_accepted(&self) -> bool { + matches!(self, Self::Accept) + } +} + +impl From for CancelResponseCode { + fn from(value: i8) -> Self { + if 0 <= value && value <= 3 { + unsafe { + // SAFETY: We have already ensured that the integer value is + // within the acceptable range for the enum, so transmuting is + // safe. + return std::mem::transmute(value); + } + } + + log_error!( + "cancel_response.from", + "Invalid integer value being cast to a cancel response: {value}. \ + Values should be in the range [0, 3]. We will set this as 1 (Reject).", + ); + CancelResponseCode::Reject + } +} + +/// This is returned by [`CancellationClient`] to inform whether a cancellation +/// of a single goal was successful. +/// +/// When a cancellation request might cancel multiple goals, [`MultiCancelResponse`] +/// will be used. +#[derive(Debug, Clone, PartialEq, PartialOrd)] +pub struct CancelResponse { + /// What kind of response was given. + pub code: CancelResponseCode, + /// What time the response took effect according to the action server. + /// This will be default-initialized if no goal was cancelled. + pub stamp: Option