diff --git a/.github/workflows/rust-minimal.yml b/.github/workflows/rust-minimal.yml index a93dfe318..abc51ed4d 100644 --- a/.github/workflows/rust-minimal.yml +++ b/.github/workflows/rust-minimal.yml @@ -37,6 +37,13 @@ jobs: steps: - uses: actions/checkout@v4 + - uses: technology-studio-forks/markdown-embed-code@main + with: + markdown: "docs/writing-your-first-rclrs-node.md" + token: ${{ secrets.GITHUB_TOKEN }} + message: "synchronizing Markdown files" + silent: true + - name: Search packages in this repository id: list_packages run: | diff --git a/.github/workflows/rust-stable.yml b/.github/workflows/rust-stable.yml index fec89e84b..ed054f14b 100644 --- a/.github/workflows/rust-stable.yml +++ b/.github/workflows/rust-stable.yml @@ -37,6 +37,13 @@ jobs: steps: - uses: actions/checkout@v4 + - uses: technology-studio-forks/markdown-embed-code@main + with: + markdown: "docs/writing-your-first-rclrs-node.md" + token: ${{ secrets.GITHUB_TOKEN }} + message: "synchronizing Markdown files" + silent: true + - name: Search packages in this repository id: list_packages run: | diff --git a/docs/writing-your-first-rclrs-node.md b/docs/writing-your-first-rclrs-node.md index 8dbd8d2fa..6ef5003ea 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,39 +113,32 @@ 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, + }) + } +}``` If that seems needlessly complicated – maybe it is, in the sense that `rclrs` could potentially introduce new abstractions to improve the ergonomics of this use case. This is to be discussed. @@ -152,45 +148,37 @@ 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, -}) +```rust:examples/minimal_pub_sub/src/first_rclrs_node.rs [5-10] +``` + +Create a publisher and add it to the newly instantiated `RepublisherNode`: + +```rust:examples/minimal_pub_sub/src/first_rclrs_node.rs [23-29] ``` 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(()) -} +```rust:examples/minimal_pub_sub/src/first_rclrs_node.rs [32-37] ``` 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 +188,7 @@ 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)) -} +```rust:examples/minimal_pub_sub/src/first_rclrs_node.rs [40-55] ``` diff --git a/examples/minimal_pub_sub/Cargo.toml b/examples/minimal_pub_sub/Cargo.toml index 52c4f0544..4092c8840 100644 --- a/examples/minimal_pub_sub/Cargo.toml +++ b/examples/minimal_pub_sub/Cargo.toml @@ -25,6 +25,11 @@ path = "src/zero_copy_subscriber.rs" name = "zero_copy_publisher" path = "src/zero_copy_publisher.rs" +[[bin]] +name = "first_rclrs_node" +path = "src/first_rclrs_node.rs" + + [dependencies] anyhow = {version = "1", features = ["backtrace"]} diff --git a/examples/minimal_pub_sub/src/first_rclrs_node.rs b/examples/minimal_pub_sub/src/first_rclrs_node.rs new file mode 100644 index 000000000..c099bf77f --- /dev/null +++ b/examples/minimal_pub_sub/src/first_rclrs_node.rs @@ -0,0 +1,55 @@ +use rclrs::*; +use std::sync::{Arc, Mutex}; +use std_msgs::msg::String as StringMsg; + +struct RepublisherNode { + _node: Arc, + _subscription: Arc>, + _publisher: Arc>, + _data: Arc>>, +} + +impl RepublisherNode { + fn new(executor: &rclrs::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<(), rclrs::RclrsError> { + if let Some(s) = &*self._data.lock().unwrap() { + self._publisher.publish(s)?; + } + Ok(()) + } +} + +fn main() -> Result<(), rclrs::RclrsError> { + let context = Context::default_from_env()?; + let mut executor = context.create_basic_executor(); + let _republisher = RepublisherNode::new(&executor)?; + std::thread::spawn(move || -> Result<(), rclrs::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()) +}