diff --git a/.github/workflows/rust-minimal.yml b/.github/workflows/rust-minimal.yml index a93dfe318..8fbb32b61 100644 --- a/.github/workflows/rust-minimal.yml +++ b/.github/workflows/rust-minimal.yml @@ -36,6 +36,14 @@ jobs: image: ${{ matrix.docker_image }} steps: - uses: actions/checkout@v4 + + - name: Markdown autodocs + uses: dineshsonachalam/markdown-autodocs@v1.0.7 + with: + commit_message: Synchronizing Markdown files + branch: ${{ github.base_ref }} + output_file_paths: '[./docs/writing-your-first-rclrs-node.md]' + categories: '[code-block]' - name: Search packages in this repository id: list_packages diff --git a/.github/workflows/rust-stable.yml b/.github/workflows/rust-stable.yml index fec89e84b..6e6282d9e 100644 --- a/.github/workflows/rust-stable.yml +++ b/.github/workflows/rust-stable.yml @@ -36,6 +36,14 @@ jobs: image: ${{ matrix.docker_image }} steps: - uses: actions/checkout@v4 + + - name: Markdown autodocs + uses: dineshsonachalam/markdown-autodocs@v1.0.7 + with: + commit_message: Synchronizing Markdown files + branch: ${{ github.base_ref }} + output_file_paths: '[./docs/writing-your-first-rclrs-node.md]' + categories: '[code-block]' - name: Search packages in this repository id: list_packages diff --git a/docs/writing-your-first-rclrs-node.md b/docs/writing-your-first-rclrs-node.md index 8dbd8d2fa..cebc05f3e 100644 --- a/docs/writing-your-first-rclrs-node.md +++ b/docs/writing-your-first-rclrs-node.md @@ -58,7 +58,6 @@ impl RepublisherNode { let data = None; let _subscription = node.create_subscription( "in_topic", - rclrs::QOS_PROFILE_DEFAULT, |msg: StringMsg| { todo!("Assign msg to self.data") }, )?; Ok(Self { @@ -74,9 +73,13 @@ Next, add a main function to launch it: ```rust fn main() -> Result<(), rclrs::RclrsError> { - let context = rclrs::Context::new(std::env::args())?; - let republisher = RepublisherNode::new(&context)?; - rclrs::spin(republisher.node) + let context = Context::default_from_env()?; + let mut executor = context.create_basic_executor(); + let _republisher = RepublisherNode::new(&executor)?; + executor + .spin(SpinOptions::default()) + .first_error() + .map_err(|err| err.into()) } ``` @@ -110,37 +113,31 @@ So, to store the received data in the struct, the following things have to chang 4. Make the closure `move`, and inside it, lock the `Mutex` and store the message ```rust +use rclrs::*; use std::sync::{Arc, Mutex}; // (1) use std_msgs::msg::String as StringMsg; - struct RepublisherNode { - node: Arc, - _subscription: Arc>, - data: Arc>>, // (2) + _node: Arc, + _subscription: Arc>, + _data: Arc>>, // (2) } - impl RepublisherNode { - fn new(context: &rclrs::Context) -> Result { - let node = rclrs::Node::new(context, "republisher")?; - let data = Arc::new(Mutex::new(None)); // (3) - let data_cb = Arc::clone(&data); - let _subscription = { - // Create a new shared pointer instance that will be owned by the closure - node.create_subscription( - "in_topic", - rclrs::QOS_PROFILE_DEFAULT, - move |msg: StringMsg| { - // This subscription now owns the data_cb variable - *data_cb.lock().unwrap() = Some(msg); // (4) - }, - )? - }; - Ok(Self { - node, - _subscription, - data, - }) - } + fn new(executor: &rclrs::Executor) -> Result { + let _node = executor.create_node("republisher")?; + let _data = Arc::new(Mutex::new(None)); // (3) + let data_cb = Arc::clone(&_data); + let _subscription = _node.create_subscription( + "in_topic".keep_last(10).transient_local(), // (4) + move |msg: StringMsg| { + *data_cb.lock().unwrap() = Some(msg); + }, + )?; + Ok(Self { + _node, + _subscription, + _data, + }) + } } ``` @@ -152,45 +149,45 @@ If you couldn't follow the explanation involving borrowing, closures etc. above, The node still doesn't republish the received messages. First, let's add a publisher to the node: -```rust -// Add this new field to the RepublisherNode struct, after the subscription: -publisher: Arc>, - -// Change the end of RepublisherNode::new() to this: -let publisher = node.create_publisher("out_topic", rclrs::QOS_PROFILE_DEFAULT)?; -Ok(Self { - node, - _subscription, - publisher, - data, -}) ``` + + +``` + +Create a publisher and add it to the newly instantiated `RepublisherNode`: + +``` + + +``` + Then, let's add a `republish()` function to the `RepublisherNode` that publishes the latest message received, or does nothing if none was received: -```rust -fn republish(&self) -> Result<(), rclrs::RclrsError> { - if let Some(s) = &*self.data.lock().unwrap() { - self.publisher.publish(s)?; - } - Ok(()) -} +``` + + ``` + What's left to do is to call this function every second. `rclrs` doesn't yet have ROS timers, which run a function at a fixed interval, but it's easy enough to achieve with a thread, a loop, and the sleep function. Change your main function to spawn a separate thread: ```rust fn main() -> Result<(), rclrs::RclrsError> { - let context = rclrs::Context::new(std::env::args())?; - let republisher = RepublisherNode::new(&context)?; + let context = Context::default_from_env()?; + let mut executor = context.create_basic_executor(); + let _republisher = RepublisherNode::new(&executor)?; std::thread::spawn(|| -> Result<(), rclrs::RclrsError> { loop { use std::time::Duration; std::thread::sleep(Duration::from_millis(1000)); - republisher.republish()?; + _republisher.republish()?; } }); - rclrs::spin(republisher.node) + executor + .spin(SpinOptions::default()) + .first_error() + .map_err(|err| err.into()) } ``` @@ -200,20 +197,9 @@ But wait, this doesn't work – there is an error about the thread closure needi The solution is also the same as above: Shared ownership with `Arc`. Only this time, `Mutex` isn't needed since both the `rclcpp::spin()` and the `republish()` function only require a shared reference: -```rust -fn main() -> Result<(), rclrs::RclrsError> { - let context = rclrs::Context::new(std::env::args())?; - let republisher = Arc::new(RepublisherNode::new(&context)?); - let republisher_other_thread = Arc::clone(&republisher); - std::thread::spawn(move || -> Result<(), rclrs::RclrsError> { - loop { - use std::time::Duration; - std::thread::sleep(Duration::from_millis(1000)); - republisher_other_thread.republish()?; - } - }); - rclrs::spin(Arc::clone(&republisher.node)) -} +``` + + ``` diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index 09f59cb62..ccff15122 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -33,6 +33,11 @@ rosidl_runtime_rs = "0.4" serde = { version = "1", optional = true, features = ["derive"] } serde-big-array = { version = "0.5.1", optional = true } +# Needed for the examples in lib.rs +[dependencies.std_msgs] +version = "*" + + [dev-dependencies] # Needed for e.g. writing yaml files in tests tempfile = "3.3.0" diff --git a/rclrs/package.xml b/rclrs/package.xml index 4c3754f48..bfbffbe52 100644 --- a/rclrs/package.xml +++ b/rclrs/package.xml @@ -20,6 +20,8 @@ rcl_interfaces rosgraph_msgs + std_msgs + test_msgs diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 73d478191..598be9a1b 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -48,3 +48,79 @@ pub use subscription::*; pub use time::*; use time_source::*; pub use wait::*; + +/// # rclrs - ROS 2 Client Library for Rust +/// +/// `rclrs` provides Rust bindings and idiomatic wrappers for ROS 2 (Robot Operating System). +/// It enables writing ROS 2 nodes, publishers, subscribers, services and clients in Rust. +/// +/// ## Features +/// +/// - Native Rust implementation of core ROS 2 concepts +/// - Safe wrappers around rcl C API +/// - Support for publishers, subscribers, services, clients +/// - Async/await support for services and clients +/// - Quality of Service (QoS) configuration +/// - Parameter services +/// - Logging integration +/// +/// ## Example +/// Here's a simple publisher-subscriber node: +use std::sync::{Arc, Mutex}; +use std_msgs::msg::String as StringMsg; + +/// ## Write the basic node structure +/// Since Rust doesn't have inheritance, it's not possible to inherit from `Node` as is common practice in `rclcpp` or `rclpy`. +/// +/// Instead, you can store the node as a regular member. Let's use a struct that contains the node, a subscription, and a field for the last message that was received to `main.rs`: +pub struct RepublisherNode { + _node: Arc, + _subscription: Arc>, + _publisher: Arc>, + _data: Arc>>, +} + +impl RepublisherNode { + fn _new(executor: &Executor) -> Result { + let _node = executor.create_node("republisher")?; + let _data = Arc::new(Mutex::new(None)); + let data_cb = Arc::clone(&_data); + let _subscription = _node.create_subscription( + "in_topic".keep_last(10).transient_local(), + move |msg: StringMsg| { + *data_cb.lock().unwrap() = Some(msg); + }, + )?; + let _publisher = _node.create_publisher::("out_topic")?; + Ok(Self { + _node, + _subscription, + _publisher, + _data, + }) + } + + fn _republish(&self) -> Result<(), RclrsError> { + if let Some(s) = &*self._data.lock().unwrap() { + self._publisher.publish(s)?; + } + Ok(()) + } +} + +fn _main() -> Result<(), RclrsError> { + let context = Context::default_from_env()?; + let mut executor = context.create_basic_executor(); + let republisher = RepublisherNode::_new(&executor)?; + std::thread::spawn(move || -> Result<(), RclrsError> { + loop { + use std::time::Duration; + std::thread::sleep(Duration::from_millis(1000)); + republisher._republish()?; + } + }); + executor + .spin(SpinOptions::default()) + .first_error() + .map_err(|err| err.into()) +}