update 初次融合+调试

COM/COM_Robot.py

@@ -1,11 +1,11 @@
 from COM.COM_TCP import TCPClient
 import queue
 import json
-from Model.RobotModel import DataAddress,DATARequest
+from Model.RobotModel import DataAddress,DATARequest,DATAReply
 
 class RobotClient(TCPClient):
 
-    def __init__(self, ip, port, command_quene, status_model):
+    def __init__(self, ip, port, command_quene, status_model:DataAddress):
         super().__init__(ip, port)
         self.command_quene = command_quene
         self.status_model = status_model
@@ -16,26 +16,27 @@ class RobotClient(TCPClient):
         return
 
     def send_Command(self):
-        q = queue.Queue
+        try:
-        q.qsize()
+            if self.command_quene.qsize()!=0:
-        while True: # 让出时间间隔
+                command = self.command_quene.get()
-            try:
+                self.client_socket.send(command.encode('utf-8'))
-                if self.command_quene.qsize()!=0:
+
-                    command = self.command_quene.get()
+                if False:
-                    self.client_socket.send(json.dumps(command).encode('utf-8'))
+                    response = self.client_socket.recv(1024).decode('utf-8')
+                    response_message = json.loads(response)
+                    if True:
+                        print('正确相应')
+                    else:
+                        self.errorCommands[json.dumps(command).encode('utf-8')] = response_message
+                        print('出错')
+                return True
+            else:
+                return True
+        except Exception as e:
+            print('连接断开')
+            return False
 
-                    if False:
-                        response = self.client_socket.recv(1024).decode('utf-8')
-                        response_message = json.loads(response)
-                        if True:
-                            print('正确相应')
-                        else:
-                            self.errorCommands[json.dumps(command).encode('utf-8')] = response_message
-                            print('出错')
-            except Exception as e:
-                print('连接断开')
 
-        #return False
 
     def send_Status(self):
         request =  DATARequest()
@@ -45,25 +46,23 @@ class RobotClient(TCPClient):
         # 移除特殊属性和方法
         request_status_json = request.toString()
         # 转字符串
-        while True:
+
-            try:
+        try:
-                self.client_socket.send(json.dumps(request_status_json).encode('utf-8'))
+            self.client_socket.send(request_status_json.encode('utf-8'))
-                if False:
+            if True:
-                    response = self.client_socket.recv(1024).decode('utf-8')
+                response = self.client_socket.recv(1024).decode('utf-8')
-                    response_message = json.loads(response)
+                response_message = json.loads(response)
-                    if True:
+                if True:
-                        data_status = DATARequest()
+                    data_status = DATAReply()
-                        data_status.__dict__ = response_message
+                    data_status.__dict__ = response_message
-                        data_address_array = data_status.queryAddr
+                    data_address_array = data_status.queryAddr
-                        data_Address = DataAddress()
+                    data_Address = DataAddress()
-                        for i in attributes.count():
+                    data_Address.setPosition(*data_address_array)
-                            setattr(data_Address,attributes[i],data_address_array[i])
+                    self.status_model.setPosition(*data_address_array)
-                        self.status_model = data_Address
+            return True
-                    else:
+        except Exception as e:
-                        print('转换失败')
+            return False
-            except Exception as e:
+
-                print('连接断开')
 
 
 
-        return False

COM/COM_TCP.py

@@ -30,10 +30,9 @@ class TCPClient:
             time.sleep(0.2)
             self.connected = self.error_count > 0
             try:
-                #time.sleep(30)
                 if (self.send_Status() and self.send_Command()):
                     self.error_count = 0
-            except:
+            except Exception as e:
                 self.error_count += 1
 
 

CU/Feeding.py

@@ -1,7 +1,8 @@
-from Model.Position import Real_Position
+from Model.Position import Real_Position, Detection_Position
 from enum import Enum
 from COM.COM_Robot import RobotClient
 from Model.RobotModel import CMDInstructRequest
+from Vision.camera_coordinate_dete import Detection
 
 
 class FeedStatus(Enum):
@@ -33,7 +34,7 @@ class FeedingConfig:
 
 
 class Feeding():
-    def __init__(self,robotClient:RobotClient):
+    def __init__(self,robotClient:RobotClient,detection: Detection):
         self.feedConfig = None
         self.feedStatus = FeedStatus.FNone
         self.robotClient = robotClient
@@ -75,8 +76,24 @@ class Feeding():
 
         elif self.feedStatus == FeedStatus.FPhoto:
             if self.feedConfig.photo_position.compare(real_position):
