Added basic handling of obstacle detection in user plugin

rosaria_msgs/msg/RobotInfoMsg.msg

@@ -2,4 +2,5 @@ std_msgs/UInt8 robot_id
 std_msgs/Float64 battery_voltage
 geometry_msgs/Twist twist
 std_msgs/Bool state
-std_msgs/Bool clutch
+std_msgs/Bool clutch
+std_msgs/Bool obstacle_detected

safety_user_plugin/scripts/qt_wrapper.py

@@ -221,6 +221,9 @@ class QtWrapper:
     def master_is_stopped_notify(self):
         self.log_error('Nie mozna wystartowac robota. MasterSTOP wcisniety!')
 
+    def obstacle_is_detected_notify(self):
+        self.log_error('Nie mozna wystartowac. Przeszkoda w polu dzialania robota')
+
     def update_restrictions_gui(self,restrictions):
         self.widget.distanceRestrictionLabel.setText("{:.2f}".format(restrictions.distance))
         self.widget.linearRestrictionLabel.setText("{:.2f}".format(restrictions.linear_velocity))
@@ -267,6 +270,13 @@ class QtWrapper:
     def connection_to_master_lost(self):
         self.log_error('Utrata polaczenia z masterstopem. Popros prowadzacego o jego uruchomienie')
 
+    def obstacle_detected(self):
+        self.log_error('Przeszkoda wykryta. Zatrzymuje robota')
+        # TODO Zmiana
+
+    def obstacle_removed(self):
+        self.log_info('Przeszkoda usunieta')
+
     def scroll_to_bottom(self):
         scrollbar = self.widget.logsBrowser.verticalScrollBar()
         scrollbar.setValue(scrollbar.maximum())

safety_user_plugin/scripts/robot_info.py

@@ -12,12 +12,14 @@ class RobotInfo:
         self.angular_velocity = None
         self.state = None
         self.clutch = None
+        self.obstacle_detected = None
 
     def update_robot_info(self,robot_info_msg):
         self.robot_id = robot_info_msg.robot_id.data
         self.battery_voltage = robot_info_msg.battery_voltage.data
         self.linear_velocity = [robot_info_msg.twist.linear.x,robot_info_msg.twist.linear.y,robot_info_msg.twist.linear.z]
         self.angular_velocity = [robot_info_msg.twist.angular.x,robot_info_msg.twist.angular.y,robot_info_msg.twist.angular.z]
+        self.obstacle_detected = robot_info_msg.obstacle_detected.data
 
         if robot_info_msg.state.data == True:
             self.state = SS.RUNNING

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.selected_robot.obstacle_detected = new_robot_info.obstacle_detected
         self.clutch_state = new_robot_info.clutch
 
     def get_user_stop_state(self):

safety_user_plugin/scripts/user_plugin.py

@@ -129,17 +129,20 @@ class UserPlugin(Plugin):
             raise RuntimeError('Cannot release. No robot selected')
 
     def handle_user_stop_state_updated(self):
-        if self._user_info.master_stop_state == SS.STOPPED:
+        if self._user_info.selected_robot.obstacle_detected == True:
-            self._qt_wrapper.master_is_stopped_notify()
+            self._qt_wrapper.obstacle_is_detected_notify()
-        elif self._user_info.master_stop_state == SS.RUNNING:
-            if self._user_info.user_stop_state == SS.RUNNING:
-                self.user_stopped()
-            elif self._user_info.user_stop_state == SS.STOPPED:
-                self.user_started()
-            else:
-                raise ValueError('user_stop_state value undefined')            
         else:
-            raise ValueError('master_stop_state value undefined') 
+            if self._user_info.master_stop_state == SS.STOPPED:
+                self._qt_wrapper.master_is_stopped_notify()
+            elif self._user_info.master_stop_state == SS.RUNNING:
+                if self._user_info.user_stop_state == SS.RUNNING:
+                    self.user_stopped()
+                elif self._user_info.user_stop_state == SS.STOPPED:
+                    self.user_started()
+                else:
+                    raise ValueError('user_stop_state value undefined')            
+            else:
+                raise ValueError('master_stop_state value undefined') 
 
     def handle_clutch_switched(self):
         if self._user_info.clutch_state == CS.ENGAGED:
@@ -227,7 +230,20 @@ class UserPlugin(Plugin):
             self.master_stopped()
             self.call_qt_wrapper_method('connection_to_master_lost')
 
+    def obstacle_detected(self):
+        self.call_qt_wrapper_method('obstacle_detected')
+        self.user_stopped()
+
+    def obstacle_removed(self):
+        self.call_qt_wrapper_method('obstacle_removed')
+
     def update_selected_robot_info(self,robot_info):
+        if self._user_info.select_robot != None:
+            if robot_info.obstacle_detected == True and self._user_info.selected_robot.obstacle_detected == False:
+                self.obstacle_detected()
+            elif robot_info.obstacle_detected == False and self._user_info.selected_robot.obstacle_detected == True:
+                self.obstacle_removed()
+
         self._user_info.update_selected_robot_info(robot_info)
         self.call_qt_wrapper_method_with_argument('update_selected_robot_info',[robot_info])
         # self._qt_wrapper.update_selected_robot_info(robot_info)