From bdf9d0f04c3241d8ea9a6b9e078789138c451251 Mon Sep 17 00:00:00 2001
From: Olek Bojda <alebojd@gmail.com>
Date: Fri, 28 Sep 2018 17:59:45 +0200
Subject: [PATCH] Small adjustments in ros topics names and fix for clutch
 handling in user plugin

---
 .../scripts/ros_master_wrapper.py             |  8 +++---
 safety_user_plugin/scripts/ros_wrapper.py     | 26 +++++++++----------
 safety_user_plugin/scripts/user_info.py       |  1 +
 3 files changed, 18 insertions(+), 17 deletions(-)

diff --git a/safety_master_plugin/scripts/ros_master_wrapper.py b/safety_master_plugin/scripts/ros_master_wrapper.py
index 9fa80be..0a51d8a 100644
--- a/safety_master_plugin/scripts/ros_master_wrapper.py
+++ b/safety_master_plugin/scripts/ros_master_wrapper.py
@@ -28,14 +28,14 @@ class ROSWrapper:
 
 
     def setup_subscribers_and_publishers(self):
-        self.master_stop_publisher = rospy.Publisher('/RosAria/master_stop',Bool,queue_size = 1)
-        self.restrictions_publisher = rospy.Publisher('/RosAria/restrictions',RestrictionsMsg,queue_size = 1)
+        self.master_stop_publisher = rospy.Publisher('/PIONIER/master_stop',Bool,queue_size = 1)
+        self.restrictions_publisher = rospy.Publisher('/PIONIER/restrictions',RestrictionsMsg,queue_size = 1)
 
         self.periodic_timer = rospy.Timer(rospy.Duration(0.5),self.publish_periodic_update)
 
     def add_robot_subscriber(self,robot_id):
         if robot_id not in self.robot_info_subscribers:
-            topic_name = '/RosAria/PIONIER{0}/robot_info'.format(robot_id)
+            topic_name = '/PIONIER{0}/RosAria/robot_info'.format(robot_id)
             new_sub = rospy.Subscriber(topic_name, RobotInfoMsg, self.handle_robot_info_update)
             self.robot_info_subscribers[robot_id] = new_sub
         else:
@@ -116,7 +116,7 @@ class ROSWrapper:
                 pass
             else:
                 robot_number = topic.split('/')
-                robot_number = robot_number[2]
+                robot_number = robot_number[1]
                 robot_number = robot_number[7:]
                 if len(robot_number) > 0:
                     robot_number = int(robot_number)
diff --git a/safety_user_plugin/scripts/ros_wrapper.py b/safety_user_plugin/scripts/ros_wrapper.py
index 8e08f33..5178d02 100644
--- a/safety_user_plugin/scripts/ros_wrapper.py
+++ b/safety_user_plugin/scripts/ros_wrapper.py
@@ -41,12 +41,12 @@ class ROSWrapper:
     def setup_subscribers_and_publishers(self,robot_id):
         robot_name = 'PIONIER' + str(robot_id)
 
-        self.robot_info_subscriber = rospy.Subscriber('/RosAria/{0}/robot_info'.format(robot_name), RobotInfoMsg, self.handle_selected_robot_info_update)
-        self.master_stop_subscriber = rospy.Subscriber('/RosAria/master_stop', Bool, self.handle_master_stop_update)
-        self.restrictions_subscriber = rospy.Subscriber('/RosAria/restrictions', RestrictionsMsg, self.handle_restrictions_update)
+        self.robot_info_subscriber = rospy.Subscriber('/{0}/RosAria/robot_info'.format(robot_name), RobotInfoMsg, self.handle_selected_robot_info_update)
+        self.master_stop_subscriber = rospy.Subscriber('/PIONIER/master_stop', Bool, self.handle_master_stop_update)
+        self.restrictions_subscriber = rospy.Subscriber('/PIONIER/restrictions', RestrictionsMsg, self.handle_restrictions_update)
 
-        self.user_stop_publisher = rospy.Publisher('/RosAria/{0}/user_stop'.format(robot_name),Bool,queue_size = 1)
-        self.clutch_publisher = rospy.Publisher('/RosAria/{0}/clutch'.format(robot_name),Bool,queue_size = 1)
+        self.user_stop_publisher = rospy.Publisher('/{0}/RosAria/user_stop'.format(robot_name),Bool,queue_size = 1)
+        self.clutch_publisher = rospy.Publisher('/{0}/RosAria/clutch'.format(robot_name),Bool,queue_size = 1)
 
         if self.periodic_timer != None:
             self.shutdown_timer(self.periodic_timer)
@@ -74,7 +74,7 @@ class ROSWrapper:
 
     def publish_periodic_update(self,event):
         stop_state = self.get_user_stop_state()
-        clutch_state = self.get_clutch_state()
+        # clutch_state = self.get_clutch_state()
 
         if stop_state == SS.RUNNING:
             self.user_started()
@@ -83,12 +83,12 @@ class ROSWrapper:
         else:
             raise ValueError('stop_state UNKNOWN when attempting to publish periodic update')
 
-        if clutch_state == CS.ENGAGED:
-            self.engage_clutch()
-        elif clutch_state == CS.DISENGAGED:
-            self.disengage_clutch()
-        else:
-            raise ValueError('clutch_state UNKNOWN when attempting to publish periodic update')
+        # if clutch_state == CS.ENGAGED:
+        #     self.engage_clutch()
+        # elif clutch_state == CS.DISENGAGED:
+        #     self.disengage_clutch()
+        # else:
+        #     raise ValueError('clutch_state UNKNOWN when attempting to publish periodic update')
 
     def handle_robot_connection_lost(self,event):
         self.shutdown_timer(self.connection_timer)
@@ -155,7 +155,7 @@ class ROSWrapper:
                 pass
             else:
                 robot_number = topic.split('/')
-                robot_number = robot_number[2]
+                robot_number = robot_number[1]
                 robot_number = robot_number[7:]
                 if len(robot_number) > 0:
                     robot_number = int(robot_number)
diff --git a/safety_user_plugin/scripts/user_info.py b/safety_user_plugin/scripts/user_info.py
index cb81450..6fcc49d 100644
--- a/safety_user_plugin/scripts/user_info.py
+++ b/safety_user_plugin/scripts/user_info.py
@@ -27,6 +27,7 @@ class UserInfo:
         self.selected_robot.angular_velocity = new_robot_info.angular_velocity
         self.selected_robot.state = new_robot_info.state
         self.selected_robot.clutch = new_robot_info.clutch
+        self.clutch_state = new_robot_info.clutch
 
     def get_user_stop_state(self):
         return self.user_stop_state