Skip to content

Commit 52b0d6a

Browse files
GuelakaisGueLaKaisGueLaKaisestevemaspe36
committed
Add simple talk/listener tutorial, matching the official ROS 2 documentation #378 (#390)
* Added another tutorial to ros2 rust. --------- Co-authored-by: GueLaKais <koroyeldores@gmail.com> Co-authored-by: GueLaKais <koryeldiores@gmail.com> Co-authored-by: Esteve Fernandez <esteve@apache.org> Co-authored-by: Esteve Fernandez <33620+esteve@users.noreply.github.com> Co-authored-by: Sam Privett <privett.sam@gmail.com>
1 parent f4e7c05 commit 52b0d6a

File tree

4 files changed

+118
-0
lines changed

4 files changed

+118
-0
lines changed

rclrs/rust_pubsub/Cargo.toml

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
[package]
2+
name = "rust_pubsub"
3+
version = "0.1.0"
4+
edition = "2021"
5+
6+
[[bin]]
7+
name="simple_publisher"
8+
path="src/simple_publisher.rs"
9+
10+
[[bin]]
11+
name="simple_subscriber"
12+
path="src/simple_subscriber.rs"
13+
14+
[dependencies]
15+
rclrs = "*"
16+
std_msgs = "*"

rclrs/rust_pubsub/package.xml

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
<package format="3">
2+
<name>rust_pubsub</name>
3+
<version>0.0.0</version>
4+
<description>TODO: Package description.</description>
5+
<maintainer email="user@todo.todo">user</maintainer>
6+
<license>TODO: License declaration.</license>
7+
8+
<depend>rclrs</depend>
9+
<depend>std_msgs</depend>
10+
11+
<export>
12+
<build_type>ament_cargo</build_type>
13+
</export>
14+
</package>
Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
use rclrs::{create_node, Context, Node, Publisher, RclrsError, QOS_PROFILE_DEFAULT};
2+
use std::{sync::Arc, thread, time::Duration};
3+
use std_msgs::msg::String as StringMsg;
4+
struct SimplePublisherNode {
5+
node: Arc<Node>,
6+
_publisher: Arc<Publisher<StringMsg>>,
7+
}
8+
impl SimplePublisherNode {
9+
fn new(context: &Context) -> Result<Self, RclrsError> {
10+
let node = create_node(context, "simple_publisher").unwrap();
11+
let _publisher = node
12+
.create_publisher("publish_hello", QOS_PROFILE_DEFAULT)
13+
.unwrap();
14+
Ok(Self { node, _publisher })
15+
}
16+
17+
fn publish_data(&self, increment: i32) -> Result<i32, RclrsError> {
18+
let msg: StringMsg = StringMsg {
19+
data: format!("Hello World {}", increment),
20+
};
21+
self._publisher.publish(msg).unwrap();
22+
Ok(increment + 1_i32)
23+
}
24+
}
25+
26+
fn main() -> Result<(), RclrsError> {
27+
let context = Context::new(std::env::args()).unwrap();
28+
let publisher = Arc::new(SimplePublisherNode::new(&context).unwrap());
29+
let publisher_other_thread = Arc::clone(&publisher);
30+
let mut count: i32 = 0;
31+
thread::spawn(move || loop {
32+
thread::sleep(Duration::from_millis(1000));
33+
count = publisher_other_thread.publish_data(count).unwrap();
34+
});
35+
rclrs::spin(publisher.node.clone())
36+
}
Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
use rclrs::{create_node, Context, Node, RclrsError, Subscription, QOS_PROFILE_DEFAULT};
2+
use std::{
3+
env,
4+
sync::{Arc, Mutex},
5+
thread,
6+
time::Duration,
7+
};
8+
use std_msgs::msg::String as StringMsg;
9+
pub struct SimpleSubscriptionNode {
10+
node: Arc<Node>,
11+
_subscriber: Arc<Subscription<StringMsg>>,
12+
data: Arc<Mutex<Option<StringMsg>>>,
13+
}
14+
impl SimpleSubscriptionNode {
15+
fn new(context: &Context) -> Result<Self, RclrsError> {
16+
let node = create_node(context, "simple_subscription").unwrap();
17+
let data: Arc<Mutex<Option<StringMsg>>> = Arc::new(Mutex::new(None));
18+
let data_mut: Arc<Mutex<Option<StringMsg>>> = Arc::clone(&data);
19+
let _subscriber = node
20+
.create_subscription::<StringMsg, _>(
21+
"publish_hello",
22+
QOS_PROFILE_DEFAULT,
23+
move |msg: StringMsg| {
24+
*data_mut.lock().unwrap() = Some(msg);
25+
},
26+
)
27+
.unwrap();
28+
Ok(Self {
29+
node,
30+
_subscriber,
31+
data,
32+
})
33+
}
34+
fn data_callback(&self) -> Result<(), RclrsError> {
35+
if let Some(data) = self.data.lock().unwrap().as_ref() {
36+
println!("{}", data.data);
37+
} else {
38+
println!("No message available yet.");
39+
}
40+
Ok(())
41+
}
42+
}
43+
fn main() -> Result<(), RclrsError> {
44+
let context = Context::new(env::args()).unwrap();
45+
let subscription = Arc::new(SimpleSubscriptionNode::new(&context).unwrap());
46+
let subscription_other_thread = Arc::clone(&subscription);
47+
thread::spawn(move || loop {
48+
thread::sleep(Duration::from_millis(1000));
49+
subscription_other_thread.data_callback().unwrap()
50+
});
51+
rclrs::spin(subscription.node.clone())
52+
}

0 commit comments

Comments
 (0)