update 修改复位，清除报警，识别等信息

CU/Feeding.py

@@ -4,20 +4,27 @@ import time
 import cv2
 from PyQt5.QtWidgets import QMessageBox
 from jedi.debug import speed
+from tifffile.tifffile import read_lsm_positions
 
 import Constant
 import Expection
+from Model.FeedModel import PositionModel
 from Model.Position import Real_Position, Detection_Position
 from enum import Enum, IntEnum
 from COM.COM_Robot import RobotClient, DetectType
 from Model.RobotModel import CMDInstructRequest, MoveType
+from Trace.handeye_calibration import getPosition
 from Util.util_math import get_distance
 from Util.util_time import CRisOrFall
 from Vision.camera_coordinate_dete import Detection
 from Util.util_log import log
 from Model.RobotModel import Instruction
 
-
+class ResetStatus(Enum):
+    RNone = 0
+    RStart = 1
+    RRunging = 2
+    ROk =3
 
 
 class FeedStatus(IntEnum):
@@ -163,8 +170,15 @@ class Feeding:
         self.cRis_photo = CRisOrFall()
         self.feed_Mid_Status = FeedMidStatus.FMid_Start
         self.is_reverse = False
+        # 复位集合
         self.run_reverse = False
         self.take_no_photo = False
+        self.reset_status = ResetStatus.RNone
+        self.reversed_positions = []
+        self.current_position = None
+
+        self.pos_index = -1
+        self.pos_near_index = -1
         pass
 
     def init_detection_image(self):
@@ -220,7 +234,7 @@ class Feeding:
         elif self.feedStatus == FeedStatus.FStart:
             log.log_message(logging.INFO, Constant.str_feed_start)
             if not self.robotClient.origin_position.compare(real_position) and not self.is_reverse:
-                QMessageBox.information(None, "提示", Constant.str_feed_start_error)
+                # QMessageBox.information(None, "提示", Constant.str_feed_start_error) # Fuck 引起异常
                 self.feedStatus = FeedStatus.FNone
                 return
             if self.is_reverse:
@@ -230,10 +244,13 @@ class Feeding:
 
             self.feedConfig.feedLine.get_position_list()
 
-            self.feedConfig.feedLine.set_take_position(real_position)
+             # = self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30,
+             #                            #                            save_img_point=0, Height_reduce=30, width_reduce=30)
+
+
+            self.feedConfig.feedLine.set_take_position(self.get_take_position())
             # TODO 获取目标位置
-            # self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30,
+            #
-            #                            save_img_point=0, Height_reduce=30, width_reduce=30)
             self.feed_Mid_Status = FeedMidStatus.FMid_Start
             self.next_position(self.is_reverse)
 
@@ -382,12 +399,87 @@ class Feeding:
                 time.sleep(self.robotClient.time_delay_put)
                 # TODO 获取目标位置
                 # self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30, save_img_point=0, Height_reduce=30, width_reduce=30)
-                self.feedConfig.feedLine.set_take_position(real_position)
+                self.feedConfig.feedLine.set_take_position(self.get_take_position())
                 self.feedConfig.num = self.feedConfig.num - 1
                 log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
                 self.next_position()
 
+    def run_reseet(self):
+        real_position = Real_Position()
+        real_position.init_position(self.robotClient.status_model.world_0,
+                                    self.robotClient.status_model.world_1,
+                                    self.robotClient.status_model.world_2,
+                                    self.robotClient.status_model.world_3,
+                                    self.robotClient.status_model.world_4,
+                                    self.robotClient.status_model.world_5)
+        if self.reset_status == ResetStatus.RNone:
+            return
+
+        if self.reset_status == ResetStatus.RStart:
+
+            if self.feedConfig == None: return
+            # for index in range(len(self.feedConfig.feedLine.positions)):
+            #     if self.feedConfig.feedLine.positions[index].status == 2:
+            #         start_index = index
+            self.pos_index = -1
+            self.pos_near_index = -1
+            self.reversed_positions = []
+            for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
+                if pos_model.get_position().compare(real_position):
+                    self.pos_index = index
+                    break
+
+            if self.pos_index == -1:
+                log.log_message(logging.ERROR, Constant.str_feed_return_original_position_fail)
+                min_distance = 99999999
+                for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
+                    if get_distance(pos_model.get_position(), real_position) < min_distance:
+                        min_distance = get_distance(pos_model.get_position(), real_position)
+                        self.pos_near_index = index
+                if self.pos_near_index != -1:
+                    self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_near_index + 1]
+                else:
+                    return False
+            else:
+                self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_index]
+            self.reversed_positions = list(reversed(self.reversed_positions))
+            self.reverse_index = 0
+            self.send_emergency_sound()
+            self.current_position = PositionModel()
+            self.current_position.init_position(real_position)
+            self.reset_status = ResetStatus.RRunging
+
+        if self.reset_status == ResetStatus.RRunging:
+            if not real_position.compare(self.current_position.get_position()):
+                return
+            pos_model = self.reversed_positions[self.reverse_index]
+            if pos_model.status == FeedStatus.FTake.value:  # 跳过取袋节点
+                print('跳过抓袋点位')
+                pos_model = self.reversed_positions[self.reverse_index + 1]
+                self.reverse_index = self.reverse_index+1
+
+            if pos_model.lineType == LineType.CureMid.value:
+                pos_model1 = self.reversed_positions[self.reverse_index + 1]
+                self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure,
+                                      real_position1=pos_model1.get_position(), speed=self.robotClient.reset_speed)
+                self.current_position = pos_model1
+                self.reverse_index = self.reverse_index + 2
+            else:
+                self.sendTargPosition(real_position=pos_model.get_position(), speed=self.robotClient.reset_speed)
+                self.current_position = pos_model
+                self.reverse_index = self.reverse_index + 1
+
+            if self.reverse_index == len(self.reversed_positions):
+                self.reset_status = ResetStatus.ROk
+        if self.reset_status == ResetStatus.ROk:
+            self.reset_status = ResetStatus.RNone
+            self.send_emergency_stop()
+
+
+
+
     def return_original_position(self):
+
         self.run_reverse = True
         real_position = Real_Position()
         real_position.init_position(self.robotClient.status_model.world_0,
@@ -425,8 +517,10 @@ class Feeding:
         reversed_positions = list(reversed(reversed_positions))
         self.reverse_index = 0
         self.send_emergency_sound()
+        current_position = PositionModel()
+        current_position.init_position(real_position)
         while self.run_reverse and self.reverse_index!=len(reversed_positions):
-            if self.reverse_index!=0 and not real_position.compare(reversed_positions[self.reverse_index-1]):
+            if self.reverse_index!=0 and not real_position.compare(current_position.get_position()):
                 continue
             print(f'复位第{self.reverse_index}')  #todo 缺少比对
             pos_model = reversed_positions[self.reverse_index]
@@ -439,9 +533,11 @@ class Feeding:
                 pos_model1 = reversed_positions[self.reverse_index + 1]
                 self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure,
                                       real_position1=pos_model1.get_position(),speed= self.robotClient.reset_speed)
+                current_position = pos_model1
                 self.reverse_index = self.reverse_index+2
             else:
                 self.sendTargPosition(real_position=pos_model.get_position(),speed=self.robotClient.reset_speed)
+                current_position = pos_model
                 self.reverse_index = self.reverse_index + 1
         self.send_emergency_stop()
         return True
@@ -502,8 +598,12 @@ class Feeding:
 
         self.robotClient.add_sendQuene(request_command)
         pass
-
+    def get_take_position(self):
-
+        _, img, xyz, uvw, points = self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30,save_img_point=0, Height_reduce=30, width_reduce=30)
+        self.detection_image = img
+        target_position,noraml_base = getPosition(*xyz,*uvw,None,points)
+        position = Real_Position().init_position(*target_position[:3],*noraml_base[:3])
+        return position
     def next_start(self,reverse=False):
         print(f'原点到初始位置的第{self.feedConfig.feedLine.origin2start_pos_index}')
         start_pos = self.feedConfig.feedLine.get_next_start_position(reverse)

Seting.ini

@@ -1,7 +1,7 @@
 [Main]
 
 [Robot_Feed]
-ipaddress = 192.168.20.4
+ipaddress = 127.0.0.1
 port = 502
 j1_min = -150
 j1_max = +150
@@ -47,7 +47,7 @@ photo_v5 = 0.0
 photo_w5 = 1.0
 linecount = 2
 remain_linename = 1
-remain_count = 0
+remain_count = 1
 solenoid_valve1_addr = 3
 solenoid_valve2_addr = 2
 solenoid_valve3_addr = 10

main.py

@@ -23,7 +23,7 @@ from Model.FeedModel import LineModel, PositionModel
 from Util.util_ini import writeFeedLine_to_ini
 import Constant
 from CU.Command import FeedCommand