-                self.feedStatus = FeedStatus.FTake
+                _, _, xyz, uvw = self.detection.get_position() #检测结果
-                pass # 发送拍照获取坐标 并 开始移动
+                if xyz!=None:
+                    self.feedStatus = FeedStatus.FTake
+                    #dp = Detection_Position().init_position(*xyz, *uvw)
+                    from Trace.handeye_calibration import R_matrix,getPosition
+                    rotation = R_matrix(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)
+
+                    # 黄老师给我的xyz和法向量
+                    target_position, noraml_base = getPosition(*xyz, *uvw,rotation)
+                    self.feedConfig.take_position = Real_Position().init_position(*target_position,*noraml_base)
+                else:
+                    print("打印日志，保存失败图像")
+                    pass # 发送拍照获取坐标 并 开始移动
 
         elif self.feedStatus == FeedStatus.FTake:
             if self.feedConfig.take_position != None and self.feedConfig.take_position.compare(real_position):

Constant.py

@@ -0,0 +1 @@
+position_accuracy=0.02

Model/Position.py

@@ -1,4 +1,4 @@
-
+from Constant import position_accuracy
 class Position:
     def __init__(self):
         self.X = 0.0

Model/RobotModel.py

@@ -5,7 +5,7 @@ class DATARequest:
         self.queryAddr = []
     def toString(self):
         model_str = '{'+f'"dsID":"{self.dsID}","reqType":"{self.reqType}","queryAddr":[' \
-                    f'"{self.queryAddr[0].toString()}"]'+'}'
+                    f'{self.queryAddr[0].toString()}]'+'}'
         return model_str
 
 
@@ -22,7 +22,12 @@ class DataAddress:
         self.output_n = ''
         self.axisNum = '6'
         self.axis_n = ''
-        self.world_n = ''
+        self.world_0 = 0
+        self.world_1 = 0
+        self.world_2 = 0
+        self.world_3 = 0
+        self.world_4 = 0
+        self.world_5 = 0
         self.curAlarm = ''
         self.curCycle = ''
         self.lastCycle = ''
@@ -35,9 +40,17 @@ class DataAddress:
         self.isMoving = ''
         self.M_n = ''
         #return
+    def setPosition(self,w0,w1,w2,w3,w4,w5):
+        self.world_0 = int(w0)
+        self.world_1 = int(w1)
+        self.world_2 = int(w2)
+        self.world_3 = int(w3)
+        self.world_4 = int(w4)
+        self.world_5 = int(w5)
+
 
     def toString(self):
-        model_str = f'"world_n"'
+        model_str = f'"world_0","world_1","world_2","world_3","world_4","world_5"'
         return model_str
 
 
@@ -48,8 +61,8 @@ class DATAReply:
     def __init__(self):
         self.dsID = ''
         self.reqType = ''
-        self.queryData = DataAddress()
+        self.queryData = []
-        return
+
 
 
     def JsonToObject(self):
@@ -72,7 +85,7 @@ class CMDReply:
 class Instruction:
     def __init__(self):
         self.oneshot = 1
-        self.action = 4 #4 自由路径 10 姿势直线 17 姿势曲线
+        self.action = 10 #4 自由路径 10 姿势直线 17 姿势曲线
         self.m0 = 0.0
         self.m1 = 0.0
         self.m2 = 0.0
@@ -80,13 +93,14 @@ class Instruction:
         self.m4 = 0.0
         self.m5 = 0.0
         self.ckStatus = '0x3F'
-        self.speed =10
+        self.speed = 10
         self.delay = 1.0
         self.smooth = 0
 
     def toString(self):
         model_str = f'"oneshot":"{self.oneshot}","action":"{self.action}","m0":"{self.m0}","m1":"{self.m1}","m2":"{self.m2}",' \
-                    f'"m3":"{self.m3}","m4":"{self.m4}","m5":"{self.m5}"'
+                    f'"m3":"{self.m3}","m4":"{self.m4}","m5":"{self.m5}","ckStatus":"{self.ckStatus}","speed":"{self.speed}",' \
+                    f'"delay":"{self.delay}","smooth":"{self.smooth}"'
         return model_str
 
 

Seting.ini

@@ -2,8 +2,8 @@
 
 
 [Robot_Feed]
-IPAddress=192.168.10.4
+IPAddress=192.168.3.5
-Port=502
+Port=8116
 j1_min=-150
 j1_max=+150
 j2_min=-150

Trace/handeye_calibration.py

@@ -58,12 +58,11 @@ def R_matrix(x,y,z,u,v,w):
 
     return transformation_matrix
 
-rotation = R_matrix(559.365,0.704,731.196,179.996,66.369,179.996)#张啸给我的值填这里
 
-# 黄老师给我的xyz和法向量
+# 图像识别结果：xyz和法向量
-def getPosition(x,y,z,a,b,c):
+def getPosition(x,y,z,a,b,c,rotation):
     target = np.asarray([x, y, z,1])
-    camera2robot = np.loadtxt('D:\BaiduNetdiskDownload\机械臂\GRCNN\\real\cam_pose.txt', delimiter=' ')
+    camera2robot = np.loadtxt('D:\BaiduNetdiskDownload\机械臂\GRCNN\\real\cam_pose.txt', delimiter=' ') #相对目录且分隔符采用os.sep
     robot2base = rotation
     camera2base = robot2base @ camera2robot
     target_position = np.dot(camera2base, target)
@@ -75,6 +74,3 @@ def getPosition(x,y,z,a,b,c):
     print(target_position, noraml_base)
 
     return target_position,noraml_base
-
-if __name__ =="__main__":
-    getPosition(-96.09919,56.339145,444.640084,-0.09619,-0.45399, 0.88579)

Vision/camera_coordinate_dete.py

@@ -15,7 +15,7 @@ from .tool.CameraRVC import camera
 from .yolo.yolov8_pt_seg import yolov8_segment
 
 
-class Detection_Position:
+class Detection:
 
     def __init__(self, model_path, device):
         self.camera_rvc = camera()

app.py

@@ -9,6 +9,7 @@ from PySide6.QtWidgets import QApplication, QMainWindow, QPushButton
 from datetime import datetime
 from CU.Command import FeedCommand
 from CU.Feeding import FeedLine, FeedingConfig, Feeding, FeedStatus
+from Vision.camera_coordinate_dete import Detection
 from ui_untitled import  Ui_MainWindow
 from COM.COM_Robot import RobotClient
 from Expection import  Error_Code
@@ -16,7 +17,7 @@ from queue import Queue
 from Model.RobotModel import *
 import time
 from queue import Queue
-from Model.Position import Real_Position
+from Model.Position import Real_Position, Detection_Position
 from threading import Thread
 from CU.Command import Status
 
@@ -64,7 +65,7 @@ class MainWindow(QMainWindow,Ui_MainWindow):
     def init_Run(self):
         self.robotClient = None
         self.configReader = configparser.ConfigParser()
-
+        self.detection = Detection()
         self.command_position_quene = Queue()
         self.status_address = DataAddress()
         self.feedLine_dict = {}
@@ -75,7 +76,7 @@ class MainWindow(QMainWindow,Ui_MainWindow):
         ip = self.configReader.get('Robot_Feed', 'IPAddress')
         port = int(self.configReader.get('Robot_Feed', 'Port'))
         self.robotClient = RobotClient(ip, port, self.command_position_quene, self.status_address)
-        self.feeding = Feeding(self.robotClient)  # 临时
+        self.feeding = Feeding(self.robotClient,self.detection)  # 临时
         self.last_time=time.time()
         try:
             self.robotClient.CreatConnect()
@@ -257,8 +258,8 @@ class MainWindow(QMainWindow,Ui_MainWindow):
         self.horizontalSlider_J6.setValue(int(self.lineEdit_j6.text()))
 
         instruction_command = CMDInstructRequest()
-        instruction_command.instructions.append(vars(position_instruction))
+        instruction_command.instructions.append(position_instruction)
-        request_command = vars(instruction_command)
+        request_command = instruction_command.toString()
         print(request_command)
         self.robotClient.add_sendQuene(request_command)
 
@@ -301,6 +302,18 @@ 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()
+
+    def updateUI_Position(self):
+        self.horizontalSlider_J1.setValue(self.status_address.world_0)
+        self.horizontalSlider_J2.setValue(self.status_address.world_1)
+        self.horizontalSlider_J3.setValue(self.status_address.world_2)
+        self.horizontalSlider_J4.setValue(self.status_address.world_3)
+        self.horizontalSlider_J5.setValue(self.status_address.world_4)
+        self.horizontalSlider_J6.setValue(self.status_address.world_5)
+
+
+
 
     def set_label_status_style(self, connected: bool):
         if connected: