diff --git a/rclrust/Cargo.toml b/rclrust/Cargo.toml index a0656731..87b631a2 100644 --- a/rclrust/Cargo.toml +++ b/rclrust/Cargo.toml @@ -13,3 +13,7 @@ keywords = ["ROS2"] categories = ["science::robotics"] [dependencies] +anyhow = "1.0" +thiserror = "1.0" +rcl-sys = { path = "../rcl-sys", version = "0.0.1" } +rclrust-msg = "0.0.1" diff --git a/rclrust/src/error.rs b/rclrust/src/error.rs new file mode 100644 index 00000000..899672eb --- /dev/null +++ b/rclrust/src/error.rs @@ -0,0 +1,212 @@ +use std::fmt; + +use anyhow::Result; + +use crate::internal::ffi::*; + +#[derive(Debug)] +pub struct RclErrorBase { + message: String, + file: String, + line: u64, + prefix: String, +} + +impl RclErrorBase { + unsafe fn new(error_state_ptr: *const rcl_sys::rcutils_error_state_t, prefix: String) -> Self { + let error_state = error_state_ptr.as_ref().unwrap(); + Self { + message: String::from_c_char(error_state.message.as_ptr()).unwrap(), + file: String::from_c_char(error_state.file.as_ptr()).unwrap(), + line: error_state.line_number, + prefix, + } + } +} + +impl fmt::Display for RclErrorBase { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + write!( + f, + "{}{}, at {}:{}", + self.prefix, self.message, self.file, self.line + ) + } +} + +#[derive(Debug, thiserror::Error)] +pub enum RclRustError { + #[error("Unspecified error.\n{0}")] + RclError(RclErrorBase), + #[error("Timeout occurred.\n{0}")] + RclTimeout(RclErrorBase), + #[error("Failed to allocate memory.\n{0}")] + RclBadAlloc(RclErrorBase), + #[error("Invalid argument.\n{0}")] + RclInvalidArgument(RclErrorBase), + #[error("Unsupported.\n{0}")] + RclUnsupported(RclErrorBase), + + // rcl specific ret codes start at 100 + #[error("rcl_init() already called.\n{0}")] + RclAlreadyInit(RclErrorBase), + #[error("rcl_init() not yet called.\n{0}")] + RclNotInit(RclErrorBase), + #[error("Mismatched rmw identifier.\n{0}")] + RclMismatchedRmwId(RclErrorBase), + #[error("Topic name does not pass validation.\n{0}")] + RclTopicNameInvalid(RclErrorBase), + #[error("Service name (same as topic name) does not pass validation.\n{0}")] + RclServiceNameInvalid(RclErrorBase), + #[error("Topic name substitution is unknown.\n{0}")] + RclUnknownSubstitution(RclErrorBase), + #[error("rcl_shutdown() already called.\n{0}")] + RclAlreadyShutdown(RclErrorBase), + + // rcl node specific ret codes in 2XX + #[error("Invalid rcl_node_t given.\n{0}")] + RclNodeInvalid(RclErrorBase), + #[error("Invalid node name.\n{0}")] + RclNodeInvalidName(RclErrorBase), + #[error("Invalid node namespace.\n{0}")] + RclNodeInvalidNamespace(RclErrorBase), + #[error("Failed to find node name.\n{0}")] + RclNodeNameNonExistent(RclErrorBase), + + // rcl publisher specific ret codes in 3XX + #[error("Invalid rcl_publisher_t given.\n{0}")] + RclPublisherInvalid(RclErrorBase), + + // rcl subscription specific ret codes in 4XX + #[error("Invalid rcl_subscription_t given.\n{0}")] + RclSubscriptionInvalid(RclErrorBase), + #[error("Failed to take a message from the subscription.\n{0}")] + RclSubscriptionTakeFailed(RclErrorBase), + + // rcl service client specific ret codes in 5XX + #[error("Invalid rcl_client_t given.\n{0}")] + RclClientInvalid(RclErrorBase), + #[error("Failed to take a response from the client.\n{0}")] + RclClientTakeFailed(RclErrorBase), + + // rcl service server specific ret codes in 6XX + #[error("Invalid rcl_service_t given.\n{0}")] + RclServiceInvalid(RclErrorBase), + #[error("Failed to take a request from the service.\n{0}")] + RclServiceTakeFailed(RclErrorBase), + + // rcl guard condition specific ret codes in 7XX + + // rcl timer specific ret codes in 8XX + #[error("Invalid rcl_timer_t given.\n{0}")] + RclTimerInvalid(RclErrorBase), + #[error("Given timer was canceled.\n{0}")] + RclTimerCanceled(RclErrorBase), + + // rcl wait and wait set specific ret codes in 9XX + #[error("Invalid rcl_wait_set_t given.\n{0}")] + RclWaitSetInvalid(RclErrorBase), + #[error("Given rcl_wait_set_t is empty.\n{0}")] + RclWaitSetEmpty(RclErrorBase), + #[error("Given rcl_wait_set_t is full.\n{0}")] + RclWaitSetFull(RclErrorBase), + + // rcl argument parsing specific ret codes in 1XXX + #[error("Argument is not a valid remap rule.\n{0}")] + RclInvalidRemapRule(RclErrorBase), + #[error("Expected one type of lexeme but got another.\n{0}")] + RclWrongLexeme(RclErrorBase), + #[error("Found invalid ros argument while parsing.\n{0}")] + RclInvalidROSArgs(RclErrorBase), + #[error("Argument is not a valid parameter rule.\n{0}")] + RclInvalidParamRule(RclErrorBase), + #[error("Argument is not a valid log level rule.\n{0}")] + RclInvalidLogLevelRule(RclErrorBase), + + // rcl event specific ret codes in 20XX + #[error("Invalid rcl_event_t given.\n{0}")] + RclEventInvalid(RclErrorBase), + #[error("Failed to take an event from the event handle.\n{0}")] + RclEventTakeFailed(RclErrorBase), + + // rcl_lifecycle state register ret codes in 30XX + #[error("rcl_lifecycle state registered.\n{0}")] + RclLifecycleStateRegistered(RclErrorBase), + #[error("rcl_lifecycle state not registered.\n{0}")] + RclLifecycleStateNotRegistered(RclErrorBase), + + #[error("Runtime Error: {0}")] + RuntimeError(&'static str), +} + +pub(crate) fn result_from_rcl_ret(ret: rcl_sys::rcl_ret_t, prefix: Option<&str>) -> Result<()> { + if ret as u32 == rcl_sys::RCL_RET_OK { + return Ok(()); + } + let error_state = unsafe { rcl_sys::rcutils_get_error_state() }; + if error_state.is_null() { + return Err(RclRustError::RuntimeError("rcl error state is not set").into()); + } + let formatted_prefix = prefix.map_or("".into(), |v| format!("[{}]: ", v)); + let base_error = unsafe { RclErrorBase::new(error_state, formatted_prefix) }; + + let error = { + use rcl_sys::*; + match ret as u32 { + RCL_RET_TIMEOUT => RclRustError::RclTimeout(base_error), + RCL_RET_BAD_ALLOC => RclRustError::RclBadAlloc(base_error), + RCL_RET_INVALID_ARGUMENT => RclRustError::RclInvalidArgument(base_error), + RCL_RET_UNSUPPORTED => RclRustError::RclUnsupported(base_error), + + RCL_RET_ALREADY_INIT => RclRustError::RclAlreadyInit(base_error), + RCL_RET_NOT_INIT => RclRustError::RclNotInit(base_error), + RCL_RET_MISMATCHED_RMW_ID => RclRustError::RclMismatchedRmwId(base_error), + RCL_RET_TOPIC_NAME_INVALID => RclRustError::RclTopicNameInvalid(base_error), + RCL_RET_SERVICE_NAME_INVALID => RclRustError::RclServiceNameInvalid(base_error), + RCL_RET_UNKNOWN_SUBSTITUTION => RclRustError::RclUnknownSubstitution(base_error), + RCL_RET_ALREADY_SHUTDOWN => RclRustError::RclAlreadyShutdown(base_error), + + RCL_RET_NODE_INVALID => RclRustError::RclNodeInvalid(base_error), + RCL_RET_NODE_INVALID_NAME => RclRustError::RclNodeInvalidName(base_error), + RCL_RET_NODE_INVALID_NAMESPACE => RclRustError::RclNodeInvalidNamespace(base_error), + RCL_RET_NODE_NAME_NON_EXISTENT => RclRustError::RclNodeNameNonExistent(base_error), + + RCL_RET_PUBLISHER_INVALID => RclRustError::RclPublisherInvalid(base_error), + + RCL_RET_SUBSCRIPTION_INVALID => RclRustError::RclSubscriptionInvalid(base_error), + RCL_RET_SUBSCRIPTION_TAKE_FAILED => RclRustError::RclSubscriptionTakeFailed(base_error), + + RCL_RET_SERVICE_INVALID => RclRustError::RclServiceNameInvalid(base_error), + RCL_RET_SERVICE_TAKE_FAILED => RclRustError::RclServiceTakeFailed(base_error), + + RCL_RET_TIMER_INVALID => RclRustError::RclTimerInvalid(base_error), + RCL_RET_TIMER_CANCELED => RclRustError::RclTimerCanceled(base_error), + + RCL_RET_WAIT_SET_INVALID => RclRustError::RclWaitSetInvalid(base_error), + RCL_RET_WAIT_SET_EMPTY => RclRustError::RclWaitSetEmpty(base_error), + RCL_RET_WAIT_SET_FULL => RclRustError::RclWaitSetFull(base_error), + + RCL_RET_INVALID_REMAP_RULE => RclRustError::RclInvalidRemapRule(base_error), + RCL_RET_WRONG_LEXEME => RclRustError::RclWrongLexeme(base_error), + RCL_RET_INVALID_ROS_ARGS => RclRustError::RclInvalidROSArgs(base_error), + RCL_RET_INVALID_PARAM_RULE => RclRustError::RclInvalidParamRule(base_error), + RCL_RET_INVALID_LOG_LEVEL_RULE => RclRustError::RclInvalidLogLevelRule(base_error), + + RCL_RET_EVENT_INVALID => RclRustError::RclEventInvalid(base_error), + RCL_RET_EVENT_TAKE_FAILED => RclRustError::RclEventTakeFailed(base_error), + + _ => RclRustError::RclError(base_error), + } + }; + Err(error.into()) +} + +pub(crate) trait ToRclRustResult { + fn to_result(self) -> Result<()>; +} + +impl ToRclRustResult for rcl_sys::rcl_ret_t { + fn to_result(self) -> Result<()> { + result_from_rcl_ret(self, Some("rclrust")) + } +} diff --git a/rclrust/src/internal.rs b/rclrust/src/internal.rs new file mode 100644 index 00000000..d8a989e4 --- /dev/null +++ b/rclrust/src/internal.rs @@ -0,0 +1 @@ +pub mod ffi; diff --git a/rclrust/src/internal/ffi.rs b/rclrust/src/internal/ffi.rs new file mode 100644 index 00000000..d2acedbc --- /dev/null +++ b/rclrust/src/internal/ffi.rs @@ -0,0 +1,38 @@ +use std::ffi::{CStr, CString}; +use std::os::raw::c_char; + +pub unsafe trait SizedFromCChar: Sized { + unsafe fn from_c_char(ptr: *const c_char) -> Option; +} + +unsafe impl SizedFromCChar for CString { + unsafe fn from_c_char(ptr: *const c_char) -> Option { + CStr::from_c_char(ptr).map(|v| v.into()) + } +} + +unsafe impl SizedFromCChar for String { + unsafe fn from_c_char(ptr: *const c_char) -> Option { + str::from_c_char(ptr).map(|v| v.into()) + } +} + +pub unsafe trait FromCChar { + unsafe fn from_c_char<'a>(ptr: *const c_char) -> Option<&'a Self>; +} + +unsafe impl FromCChar for CStr { + unsafe fn from_c_char<'a>(ptr: *const c_char) -> Option<&'a Self> { + if ptr.is_null() { + None + } else { + Some(Self::from_ptr(ptr)) + } + } +} + +unsafe impl FromCChar for str { + unsafe fn from_c_char<'a>(ptr: *const c_char) -> Option<&'a Self> { + CStr::from_c_char(ptr).map(|v| v.to_str().expect("expect UTF-8 string")) + } +} diff --git a/rclrust/src/lib.rs b/rclrust/src/lib.rs index 31e1bb20..655bc49f 100644 --- a/rclrust/src/lib.rs +++ b/rclrust/src/lib.rs @@ -1,7 +1,9 @@ -#[cfg(test)] -mod tests { - #[test] - fn it_works() { - assert_eq!(2 + 2, 4); - } -} +#![warn( + rust_2018_idioms, + elided_lifetimes_in_paths, + clippy::all, + clippy::nursery +)] + +pub mod error; +pub(crate) mod internal;