-from CU.Feeding import FeedLine, FeedingConfig, Feeding, FeedStatus, FeedMidStatus
+from CU.Feeding import FeedLine, FeedingConfig, Feeding, FeedStatus, FeedMidStatus, ResetStatus
 from Util.util_log import QTextEditLogger
 from Util.util_time import CRisOrFall
 from Vision.camera_coordinate_dete import Detection
@@ -39,6 +39,9 @@ from CU.Command import Status
 from Util.util_log import log
 from Vision.detect_person import DetectionPerson
 from ui_MainWin import Ui_MainWindow
+from view.ResetView import StopDialog
+
+
 #from view.ResetView import StopDialog
 
 
@@ -542,13 +545,13 @@ class MainWindow(QMainWindow, Ui_MainWindow):
     def init_Run(self):
         self.robotClient = None
         self.configReader = configparser.ConfigParser()
-        self.detection = None # Detection() #TODO 关闭图像
+        self.detection =Detection() #TODO 关闭图像
         self.command_position_quene = Queue()
         self.status_address = DataAddress()
         self.feedLine_dict = {}
         self.command_quene = Queue()
         self.main_threading = None
-        self.detection_person = None #DetectionPerson()
+        self.detection_person = None # DetectionPerson()
         self.cton_take_no_photo = CRisOrFall()
 
         self.configReader.read(Constant.set_ini)
@@ -1133,6 +1136,9 @@ class MainWindow(QMainWindow, Ui_MainWindow):
             self.send_start_tool_command()
 
     def send_reset_button_click(self):
+        # TODO 清楚痕迹
+        # TODO 开启自动
+
         line_head = self.comboBox_lineIndex.currentData()
         #safe_position = self.feedLine_dict[line_head].safe_position
         # self.send_position_command(safe_position.X, safe_position.Y, safe_position.Z, safe_position.U, safe_position.V, safe_position.W,move_type=MoveType.WORLD)
@@ -1149,16 +1155,18 @@ class MainWindow(QMainWindow, Ui_MainWindow):
             else:
                 log.log_message(logging.ERROR, Constant.str_feed_reset_no_line_error)
                 return
+            self.feeding.reset_status = ResetStatus.RStart
+            dialog_reset = StopDialog()
+            dialog_reset.stop_thread_signal.connect(self.stop_reset_thread)
         else:
             log.log_message(logging.ERROR, Constant.str_feed_reset_no_line_error)
             return
-        thread = threading.Thread(target=self.feeding.return_original_position())
+        # thread = threading.Thread(target=self.feeding.return_original_position())
-        thread.start()
+        # thread.start()
-        # dialog_reset = StopDialog()
+
-        # dialog_reset.stop_thread_signal.connect(self.stop_reset_thread)
 
     def stop_reset_thread(self):
-        self.feeding.run_reverse = False
+        self.feeding.reset_status = ResetStatus.ROk
         self.send_clear_auto_command()
         self.send_emergency_alarm_command()
 
@@ -1256,6 +1264,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
 
             try:
                 self.feeding.run()
+                self.feeding.run_reseet()
             except:
                 print(Error_Code.SYS_NONEPoint)
 
@@ -1277,7 +1286,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
                 #     return
                 log.log_message(logging.ERROR, '人员进入安全区')
 
-            time.sleep(3)
+            time.sleep(1)
         pass
 
     def updateUI(self):
@@ -1305,12 +1314,15 @@ class MainWindow(QMainWindow, Ui_MainWindow):
             self.label_date.setText(datetime.now().strftime("%Y-%m-%d"))
             self.label_time.setText(datetime.now().strftime("%H:%M:%S"))
 
-            self.updateUI_Position()
+            # self.updateUI_Position()
-            self.updateUI_label_detection()
+            # self.updateUI_label_detection()
-            self.updateUI_label_status()
+            # self.updateUI_label_status()
-            self.updateUI_frame_sign(self.feeding.feedStatus)
+            # self.updateUI_frame_sign(self.feeding.feedStatus)
-            self.updateUI_IOPanel()
+            # self.updateUI_IOPanel()
-            self.updateUI_InfoMB()
+            # self.updateUI_InfoMB()
+
+
+
     def updateUI_InfoMB(self):
         if self.cton_take_no_photo.Q(self.feeding.take_no_photo,True):
             self.show_infomessage_box("未识别到料袋报警，请重新放料后，点击继续")
@@ -1533,7 +1545,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
     def send_clear_alarm_command(self, pause: bool):
         self.send_start_tool_command()
         pause_command = CMDRequest()
-        pause_command.cmdData.append("clearAlarmContinue")
+        pause_command.cmdData.append("StopButton")
         pause_command.cmdData.append("1")
         request_command = pause_command.toString()
         print(request_command)