From ca526b54279727a1199060e155a65aaa2391d601 Mon Sep 17 00:00:00 2001
From: Philipp Caspers <philipp.caspers@vorausrobotik.com>
Date: Mon, 8 Jul 2024 13:01:02 +0000
Subject: [PATCH] test: Implement helper ros_subscriber

Co-authored-by: Jan-Niklas Burfeind <jan-niklas.burfeind@vorausrobotik.com>
---
 tests/helpers/ros_subscriber.rs | 57 +++++++++++++++++++++++++++++++++
 1 file changed, 57 insertions(+)
 create mode 100644 tests/helpers/ros_subscriber.rs

diff --git a/tests/helpers/ros_subscriber.rs b/tests/helpers/ros_subscriber.rs
new file mode 100644
index 0000000..72ad65d
--- /dev/null
+++ b/tests/helpers/ros_subscriber.rs
@@ -0,0 +1,57 @@
+use rclrs::{create_node, Context, Node, RclrsError, Subscription, QOS_PROFILE_DEFAULT};
+use sensor_msgs::msg::JointState as JointStateMsg;
+use std::{
+    env,
+    sync::{
+        atomic::{AtomicU32, Ordering},
+        Arc, Mutex,
+    },
+};
+
+pub struct Subscriber {
+    pub num_messages: AtomicU32,
+    pub data: Arc<Mutex<Option<JointStateMsg>>>,
+    pub node: Arc<Node>,
+    subscription: Mutex<Option<Arc<Subscription<JointStateMsg>>>>,
+}
+
+impl Subscriber {
+    pub fn new(name: &str, topic: &str) -> Result<Arc<Self>, RclrsError> {
+        let context = Context::new(env::args())?;
+        let node = create_node(&context, name)?;
+        let subscriber = Arc::new(Subscriber {
+            num_messages: 0.into(),
+            data: Arc::new(Mutex::new(None)),
+            node,
+            subscription: None.into(),
+        });
+
+        let minimal_subscriber_aux = Arc::clone(&subscriber);
+        let subscription = subscriber.node.create_subscription::<JointStateMsg, _>(
+            topic,
+            QOS_PROFILE_DEFAULT,
+            move |msg: JointStateMsg| {
+                minimal_subscriber_aux.callback(msg);
+            },
+        )?;
+        *subscriber.subscription.lock().unwrap() = Some(subscription);
+        Ok(subscriber)
+    }
+    fn callback(&self, msg: JointStateMsg) {
+        self.num_messages.fetch_add(1, Ordering::SeqCst);
+        println!("msg {}", msg.clone().position.first().unwrap());
+        *self.data.lock().unwrap() = Some(msg);
+
+        println!(
+            "data {}",
+            self.data
+                .lock()
+                .unwrap()
+                .as_mut()
+                .unwrap()
+                .position
+                .first()
+                .unwrap()
+        );
+    }
+}