Further changes in user plugin

safety_user_plugin/scripts/qt_wrapper.py

@@ -25,17 +25,17 @@ class QtWrapper:
     def update_selected_robot_info(self,robot_info):
         raise NotImplementedError
 
-    def master_stopped():
+    def master_stopped(self):
         raise NotImplementedError
 
-    def master_started():
+    def master_started(self):
         raise NotImplementedError
 
-    def user_stopped():
+    def user_stopped(self):
         raise NotImplementedError
 
-    def user_started():
+    def user_started(self):
         raise NotImplementedError
 
-    def master_is_stopped_notify():
+    def master_is_stopped_notify(self):
         raise NotImplementedError

safety_user_plugin/scripts/restrictions.py

@@ -0,0 +1,8 @@
+#!/usr/bin/env python
+
+class Restrictions:
+
+	def __init__(self,restrictions_msg):
+		self.distance = restrictions_msg.distance
+		self.linear_velocity = restrictions_msg.linear_velocity
+		self.angular_velocity = restrictions_msg.angular_velocity

safety_user_plugin/scripts/robot_info.py

@@ -1,9 +1,11 @@
 #!/usr/bin/env python
 
-
-
 class RobotInfo:
 
-    def __init__(self,robot_id):
-        self.robot_id = robot_id
-        raise(NotImplementedError)
+    def __init__(self,robot_info_msg):
+        self.robot_id = robot_info_msg.robot_id
+        self.battery_voltage = robot_info_msg.battery_voltage
+        self.linear_velocity = robot_info_msg.linear_velocity
+        self.angular_velocity = robot_info_msg.angular_velocity
+        self.state = robot_info_msg.state
+        self.clutch = robot_info_msg.clutch

safety_user_plugin/scripts/ros_wrapper.py

@@ -1,31 +1,206 @@
 #!/usr/bin/env python
+import rospy
+import rospkg
+
+from std_msgs.msg import Bool
+from safety_user_plugin.msg import RobotInfoMsg
+from safety_user_plugin.msg import RestrictionsMsg
+
+from robot_info import RobotInfo
+from restrictions import Restrictions
+
+from enums.clutch_state import ClutchState as CS
+from enums.stop_state import StopState as SS
 
 class ROSWrapper:
 
     def __init__(self):
-        pass
+        self.robots_list_update_callback = None
+        self.selected_robot_info_update_callback = None
+        self.master_stop_update_callback = None
+        self.restrictions_update_callback = None
+
+        self.get_user_stop_state = None
+        self.get_clutch_state = None
+
+        self.robot_info_subscriber = None
+        self.master_stop_subscriber = None
+        self.restrictions_subscriber = None
+
+        self.user_stop_publisher = None
+        self.clutch_publisher = None
+
+        self.periodic_timer = None
 
     def setup_node(self):
+        rospy.init_node('safety_user_plugin', anonymous=True)
+
+    def setup_subscribers_and_publishers(self,robot_id):
+        robot_name = 'PIONIER' + str(robot_id)
+
+        self.robot_info_subscriber = rospy.Subscriber('/RosAria/'+robot_name+'/robot_info', 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.user_stop_publisher = rospy.Publisher('/RosAria/'+robot_name+'/user_stop',Bool,queue_size = 1)
+        self.clutch_publisher = rospy.Publisher('/RosAria/'+robot_name+'/clutch',Bool,queue_size = 1)
+
+        self.periodic_timer = rospy.Timer(rospy.Duration(0.2),self.publish_periodic_update)
+
+    def unsubscribe(self,subscriber):
+        if subscriber != None:
+            subscriber.unsubscribe()
+
+    def unregister_publisher(self,publisher):
+        if publisher != None:
+            publisher.unregister()
+
+    def shutdown_timer(self,timer):
+        if timer != None:
+            timer.shutdown()
+
+    def publish_periodic_update(self,event):
+        stop_state = self.get_user_stop_state()
+        clutch_state = self.get_clutch_state()
+
+        if stop_state == SS.RUNNING:
+            self.user_started()
+        elif stop_state == SS.STOPPED:
+            self.user_stopped()
+        else:
+            # TODO Raise wrong value error
             raise NotImplementedError
 
+        if clutch_state == CS.ENGAGED:
+            self.engage_clutch()
+        elif clutch_state == CS.DISENGAGED:
+            self.disengage_clutch()
+        else:
+            # TODO Raise wrong value error
+            raise NotImplementedError  
+
+    def cancel_subscribers_and_publishers(self):
+        self.shutdown_timer(self.periodic_timer)
+        self.unsubscribe(self.robot_info_subscriber)
+        self.unsubscribe(self.master_stop_subscriber)
+        self.unsubscribe(self.restrictions_subscriber)
+        self.unregister_publisher(self.user_stop_publisher)
+        self.unregister_publisher(self.clutch_publisher)
+
+        self.periodic_timer = None
+        self.robot_info_subscriber = None
+        self.master_stop_subscriber = None
+        self.restrictions_subscriber = None
+        self.user_stop_publisher = None
+        self.clutch_publisher = None
+
+    def unregister_node(self):
+        self.cancel_subscribers_and_publishers()
+        rospy.signal_shutdown('Closing safety user plugin')
+
     def select_robot(self,robot_id):
-        raise NotImplementedError
-        return False
+        self.setup_subscribers_and_publishers(robot_id)
         
+    def setup_get_user_stop_state_function(self,function):
+        self.get_user_stop_state = function
+
+    def setup_get_clutch_state_function(self,function):
+        self.get_clutch_state = function
+
+
+    def scan_for_robots(self):
+        robots_list = []
+
+        published_topics_list = rospy.get_published_topics(namespace='/')
+        published_topics = []
+
+
+        for list_ in published_topics_list:
+            published_topics.append(list_[0])
+
+
+        for topic in published_topics:
+            if topic.find('RosAria') ==-1 or topic.find('robot_state') == -1:
+                pass
+            else:
+                robot_number = topic.split('/')
+                robot_number = robot_number[1]
+                robot_number = robot_number[7:]
+                if len(robot_number) > 0:
+                    robot_number = int(robot_number)
+                    robots_list.append(robot_number)
+
+        return robots_list
+
+    # ROSWrapper Callbacks
+    def get_robots_list(self):
+        robots_id_list = []
+
+        published_topics_list = rospy.get_published_topics(namespace='/')
+        published_topics = []
+
+        for list_ in published_topics_list:
+            published_topics.append(list_[0])
+
+        for topic in published_topics:
+            if topic_find('RosAria') and topic_find('robot_state'):
+                robot_number = topic.split('/')
+                robot_number = robot_number[1]
+                robot_number = robot_number[7:]
+                if len(robot_number) > 0:
+                    robot_number = int(robot_number)
+                    robots_id_list.append(robot_number)
+
+        self.robots_list_update_callback(robots_id_list)
+
+    def handle_selected_robot_info_update(self,msg):
+        _robot_info = RobotInfo(msg)
+        self.selected_robot_info_update_callback(_robot_info)
+
+    def handle_master_stop_update(self,msg):
+        master_stop_state = SS.UNKNOWN
+        if msg.data == True:
+            master_stop_state = SS.RUNNING
+        else:
+            master_stop_state = SS.STOPPED
+
+        self.master_stop_update_callback(master_stop_state)
+
+    def handle_restrictions_update(self,msg):
+        restrictions = Restrictions(msg)
+        restrictions_update_callback(restrictions)
+
+    # UserPlugin Callbacks
     def set_robots_list_update_callback(self,callback_function):
         self.robots_list_update_callback = callback_function
 
-    def unregister_node(self):
-        raise NotImplementedError
+    def set_selected_robot_info_update_callback(self,callback_function):
+        self.selected_robot_info_update_callback = callback_function
+
+    def set_master_stop_update_callback(self,callback_function):
+        self.master_stop_update_callback = callback_function
+
+    def set_restrictions_update_callback(self,callback_function):
+        self.restrictions_update_callback = callback_function
+
 
     def engage_clutch(self):
-        raise NotImplementedError
+        msg = Bool()
+        msg.data = True
+        self.clutch_publisher.Publish(msg)
 
     def disengage_clutch(self):
-        raise NotImplementedError
-
-    def user_stopped():
-        raise NotImplementedError
+        msg = Bool()
+        msg.data = False
+        self.clutch_publisher.Publish(msg)
 
     def user_started():
-        raise NotImplementedError
+        msg = Bool()
+        msg.data = True
+        self.user_stop_publisher.Publish(msg)
+
+    def user_stopped():
+        msg = Bool()
+        msg.data = False
+        self.user_stop_publisher.Publish(msg)
+

safety_user_plugin/scripts/user_info.py

@@ -19,3 +19,12 @@ class UserInfo:
 
     def update_robots_id_list(self,robots_id_list):
         self.robots_id_list = robots_id_list
+
+    def update_selected_robot_info(self,new_robot_info):
+        raise NotImplementedError
+
+    def get_user_stop_state(self):
+        return self.user_stop_state
+
+    def get_clutch_state(self):
+        return self.clutch_state

safety_user_plugin/scripts/user_plugin.py

@@ -1,8 +1,6 @@
 #!/usr/bin/env python
 
 import os
-import rospy
-import rospkg
 
 from qt_gui.plugin import Plugin
 
@@ -29,19 +27,31 @@ class UserPlugin(Plugin):
         self._user_info.master_stop_state = SS.UNKNOWN
         self._user_info.clutch_state = CS.UNKNOWN
 
-        # self.register_callback_functions()
+        # Setup functions to get _user_info states from _ros_wrapper
+        self._ros_wrapper.setup_get_user_stop_state_function(self._user_info.get_user_stop_state)
+        self._ros_wrapper.setup_get_clutch_state_function(self._user_info.get_clutch_state)
+
+        # self.set_callback_functions()
 
         # At the end!
         # self._qt_wrapper.connect_signals()
         # self._ros_wrapper.setup_node()
 
     def shutdown_plugin(self):
-        # TODO unregister all publishers here
         self._ros_wrapper.unregister_node()
 
-    def register_callback_functions(self):
+    def set_callback_functions(self):
+        self.set_Qt_callback_functions()
+        self.set_ROS_callback_functions()
+        
+    def set_ROS_callback_functions(self):
+        self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
+        self._ros_wrapper.set_selected_robot_info_update_callback(self.handle_selected_robot_info_update)
+        self._ros_wrapper.set_master_stop_update_callback(self.handle_master_stop_update)
+        self._ros_wrapper.set_restrictions_update_callback(self.handle_restrictions_update)
+
+    def set_Qt_callback_functions(self):
         raise NotImplementedError
-        # self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
 
 
     # ROS callback functions
@@ -50,7 +60,8 @@ class UserPlugin(Plugin):
         self._qt_wrapper.update_robots_list_gui(robots_id_list)
 
     def handle_selected_robot_info_update(self,robot_info):
-        self._qt_wrapper.update_selected_robot_info(self,robot_info)
+        self._user_info.update_selected_robot_info(robot_info)
+        self._qt_wrapper.update_selected_robot_info(robot_info)
 
     def handle_master_stop_update(self,master_stop_state):
         self._user_info.master_stop_state = master_stop_state
@@ -126,9 +137,6 @@ class UserPlugin(Plugin):
         self._ros_wrapper.disengage_clutch()
 
     def select_robot(self):
-        if self._ros_wrapper.select_robot(robot_id) == True:
+        self._ros_wrapper.select_robot(robot_id)
         self._qt_wrapper.select_robot(robot_id)
         self._user_info.select_robot(robot_id)
-        else:
-            # TODO - raise ROS subscriptions problem exception
-            raise NotImplementedError