diff --git a/COM/COM_Robot.py b/COM/COM_Robot.py index 07e692a..1a5f2de 100644 --- a/COM/COM_Robot.py +++ b/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 - q.qsize() - while True: # 让出时间间隔 - try: - if self.command_quene.qsize()!=0: - command = self.command_quene.get() - self.client_socket.send(json.dumps(command).encode('utf-8')) + try: + if self.command_quene.qsize()!=0: + command = self.command_quene.get() + self.client_socket.send(command.encode('utf-8')) + + 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('出错') + 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: - self.client_socket.send(json.dumps(request_status_json).encode('utf-8')) - if False: - response = self.client_socket.recv(1024).decode('utf-8') - response_message = json.loads(response) - if True: - data_status = DATARequest() - data_status.__dict__ = response_message - data_address_array = data_status.queryAddr - data_Address = DataAddress() - for i in attributes.count(): - setattr(data_Address,attributes[i],data_address_array[i]) - self.status_model = data_Address - else: - print('转换失败') - except Exception as e: - print('连接断开') + + try: + self.client_socket.send(request_status_json.encode('utf-8')) + if True: + response = self.client_socket.recv(1024).decode('utf-8') + response_message = json.loads(response) + if True: + data_status = DATAReply() + data_status.__dict__ = response_message + data_address_array = data_status.queryAddr + data_Address = DataAddress() + data_Address.setPosition(*data_address_array) + self.status_model.setPosition(*data_address_array) + return True + except Exception as e: + return False + - return False diff --git a/COM/COM_TCP.py b/COM/COM_TCP.py index 3eea299..92614d6 100644 --- a/COM/COM_TCP.py +++ b/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 diff --git a/CU/Feeding.py b/CU/Feeding.py index f99b597..59d76d2 100644 --- a/CU/Feeding.py +++ b/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 - pass # 发送拍照获取坐标 并 开始移动 + _, _, xyz, uvw = self.detection.get_position() #检测结果 + 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): diff --git a/Constant.py b/Constant.py new file mode 100644 index 0000000..abf8950 --- /dev/null +++ b/Constant.py @@ -0,0 +1 @@ +position_accuracy=0.02 \ No newline at end of file diff --git a/Model/Position.py b/Model/Position.py index 41a379e..3ca27c1 100644 --- a/Model/Position.py +++ b/Model/Position.py @@ -1,4 +1,4 @@ - +from Constant import position_accuracy class Position: def __init__(self): self.X = 0.0 diff --git a/Model/RobotModel.py b/Model/RobotModel.py index 9f92f2e..0960db0 100644 --- a/Model/RobotModel.py +++ b/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() - return + self.queryData = [] + 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 diff --git a/Seting.ini b/Seting.ini index dff6486..4a6126b 100644 --- a/Seting.ini +++ b/Seting.ini @@ -2,8 +2,8 @@ [Robot_Feed] -IPAddress=192.168.10.4 -Port=502 +IPAddress=192.168.3.5 +Port=8116 j1_min=-150 j1_max=+150 j2_min=-150 diff --git a/Trace/handeye_calibration.py b/Trace/handeye_calibration.py index 2253259..6d50e85 100644 --- a/Trace/handeye_calibration.py +++ b/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和法向量 -def getPosition(x,y,z,a,b,c): +# 图像识别结果：xyz和法向量 +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) \ No newline at end of file diff --git a/Vision/camera_coordinate_dete.py b/Vision/camera_coordinate_dete.py index 63ef1c1..7c0c0f7 100644 --- a/Vision/camera_coordinate_dete.py +++ b/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() diff --git a/app.py b/app.py index 3bda1fe..e2a36a4 100644 --- a/app.py +++ b/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)) - request_command = vars(instruction_command) + instruction_command.instructions.append(position_instruction) + 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: