From 643f7ac2bd67425db6213dcdef8b8d31f08712c8 Mon Sep 17 00:00:00 2001 From: Yuma Hiramatsu Date: Sat, 24 Jul 2021 11:32:46 +0900 Subject: [PATCH 002/230] Add .gitignore --- .gitignore | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..084bebe5 --- /dev/null +++ b/.gitignore @@ -0,0 +1,21 @@ + +# Created by https://www.toptal.com/developers/gitignore/api/rust +# Edit at https://www.toptal.com/developers/gitignore?templates=rust + +### Rust ### +# Generated by Cargo +# will have compiled files and executables +debug/ +target/ + +# Remove Cargo.lock from gitignore if creating an executable, leave it for libraries +# More information here https://doc.rust-lang.org/cargo/guide/cargo-toml-vs-cargo-lock.html +Cargo.lock + +# These are backup files generated by rustfmt +**/*.rs.bk + +# MSVC Windows builds of rustc generate these, which store debugging information +*.pdb + +# End of https://www.toptal.com/developers/gitignore/api/rust From 2f336774e00d106aef7eb4ff868adb1e0a7e9fda Mon Sep 17 00:00:00 2001 From: Yuma Hiramatsu Date: Sat, 24 Jul 2021 11:33:14 +0900 Subject: [PATCH 003/230] Create LICENSE --- LICENSE | 201 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 201 insertions(+) create mode 100644 LICENSE diff --git a/LICENSE b/LICENSE new file mode 100644 index 00000000..261eeb9e --- /dev/null +++ b/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. From 86cae85069a3f4c774314e8c966466530a2b5364 Mon Sep 17 00:00:00 2001 From: Yuma Hiramatsu Date: Sat, 24 Jul 2021 11:35:40 +0900 Subject: [PATCH 004/230] Init two packages --- Cargo.toml | 5 +++++ rcl-sys/Cargo.toml | 11 +++++++++++ rcl-sys/src/lib.rs | 7 +++++++ rclrust/Cargo.toml | 13 +++++++++++++ rclrust/src/lib.rs | 7 +++++++ 5 files changed, 43 insertions(+) create mode 100644 Cargo.toml create mode 100644 rcl-sys/Cargo.toml create mode 100644 rcl-sys/src/lib.rs create mode 100644 rclrust/Cargo.toml create mode 100644 rclrust/src/lib.rs diff --git a/Cargo.toml b/Cargo.toml new file mode 100644 index 00000000..be729e1d --- /dev/null +++ b/Cargo.toml @@ -0,0 +1,5 @@ +[workspace] +members = [ + "rclrust", + "rcl-sys", +] diff --git a/rcl-sys/Cargo.toml b/rcl-sys/Cargo.toml new file mode 100644 index 00000000..5d9bb443 --- /dev/null +++ b/rcl-sys/Cargo.toml @@ -0,0 +1,11 @@ +[package] +name = "rcl-sys" +version = "0.0.0" +edition = "2018" +authors = ["Yuma Hiramatsu "] + +description = "TODO" +repository = "https://github.com/ros2rust/rclrust" +license = "Apache-2.0" + +[dependencies] diff --git a/rcl-sys/src/lib.rs b/rcl-sys/src/lib.rs new file mode 100644 index 00000000..31e1bb20 --- /dev/null +++ b/rcl-sys/src/lib.rs @@ -0,0 +1,7 @@ +#[cfg(test)] +mod tests { + #[test] + fn it_works() { + assert_eq!(2 + 2, 4); + } +} diff --git a/rclrust/Cargo.toml b/rclrust/Cargo.toml new file mode 100644 index 00000000..952d7ad0 --- /dev/null +++ b/rclrust/Cargo.toml @@ -0,0 +1,13 @@ +[package] +name = "rclrust" +version = "0.0.0" +edition = "2018" +authors = ["Yuma Hiramatsu "] + +description = "TODO" +repository = "https://github.com/ros2rust/rclrust" +license = "Apache-2.0" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] diff --git a/rclrust/src/lib.rs b/rclrust/src/lib.rs new file mode 100644 index 00000000..31e1bb20 --- /dev/null +++ b/rclrust/src/lib.rs @@ -0,0 +1,7 @@ +#[cfg(test)] +mod tests { + #[test] + fn it_works() { + assert_eq!(2 + 2, 4); + } +} From 8807f040ba29564cb0b6fdf6f58d83c608b4c4e1 Mon Sep 17 00:00:00 2001 From: Yuma Hiramatsu Date: Sat, 24 Jul 2021 11:51:38 +0900 Subject: [PATCH 005/230] Impl sys-crate --- rcl-sys/Cargo.toml | 10 +++++++--- rcl-sys/build.rs | 34 ++++++++++++++++++++++++++++++++++ rcl-sys/src/lib.rs | 17 ++++++++++------- rcl-sys/src/wrapper.h | 2 ++ 4 files changed, 53 insertions(+), 10 deletions(-) create mode 100644 rcl-sys/build.rs create mode 100644 rcl-sys/src/wrapper.h diff --git a/rcl-sys/Cargo.toml b/rcl-sys/Cargo.toml index 5d9bb443..2a203832 100644 --- a/rcl-sys/Cargo.toml +++ b/rcl-sys/Cargo.toml @@ -1,11 +1,15 @@ [package] name = "rcl-sys" -version = "0.0.0" +version = "0.0.1" edition = "2018" authors = ["Yuma Hiramatsu "] -description = "TODO" +description = "The C API binding of the ROS client library (rcl)" repository = "https://github.com/ros2rust/rclrust" license = "Apache-2.0" -[dependencies] +[build-dependencies] +bindgen = "0.59" + +[lib] +doctest = false diff --git a/rcl-sys/build.rs b/rcl-sys/build.rs new file mode 100644 index 00000000..8d03c96d --- /dev/null +++ b/rcl-sys/build.rs @@ -0,0 +1,34 @@ +use std::path::PathBuf; + +use bindgen::{Builder, EnumVariation}; + +fn main() { + println!("cargo:rerun-if-env-changed=AMENT_PREFIX_PATH"); + println!("cargo:rerun-if-changed=./src/wrapper.h"); + + let mut builder = Builder::default() + .header("src/wrapper.h") + .size_t_is_usize(true) + .derive_copy(false) + .default_enum_style(EnumVariation::Rust { + non_exhaustive: false, + }); + + let ament_prefix_paths = + std::env::var("AMENT_PREFIX_PATH").expect("$AMENT_PREFIX_PATH is supposed to be set."); + for ament_prefix_path in ament_prefix_paths.split(':') { + builder = builder.clang_arg(format!("-I{}/include", ament_prefix_path)); + println!("cargo:rustc-link-search=native={}/lib", ament_prefix_path); + } + + println!("cargo:rustc-link-lib=dylib=rcl"); + println!("cargo:rustc-link-lib=dylib=rcutils"); + println!("cargo:rustc-link-lib=dylib=rmw"); + + let out_path = PathBuf::from(std::env::var("OUT_DIR").unwrap()).join("bindings.rs"); + builder + .generate() + .expect("Unable to generate bindings") + .write_to_file(out_path) + .expect("Couldn't write bindings!"); +} diff --git a/rcl-sys/src/lib.rs b/rcl-sys/src/lib.rs index 31e1bb20..5ec864a9 100644 --- a/rcl-sys/src/lib.rs +++ b/rcl-sys/src/lib.rs @@ -1,7 +1,10 @@ -#[cfg(test)] -mod tests { - #[test] - fn it_works() { - assert_eq!(2 + 2, 4); - } -} +#![allow( + improper_ctypes, + non_camel_case_types, + non_snake_case, + non_upper_case_globals, + invalid_codeblock_attributes, + clippy::all +)] + +include!(concat!(env!("OUT_DIR"), "/bindings.rs")); diff --git a/rcl-sys/src/wrapper.h b/rcl-sys/src/wrapper.h new file mode 100644 index 00000000..0c94dec3 --- /dev/null +++ b/rcl-sys/src/wrapper.h @@ -0,0 +1,2 @@ +#include +#include From 49072c341c315879ed1673a4fdf92546c088c6c3 Mon Sep 17 00:00:00 2001 From: Yuma Hiramatsu Date: Sat, 24 Jul 2021 15:11:16 +0900 Subject: [PATCH 006/230] Update manifest file --- README.md | 1 + rclrust/Cargo.toml | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 00000000..3b071716 --- /dev/null +++ b/README.md @@ -0,0 +1 @@ +# rclrust diff --git a/rclrust/Cargo.toml b/rclrust/Cargo.toml index 952d7ad0..a0656731 100644 --- a/rclrust/Cargo.toml +++ b/rclrust/Cargo.toml @@ -7,7 +7,9 @@ authors = ["Yuma Hiramatsu "] description = "TODO" repository = "https://github.com/ros2rust/rclrust" license = "Apache-2.0" +readme = "../README.md" -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html +keywords = ["ROS2"] +categories = ["science::robotics"] [dependencies] From 13c3b4140b2c587e9a2db0712c7d191e215be404 Mon Sep 17 00:00:00 2001 From: Yuma Hiramatsu Date: Sat, 24 Jul 2021 15:12:52 +0900 Subject: [PATCH 007/230] Impl wrapper of rcl errors --- rclrust/Cargo.toml | 4 + rclrust/src/error.rs | 212 ++++++++++++++++++++++++++++++++++++ rclrust/src/internal.rs | 1 + rclrust/src/internal/ffi.rs | 38 +++++++ rclrust/src/lib.rs | 16 +-- 5 files changed, 264 insertions(+), 7 deletions(-) create mode 100644 rclrust/src/error.rs create mode 100644 rclrust/src/internal.rs create mode 100644 rclrust/src/internal/ffi.rs 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; From 5c4f224ac0377377758c65bb3a1694684fe51384 Mon Sep 17 00:00:00 2001 From: Yuma Hiramatsu Date: Sat, 24 Jul 2021 18:00:21 +0900 Subject: [PATCH 008/230] Impl logging macro --- rclrust/src/internal.rs | 1 + rclrust/src/internal/macros.rs | 33 ++++ rclrust/src/lib.rs | 1 + rclrust/src/log.rs | 305 +++++++++++++++++++++++++++++++++ 4 files changed, 340 insertions(+) create mode 100644 rclrust/src/internal/macros.rs create mode 100644 rclrust/src/log.rs diff --git a/rclrust/src/internal.rs b/rclrust/src/internal.rs index d8a989e4..f473d1d8 100644 --- a/rclrust/src/internal.rs +++ b/rclrust/src/internal.rs @@ -1 +1,2 @@ pub mod ffi; +mod macros; diff --git a/rclrust/src/internal/macros.rs b/rclrust/src/internal/macros.rs new file mode 100644 index 00000000..0f08bee2 --- /dev/null +++ b/rclrust/src/internal/macros.rs @@ -0,0 +1,33 @@ +#[macro_export] +macro_rules! impl_from_trait_for_enum { + ( + $new_type: ty, + $old_type: ty, + $( $new_item: ident := $old_item: ident ),+ + $(,)? + ) => { + impl From<$old_type> for $new_type { + #![allow(deprecated)] + fn from(item: $old_type) -> Self { + match item { + $(<$old_type>::$old_item => Self::$new_item),+ + } + } + } + + impl From<$new_type> for $old_type { + #![allow(deprecated)] + fn from(item: $new_type) -> Self { + match item { + $(<$new_type>::$new_item => Self::$old_item),+ + } + } + } + + impl From<$new_type> for std::os::raw::c_int { + fn from(item: $new_type) -> Self { + <$old_type>::from(item) as Self + } + } + }; +} diff --git a/rclrust/src/lib.rs b/rclrust/src/lib.rs index 655bc49f..a97adac9 100644 --- a/rclrust/src/lib.rs +++ b/rclrust/src/lib.rs @@ -7,3 +7,4 @@ pub mod error; pub(crate) mod internal; +pub mod log; diff --git a/rclrust/src/log.rs b/rclrust/src/log.rs new file mode 100644 index 00000000..1812439c --- /dev/null +++ b/rclrust/src/log.rs @@ -0,0 +1,305 @@ +use std::convert::{TryFrom, TryInto}; +use std::ffi::CString; +use std::os::raw::{c_char, c_int}; + +use anyhow::{Context, Result}; + +use crate::error::{RclRustError, ToRclRustResult}; +use crate::impl_from_trait_for_enum; + +#[derive(Debug, Clone, Copy, PartialEq)] +pub enum LogSeverity { + Unset, + Debug, + Info, + Warn, + Error, + Fatal, +} + +impl_from_trait_for_enum! { + LogSeverity, + rcl_sys::RCUTILS_LOG_SEVERITY, + Unset := RCUTILS_LOG_SEVERITY_UNSET, + Debug := RCUTILS_LOG_SEVERITY_DEBUG, + Info := RCUTILS_LOG_SEVERITY_INFO, + Warn := RCUTILS_LOG_SEVERITY_WARN, + Error := RCUTILS_LOG_SEVERITY_ERROR, + Fatal := RCUTILS_LOG_SEVERITY_FATAL, +} + +impl TryFrom for LogSeverity { + type Error = anyhow::Error; + + fn try_from(from: c_int) -> Result { + use rcl_sys::RCUTILS_LOG_SEVERITY; + + if from == RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_UNSET as u32 as i32 { + Ok(Self::Unset) + } else if from == RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_DEBUG as u32 as i32 { + Ok(Self::Debug) + } else if from == RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_INFO as u32 as i32 { + Ok(Self::Info) + } else if from == RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_WARN as u32 as i32 { + Ok(Self::Warn) + } else if from == RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_ERROR as u32 as i32 { + Ok(Self::Error) + } else if from == RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_FATAL as u32 as i32 { + Ok(Self::Fatal) + } else { + return Err(RclRustError::RuntimeError("").into()); + } + } +} + +pub fn logging_autoinit() { + unsafe { + if !rcl_sys::g_rcutils_logging_initialized { + rcl_sys::rcutils_logging_initialize() + .to_result() + .expect("rcutils_logging_initialize() should succeed"); + } + } +} + +pub struct Logger { + name: CString, +} + +impl Logger { + pub fn new(name: &str) -> Self { + Self { + name: CString::new(name).unwrap(), + } + } + + pub fn empty_name() -> Self { + Self { + name: CString::default(), + } + } + + pub fn log_common(&self, severity: LogSeverity, msg: &str, file: &str, line: u32) { + logging_autoinit(); + + if !self.is_enable_for(severity) { + return; + } + + // TODO: How to get function name? + let function_name = CString::new("()").unwrap(); + let file = CString::new(file).unwrap(); + let msg = CString::new(msg).unwrap(); + + let logging_location = rcl_sys::rcutils_log_location_t { + function_name: function_name.as_ptr(), + file_name: file.as_ptr(), + line_number: line.try_into().unwrap(), + }; + + unsafe { + rcl_sys::rcutils_log( + &logging_location, + severity.into(), + self.get_name_ptr(), + msg.as_ptr(), + ); + } + } + + pub fn get_name(&self) -> &str { + &self.name.to_str().unwrap() + } + + fn get_name_ptr(&self) -> *const c_char { + self.name.as_ptr() + } + + pub fn get_child(&self, suffix: &str) -> Self { + if self.name.to_str().unwrap().is_empty() { + Self::empty_name() + } else { + Self::new(&format!( + "{}.{}", + self.name.clone().into_string().unwrap(), + suffix + )) + } + } + + pub fn set_level(&self, level: LogSeverity) -> Result<()> { + unsafe { + rcl_sys::rcutils_logging_set_logger_level(self.get_name_ptr(), level.into()).to_result() + } + } + + pub fn get_level(&self) -> Result { + LogSeverity::try_from(unsafe { + rcl_sys::rcutils_logging_get_logger_level(self.get_name_ptr()) + }) + .with_context(|| format!("{:?}", self.name)) + } + + pub fn get_effective_level(&self) -> Result { + LogSeverity::try_from(unsafe { + rcl_sys::rcutils_logging_get_logger_effective_level(self.get_name_ptr()) + }) + .with_context(|| format!("{:?}", self.name)) + } + + pub fn is_enable_for(&self, severity: LogSeverity) -> bool { + unsafe { + rcl_sys::rcutils_logging_logger_is_enabled_for(self.get_name_ptr(), severity.into()) + } + } +} + +#[macro_export] +macro_rules! rclrust_debug { + ($logger:expr, $($arg:tt),+) => { + $logger.log_common($crate::log::LogSeverity::Debug, &format!($($arg),+), file!(), line!()); + }; +} + +#[macro_export] +macro_rules! rclrust_info { + ($logger:expr, $($arg:tt),+) => { + $logger.log_common($crate::log::LogSeverity::Info, &format!($($arg),+), file!(), line!()); + }; +} + +#[macro_export] +macro_rules! rclrust_warn { + ($logger:expr, $($arg:tt),+) => { + $logger.log_common($crate::log::LogSeverity::Warn, &format!($($arg),+), file!(), line!()); + }; +} + +#[macro_export] +macro_rules! rclrust_error { + ($logger:expr, $($arg:tt),+) => { + $logger.log_common($crate::log::LogSeverity::Error, &format!($($arg),+), file!(), line!()); + }; +} + +#[macro_export] +macro_rules! rclrust_fatal { + ($logger:expr, $($arg:tt),+) => { + $logger.log_common($crate::log::LogSeverity::Fatal, &format!($($arg),+), file!(), line!()); + }; +} + +#[cfg(test)] +mod test { + use super::*; + + #[test] + fn named_logger_init() -> Result<()> { + let logger = Logger::new("test"); + assert_eq!(logger.get_name(), "test"); + Ok(()) + } + + #[test] + fn get_child_logger() -> Result<()> { + let logger = Logger::new("test"); + let child_logger = logger.get_child("child"); + assert_eq!(child_logger.get_name(), "test.child"); + let grandchild_logger = child_logger.get_child("child2"); + assert_eq!(grandchild_logger.get_name(), "test.child.child2"); + + Ok(()) + } + + #[test] + fn empty_logger_level() -> Result<()> { + let logger = Logger::empty_name(); + + logger.set_level(LogSeverity::Debug)?; + assert_eq!( + logger.get_level()?, + LogSeverity::Debug, + "get_level() should return LogSeverity::Debug" + ); + assert_eq!( + logger.get_effective_level()?, + LogSeverity::Debug, + "get_effective_level() should return LogSeverity::Debug" + ); + + // rollback + logger.set_level(LogSeverity::Info)?; + + Ok(()) + } + + #[test] + fn named_logger_level() -> Result<()> { + let root_logger = Logger::empty_name(); + + let logger = Logger::new("test"); + assert_eq!(logger.get_level()?, LogSeverity::Unset); + assert_eq!(logger.get_effective_level()?, root_logger.get_level()?); + + logger.set_level(LogSeverity::Info)?; + assert_eq!(logger.get_level()?, LogSeverity::Info); + assert_eq!(logger.get_effective_level()?, LogSeverity::Info); + + // rollback + logger.set_level(LogSeverity::Unset)?; + + Ok(()) + } + + #[test] + fn empty_logger_is_enable_for() -> Result<()> { + let logger = Logger::empty_name(); + + logger.set_level(LogSeverity::Unset)?; + assert!(logger.is_enable_for(LogSeverity::Fatal)); + assert!(logger.is_enable_for(LogSeverity::Error)); + assert!(logger.is_enable_for(LogSeverity::Warn)); + assert!(logger.is_enable_for(LogSeverity::Info)); + assert!(logger.is_enable_for(LogSeverity::Debug)); + + logger.set_level(LogSeverity::Fatal)?; + assert!(logger.is_enable_for(LogSeverity::Fatal)); + assert!(!logger.is_enable_for(LogSeverity::Error)); + assert!(!logger.is_enable_for(LogSeverity::Warn)); + assert!(!logger.is_enable_for(LogSeverity::Info)); + assert!(!logger.is_enable_for(LogSeverity::Debug)); + + logger.set_level(LogSeverity::Error)?; + assert!(logger.is_enable_for(LogSeverity::Fatal)); + assert!(logger.is_enable_for(LogSeverity::Error)); + assert!(!logger.is_enable_for(LogSeverity::Warn)); + assert!(!logger.is_enable_for(LogSeverity::Info)); + assert!(!logger.is_enable_for(LogSeverity::Debug)); + + logger.set_level(LogSeverity::Warn)?; + assert!(logger.is_enable_for(LogSeverity::Fatal)); + assert!(logger.is_enable_for(LogSeverity::Error)); + assert!(logger.is_enable_for(LogSeverity::Warn)); + assert!(!logger.is_enable_for(LogSeverity::Info)); + assert!(!logger.is_enable_for(LogSeverity::Debug)); + + logger.set_level(LogSeverity::Info)?; + assert!(logger.is_enable_for(LogSeverity::Fatal)); + assert!(logger.is_enable_for(LogSeverity::Error)); + assert!(logger.is_enable_for(LogSeverity::Warn)); + assert!(logger.is_enable_for(LogSeverity::Info)); + assert!(!logger.is_enable_for(LogSeverity::Debug)); + + logger.set_level(LogSeverity::Debug)?; + assert!(logger.is_enable_for(LogSeverity::Fatal)); + assert!(logger.is_enable_for(LogSeverity::Error)); + assert!(logger.is_enable_for(LogSeverity::Warn)); + assert!(logger.is_enable_for(LogSeverity::Info)); + assert!(logger.is_enable_for(LogSeverity::Debug)); + + // rollback + logger.set_level(LogSeverity::Info)?; + + Ok(()) + } +} From 59a69817f077463dd6909eb32e9af4155b23c2fe Mon Sep 17 00:00:00 2001 From: Yuma Hiramatsu Date: Sat, 24 Jul 2021 23:44:54 +0900 Subject: [PATCH 009/230] Logging process is now thread-safe --- rcl-sys/src/wrapper.h | 1 + rclrust/Cargo.toml | 4 +++- rclrust/src/log.rs | 31 +++++++++++++++++++------------ 3 files changed, 23 insertions(+), 13 deletions(-) diff --git a/rcl-sys/src/wrapper.h b/rcl-sys/src/wrapper.h index 0c94dec3..a7cc13b8 100644 --- a/rcl-sys/src/wrapper.h +++ b/rcl-sys/src/wrapper.h @@ -1,2 +1,3 @@ +#include #include #include diff --git a/rclrust/Cargo.toml b/rclrust/Cargo.toml index 87b631a2..ad02c5e8 100644 --- a/rclrust/Cargo.toml +++ b/rclrust/Cargo.toml @@ -14,6 +14,8 @@ categories = ["science::robotics"] [dependencies] anyhow = "1.0" -thiserror = "1.0" +once_cell = "1.8" +parking_lot = "0.11" rcl-sys = { path = "../rcl-sys", version = "0.0.1" } rclrust-msg = "0.0.1" +thiserror = "1.0" diff --git a/rclrust/src/log.rs b/rclrust/src/log.rs index 1812439c..59b53a36 100644 --- a/rclrust/src/log.rs +++ b/rclrust/src/log.rs @@ -3,6 +3,8 @@ use std::ffi::CString; use std::os::raw::{c_char, c_int}; use anyhow::{Context, Result}; +use once_cell::sync::Lazy; +use parking_lot::ReentrantMutex; use crate::error::{RclRustError, ToRclRustResult}; use crate::impl_from_trait_for_enum; @@ -47,21 +49,14 @@ impl TryFrom for LogSeverity { } else if from == RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_FATAL as u32 as i32 { Ok(Self::Fatal) } else { - return Err(RclRustError::RuntimeError("").into()); + Err(RclRustError::RuntimeError("cast error: LogSeverity").into()) } } } -pub fn logging_autoinit() { - unsafe { - if !rcl_sys::g_rcutils_logging_initialized { - rcl_sys::rcutils_logging_initialize() - .to_result() - .expect("rcutils_logging_initialize() should succeed"); - } - } -} +pub(crate) static LOGGER_MUTEX: Lazy> = Lazy::new(|| ReentrantMutex::new(())); +#[derive(Debug)] pub struct Logger { name: CString, } @@ -80,7 +75,15 @@ impl Logger { } pub fn log_common(&self, severity: LogSeverity, msg: &str, file: &str, line: u32) { - logging_autoinit(); + let _guard = LOGGER_MUTEX.lock(); + + unsafe { + if !rcl_sys::g_rcutils_logging_initialized { + rcl_sys::rcutils_logging_initialize() + .to_result() + .expect("rcutils_logging_initialize() should succeed"); + } + } if !self.is_enable_for(severity) { return; @@ -108,7 +111,7 @@ impl Logger { } pub fn get_name(&self) -> &str { - &self.name.to_str().unwrap() + self.name.to_str().unwrap() } fn get_name_ptr(&self) -> *const c_char { @@ -128,6 +131,7 @@ impl Logger { } pub fn set_level(&self, level: LogSeverity) -> Result<()> { + let _guard = LOGGER_MUTEX.lock(); unsafe { rcl_sys::rcutils_logging_set_logger_level(self.get_name_ptr(), level.into()).to_result() } @@ -135,6 +139,7 @@ impl Logger { pub fn get_level(&self) -> Result { LogSeverity::try_from(unsafe { + let _guard = LOGGER_MUTEX.lock(); rcl_sys::rcutils_logging_get_logger_level(self.get_name_ptr()) }) .with_context(|| format!("{:?}", self.name)) @@ -142,12 +147,14 @@ impl Logger { pub fn get_effective_level(&self) -> Result { LogSeverity::try_from(unsafe { + let _guard = LOGGER_MUTEX.lock(); rcl_sys::rcutils_logging_get_logger_effective_level(self.get_name_ptr()) }) .with_context(|| format!("{:?}", self.name)) } pub fn is_enable_for(&self, severity: LogSeverity) -> bool { + let _guard = LOGGER_MUTEX.lock(); unsafe { rcl_sys::rcutils_logging_logger_is_enabled_for(self.get_name_ptr(), severity.into()) } From d244589441f2282adb91da1f1d8699ed812dc158 Mon Sep 17 00:00:00 2001 From: Yuma Hiramatsu Date: Sun, 25 Jul 2021 11:04:47 +0900 Subject: [PATCH 010/230] Make to reset error after getting RclRustError --- rclrust/src/error.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/rclrust/src/error.rs b/rclrust/src/error.rs index 899672eb..067ef2bc 100644 --- a/rclrust/src/error.rs +++ b/rclrust/src/error.rs @@ -149,6 +149,7 @@ pub(crate) fn result_from_rcl_ret(ret: rcl_sys::rcl_ret_t, prefix: Option<&str>) } let formatted_prefix = prefix.map_or("".into(), |v| format!("[{}]: ", v)); let base_error = unsafe { RclErrorBase::new(error_state, formatted_prefix) }; + unsafe { rcl_sys::rcutils_reset_error() } let error = { use rcl_sys::*; From 61726ea5d1dc2c8cdb845b6d9575d236f5496806 Mon Sep 17 00:00:00 2001 From: Yuma Hiramatsu Date: Fri, 30 Jul 2021 00:53:46 +0900 Subject: [PATCH 011/230] Impl time and clock modules --- rclrust/src/clock.rs | 182 +++++++++++++++++++++++++++++++++++++++++++ rclrust/src/lib.rs | 5 ++ rclrust/src/time.rs | 108 +++++++++++++++++++++++++ 3 files changed, 295 insertions(+) create mode 100644 rclrust/src/clock.rs create mode 100644 rclrust/src/time.rs diff --git a/rclrust/src/clock.rs b/rclrust/src/clock.rs new file mode 100644 index 00000000..79c4e597 --- /dev/null +++ b/rclrust/src/clock.rs @@ -0,0 +1,182 @@ +use anyhow::{ensure, Result}; + +use crate::error::ToRclRustResult; +use crate::impl_from_trait_for_enum; +use crate::log::Logger; +use crate::rclrust_error; +use crate::time::Time; + +/// Time source type, used to indicate the source of a time measurement. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum ClockType { + /// Clock is uninitialized yet. + Uninitialized, + /// Clock of this type will report the latest value reported by a ROS time source, or + /// if a ROS time source is not active it reports the same as RCL_SYSTEM_TIME. + /// For more information about ROS time sources, refer to the design document: + /// http://design.ros2.org/articles/clock_and_time.html + RosTime, + /// Clock of this type reports the same value as the system clock. + SystemTime, + /// Clock of this type reports a value from a monotonically increasing clock. + SteadyTime, +} + +impl_from_trait_for_enum! { + ClockType, + rcl_sys::rcl_clock_type_t, + Uninitialized := RCL_CLOCK_UNINITIALIZED, + RosTime := RCL_ROS_TIME, + SystemTime := RCL_SYSTEM_TIME, + SteadyTime := RCL_STEADY_TIME, +} + +#[derive(Debug)] +pub(crate) struct RclClock(rcl_sys::rcl_clock_t); + +unsafe impl Send for RclClock {} + +impl RclClock { + pub fn new(clock_type: ClockType) -> Result { + ensure!( + clock_type != ClockType::Uninitialized, + "`ClockType::Uninitialized` is invalid type." + ); + + let mut clock = rcl_sys::rcl_clock_t { + type_: ClockType::Uninitialized.into(), + jump_callbacks: std::ptr::null_mut(), + num_jump_callbacks: 0, + get_now: None, + data: std::ptr::null_mut(), + allocator: unsafe { rcl_sys::rcutils_get_default_allocator() }, + }; + + let mut allocator = unsafe { rcl_sys::rcutils_get_default_allocator() }; + unsafe { + rcl_sys::rcl_clock_init(clock_type.into(), &mut clock, &mut allocator).to_result()?; + } + Ok(Self(clock)) + } + + pub fn now(&mut self) -> Result