import cv2 import numpy as np import time import os,sys import math import rospy from visualization_msgs.msg import Marker from moving_utils import Movement import pymycobot from packaging import version # min low version require MIN_REQUIRE_VERSION = '3.6.1' current_verison = pymycobot.__version__ print('current pymycobot library version: {}'.format(current_verison)) if version.parse(current_verison) < version.parse(MIN_REQUIRE_VERSION): raise RuntimeError('The version of pymycobot library must be greater than {} or higher. The current version is {}. Please upgrade the library version.'.format(MIN_REQUIRE_VERSION, current_verison)) else: print('pymycobot library version meets the requirements!') from pymycobot import MyCobot320 IS_CV_4 = cv2.__version__[0] == '4' __version__ = "1.0" # Adaptive seeed class Object_detect(Movement): def __init__(self, camera_x = 260, camera_y = 0): # inherit the parent class super(Object_detect, self).__init__() # declare mycobot320 self.mc = None # 移动角度 self.move_angles = [ [0.61, 45.87, -92.37, -41.3, 89.56, 9.58], # init the point [18.8, -7.91, -54.49, -23.02, 89.56, -14.76], # point to grab [17.22, -5.27, -52.47, -25.75, 89.73, -0.26], ] # 移动坐标 self.move_coords = [ [32, -228.3, 201.6, -168.07, -7.17, -92.56], # D Sorting area [266.5, -219.7, 209.3, -170, -3.64, -94.62], # C Sorting area [253.8, 236.8, 224.6, -170, 6.87, -77.91], # A Sorting area [35.9, 235.4, 211.8, -169.33, -9.27, 88.3], # B Sorting area ] # which robot: USB* is m5; ACM* is wio; AMA* is raspi 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.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1] # choose place to set cube self.color = 0 # parameters to calculate camera clipping parameters self.x1 = self.x2 = self.y1 = self.y2 = 0 # set cache of real coord self.cache_x = self.cache_y = 0 # use to calculate coord between cube and mycobot self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0 # The coordinates of the grab center point relative to the mycobot self.camera_x, self.camera_y = camera_x, camera_y # The coordinates of the cube relative to the mycobot self.c_x, self.c_y = 0, 0 # The ratio of pixels to actual values self.ratio = 0 # Get ArUco marker dict that can be detected. self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250) # Get ArUco marker params. self.aruco_params = cv2.aruco.DetectorParameters_create() # 初始化背景减法器 self.mog =cv2.bgsegm.createBackgroundSubtractorMOG() # init a node and a publisher rospy.init_node("marker", anonymous=True) self.pub = rospy.Publisher('/cube', Marker, queue_size=1) # init a Marker self.marker = Marker() self.marker.header.frame_id = "base" self.marker.ns = "cube" self.marker.type = self.marker.CUBE self.marker.action = self.marker.ADD self.marker.scale.x = 0.04 self.marker.scale.y = 0.04 self.marker.scale.z = 0.04 self.marker.color.a = 1.0 self.marker.color.g = 1.0 self.marker.color.r = 1.0 # marker position initial self.marker.pose.position.x = 0 self.marker.pose.position.y = 0 self.marker.pose.position.z = 0.03 self.marker.pose.orientation.x = 0 self.marker.pose.orientation.y = 0 self.marker.pose.orientation.z = 0 self.marker.pose.orientation.w = 1.0 # publish marker def pub_marker(self, x, y, z=0.03): self.marker.header.stamp = rospy.Time.now() self.marker.pose.position.x = x self.marker.pose.position.y = y self.marker.pose.position.z = z self.marker.color.g = self.color self.pub.publish(self.marker) # pump_control pi def gpio_status(self, flag): if flag: """start the suction pump""" self.mc.set_basic_output(1, 0) self.mc.set_basic_output(2, 1) else: """stop suction pump""" self.mc.set_basic_output(1, 1) self.mc.set_basic_output(2, 0) time.sleep(1) self.mc.set_basic_output(2, 1) def pump_on(self): """Start the suction pump""" self.mc.set_basic_output(1, 0) self.mc.set_basic_output(2, 1) def pump_off(self): """stop suction pump m5""" self.mc.set_basic_output(1, 1) self.mc.set_basic_output(2, 0) time.sleep(1) self.mc.set_basic_output(2, 1) # Grasping motion def move(self, x, y, color): # send Angle to move mycobot320 print(color) print('x, y:', round(x, 2), round(y, 2)) # send Angle to move mycobot320 self.mc.send_angles(self.move_angles[2], 50) time.sleep(3) # send coordinates to move mycobot self.mc.send_coords([x, y, 230, -173.84, -0.14, -74.37], 100, 1) time.sleep(2.5) self.mc.send_coords([x, y, 100, -173.84, -0.14, -74.37], 100, 1) # time.sleep(3) # open pump self.gpio_status(True) time.sleep(1.5) tmp = [] while True: if not tmp: tmp = self.mc.get_angles() else: break time.sleep(0.5) # print(tmp) self.mc.send_angles([tmp[0], -0.71, -54.49, -23.02, 89.56, tmp[5]],25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76] time.sleep(3) self.pub_marker(self.move_coords[2][0]/1000.0, self.move_coords[2][1]/1000.0, self.move_coords[2][2]/1000.0) self.mc.send_coords(self.move_coords[color], 100, 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) time.sleep(6.5) # close pump self.gpio_status(False) time.sleep(6.5) if color == 1: self.pub_marker( self.move_coords[color][0]/1000.0+0.03, 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.mc.send_angles(self.move_angles[0], 25) time.sleep(4.5) # decide whether grab cube def decide_move(self, x, y, color): print(x, y, self.cache_x, self.cache_y) # detect the cube status move or run if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm self.cache_x, self.cache_y = x, y return else: self.cache_x = self.cache_y = 0 # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动 self.move(x, y, color) # init mycobot320 def run(self): if "dev" in self.robot_raspi: self.mc = MyCobot320(self.robot_raspi, 115200) self.gpio_status(False) self.mc.send_angles([0.61, 45.87, -92.37, -41.3, 89.56, 9.58], 20) time.sleep(2.5) # draw aruco def draw_marker(self, img, x, y): # draw rectangle on img cv2.rectangle( img, (x - 20, y - 20), (x + 20, y + 20), (0, 255, 0), thickness=2, lineType=cv2.FONT_HERSHEY_COMPLEX, ) # add text on rectangle cv2.putText(img, "({},{})".format(x, y), (x, y), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,) # get points of two aruco def get_calculate_params(self, img): # Convert the image to a gray image gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # Detect ArUco marker. corners, ids, rejectImaPoint = cv2.aruco.detectMarkers( gray, self.aruco_dict, parameters=self.aruco_params ) """ Two Arucos must be present in the picture and in the same order. There are two Arucos in the Corners, and each aruco contains the pixels of its four corners. Determine the center of the aruco by the four corners of the aruco. """ if len(corners) > 0: if ids is not None: if len(corners) <= 1 or ids[0] == 1: return None x1 = x2 = y1 = y2 = 0 point_11, point_21, point_31, point_41 = corners[0][0] x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int( (point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0) point_1, point_2, point_3, point_4 = corners[1][0] x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int( (point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0) return x1, x2, y1, y2 return None # set camera clipping parameters def set_cut_params(self, x1, y1, x2, y2): self.x1 = int(x1) self.y1 = int(y1) self.x2 = int(x2) self.y2 = int(y2) print(self.x1, self.y1, self.x2, self.y2) # set parameters to calculate the coords between cube and mycobot320 def set_params(self, c_x, c_y, ratio): self.c_x = c_x self.c_y = c_y self.ratio = 220.0/ratio # calculate the coords between cube and mycobot320 def get_position(self, x, y): return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y) """ Calibrate the camera according to the calibration parameters. Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times. If two ARuco values have been calculated, clip the video. """ def transform_frame(self, frame): # enlarge the image by 1.5 times fx = 1.5 fy = 1.5 frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy, interpolation=cv2.INTER_CUBIC) if self.x1 != self.x2: # the cutting ratio here is adjusted according to the actual situation frame = frame[int(self.y2*0.78):int(self.y1*1.1), int(self.x1*0.86):int(self.x2*1.08)] return frame # 检测物体的形状 def shape_detect(self,img): x = 0 y = 0 Alpha = 65.6 Gamma=-8191.5 cal = cv2.addWeighted(img, Alpha,img, 0, Gamma) gray = cv2.cvtColor(cal, cv2.COLOR_BGR2GRAY) # 转换为灰度图片 #ray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) # a etching operation on a picture to remove edge roughness erosion = cv2.erode(gray, np.ones((2, 2), np.uint8), iterations=2) # the image for expansion operation, its role is to deepen the color depth in the picture dilation = cv2.dilate(erosion, np.ones( (1, 1), np.uint8), iterations=2) # 设定灰度图的阈值 175, 255 _, threshold = cv2.threshold(dilation, 175, 255, cv2.THRESH_BINARY) # 边缘检测 edges = cv2.Canny(threshold,50,100) # 检测物体边框 contours,_ = cv2.findContours( edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) if len(contours)>0: for cnt in contours: # if 6000>cv2.contourArea(cnt) and cv2.contourArea(cnt)>4500: if cv2.contourArea(cnt)>5500: objectType = None peri = cv2.arcLength(cnt,True) approx = cv2.approxPolyDP(cnt, 0.02 * peri, True) objCor = len(approx) x, y, w, h = cv2.boundingRect(approx) boxes = [ box for box in [cv2.boundingRect(c) for c in contours] if min(img.shape[0], img.shape[1]) / 10 < min(box[2], box[3]) < min(img.shape[0], img.shape[1]) / 1 ] if boxes: for box in boxes: x, y, w, h = box # find the largest object that fits the requirements c = max(contours, key=cv2.contourArea) rect = cv2.minAreaRect(c) box = cv2.boxPoints(rect) box = np.int0(box) cv2.drawContours(img, [box], 0, (153, 153, 0), 2) x = int(rect[0][0]) y = int(rect[0][1]) if objCor==3: objectType = "Triangle(三角形)" cv2.drawContours(img, [cnt], 0, (0, 0, 255), 3) self.color = 3 elif objCor==4: box = cv2.boxPoints(rect) box = np.int0(box) _W = math.sqrt(math.pow((box[0][0] - box[1][0]), 2) + math.pow((box[0][1] - box[1][1]), 2)) _H = math.sqrt(math.pow((box[0][0] - box[3][0]), 2) + math.pow((box[0][1] - box[3][1]), 2)) aspRatio = _W/float(_H) if 0.98 < aspRatio < 1.03: objectType = "Square(正方形)" cv2.drawContours(img, [cnt], 0, (0, 0, 255), 3) self.color=1 else: objectType = "Rectangle(长方形)" cv2.drawContours(img, [cnt], 0, (0, 0, 255), 3) self.color=2 elif objCor>=8: objectType = "Circle(圆形)" self.color=0 cv2.drawContours(img, [cnt], 0, (0, 0, 255), 3) else: print('not recognized!') print(objectType) if abs(x) + abs(y) > 0: return x, y else: return None if __name__ == "__main__": # open the camera cap_num = 0 cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L) # cap = cv2.VideoCapture(cap_num) cap.set(3, 640) cap.set(4, 480) if not cap.isOpened(): cap.open() # init a class of Object_detect detect = Object_detect() # init mycobot320 detect.run() _init_ = 20 init_num = 0 nparams = 0 num = 0 real_sx = real_sy = 0 while cv2.waitKey(1) < 0: # read camera _, frame = cap.read() # deal img frame = detect.transform_frame(frame) if _init_ > 0: _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) 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 continue 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 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0 init_num += 1 continue # calculate params of the coords between cube and mycobot320 if nparams < 10: if detect.get_calculate_params(frame) is None: 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 continue elif nparams == 10: nparams += 1 # calculate and set params of calculating real coord between cube and mycobot320 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 ) print("ok") continue # get detect result # detect_result = detect.color_detect(frame) # print('调用检测') detect_result = detect.shape_detect(frame) # print("完成检测") if detect_result is None: cv2.imshow("figure", frame) continue else: x, y = detect_result # calculate real coord between cube and mycobot320 real_x, real_y = detect.get_position(x, y) if num == 20: detect.pub_marker(real_sx/20.0/1000.0, real_sy/20.0/1000.0) detect.decide_move(real_sx/20.0, real_sy/20.0, detect.color) num = real_sx = real_sy = 0 else: num += 1 real_sy += real_y real_sx += real_x cv2.imshow("figure", frame) # close the window if cv2.waitKey(1) & 0xFF == ord('q'): cap.release() cv2.destroyAllWindows() sys.exit()