update 加入并行拍照，加入平滑

CU/Catch.py

@@ -34,8 +34,8 @@ class Catch:
             if Constant.Debug or self.robotClient.check_outputQ(self.robotClient.con_ios[1]) and not self.robotClient.check_outputQ(self.robotClient.con_ios[0]):
                 self.catch_status = CatchStatus.COk
         if self.catch_status == CatchStatus.CShake:
-            if not self.shake_continue.Q(True, 2000):
+            if not self.shake_continue.Q(True, 1500):
-                shakeQ = self.shakePulse.Q(True, 100, 100)
+                shakeQ = self.shakePulse.Q(True, 50, 50)
                 self.robotClient.sendIOControl(self.robotClient.con_ios[2], 1 if shakeQ else 0)
                 print("正在震动" if shakeQ else "震动结束")
             else:

CU/Detect.py

@@ -0,0 +1,40 @@
+from enum import Enum
+
+import Constant
+from Model.Position import Real_Position
+from Trace.handeye_calibration import getPosition
+from Util.util_time import CClockPulse, CTon
+
+
+class DetectStatus(Enum):
+    DNone = 0
+    DDetect = 1
+    DOk = 2
+
+class Detect:
+    def __init__(self,detection):
+        self.detection = detection
+        self.detect_status = DetectStatus.DNone
+        self.detect_position = None
+    def run(self):
+        if self.detect_status == DetectStatus.DNone:
+            return
+        if self.detect_status == DetectStatus.DDetect:
+
+            _, 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.copy()
+            if xyz == None or uvw == None or points == None:
+                self.detect_position = None
+                self.detect_status = DetectStatus.DOk
+                return
+            target_position, noraml_base = getPosition(*xyz, *uvw, None, points)
+
+            position = Real_Position().init_position(*target_position[:3], *noraml_base[:3])
+            position.Z = position.Z + 200
+            self.detect_position = position
+            self.detect_status = DetectStatus.DOk
+
+        if self.detect_status == DetectStatus.DOk:
+            return

CU/Feeding.py

@@ -1,5 +1,6 @@
 import copy
 import logging
+import threading
 import time
 
 import cv2
@@ -9,6 +10,7 @@ from PySide6.QtCore import Signal, QObject
 import Constant
 import Expection
 from CU.Catch import Catch, CatchStatus
+from CU.Detect import Detect, DetectStatus
 from Model.FeedModel import PositionModel
 from Model.Position import Real_Position, Detection_Position
 from enum import Enum, IntEnum
@@ -185,6 +187,9 @@ class Feeding(QObject):
         self.pos_index = -1
         self.pos_near_index = -1
         self.catch = Catch(self.robotClient)
+        self.detect = Detect(self.detection)
+        self.detect_thread_img = threading.Thread(target=self.detect.run)
+        self.detect_thread_img.start()
         pass
 
     def init_detection_image(self):
@@ -258,12 +263,11 @@ class Feeding(QObject):
                 return
 
             self.feedConfig.feedLine.get_position_list()
-
+            self.detect.detect_status = DetectStatus.DNone
              # = 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)
 
-            if self.feedConfig.feedLine.get_take_position().get_position() != None:
+
-                self.feedConfig.feedLine.set_take_position(self.get_take_position())
             # TODO 获取目标位置
             #
             self.feed_Mid_Status = FeedMidStatus.FMid_Start
@@ -291,7 +295,17 @@ class Feeding(QObject):
 
             if self.robotClient.type_detection == DetectType.EyeOutHand:
                 self.feed_Mid_Status = FeedMidStatus.FMid_Feed
-                self.next_position()
+                if self.detect.detect_status == DetectStatus.DNone:
+                    self.detect.detect_status = DetectStatus.DDetect
+                elif self.detect.detect_status == DetectStatus.DOk:
+                    if self.detect.detect_position != None:
+                        log.log_message(logging.INFO, Constant.str_feed_takePhoto_success)
+                        self.feedConfig.feedLine.set_take_position(self.detect.detect_position)
+                        self.next_position()
+                        self.detect.detect_status = DetectStatus.DNone
+                    else:
+                        log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail)
+                        self.take_no_photo_sigal.emit()
                 return
             detect_pos_list = []
             if not Constant.Debug:
@@ -386,10 +400,11 @@ class Feeding(QObject):
             else:
                 log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail)
                 # self.take_no_photo = True
-                self.take_no_photo_sigal.emit()
+
                 # 继续获取图像
                 # TODO
-                self.feedConfig.feedLine.set_take_position(self.get_take_position())
+                # self.feedConfig.feedLine.set_take_position(self.get_take_position())
+
 
 
 
@@ -437,7 +452,9 @@ class Feeding(QObject):
                     self.catch.catch_status = CatchStatus.CNone
                     time.sleep(self.robotClient.time_delay_put)
                     # 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())
+                    # self.feedConfig.feedLine.set_take_position(self.get_take_position())
+                    if self.detect.detect_status == DetectStatus.DNone:
+                        self.detect.detect_status = DetectStatus.DDetect
                     self.feedConfig.num = self.feedConfig.num - 1
                     log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
                     self.next_position()
@@ -607,6 +624,7 @@ class Feeding(QObject):
         position_instruction.m3 = real_position.U
         position_instruction.m4 = real_position.V
         position_instruction.m5 = real_position.W
+        position_instruction.smooth = 4
 
         position_instruction.action = move_type.value
         if position_instruction.action == 17:
@@ -636,18 +654,18 @@ class Feeding(QObject):
 
         self.robotClient.add_sendQuene(request_command)
         pass
-    def get_take_position(self):
+    # def get_take_position(self):
-        if Constant.Debug:
+    #     if Constant.Debug:
-            return self.robotClient.status_model.getRealPosition()
+    #         return self.robotClient.status_model.getRealPosition()
-        _, 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)
+    #     _, 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.copy()
+    #     self.detection_image = img.copy()
-        if xyz ==None or uvw==None or points==None:
+    #     if xyz ==None or uvw==None or points==None:
-            return None
+    #         return None
-        target_position,noraml_base = getPosition(*xyz,*uvw,None,points)
+    #     target_position,noraml_base = getPosition(*xyz,*uvw,None,points)
-
+    #
-        position = Real_Position().init_position(*target_position[:3],*noraml_base[:3])
+    #     position = Real_Position().init_position(*target_position[:3],*noraml_base[:3])
-        position.Z = position.Z+200
+    #     position.Z = position.Z+200
-        return position
+    #     return position
     def next_start(self,reverse=False):
         start_pos = self.feedConfig.feedLine.get_next_start_position(reverse)
         self.feedStatus = FeedStatus(start_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone

Constant.py

@@ -61,6 +61,7 @@ str_feed_start_error = '请先复位到原点'
 str_feed_return_original_position_fail = '机械臂未按照实际路线点移动，未能寻找到适配路径点位'
 str_feed_reset_no_line_error = '未定义的线段'
 str_feed_angle_error = '角度差距过大，停止运行'
+str_feed_reset_start = '开始复位'
 str_sys_start = '进入系统'
 str_sys_exit = '退出系统'
 str_sys_switch_tool = '切换到工具坐标'

main.py

@@ -1181,8 +1181,6 @@ class MainWindow(QMainWindow, Ui_MainWindow):
             self.send_start_tool_command()
 
     def send_reset_button_click(self):
-        # TODO 清楚痕迹
-        # TODO 开启自动
         # 触发自动运行
         if self.robotClient.status_model.curMode != 7:
             self.send_switch_tool_command()
@@ -1193,6 +1191,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
 
         line_head = self.comboBox_lineIndex.currentData()
         self.send_clear_auto_command()
+        log.log_message(logging.INFO, Constant.str_feed_reset_start)
         #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)
         if self.remain_lineName != '':