diff --git a/mycobot_ai/local_photo/img/goal1.jpeg b/mycobot_ai/local_photo/img/goal1.jpeg new file mode 100644 index 0000000..da80fc6 Binary files /dev/null and b/mycobot_ai/local_photo/img/goal1.jpeg differ diff --git a/mycobot_ai/local_photo/img/goal2.jpeg b/mycobot_ai/local_photo/img/goal2.jpeg new file mode 100644 index 0000000..2f07252 Binary files /dev/null and b/mycobot_ai/local_photo/img/goal2.jpeg differ diff --git a/mycobot_ai/local_photo/img/goal3.jpeg b/mycobot_ai/local_photo/img/goal3.jpeg new file mode 100644 index 0000000..fae4365 Binary files /dev/null and b/mycobot_ai/local_photo/img/goal3.jpeg differ diff --git a/mycobot_ai/local_photo/img/goal4.jpeg b/mycobot_ai/local_photo/img/goal4.jpeg new file mode 100644 index 0000000..787c61b Binary files /dev/null and b/mycobot_ai/local_photo/img/goal4.jpeg differ diff --git a/mycobot_ai/local_photo/img/goal5.jpeg b/mycobot_ai/local_photo/img/goal5.jpeg new file mode 100644 index 0000000..51ec096 Binary files /dev/null and b/mycobot_ai/local_photo/img/goal5.jpeg differ diff --git a/mycobot_ai/scripts/ai_windows.py b/mycobot_ai/scripts/ai_windows.py index 340ecec..04afca4 100644 --- a/mycobot_ai/scripts/ai_windows.py +++ b/mycobot_ai/scripts/ai_windows.py @@ -3,7 +3,8 @@ from tkinter import ttk from tkinter import * -import os, time +import os +import time import threading from multiprocessing import Process @@ -34,7 +35,7 @@ class Application(object): # 设置标题 self.win.title("aikit启动工具") self.win.geometry( - "500x300+100+100") # 290 160为窗口大小，+10 +10 定义窗口弹出时的默认展示位置 + "550x350+100+100") # 290 160为窗口大小，+10 +10 定义窗口弹出时的默认展示位置 # 打开ros按钮 self.btn = Button(self.win, text="open ROS", command=self.open_ros) self.btn.grid(row=0) @@ -65,7 +66,7 @@ class Application(object): # self.e2 = Entry(self.win,textvariable=self.v2, width=10) # self.e2.insert(0,0) # self.e2.grid(row=3,column=1) - self.tips = "1、首先打开ros\n2、选择所要运行的程序点击运行即可" + self.tips = "1、首先打开ros,大概需要等待15s\n2、选择所要运行的程序点击运行即可,开启大概需要10秒,可以通过查看终端查看开启情况。\n\n添加新的图像：\n1、点击按钮，等待开启摄像头\n2、选中图像框，按z键拍照\n3、使用鼠标框出需要识别的图像区域\n4、按Enter键提取图像\n5、再次按Enter键保存即可" self.btn = Button(self.win, text="运行", command=self.start_run) self.btn.grid(row=5) @@ -87,6 +88,7 @@ class Application(object): print(u"开始运行") one = self.myCombox.get() if one == u"颜色识别": + self.run_py = "detect_obj_color.py" t2 = threading.Thread(target=self.open_py1) t2.setDaemon(True) t2.start() diff --git a/mycobot_ai/scripts/detect_encode.py b/mycobot_ai/scripts/detect_encode.py index 18b1cd2..d469294 100644 --- a/mycobot_ai/scripts/detect_encode.py +++ b/mycobot_ai/scripts/detect_encode.py @@ -1,4 +1,4 @@ -#encoding: UTF-8 +# encoding: UTF-8 #!/usr/bin/env python2 import cv2 as cv import os @@ -13,19 +13,32 @@ pump_y = -55 # x轴偏移量 pump_x = 15 + class Detect_marker(Movement): def __init__(self): super(Detect_marker, self).__init__() # set cache of real coord self.cache_x = self.cache_y = 0 - + # which robot - self.robot = os.popen("ls /dev/ttyUSB*") - if "dev" in self.robot: - self.Pin = [2,5] - else: - self.Pin = [20,21] - + self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1] + self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1] + self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1] + self.raspi = False + if "dev" in self.robot_m5: + self.Pin = [2, 5] + elif "dev" in self.robot_wio: + self.Pin = [20, 21] + for i in self.move_coords: + i[2] -= 20 + elif "dev" in self.robot_raspi: + import RPi.GPIO as GPIO + GPIO.setmode(GPIO.BCM) + GPIO.setup(20, GPIO.OUT) + GPIO.setup(21, GPIO.OUT) + self.raspi = True + pump_y -= 5 + # Creating a Camera Object cap_num = 0 self.cap = cv.VideoCapture(cap_num) @@ -33,7 +46,8 @@ class Detect_marker(Movement): self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) # Get ArUco marker params. self.aruco_params = cv.aruco.DetectorParameters_create() - self.calibrationParams = cv.FileStorage("calibrationFileName.xml", cv.FILE_STORAGE_READ) + self.calibrationParams = cv.FileStorage( + "calibrationFileName.xml", cv.FILE_STORAGE_READ) # Get distance coefficient. self.dist_coeffs = self.calibrationParams.getNode("distCoeffs").mat() @@ -66,7 +80,6 @@ class Detect_marker(Movement): self.marker.color.g = 0.3 self.marker.color.b = 0.3 - # marker position initial self.marker.pose.position.x = 0 self.marker.pose.position.y = 0 @@ -78,11 +91,16 @@ class Detect_marker(Movement): # Grasping motion def move(self, x, y): - - coords = [ - [135.0, -65.5, 280.1, 178.99, 5.38, -179.9], - [136.1, -141.6, 243.9, 178.99, 5.38, -179.9] - ] + if self.raspi: + coords = [ + [145.6, -64.9, 285.2, 179.88, 7.67, -177.06], + [130.1, -155.6, 243.9, 178.99, 5.38, -179.9] + ] + else: + coords = [ + [135.0, -65.5, 280.1, 178.99, 5.38, -179.9], + [136.1, -141.6, 243.9, 178.99, 5.38, -179.9] + ] # publish marker self.marker.header.stamp = rospy.Time.now() @@ -93,21 +111,32 @@ class Detect_marker(Movement): # send coordinates to move mycobot self.pub_coords(coords[0], 30, 1) time.sleep(2) - self.pub_coords([coords[0][0]-x, coords[0][1]-y, 240, 178.99, 5.38, -179.9], 25, 1) + self.pub_coords([coords[0][0]-x, coords[0][1]-y, + 240, 178.99, 5.38, -179.9], 25, 1) time.sleep(2) - self.pub_coords([coords[0][0]-x, coords[0][1]-y, 200, 178.99, 5.38, -179.9], 25, 1) + self.pub_coords([coords[0][0]-x, coords[0][1]-y, + 200, 178.99, 5.38, -179.9], 25, 1) time.sleep(2) - if "dev" in self.robot: - self.pub_coords([coords[0][0]-x, coords[0][1]-y, 90, 178.99, 5.38, -179.9], 25, 1) + if "dev" in self.robot_m5 or self.raspi: + self.pub_coords([coords[0][0]-x, coords[0][1]-y, + 90, 178.99, 5.38, -179.9], 25, 1) + elif "dev" in self.robot_wio: + self.pub_coords([coords[0][0]-x+20, coords[0][1] - + y-10, 70, 178.99, 5.38, -179.9], 25, 1) + time.sleep(2) + if self.raspi: + self.gpio_status(True) else: - self.pub_coords([coords[0][0]-x+20, coords[0][1]-y-10, 70, 178.99, 5.38, -179.9], 25, 1) - time.sleep(3.5) - self.pub_pump(True,self.Pin) + self.pub_pump(True, self.Pin) + time.sleep(1) self.pub_coords(coords[0], 30, 1) time.sleep(3) self.pub_coords(coords[1], 30, 1) time.sleep(2) - self.pub_pump(False,self.Pin) + if self.raspi: + self.gpio_status(False) + else: + self.pub_pump(False, self.Pin) # publish marker self.marker.header.stamp = rospy.Time.now() self.marker.pose.position.x = coords[1][0]/1000.0 @@ -117,12 +146,20 @@ class Detect_marker(Movement): self.pub_coords(coords[0], 30, 1) time.sleep(2) + def gpio_status(self, flag): + if flag: + GPIO.output(20, 0) + GPIO.output(21, 0) + else: + GPIO.output(20, 1) + GPIO.output(21, 1) + # decide whether grab cube def decide_move(self, x, y): - print(x,y) + print(x, y) # detect the cube status move or run - if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm + if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm self.cache_x, self.cache_y = x, y return else: @@ -131,35 +168,37 @@ class Detect_marker(Movement): # init mycobot def init_mycobot(self): - self.pub_pump(False,self.Pin) + if self.raspi: + self.gpio_status(False) + else: + self.pub_pump(False, self.Pin) for _ in range(5): print _ - self.pub_coords([135.0, -65.5, 280.1, 178.99, 5.38, -179.9], 20, 1) - time.sleep(0.5) - + self.pub_coords([145.6, -64.9, 285.2, 179.88, 7.67, -177.06], 20, 1), 20, 1) + time.sleep(0.5) def run(self): global pump_y, pump_x - self.init_mycobot() - num = sum_x = sum_y = 0 + self.init_mycobot() + num=sum_x=sum_y=0 while cv.waitKey(1) < 0: - success, img = self.cap.read() + success, img=self.cap.read() if not success: print("It seems that the image cannot be acquired correctly.") break # transfrom the img to model of gray - gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) + gray=cv.cvtColor(img, cv.COLOR_BGR2GRAY) # Detect ArUco marker. - corners, ids, rejectImaPoint = cv.aruco.detectMarkers( - gray, self.aruco_dict, parameters=self.aruco_params + corners, ids, rejectImaPoint=cv.aruco.detectMarkers( + gray, self.aruco_dict, parameters = self.aruco_params ) if len(corners) > 0: if ids is not None: # get informations of aruco - ret = cv.aruco.estimatePoseSingleMarkers( + ret=cv.aruco.estimatePoseSingleMarkers( corners, 0.03, self.camera_matrix, self.dist_coeffs ) # rvec:rotation offset,tvec:translation deviator @@ -167,11 +206,11 @@ class Detect_marker(Movement): (rvec - tvec).any() xyz = tvec[0, 0, :] # calculate the coordinates of the aruco relative to the pump - xyz = [round(xyz[0]*1000+pump_y, 2), round(xyz[1]*1000+pump_x, 2), round(xyz[2]*1000, 2)] - + xyz = [round(xyz[0]*1000+pump_y, 2), round(xyz[1] + * 1000+pump_x, 2), round(xyz[2]*1000, 2)] for i in range(rvec.shape[0]): - # draw the aruco on img + # draw the aruco on img cv.aruco.drawDetectedMarkers(img, corners) cv.aruco.drawAxis( img, @@ -182,16 +221,19 @@ class Detect_marker(Movement): 0.03, ) - if num < 40 : + if num < 40: + if self.raspi: + sum_x -= 30 sum_x += xyz[1] sum_y += xyz[0] num += 1 - elif num ==40 : + elif num == 40: self.decide_move(sum_x/40.0, sum_y/40.0) num = sum_x = sum_y = 0 cv.imshow("encode_image", img) + if __name__ == "__main__": detect = Detect_marker() detect.run() diff --git a/mycobot_ai/scripts/detect_obj_color.py b/mycobot_ai/scripts/detect_obj_color.py index 83c5c3e..8a27d06 100644 --- a/mycobot_ai/scripts/detect_obj_color.py +++ b/mycobot_ai/scripts/detect_obj_color.py @@ -1,4 +1,5 @@ # encoding:utf-8 +#!/usr/bin/env python2 from tokenize import Pointfloat import cv2 @@ -33,7 +34,7 @@ class Object_detect(Movement): self.move_coords = [ [120.1, -141.6, 240.9, -173.34, -8.15, -83.11], # above the red bucket # above the yello bucket - [208.2, -127.8, 260.9, -157.51, -17.5, -71.18], + [215.2, -127.8, 260.9, -157.51, -17.5, -71.18], [209.7, -18.6, 230.4, -168.48, -9.86, -39.38], [196.9, -64.7, 232.6, -166.66, -9.44, -52.47], [126.6, -118.1, 305.0, -157.57, -13.72, -75.3], @@ -140,7 +141,7 @@ class Object_detect(Movement): time.sleep(1.5) if "dev" in self.robot_m5 or self.raspi: self.pub_coords([x, y, 90, -178.9, -1.57, -25.95], 20, 1) - else: + elif "dev" in self.robot_wio: h = 0 if 165 < x < 180: @@ -172,7 +173,6 @@ class Object_detect(Movement): self.pub_marker( self.move_coords[4][0]/1000.0, self.move_coords[4][1]/1000.0, self.move_coords[4][2]/1000.0) - print 'down:', self.move_coords[color] self.pub_coords(self.move_coords[color], 20, 1) self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color] [1]/1000.0, self.move_coords[color][2]/1000.0) @@ -202,7 +202,8 @@ class Object_detect(Movement): return else: self.cache_x = self.cache_y = 0 - if "dev" not in self.robot: + # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动 + if "dev" not in self.robot_wio: if (y < -30 and x > 140) or (x > 150 and y < -10): x -= 10 diff --git a/mycobot_ai/scripts/detect_obj_img.py b/mycobot_ai/scripts/detect_obj_img.py index 8cd76ba..eed80a2 100644 --- a/mycobot_ai/scripts/detect_obj_img.py +++ b/mycobot_ai/scripts/detect_obj_img.py @@ -1,4 +1,5 @@ # encoding:utf-8 +#!/usr/bin/env python2 from tokenize import Pointfloat import cv2 @@ -24,28 +25,38 @@ class Object_detect(Movement): super(Object_detect, self).__init__() # get path of file dir_path = os.path.dirname(__file__) - # 移动角度 + # 移动角度 self.move_angles = [ - [-7.11, -6.94, -55.01, -24.16, 0, -38.84], # init the point + [-7.11, -6.94, -55.01, -24.16, 0, -38.84], # init the point [-1.14, -10.63, -87.8, 9.05, -3.07, -37.7], # point to grab [17.4, -10.1, -87.27, 5.8, -2.02, -37.7], # point to grab ] # 移动坐标 self.move_coords = [ - [120.1, -141.6, 240.9, -173.34, -8.15, -83.11], # above the red bucket - [208.2, -127.8, 260.9, -157.51, -17.5, -71.18], # above the yello bucket - [209.7, -18.6, 230.4, -168.48, -9.86, -39.38], - [196.9, -64.7, 232.6, -166.66, -9.44, -52.47], - [126.6, -118.1, 305.0, -157.57, -13.72, -75.3], + [120.1, -141.6, 240.9, -173.34, -8.15, -83.11], # above the red bucket + # above the yello bucket + [208.2, -127.8, 260.9, -157.51, -17.5, -71.18], + [209.7, -18.6, 230.4, -168.48, -9.86, -39.38], + [196.9, -64.7, 232.6, -166.66, -9.44, -52.47], + [126.6, -118.1, 305.0, -157.57, -13.72, -75.3], ] # 判断连接设备:ttyUSB*为M5，ttyACM*为seeed - self.robot = os.popen("ls /dev/ttyUSB*").readline()[:-1] - if "dev" in self.robot: + self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1] + self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1] + self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1] + self.raspi = False + if "dev" in self.robot_m5: self.Pin = [2, 5] - else: + elif "dev" in self.robot_wio: self.Pin = [20, 21] for i in self.move_coords: i[2] -= 20 + elif "dev" in self.robot_raspi: + import RPi.GPIO as GPIO + GPIO.setmode(GPIO.BCM) + GPIO.setup(20, GPIO.OUT) + GPIO.setup(21, GPIO.OUT) + self.raspi = True # choose place to set cube self.color = 0 @@ -113,6 +124,14 @@ class Object_detect(Movement): self.marker.color.g = self.color self.pub.publish(self.marker) + def gpio_status(self, flag): + if flag: + GPIO.output(20, 0) + GPIO.output(21, 0) + else: + GPIO.output(20, 1) + GPIO.output(21, 1) + # Grasping motion def move(self, x, y, color): # send Angle to move mycobot @@ -125,11 +144,10 @@ class Object_detect(Movement): # send coordinates to move mycobot self.pub_coords([x, y, 165, -178.9, -1.57, -25.95], 20, 1) time.sleep(1.5) - - if "dev" in self.robot: + # 根据不同底板机械臂，调整吸泵高度 + if "dev" in self.robot_m5: self.pub_coords([x, y, 90, -178.9, -1.57, -25.95], 20, 1) - else: - + elif "dev" in self.robot_wio: h = 0 if 165 < x < 180: h = 10 @@ -137,11 +155,13 @@ class Object_detect(Movement): h = 20 elif x < 135: h = -20 - # print 'down_1:',[x, y, 31.9+h, -178.9, -1, -25.95] self.pub_coords([x, y, 31.9+h, -178.9, -1, -25.95], 20, 1) time.sleep(1.5) # open pump - self.pub_pump(True, self.Pin) + if self.raspi: + self.gpio_status(True) + else: + self.pub_pump(True, self.Pin) time.sleep(0.5) self.pub_angles(self.move_angles[2], 20) time.sleep(3) @@ -163,14 +183,16 @@ class Object_detect(Movement): [1]/1000.0, self.move_coords[color][2]/1000.0) time.sleep(2) # close pump - self.pub_pump(False, self. -Pin) + if self.raspi: + self.gpio_status(False) + else: + self.pub_pump(False, self.Pin) if color == 1: - self.pub_marker( - self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02) + self.pub_marker( + self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02) elif color == 0: - self.pub_marker( - self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0) + self.pub_marker( + self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0) self.pub_angles(self.move_angles[0], 20) time.sleep(3) @@ -184,7 +206,8 @@ Pin) return else: self.cache_x = self.cache_y = 0 - if "dev" not in self.robot: + # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动 + if "dev" not in self.robot_wio: if (y < -30 and x > 140) or (x > 150 and y < -10): x -= 10 y += 10 @@ -193,7 +216,7 @@ Pin) elif x > 170: x -= 10 y += 10 - else: + elif "dev" not in self.robot_m5: y += 10 x -= 5 if y < -20: @@ -203,13 +226,17 @@ Pin) # init mycobot def run(self): + if self.raspi: + self.gpio_status(False) + else: + self.pub_pump(False, self.Pin) for _ in range(5): self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -38.84], 20) print(_) time.sleep(0.5) - self.pub_pump(False, self.Pin) # draw aruco + def draw_marker(self, img, x, y): # draw rectangle on img cv2.rectangle( @@ -285,14 +312,14 @@ Pin) if self.x1 != self.x2: # the cutting ratio here is adjusted according to the actual situation frame = frame[int(self.y2*0.2):int(self.y1*1.15), - int(self.x1*0.7):int(self.x2*1.15)] + int(self.x1*0.7):int(self.x2*1.15)] return frame # according the class_id to get object name def id_class_name(self, class_id): for key, value in self.labels.items(): - if class_id == int(key): - return value + if class_id == int(key): + return value # detect object def obj_detect(self, img, goal): @@ -405,107 +432,20 @@ Pin) except Exception as e: pass - else: - if(len(good) < MIN_MATCH_COUNT): + # else: + # if(len(good) < MIN_MATCH_COUNT): - i += 1 - if(i % 10 == 0): - print("Not enough matches are found - %d/%d" % - (len(good), MIN_MATCH_COUNT)) + # i += 1 + # if(i % 10 == 0): + # print("Not enough matches are found - %d/%d" % + # (len(good), MIN_MATCH_COUNT)) - matchesMask = None + # matchesMask = None if x+y > 0: return x, y else: return None - def take_photo(self): - # 提醒用户操作字典 - print("*********************************************") - print("* 热键(请在摄像头的窗口使用)： *") - print("* z: 拍摄图片 *") - print("* q: 退出 *") - print("*********************************************") - - # 创建/使用local_photo文件夹 - class_name = "local_photo" - if(os.path.exists("local_photo")): - pass - else: - os.mkdir(class_name) - - # 设置特定值 - - index = 'takephoto' - cap = cv2.VideoCapture(0) - - while True: - # 读入每一帧 - ret, frame = cap.read() - - cv2.imshow("capture", frame) - - # 存储 - input = cv2.waitKey(1) & 0xFF - # 拍照 - if input == ord('z'): - cv2.imwrite("%s/%s.jpeg" % (class_name, index), - cv2.resize(frame, (600, 480), interpolation=cv2.INTER_AREA)) - break - - # 退出 - if input == ord('q'): - break - - # 关闭窗口 - cap.release() - cv2.destroyAllWindows() - - def cut_photo(self): - path = os.getcwd()+'/local_photo/img' - print path - for i,j,k in os.walk(path): - file_len = len(k) - print("请截取要识别的部分") - # root = tk.Tk() - # root.withdraw() - # temp1=filedialog.askopenfilename(parent=root) #rgb - # temp2=Image.open(temp1,mode='r') - # temp2= cv.cvtColor(np.asarray(temp2),cv.COLOR_RGB2BGR) - # cut = np.array(temp2) - - cut = cv2.imread(r"local_photo/takephoto.jpeg") - - cv2.imshow('original', cut) - # C:\Users\Elephant\Desktop\pymycobot+opencv\local_photo/takephoto.jpeg - - # 选择ROI - roi = cv2.selectROI(windowName="original", img=cut, - showCrosshair=False, fromCenter=False) - x, y, w, h = roi - print(roi) - - # 显示ROI并保存图片 - if roi != (0, 0, 0, 0): - crop = cut[y:y+h, x:x+w] - cv2.imshow('crop', crop) - cv2.imwrite('local_photo/img/goal{}.jpeg'.format(str(file_len+1)), crop) - print('Saved!') - - # 退出 - cv2.waitKey(0) - cv2.destroyAllWindows() - - def distinguist(self): - print("请选择要识别的物体图片") - root = tk.Tk() # 显式创建根窗体 - root.withdraw() # 将根窗体隐藏 - file = filedialog.askopenfilename(parent=root) - load = Image.open(file, mode='r') - load = cv.cvtColor(np.asarray(load), cv.COLOR_RGB2BGR) - goal = np.array(load) - return goal - def run(): @@ -515,7 +455,7 @@ def run(): goal = [] path = os.getcwd()+'/local_photo/img' - for i,j,k in os.walk(path): + for i, j, k in os.walk(path): for l in k: goal.append(cv2.imread('local_photo/img/{}'.format(l))) cap_num = 0 @@ -526,78 +466,77 @@ def run(): detect = Object_detect() # init mycobot detect.run() - - _init_ = 20 # + + _init_ = 20 # init_num = 0 nparams = 0 num = 0 real_sx = real_sy = 0 while cv2.waitKey(1) < 0: # read camera - _,frame = cap.read() + _, frame = cap.read() # deal img frame = detect.transform_frame(frame) - if _init_ > 0: - _init_-=1 + _init_ -= 1 continue # calculate the parameters of camera clipping if init_num < 20: if detect.get_calculate_params(frame) is None: - cv2.imshow("figure",frame) + cv2.imshow("figure", frame) continue else: - x1,x2,y1,y2 = detect.get_calculate_params(frame) - detect.draw_marker(frame,x1,y1) - detect.draw_marker(frame,x2,y2) - detect.sum_x1+=x1 - detect.sum_x2+=x2 - detect.sum_y1+=y1 - detect.sum_y2+=y2 - init_num+=1 + x1, x2, y1, y2 = detect.get_calculate_params(frame) + detect.draw_marker(frame, x1, y1) + detect.draw_marker(frame, x2, y2) + detect.sum_x1 += x1 + detect.sum_x2 += x2 + detect.sum_y1 += y1 + detect.sum_y2 += y2 + init_num += 1 continue - elif init_num==20: + elif init_num == 20: detect.set_cut_params( - (detect.sum_x1)/20.0, - (detect.sum_y1)/20.0, - (detect.sum_x2)/20.0, - (detect.sum_y2)/20.0, + (detect.sum_x1)/20.0, + (detect.sum_y1)/20.0, + (detect.sum_x2)/20.0, + (detect.sum_y2)/20.0, ) detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0 - init_num+=1 + init_num += 1 continue # calculate params of the coords between cube and mycobot if nparams < 10: if detect.get_calculate_params(frame) is None: - cv2.imshow("figure",frame) + cv2.imshow("figure", frame) continue else: - x1,x2,y1,y2 = detect.get_calculate_params(frame) - detect.draw_marker(frame,x1,y1) - detect.draw_marker(frame,x2,y2) - detect.sum_x1+=x1 - detect.sum_x2+=x2 - detect.sum_y1+=y1 - detect.sum_y2+=y2 - nparams+=1 + x1, x2, y1, y2 = detect.get_calculate_params(frame) + detect.draw_marker(frame, x1, y1) + detect.draw_marker(frame, x2, y2) + detect.sum_x1 += x1 + detect.sum_x2 += x2 + detect.sum_y1 += y1 + detect.sum_y2 += y2 + nparams += 1 continue - elif nparams==10: - nparams+=1 + elif nparams == 10: + nparams += 1 # calculate and set params of calculating real coord between cube and mycobot detect.set_params( - (detect.sum_x1+detect.sum_x2)/20.0, - (detect.sum_y1+detect.sum_y2)/20.0, - abs(detect.sum_x1-detect.sum_x2)/10.0+abs(detect.sum_y1-detect.sum_y2)/10.0 + (detect.sum_x1+detect.sum_x2)/20.0, + (detect.sum_y1+detect.sum_y2)/20.0, + abs(detect.sum_x1-detect.sum_x2)/10.0 + + abs(detect.sum_y1-detect.sum_y2)/10.0 ) print "ok" continue - # get detect result - detect_result = detect.obj_detect(frame,goal) + detect_result = detect.obj_detect(frame, goal) if detect_result is None: - cv2.imshow("figure",frame) + cv2.imshow("figure", frame) continue else: x, y = detect_result @@ -613,11 +552,10 @@ def run(): real_sy += real_y real_sx += real_x - cv2.imshow("figure",frame) - - + cv2.imshow("figure", frame) + + if __name__ == "__main__": run() # Object_detect().take_photo() # Object_detect().cut_photo() -