Skip to content

Commit 625357f

Browse files
author
GueLaKais
committed
added cargo manifest
1 parent 7d4c713 commit 625357f

File tree

3 files changed

+152
-0
lines changed

3 files changed

+152
-0
lines changed

examples/your_package_name/Cargo.toml

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
[package]
2+
name = "your_package_name"
3+
version = "0.1.0"
4+
edition = "2021"
5+
6+
# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
7+
[[bin]]
8+
name="simple_publisher"
9+
path="src/main.rs"
10+
[[bin]]
11+
name="simple_subscriber"
12+
path="src/simple_subscriber.rs"
13+
[dependencies]
14+
rclrs = "*"
15+
std_msgs = "*"
Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
/// Creates a SimplePublisherNode, initializes a node and publisher, and provides
2+
/// methods to publish a simple "Hello World" message on a loop in separate threads.
3+
4+
/// Imports the Arc type from std::sync, used for thread-safe reference counting pointers,
5+
/// and the StringMsg message type from std_msgs for publishing string messages.
6+
use std::{sync::Arc,time::Duration,iter,thread};
7+
use rclrs::{RclrsError,QOS_PROFILE_DEFAULT,Context,create_node,Node,Publisher};
8+
use std_msgs::msg::String as StringMsg;
9+
// / SimplePublisherNode struct contains node and publisher members.
10+
// / Used to initialize a ROS 2 node and publisher, and publish messages.
11+
struct SimplePublisherNode {
12+
node: Arc<Node>,
13+
_publisher: Arc<Publisher<StringMsg>>,
14+
}
15+
/// The `new` function takes a context and returns a Result containing the
16+
/// initialized SimplePublisherNode or an error. It creates a node with the
17+
/// given name and creates a publisher on the "publish_hello" topic.
18+
///
19+
/// The SimplePublisherNode contains the node and publisher members.
20+
impl SimplePublisherNode {
21+
/// Creates a new SimplePublisherNode by initializing a node and publisher.
22+
///
23+
/// This function takes a context and returns a Result containing the
24+
/// initialized SimplePublisherNode or an error. It creates a node with the
25+
/// given name and creates a publisher on the "publish_hello" topic.
26+
///
27+
/// The SimplePublisherNode contains the node and publisher members.
28+
fn new(context: &Context) -> Result<Self,RclrsError> {
29+
let node = create_node(context, "simple_publisher").unwrap();
30+
let _publisher = node
31+
.create_publisher("publish_hello", QOS_PROFILE_DEFAULT)
32+
.unwrap();
33+
Ok(Self { node, _publisher, })
34+
}
35+
36+
/// Publishes a "Hello World" message on the publisher.
37+
///
38+
/// Creates a StringMsg with "Hello World" as the data, publishes it on
39+
/// the `_publisher`, and returns a Result. This allows regularly publishing
40+
/// a simple message on a loop.
41+
fn publish_data(&self,inkrement:i32) -> Result<i32,RclrsError> {
42+
43+
let msg: StringMsg = StringMsg {
44+
data: format!("Hello World {}",inkrement),
45+
};
46+
self._publisher.publish(msg).unwrap();
47+
Ok(inkrement+1_i32)
48+
}
49+
}
50+
51+
/// The main function initializes a ROS 2 context, node and publisher,
52+
/// spawns a thread to publish messages repeatedly, and spins the node
53+
/// to receive callbacks.
54+
///
55+
/// It creates a context, initializes a SimplePublisherNode which creates
56+
/// a node and publisher, clones the publisher to pass to the thread,
57+
/// spawns a thread to publish "Hello World" messages repeatedly, and
58+
/// calls spin() on the node to receive callbacks. This allows publishing
59+
/// messages asynchronously while spinning the node.
60+
fn main() -> Result<(),RclrsError> {
61+
let context = Context::new(std::env::args()).unwrap();
62+
let publisher = Arc::new(SimplePublisherNode::new(&context).unwrap());
63+
let publisher_other_thread = Arc::clone(&publisher);
64+
let mut iterator: i32=0;
65+
thread::spawn(move || -> () {
66+
iter::repeat(()).for_each(|()| {
67+
thread::sleep(Duration::from_millis(1000));
68+
iterator=publisher_other_thread.publish_data(iterator).unwrap();
69+
});
70+
});
71+
rclrs::spin(publisher.node.clone())
72+
}
Lines changed: 65 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,65 @@
1+
use rclrs::{create_node, Context, Node, RclrsError, Subscription, QOS_PROFILE_DEFAULT};
2+
use std::{
3+
env,
4+
iter,thread,
5+
sync::{Arc, Mutex},
6+
time::Duration,
7+
};
8+
use std_msgs::msg::String as StringMsg;
9+
/// A simple ROS2 subscriber node that receives and prints "hello" messages.
10+
///
11+
/// This node creates a subscription to the "publish_hello" topic and prints the
12+
/// received messages to the console. It runs the subscription in a separate
13+
/// thread, while the main thread calls `rclrs::spin()` to keep the node running.
14+
pub struct SimpleSubscriptionNode {
15+
node: Arc<Node>,
16+
_subscriber: Arc<Subscription<StringMsg>>,
17+
data: Arc<Mutex<Option<StringMsg>>>,
18+
}
19+
/// Implements a simple ROS2 subscriber node that receives and prints "hello" messages.
20+
/// The `new` function creates the node and the subscription, and returns a `SimpleSubscriptionNode`
21+
/// instance. The `data_callback` function can be used to access the latest received message.
22+
impl SimpleSubscriptionNode {
23+
fn new(context: &Context) -> Result<Self, RclrsError> {
24+
let node = create_node(context, "simple_subscription").unwrap();
25+
let data: Arc<Mutex<Option<StringMsg>>> = Arc::new(Mutex::new(None));
26+
let data_mut: Arc<Mutex<Option<StringMsg>>> = Arc::clone(&data);
27+
let _subscriber = node
28+
.create_subscription::<StringMsg, _>(
29+
"publish_hello",
30+
QOS_PROFILE_DEFAULT,
31+
move |msg: StringMsg| {
32+
*data_mut.lock().unwrap() = Some(msg);
33+
},
34+
)
35+
.unwrap();
36+
Ok(Self {
37+
node,
38+
_subscriber,
39+
data,
40+
})
41+
}
42+
fn data_callback(&self) -> Result<(), RclrsError> {
43+
if let Some(data) = self.data.lock().unwrap().as_ref() {
44+
println!("{}", data.data);
45+
} else {
46+
println!("No message available yet.");
47+
}
48+
Ok(())
49+
}
50+
}
51+
/// The `main` function creates a new ROS2 context, a `SimpleSubscriptionNode` instance, and starts a separate thread to periodically call the `data_callback` method on the subscription. The main thread then calls `rclrs::spin()` to keep the node running and receive messages.
52+
///
53+
/// The separate thread is used to ensure that the `data_callback` method is called regularly, even if the main thread is blocked in `rclrs::spin()`. This allows the subscriber to continuously process and print the received "hello" messages.
54+
fn main() -> Result<(), RclrsError> {
55+
let context = Context::new(env::args()).unwrap();
56+
let subscription = Arc::new(SimpleSubscriptionNode::new(&context).unwrap());
57+
let subscription_other_thread = Arc::clone(&subscription);
58+
thread::spawn(move || -> () {
59+
iter::repeat(()).for_each(|()| {
60+
thread::sleep(Duration::from_millis(1000));
61+
subscription_other_thread.data_callback().unwrap()
62+
});
63+
});
64+
rclrs::spin(subscription.node.clone())
65+
}

0 commit comments

Comments
 (0)