From 11d64cb0cf4cab159cb565004e3df5de415da593 Mon Sep 17 00:00:00 2001
From: Jakub Delicat <jakub.delicat@pwr.edu.pl>
Date: Wed, 13 Sep 2023 12:16:11 +0200
Subject: [PATCH] Implemented can't get robot:

Signed-off-by: Jakub Delicat <jakub.delicat@pwr.edu.pl>
---
 safety_user_plugin/ros_wrapper.py | 52 +++++++++++++++++++++----------
 1 file changed, 35 insertions(+), 17 deletions(-)

diff --git a/safety_user_plugin/ros_wrapper.py b/safety_user_plugin/ros_wrapper.py
index b11e400..26b8fe3 100644
--- a/safety_user_plugin/ros_wrapper.py
+++ b/safety_user_plugin/ros_wrapper.py
@@ -23,7 +23,6 @@ from safety_user_plugin.enums.stop_state import StopState as SS
 class ROSWrapper(Node):
     def __init__(self):
         # TODO: mogą być problemy z tymi samymi nazwami node'ów, gry różne kompy będą miały odpalony
-        # safety user system
         super().__init__("safety_user_plugin")
         self.robots_list_update_callback = None
         self.selected_robot_info_update_callback = None
@@ -47,11 +46,18 @@ class ROSWrapper(Node):
         self.connection_timer = None
         self.master_connection_timer = None
         self.robot_stop_timer = None
-        # self.hz_update_timer = self.create_timer(0.2, self.publish_user_stop_and_update_hz_values)
 
-        self.robots_hz_subscribers_dict = {}
-        self.rostopics_hz_dict = {}
-        self.robots_hz_value = {}
+        self.time_of_last_msg_of_user_stop = [
+            None for i in range(6)
+        ]
+        self.user_stop_subscribers = [
+            None for i in range(6)
+        ]
+        self.robots_hz_value = [
+            None for i in range(6)
+        ]
+        
+        self.hz_update_timer = self.create_timer(0.1, self.update_taken_robots)
 
         self._qos = QoSProfile(
             reliability=QoSReliabilityPolicy.RELIABLE,
@@ -60,6 +66,20 @@ class ROSWrapper(Node):
             liveliness=QoSLivelinessPolicy.AUTOMATIC,
             depth=10,
         )
+        self._create_user_stops_subscribers_and_callbacks()
+
+    def _create_user_stops_subscribers_and_callbacks(self):
+        for i in range(6):
+            self.user_stop_subscribers[i] = self.create_subscription(
+                Bool,
+                f"/pioneer{i}/user_stop",
+                lambda msg, i_in = i: self._update_user_stop_period(msg, i_in),
+                qos_profile=self._qos,
+            )
+
+    def _update_user_stop_period(self, msg, robot_id):
+        print(f"Got user stop message for {robot_id}: {msg.data}")
+        self.time_of_last_msg_of_user_stop[robot_id] = self.get_clock().now()
 
     def setup_subscribers_and_publishers(self, robot_id):
         self.robot_info_subscriber = self.create_subscription(
@@ -96,16 +116,13 @@ class ROSWrapper(Node):
 
         self.shutdown_timer(self.master_connection_timer)
         self.master_connection_timer = self.create_timer(
-            1.5, self.handle_master_connection_lost()
+            1.5, self.handle_master_connection_lost
         )
 
         self.shutdown_timer(self.robot_stop_timer)
         self.robot_stop_timer = self.create_timer(
             0.1, self.publish_robot_stop
         )
-        # if self.hz_update_timer != None:
-        #     self.shutdown_timer(self.hz_update_timer)
-        # self.hz_update_timer = rospy.Timer(rospy.Duration(0.2), self.update_hz_values)
 
     def unsubscribe(self, subscriber):
         if subscriber is not None:
@@ -128,15 +145,16 @@ class ROSWrapper(Node):
             msg.data = self.user_stop_state_to_publish
             self.user_stop_publisher.publish(msg)
     
-    def publish_user_stop_and_update_hz_values(self):
-        print("update_hz_values")
+    def update_taken_robots(self):
+        for i, last_time in zip(range(6), self.time_of_last_msg_of_user_stop):
+            if last_time is None:
+                self.robots_hz_value[i] = None
+                continue
 
-
-
-        # for key in self.rostopics_hz_dict:
-        #     self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz(
-        #         "/PIONEER{0}/RosAria/user_stop".format(key)
-        #     )
+            self.robots_hz_value[i] =  1.0/float((self.get_clock().now() - last_time).nanoseconds) * 10e9
+            if self.robots_hz_value[i] < 20.0:
+                self.robots_hz_value[i] = None
+            print(f"hz values: {self.robots_hz_value}")
 
     def handle_robot_connection_lost(self):
         self.shutdown_timer(self.connection_timer)