diff --git a/rosaria_msgs/msg/RobotInfoMsg.msg b/rosaria_msgs/msg/RobotInfoMsg.msg index 00b7665..f3ded53 100644 --- a/rosaria_msgs/msg/RobotInfoMsg.msg +++ b/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 \ No newline at end of file +std_msgs/Bool clutch +std_msgs/Bool obstacle_detected \ No newline at end of file diff --git a/safety_user_plugin/scripts/qt_wrapper.py b/safety_user_plugin/scripts/qt_wrapper.py index ba0c5f8..bab0f79 100644 --- a/safety_user_plugin/scripts/qt_wrapper.py +++ b/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()) diff --git a/safety_user_plugin/scripts/robot_info.py b/safety_user_plugin/scripts/robot_info.py index 4f50b36..6669655 100644 --- a/safety_user_plugin/scripts/robot_info.py +++ b/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 diff --git a/safety_user_plugin/scripts/user_info.py b/safety_user_plugin/scripts/user_info.py index 6fcc49d..68cd98d 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.selected_robot.obstacle_detected = new_robot_info.obstacle_detected self.clutch_state = new_robot_info.clutch def get_user_stop_state(self): diff --git a/safety_user_plugin/scripts/user_plugin.py b/safety_user_plugin/scripts/user_plugin.py index c982be3..f6c806b 100755 --- a/safety_user_plugin/scripts/user_plugin.py +++ b/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: - 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') + if self._user_info.selected_robot.obstacle_detected == True: + self._qt_wrapper.obstacle_is_detected_notify() 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) \ No newline at end of file