From 504000d49680fb15e557c65b3e75fd968b7bc3af Mon Sep 17 00:00:00 2001
From: Olek Bojda <alebojd@gmail.com>
Date: Mon, 22 Oct 2018 19:50:03 +0200
Subject: [PATCH] Added functionality to check if robot is already selected by
 another group

---
 safety_user_plugin/scripts/qt_wrapper.py  |  3 ++
 safety_user_plugin/scripts/ros_wrapper.py | 35 ++++++++++++++++++++++-
 safety_user_plugin/scripts/user_plugin.py |  6 +++-
 3 files changed, 42 insertions(+), 2 deletions(-)

diff --git a/safety_user_plugin/scripts/qt_wrapper.py b/safety_user_plugin/scripts/qt_wrapper.py
index 2791c26..fe0c18a 100644
--- a/safety_user_plugin/scripts/qt_wrapper.py
+++ b/safety_user_plugin/scripts/qt_wrapper.py
@@ -236,6 +236,9 @@ class QtWrapper:
     def obstacle_is_detected_notify(self):
         self.log_error('Nie mozna wystartować. Przeszkoda w polu działania robota')
 
+    def robot_selected_by_another_group_notify(self):
+        self.log_error('Nie mozna wybrać robota. Robot został już wybrany przez inną grupę')
+
     def update_restrictions_gui(self,restrictions):
         self.widget.distanceRestrictionLabel.setText("{:.2f}".format(restrictions.distance))
         self.widget.linearRestrictionLabel.setText("{:.2f}".format(restrictions.linear_velocity))
diff --git a/safety_user_plugin/scripts/ros_wrapper.py b/safety_user_plugin/scripts/ros_wrapper.py
index 5178d02..f4bfa19 100644
--- a/safety_user_plugin/scripts/ros_wrapper.py
+++ b/safety_user_plugin/scripts/ros_wrapper.py
@@ -1,6 +1,7 @@
 #!/usr/bin/env python
 import rospy
 import rospkg
+import rostopic
 
 from std_msgs.msg import Bool
 from rosaria_msgs.msg import RobotInfoMsg
@@ -34,6 +35,11 @@ class ROSWrapper:
         self.robots_list_timer = None
         self.connection_timer = None
         self.master_connection_timer = None
+        self.hz_update_timer = rospy.Timer(rospy.Duration(0.2),self.update_hz_values)
+
+        self.robots_hz_subscribers_dict = {}
+        self.rostopics_hz_dict = {}
+        self.robots_hz_value = {}
 
     # def setup_node(self):
     #     rospy.init_node('safety_user_plugin', anonymous=True)
@@ -50,7 +56,7 @@ class ROSWrapper:
 
         if self.periodic_timer != None:
             self.shutdown_timer(self.periodic_timer)
-        self.periodic_timer = rospy.Timer(rospy.Duration(0.2),self.publish_periodic_update)
+        self.periodic_timer = rospy.Timer(rospy.Duration(0.1),self.publish_periodic_update)
 
         if self.connection_timer != None:
             self.shutdown_timer(self.connection_timer)        
@@ -60,6 +66,10 @@ class ROSWrapper:
             self.shutdown_timer(self.master_connection_timer)       
         self.master_connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_master_connection_lost)
 
+        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 != None:
             subscriber.unregister()
@@ -90,6 +100,11 @@ class ROSWrapper:
         # else:
         #     raise ValueError('clutch_state UNKNOWN when attempting to publish periodic update')
 
+    def update_hz_values(self,event):
+        for key in self.rostopics_hz_dict:
+            self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz('/PIONIER{0}/RosAria/user_stop'.format(key))
+
+
     def handle_robot_connection_lost(self,event):
         self.shutdown_timer(self.connection_timer)
         self.robot_connection_lost_callback()
@@ -161,6 +176,24 @@ class ROSWrapper:
                     robot_number = int(robot_number)
                     robots_id_list.append(robot_number)
 
+
+        # Adding new hz subscribers
+        for robot_id in robots_id_list:
+            if robot_id not in self.robots_hz_subscribers_dict:
+                # Add hz subscriber
+                self.rostopics_hz_dict[robot_id] = rostopic.ROSTopicHz(-1)
+                self.robots_hz_subscribers_dict[robot_id] = rospy.Subscriber('/PIONIER{0}/RosAria/user_stop'.format(robot_id),Bool,
+                    self.rostopics_hz_dict[robot_id].callback_hz,callback_args='/PIONIER{0}/RosAria/user_stop'.format(robot_id))
+
+        # Removing old hz subscribers
+        for robot_key in self.robots_hz_subscribers_dict:
+            if robot_key not in robots_id_list:
+                # Remove old subscriber
+                self.unsubscribe(self.robots_hz_subscribers_dict[robot_id])
+                self.robots_hz_subscribers_dict[robot_id] = None
+                self.rostopics_hz_dict[robot_id] = None
+
+
         self.robots_list_update_callback(robots_id_list)
 
 
diff --git a/safety_user_plugin/scripts/user_plugin.py b/safety_user_plugin/scripts/user_plugin.py
index f2ae378..cc7169a 100755
--- a/safety_user_plugin/scripts/user_plugin.py
+++ b/safety_user_plugin/scripts/user_plugin.py
@@ -118,7 +118,11 @@ class UserPlugin(Plugin):
     # Qt callback functions
     def handle_robot_selection(self,robot_id):
         if self._user_info.selected_robot == None:
-            self.select_robot(robot_id)
+            if self._ros_wrapper.robots_hz_value[robot_id] == None:
+                    self.select_robot(robot_id)
+            else:
+                self._qt_wrapper.robot_selected_by_another_group_notify()
+        
         else:
             raise RuntimeError('User already selected robot')