safe_drive/
error.rs

1//! Errors returned by ROS2.
2
3use crate::rcl;
4use num_derive::{FromPrimitive, ToPrimitive};
5use num_traits::FromPrimitive;
6use std::{
7    error::Error,
8    fmt::{self, Debug},
9};
10
11#[repr(u32)]
12#[derive(Debug, Clone, Copy, PartialEq, Eq, FromPrimitive, ToPrimitive)]
13pub enum RCLError {
14    Error = rcl::RCL_RET_ERROR,
15    Timeout = rcl::RCL_RET_TIMEOUT,
16    BadAlloc = rcl::RCL_RET_BAD_ALLOC,
17    InvalidArgument = rcl::RCL_RET_INVALID_ARGUMENT,
18    Unsupported = rcl::RCL_RET_UNSUPPORTED,
19    AlreadyInit = rcl::RCL_RET_ALREADY_INIT,
20    NotInit = rcl::RCL_RET_NOT_INIT,
21    MismatchedRmwId = rcl::RCL_RET_MISMATCHED_RMW_ID,
22    TopicNameInvalid = rcl::RCL_RET_TOPIC_NAME_INVALID,
23    ServiceNameInvalid = rcl::RCL_RET_SERVICE_NAME_INVALID,
24    UnknownSubstitution = rcl::RCL_RET_UNKNOWN_SUBSTITUTION,
25    AlreadyShutdown = rcl::RCL_RET_ALREADY_SHUTDOWN,
26    NodeInvalid = rcl::RCL_RET_NODE_INVALID,
27    NodeInvalidName = rcl::RCL_RET_NODE_INVALID_NAME,
28    NodeInvalidNamespace = rcl::RCL_RET_NODE_INVALID_NAMESPACE,
29    NodeNameNonExistent = rcl::RCL_RET_NODE_NAME_NON_EXISTENT,
30    PublisherInvalid = rcl::RCL_RET_PUBLISHER_INVALID,
31    SubscriptionInvalid = rcl::RCL_RET_SUBSCRIPTION_INVALID,
32    SubscriptionTakeFailed = rcl::RCL_RET_SUBSCRIPTION_TAKE_FAILED,
33    ClientInvalid = rcl::RCL_RET_CLIENT_INVALID,
34    ClientTakeFailed = rcl::RCL_RET_CLIENT_TAKE_FAILED,
35    ServiceInvalid = rcl::RCL_RET_SERVICE_INVALID,
36    ServiceTakeFailed = rcl::RCL_RET_SERVICE_TAKE_FAILED,
37    TimerInvalid = rcl::RCL_RET_TIMER_INVALID,
38    TimerCanceled = rcl::RCL_RET_TIMER_CANCELED,
39    WaitSetInvalid = rcl::RCL_RET_WAIT_SET_INVALID,
40    WaitSetEmpty = rcl::RCL_RET_WAIT_SET_EMPTY,
41    WaitSetFull = rcl::RCL_RET_WAIT_SET_FULL,
42    InvalidRemapRule = rcl::RCL_RET_INVALID_REMAP_RULE,
43    WrongLexeme = rcl::RCL_RET_WRONG_LEXEME,
44    InvalidRosArgs = rcl::RCL_RET_INVALID_ROS_ARGS,
45    InvalidParamRule = rcl::RCL_RET_INVALID_PARAM_RULE,
46    InvalidLogLevelRule = rcl::RCL_RET_INVALID_LOG_LEVEL_RULE,
47    EventInvalid = rcl::RCL_RET_EVENT_INVALID,
48    EventTakeFailed = rcl::RCL_RET_EVENT_TAKE_FAILED,
49    LifecycleStateRegistered = rcl::RCL_RET_LIFECYCLE_STATE_REGISTERED,
50    LifecycleStateNotRegistered = rcl::RCL_RET_LIFECYCLE_STATE_NOT_REGISTERED,
51    InvalidRetVal = !0,
52}
53
54impl fmt::Display for RCLError {
55    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
56        write!(f, "{:?} ({})", self, *self as u32)
57    }
58}
59
60impl Error for RCLError {}
61
62/// Result type to RCLError when encountering error.
63pub type RCLResult<T> = Result<T, RCLError>;
64
65/// Dynamic type which can be sent and shared between threads.
66pub type DynError = Box<dyn Error + Send + Sync + 'static>;
67
68/// Convert a rcl-style, C-style, return value to a Rust-style value.
69/// If `n` indicates successful, this returns Ok(()),
70/// otherwise returns Err(_).
71pub(crate) fn ret_val_to_err(n: rcl::rcl_ret_t) -> RCLResult<()> {
72    let n = n as u32;
73    if n == rcl::RCL_RET_OK {
74        Ok(())
75    } else {
76        Err(FromPrimitive::from_u32(n).unwrap_or(RCLError::InvalidRetVal))
77    }
78}
79
80//
81// Some errors in rcl and rcl_action have the same error code (e.g. RCL_RET_ACTION_NAME_INVALID ==
82// RCL_RET_EVENT_INVALID == 2000) so errors in actions are referenced through RCLActionError.
83#[repr(u32)]
84#[derive(Debug, Clone, Copy, PartialEq, Eq)]
85pub enum RCLActionError {
86    NameInvalid,
87    // RCL_RET_ACTION_GOAL_ACCEPTED and RCL_RET_ACTION_GOAL_REJECTED are not used in RCL
88    // and do not represent errors, but are kept here for consistency.
89    GoalAccepted,
90    GoalRejected,
91    ClientInvalid,
92    ClientTakeFailed,
93    ServerInvalid,
94    ServerTakeFailed,
95    GoalHandleInvalid,
96    GoalEventInvalid,
97    RCLError(RCLError),
98    InvalidRetVal,
99}
100
101impl fmt::Display for RCLActionError {
102    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
103        write!(f, "{:?} ({})", self, Into::<u32>::into(*self))
104    }
105}
106
107impl Error for RCLActionError {}
108
109/// Result type to RCLActionError when encountering error.
110pub type RCLActionResult<T> = Result<T, RCLActionError>;
111
112pub(crate) fn action_ret_val_to_err(n: rcl::rcl_ret_t) -> RCLActionResult<()> {
113    match n as u32 {
114        rcl::RCL_RET_OK => Ok(()),
115        rcl::RCL_RET_ACTION_NAME_INVALID => Err(RCLActionError::NameInvalid),
116        rcl::RCL_RET_ACTION_GOAL_ACCEPTED => Err(RCLActionError::GoalAccepted),
117        rcl::RCL_RET_ACTION_GOAL_REJECTED => Err(RCLActionError::GoalRejected),
118        rcl::RCL_RET_ACTION_CLIENT_INVALID => Err(RCLActionError::ClientInvalid),
119        rcl::RCL_RET_ACTION_CLIENT_TAKE_FAILED => Err(RCLActionError::ClientTakeFailed),
120        rcl::RCL_RET_ACTION_SERVER_INVALID => Err(RCLActionError::ServerInvalid),
121        rcl::RCL_RET_ACTION_SERVER_TAKE_FAILED => Err(RCLActionError::ServerTakeFailed),
122        rcl::RCL_RET_ACTION_GOAL_HANDLE_INVALID => Err(RCLActionError::GoalHandleInvalid),
123        rcl::RCL_RET_ACTION_GOAL_EVENT_INVALID => Err(RCLActionError::GoalEventInvalid),
124
125        _ => ret_val_to_err(n).map_err(RCLActionError::RCLError),
126    }
127}
128
129impl From<RCLActionError> for u32 {
130    fn from(val: RCLActionError) -> Self {
131        match val {
132            RCLActionError::NameInvalid => rcl::RCL_RET_ACTION_NAME_INVALID,
133            RCLActionError::GoalAccepted => rcl::RCL_RET_ACTION_GOAL_ACCEPTED,
134            RCLActionError::GoalRejected => rcl::RCL_RET_ACTION_GOAL_REJECTED,
135            RCLActionError::ClientInvalid => rcl::RCL_RET_ACTION_CLIENT_INVALID,
136            RCLActionError::ClientTakeFailed => rcl::RCL_RET_ACTION_CLIENT_TAKE_FAILED,
137            RCLActionError::ServerInvalid => rcl::RCL_RET_ACTION_SERVER_INVALID,
138            RCLActionError::ServerTakeFailed => rcl::RCL_RET_ACTION_SERVER_TAKE_FAILED,
139            RCLActionError::GoalHandleInvalid => rcl::RCL_RET_ACTION_GOAL_HANDLE_INVALID,
140            RCLActionError::GoalEventInvalid => rcl::RCL_RET_ACTION_GOAL_EVENT_INVALID,
141            RCLActionError::RCLError(err) => err as u32,
142            RCLActionError::InvalidRetVal => !0,
143        }
144    }
145}
146
147impl From<RCLError> for RCLActionError {
148    fn from(err: RCLError) -> Self {
149        RCLActionError::RCLError(err)
150    }
151}
152
153impl std::fmt::Display for rcl::rcutils_error_string_t {
154    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
155        let s = self.str_;
156        let inner: &[u8] = unsafe { std::slice::from_raw_parts(s.as_ptr() as *const u8, s.len()) };
157        let s = String::from_utf8(inner.to_vec()).unwrap();
158        write!(f, "{}", s)
159    }
160}
161
162impl Error for rcl::rcutils_error_string_t {}
safe_drive/error.rs

safe_drive/
error.rs
