Skip to content

Commit 942e310

Browse files
Refactor first_rclrs_node example for improved clarity and structure
1 parent 92edc21 commit 942e310

File tree

1 file changed

+76
-86
lines changed

1 file changed

+76
-86
lines changed

examples/minimal_pub_sub/src/first_rclrs_node.rs

Lines changed: 76 additions & 86 deletions
Original file line numberDiff line numberDiff line change
@@ -5,12 +5,11 @@
55
//!
66
//! ## Create a package
77
//! In ROS 2, `ros2 pkg create` is the standard way of creating packages. However, the functionality to create Rust packages with this tool is not yet implemented, so they need to be created manually.
8-
//!
8+
//!
99
//! You can use the current package that has already been created, or you can create a new package using `cargo` as usual:
1010
//! ```console
1111
//! cargo new republisher_node && cd republisher_node
1212
//! ```
13-
1413
//! In the `Cargo.toml` file, add dependencies for `rclrs = "*"` and `std_msgs = "*"`, if they are not already included.
1514
//!
1615
//! Additionally, a new `package.xml` needs to be created if you want your node to be buildable with `colcon` and make sure to change the build type to `ament_cargo` and to include the two packages mentioned above in the dependencies, as such:
@@ -62,23 +61,19 @@
6261
//! _data,
6362
//! })
6463
//! }
65-
//!}
66-
//! ```
67-
64+
//!}```
6865
//! Next, add a main function to launch it:
6966
70-
//!```rust
67+
//! ```rust
7168
//! fn main() -> Result<(), rclrs::RclrsError> {
72-
//! let context = Context::default_from_env()?;
73-
//! let mut executor = context.create_basic_executor();
74-
//! let _republisher = RepublisherNode::new(&executor)?;
75-
//! executor
76-
//! .spin(SpinOptions::default())
77-
//! .first_error()
78-
//! .map_err(|err| err.into())
79-
//!}
80-
//! ```
81-
69+
//! let context = Context::default_from_env()?;
70+
//! let mut executor = context.create_basic_executor();
71+
//! let _republisher = RepublisherNode::new(&executor)?;
72+
//! executor
73+
//! .spin(SpinOptions::default())
74+
//! .first_error()
75+
//! .map_err(|err| err.into())
76+
//! }```
8277
8378
//! ## Run the node
8479
//! You should now be able to run this node with `cargo build` and then `cargo run`. However, the subscription callback still has a `todo!` in it, so it will exit with an error when it receives a message.
@@ -89,17 +84,15 @@
8984
//! ```rust
9085
//! |msg: StringMsg| {
9186
//! data = Some(msg);
92-
//!},
93-
//!```
94-
95-
87+
//! }
88+
//! ```
9689
//! This is a standard pattern in C++, but doesn't work in Rust. Why not?
9790
//!
98-
//! Written like this, `data` is *borrowed* by the callback, but `data` is a local variable which only exists in its current form until the end of `RepublisherNode::new()`.
91+
//! Written like this, `data` is *borrowed* by the callback, but `data` is a local variable which only exists in its current form until the end of `RepublisherNode::new()`.
9992
//! The subscription callback is required to not borrow any variables because the subscription, and therefore the callback, could live indefinitely.
100-
//! > 💡 As an aside, this requirement is expressed by the `'static` bound on the generic parameter `F` for the callback in `Node::create_subscription()`.
93+
//! > 💡 As an aside, this requirement is expressed by the `'static` bound on the generic parameter `F` for the callback in `Node::create_subscription()`.
10194
//!
102-
//! You might think "I don't want to borrow from the local variable `data` anyway, I want to borrow from the `data` field in `RepublisherNode`!" and you would be right.
95+
//! You might think "I don't want to borrow from the local variable `data` anyway, I want to borrow from the `data` field in `RepublisherNode`!" and you would be right.
10396
//! That variable lives considerably longer, but also not forever, so it wouldn't be enough. A secondary problem is that it would be a *self-referential struct*, which is not allowed in Rust.
10497
//!
10598
//! The solution is _shared ownership_ of the data by the callback and the node. The `Arc` type provides shared ownership, but since it only gives out shared references to its data,
@@ -109,7 +102,7 @@
109102
//! 1. Import `Mutex`
110103
//! 2. Adjust the type of the `data` field
111104
//! 3. Create two pointers to the same data (wrapped in a `Mutex`)
112-
//! 4. Make the closure `move`, and inside it, lock the `Mutex` and store the message
105+
//! 4. Make the closure `move`, and inside it, lock the `Mutex` and store the message
113106
114107
//! ```rust
115108
//! use std::sync::{Arc, Mutex}; // (1)
@@ -140,20 +133,17 @@
140133
//! })
141134
//! }
142135
//! }
143-
144136
//! ```
145137
//!
146-
147138
//! 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.
148-
//!
139+
//!
149140
//! If you couldn't follow the explanation involving borrowing, closures etc. above, an explanation of these concepts is unfortunately out of scope of this tutorial.
150141
//! There are many good Rust books and tutorials that can help you understand these crucial features. The online book [*The Rust Programming Language*](https://doc.rust-lang.org/book/) is a good place to start for most topics.
151-
152-
142+
//!
153143
//! ## Periodically run a republishing function
154-
//!
144+
//!
155145
//! The node still doesn't republish the received messages. First, let's add a publisher to the node:
156-
146+
//!
157147
//! ```rust
158148
//! // Add this new field to the RepublisherNode struct, after the subscription:
159149
//! _publisher: Arc<rclrs::Publisher<StringMsg>>,
@@ -167,7 +157,6 @@
167157
//! _data,
168158
//! })
169159
//! ```
170-
171160
//! Then, let's add a `republish()` function to the `RepublisherNode` struct that periodically republishes the last message received, or does nothing if none was received:
172161
//!
173162
//! ```rust
@@ -178,36 +167,34 @@
178167
//! Ok(())
179168
//! }
180169
//! ```
181-
182170
//! 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,
183171
//! 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:
184172
//!
185173
//! ```rust
186174
//! fn main() -> Result<(), rclrs::RclrsError> {
187-
//! let context = Context::default_from_env()?;
188-
//! let mut executor = context.create_basic_executor();
189-
//! let _republisher = RepublisherNode::new(&executor)?;
190-
//! std::thread::spawn(|| -> Result<(), rclrs::RclrsError> {
191-
//! loop {
192-
//! use std::time::Duration;
193-
//! std::thread::sleep(Duration::from_millis(1000));
194-
//! _republisher.republish()?;
195-
//! }
196-
//! });
197-
//! executor
198-
//! .spin(SpinOptions::default())
199-
//! .first_error()
200-
//! .map_err(|err| err.into())
201-
//!}
202-
//!```
203-
204-
205-
//! But wait, this doesn't work – there is an error about the thread closure needing to outlive `'static`.
206-
//! That's again the same issue as above: Rust doesn't allow borrowing variables in this closure,
175+
//! let context = Context::default_from_env()?;
176+
//! let mut executor = context.create_basic_executor();
177+
//! let _republisher = RepublisherNode::new(&executor)?;
178+
//! std::thread::spawn(move || -> Result<(), rclrs::RclrsError> {
179+
//! loop {
180+
//! use std::time::Duration;
181+
//! std::thread::sleep(Duration::from_millis(1000));
182+
//! _republisher.republish()?;
183+
//! }
184+
//! });
185+
//! executor
186+
//! .spin(SpinOptions::default())
187+
//! .first_error()
188+
//! .map_err(|err| err.into())
189+
//! }
190+
//! ```
191+
//!
192+
//! But wait, this doesn't work – there is an error about the thread closure needing to outlive `'static`.
193+
//! That's again the same issue as above: Rust doesn't allow borrowing variables in this closure,
207194
//! because the function that the variable is coming from might return before the thread that borrows the variable ends.
208195
//! > 💡 Of course, you could argue that this cannot really happen here, because returning from `main()` will also terminate the other threads, but Rust isn't that smart.
209-
//!
210-
//! The solution is also the same as above: Shared ownership with `Arc`. Only this time, `Mutex` isn't needed since both the `rclcpp::spin()`
196+
//!
197+
//! The solution is also the same as above: Shared ownership with `Arc`. Only this time, `Mutex` isn't needed since both the `rclcpp::spin()`
211198
//! and the `republish()` function only require a shared reference:
212199
//! ```rust
213200
//! fn main() -> Result<(), rclrs::RclrsError> {
@@ -231,43 +218,46 @@
231218
//! ## Try it out
232219
//! In separate terminals, run `cargo run --bin first_rclrs_node` if running the current node or `cargo run` otherwise and `ros2 topic echo /out_topic`. Nothing will be shown yet, since our node hasn't received any data yet.
233220
//!
234-
//! In another terminal, publish a single message with `ros2 topic pub /in_topic std_msgs/msg/String '{data: "Bonjour"}' -1`.
221+
//! In another terminal, publish a single message with `ros2 topic pub /in_topic std_msgs/msg/String '{data: "Bonjour"}' -1`.
235222
//! The terminal with `ros2 topic echo` should now receive a new `Bonjour` message every second.
236223
//!
237224
//! Now publish another message, e.g. `ros2 topic pub /in_topic std_msgs/msg/String '{data: "Servus"}' -1` and observe the `ros2 topic echo` terminal receiving that message from that point forward.
238225
239-
use std::sync::{Arc, Mutex}; // (1)
240-
use std_msgs::msg::String as StringMsg;
241226
use rclrs::*;
227+
use std::sync::{Arc, Mutex};
228+
use std_msgs::msg::String as StringMsg;
229+
242230
struct RepublisherNode {
243-
_node: Arc<rclrs::Node>,
244-
_subscription: Arc<rclrs::Subscription<StringMsg>>,
245-
_publisher: Arc<rclrs::Publisher<StringMsg>>,
246-
_data: Arc<Mutex<Option<StringMsg>>>, // (2)
231+
_node: Arc<rclrs::Node>,
232+
_subscription: Arc<rclrs::Subscription<StringMsg>>,
233+
_publisher: Arc<rclrs::Publisher<StringMsg>>,
234+
_data: Arc<Mutex<Option<StringMsg>>>,
247235
}
236+
248237
impl RepublisherNode {
249-
fn new(executor: &rclrs::Executor) -> Result<Self, rclrs::RclrsError> {
250-
let _node = executor.create_node("republisher")?;
251-
let _data = Arc::new(Mutex::new(None)); // (3)
252-
let data_cb = Arc::clone(&_data);
253-
let _subscription = _node.create_subscription(
254-
"in_topic".keep_last(10).transient_local(), // (4)
255-
move |msg: StringMsg| {
256-
*data_cb.lock().unwrap() = Some(msg);
257-
},
258-
)?;
259-
let _publisher = _node.create_publisher::<std_msgs::msg::String>("out_topic")?;
260-
Ok(Self {
261-
_node,
262-
_subscription,
263-
_publisher,
264-
_data,
265-
})
266-
}
267-
fn republish(&self) -> Result<(), rclrs::RclrsError> {
238+
fn new(executor: &rclrs::Executor) -> Result<Self, rclrs::RclrsError> {
239+
let _node = executor.create_node("republisher")?;
240+
let _data = Arc::new(Mutex::new(None));
241+
let data_cb = Arc::clone(&_data);
242+
let _subscription = _node.create_subscription(
243+
"in_topic".keep_last(10).transient_local(),
244+
move |msg: StringMsg| {
245+
*data_cb.lock().unwrap() = Some(msg);
246+
},
247+
)?;
248+
let _publisher = _node.create_publisher::<std_msgs::msg::String>("out_topic")?;
249+
Ok(Self {
250+
_node,
251+
_subscription,
252+
_publisher,
253+
_data,
254+
})
255+
}
256+
257+
fn republish(&self) -> Result<(), rclrs::RclrsError> {
268258
if let Some(s) = &*self._data.lock().unwrap() {
269259
self._publisher.publish(s)?;
270-
}
260+
}
271261
Ok(())
272262
}
273263
}
@@ -283,7 +273,7 @@ fn main() -> Result<(), rclrs::RclrsError> {
283273
}
284274
});
285275
executor
286-
.spin(SpinOptions::default())
287-
.first_error()
288-
.map_err(|err| err.into())
289-
}
276+
.spin(SpinOptions::default())
277+
.first_error()
278+
.map_err(|err| err.into())
279+
}

0 commit comments

Comments
 (0)