diff --git a/mecharm/mecharm_communication/scripts/mecharm_topics.py b/mecharm/mecharm_communication/scripts/mecharm_topics.py index 02c3934..5039b0d 100755 --- a/mecharm/mecharm_communication/scripts/mecharm_topics.py +++ b/mecharm/mecharm_communication/scripts/mecharm_topics.py @@ -71,6 +71,8 @@ class MycobotTopics(object): rospy.init_node("mycobot_topics") rospy.loginfo("start ...") port = rospy.get_param("~port", os.popen("ls /dev/ttyUSB*").readline()[:-1]) + if not port: + port = rospy.get_param("~port", os.popen("ls /dev/ttyACM*").readline()[:-1]) baud = rospy.get_param("~baud", 115200) rospy.loginfo("%s,%s" % (port, baud)) self.mc = MyCobot(port, baud) diff --git a/mycobot_ai/ai_mecharm_270/launch/vision_wio.launch b/mycobot_ai/ai_mecharm_270/launch/vision_wio.launch index 51eb78b..6b6f132 100644 --- a/mycobot_ai/ai_mecharm_270/launch/vision_wio.launch +++ b/mycobot_ai/ai_mecharm_270/launch/vision_wio.launch @@ -2,8 +2,8 @@ - - + + @@ -14,13 +14,13 @@ - + - + diff --git a/mycobot_ai/ai_mecharm_270/scripts/advance_detect_obj_color.py b/mycobot_ai/ai_mecharm_270/scripts/advance_detect_obj_color.py new file mode 100644 index 0000000..ec46a57 --- /dev/null +++ b/mycobot_ai/ai_mecharm_270/scripts/advance_detect_obj_color.py @@ -0,0 +1,493 @@ +#!/usr/bin/env python2 +# -*- coding:utf-8 -*- +import cv2 +import numpy as np +import time +import os,sys +import rospy +from visualization_msgs.msg import Marker +from pymycobot.mycobot import MyCobot +from moving_utils import Movement + + +IS_CV_4 = cv2.__version__[0] == '4' +__version__ = "1.0" +# Adaptive seeed + + +class Object_detect(Movement): + + def __init__(self, camera_x = 140, camera_y = 5): # m5 + # def __init__(self, camera_x = 140, camera_y = -5): # pi + # inherit the parent class + super(Object_detect, self).__init__() + # get path of file + dir_path = os.path.dirname(__file__) + + # declare 270 + self.mc = None + + # 移动角度 + self.move_angles = [ + [0, 0, 0, 0, 90, 0], # point to grab + [-33.31, 2.02, -10.72, -0.08, 95, -54.84], # init the point + ] + + # 移动坐标 + self.move_coords = [ + [92.3, -104.9, 211.4, -179.6, 28.91, 131.29], # above the red bucket + [165.0, -93.6, 201.4, -173.43, 46.23, 160.65], # above the green bucket + [88.1, 126.3, 193.4, 162.15, 2.23, 156.02], # above the blue bucket + [-5.4, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket + ] + + # 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] + self.raspi = False + if "dev" in self.robot_m5: + self.Pin = [2, 5] + # self.Pin = [5] + elif "dev" in self.robot_wio: + # self.Pin = [20, 21] + self.Pin = [2, 5] + for i in self.move_coords: + i[2] -= 20 + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + import RPi.GPIO as GPIO + GPIO.setwarnings(False) + self.GPIO = GPIO + GPIO.setmode(GPIO.BCM) + GPIO.setup(20, GPIO.OUT) + GPIO.setup(21, GPIO.OUT) + GPIO.output(20, 1) + GPIO.output(21, 1) + self.raspi = True + if self.raspi: + self.gpio_status(False) + # 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 + # set color HSV + self.HSV = { + "yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])], + "red": [np.array([0, 43, 46]), np.array([8, 255, 255])], + "green": [np.array([35, 43, 46]), np.array([77, 255, 255])], + "blue": [np.array([100, 43, 46]), np.array([124, 255, 255])], + "cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])], + } + # use to calculate coord between cube and 270 + self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0 + # The coordinates of the grab center point relative to the 270 + self.camera_x, self.camera_y = camera_x, camera_y + # The coordinates of the cube relative to the 270 + 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() + + # 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 = "/joint1" + 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: + # self.GPIO.output(20, 0) + self.GPIO.output(21, 0) + else: + # self.GPIO.output(20, 1) + self.GPIO.output(21, 1) + + # 开启吸泵 m5 + def pump_on(self): + # 让2号位工作 + # self.mc.set_basic_output(2, 0) + # 让5号位工作 + self.mc.set_basic_output(5, 0) + + # 停止吸泵 m5 + def pump_off(self): + # 让2号位停止工作 + # self.mc.set_basic_output(2, 1) + # 让5号位停止工作 + self.mc.set_basic_output(5, 1) + + # Grasping motion + def move(self, x, y, color): + # send Angle to move 270 + print(color) + self.mc.send_angles(self.move_angles[0], 30) + time.sleep(4) + + # send coordinates to move 270 + self.mc.send_coords([x, y, 140, 179.12, -0.18, 179.46], 30, 0) + time.sleep(3) + self.pub_marker(x/1000.0, y/1000.0, 140/1000.0) + + + self.mc.send_coords([x, y, 103, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 m5 + # self.mc.send_coords([x, y, 90, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 pi + time.sleep(3) + self.pub_marker(x/1000.0, y/1000.0, 90/1000.0) + + + # open pump + if "dev" in self.robot_m5 or "dev" in self.robot_wio: + self.pump_on() + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + 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], 17.22, -32.51, tmp[3], 97, tmp[5]],30) + time.sleep(3) + + + + self.mc.send_coords(self.move_coords[color], 30, 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(3) + + # close pump + if "dev" in self.robot_m5 or "dev" in self.robot_wio: + self.pump_off() + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + self.gpio_status(False) + time.sleep(6) + + if color == 1: + 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.mc.send_angles(self.move_angles[1], 30) + time.sleep(1.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 270 + def run(self): + if "dev" in self.robot_m5: + self.mc = MyCobot(self.robot_m5, 115200) + elif "dev" in self.robot_wio: + self.mc = MyCobot(self.robot_wio, 115200) + elif "dev" in self.robot_raspi: + self.mc = MyCobot(self.robot_raspi, 1000000) + if not self.raspi: + self.pub_pump(False, self.Pin) + self.mc.send_angles([-33.31, 2.02, -10.72, -0.08, 95, -54.84], 30) + time.sleep(3) + + # 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 270 + 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 270 + 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 + + # detect cube color + def color_detect(self, img): + # set the arrangement of color'HSV + x = y = 0 + for mycolor, item in self.HSV.items(): + # print("mycolor:",mycolor) + redLower = np.array(item[0]) + redUpper = np.array(item[1]) + + # transfrom the img to model of gray + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + # print("hsv",hsv) + + # wipe off all color expect color in range + mask = cv2.inRange(hsv, item[0], item[1]) + + # a etching operation on a picture to remove edge roughness + erosion = cv2.erode(mask, np.ones((1, 1), 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) + + # adds pixels to the image + target = cv2.bitwise_and(img, img, mask=dilation) + + # the filtered image is transformed into a binary image and placed in binary + ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY) + + # get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected + contours, hierarchy = cv2.findContours( + dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + if len(contours) > 0: + # do something about misidentification + 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) + # get the lower left and upper right points of the positioning object + x, y, w, h = cv2.boundingRect(c) + # locate the target by drawing rectangle + cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2) + # calculate the rectangle center + x, y = (x*2+w)/2, (y*2+h)/2 + # calculate the real coordinates of 270 relative to the target + + if mycolor == "red": + self.color = 0 + elif mycolor == "green": + self.color = 1 + elif mycolor == "cyan": + self.color = 2 + else: + self.color = 3 + + 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) + if not cap.isOpened(): + cap.open() + # init a class of Object_detect + detect = Object_detect() + # init 270 + 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 270 + 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 270 + 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) + if detect_result is None: + cv2.imshow("figure", frame) + continue + else: + x, y = detect_result + # calculate real coord between cube and 270 + 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() \ No newline at end of file diff --git a/mycobot_ai/ai_mecharm_270/scripts/advance_detect_obj_img_folder.py b/mycobot_ai/ai_mecharm_270/scripts/advance_detect_obj_img_folder.py new file mode 100644 index 0000000..f2bc2c1 --- /dev/null +++ b/mycobot_ai/ai_mecharm_270/scripts/advance_detect_obj_img_folder.py @@ -0,0 +1,644 @@ +#!/usr/bin/env python2 +# encoding:utf-8 +from multiprocessing import Process, Pipe +import cv2 +import numpy as np +import time +import os,sys +import rospy +from visualization_msgs.msg import Marker +from moving_utils import Movement +from pymycobot.mycobot import MyCobot + +IS_CV_4 = cv2.__version__[0] == '4' +__version__ = "1.0" # Adaptive seeed + + +class Object_detect(Movement): + + def __init__(self, camera_x = 145, camera_y = -5): + # inherit the parent class + super(Object_detect, self).__init__() + # get path of file + dir_path = os.path.dirname(__file__) + + # declare 270 + self.mc = None + # 移动角度 + self.move_angles = [ + [0, 0, 0, 0, 90, 0], # point to grab + [-33.31, 2.02, -10.72, -0.08, 95, -54.84], # init the point + ] + + # 移动坐标 + self.move_coords = [ + [92.3, -104.9, 211.4, -179.6, 28.91, 131.29], # above the red bucket + [165.0, -93.6, 201.4, -173.43, 46.23, 160.65], # above the green bucket + [88.1, 126.3, 193.4, 162.15, 2.23, 156.02], # above the blue bucket + [-5.4, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket + ] + + # 判断连接设备:ttyUSB*为M5，ttyACM*为seeed + self.raspi = False + 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] + if "dev" in self.robot_m5: + self.Pin = [2, 5] + elif "dev" in self.robot_wio: + # self.Pin = [20, 21] + self.Pin = [2, 5] + for i in self.move_coords: + i[2] -= 20 + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + import RPi.GPIO as GPIO + GPIO.setwarnings(False) + self.GPIO = GPIO + GPIO.setmode(GPIO.BCM) + GPIO.setup(20, GPIO.OUT) + GPIO.setup(21, GPIO.OUT) + GPIO.output(20, 1) + GPIO.output(21, 1) + self.raspi = True + if self.raspi: + self.gpio_status(False) + + # 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() + + # 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 = "/joint1" + 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 + + self.cache_x = self.cache_y = 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: + # self.GPIO.output(20, 0) + self.GPIO.output(21, 0) + else: + # self.GPIO.output(20, 1) + self.GPIO.output(21, 1) + + # 开启吸泵 m5 + def pump_on(self): + # 让2号位工作 + # self.mc.set_basic_output(2, 0) + # 让5号位工作 + self.mc.set_basic_output(5, 0) + + # 停止吸泵 m5 + def pump_off(self): + # 让2号位停止工作 + # self.mc.set_basic_output(2, 1) + # 让5号位停止工作 + self.mc.set_basic_output(5, 1) + + # Grasping motion + def move(self, x, y, color): + # send Angle to move 270 + self.mc.send_angles(self.move_angles[0], 30) + time.sleep(7) + + print("x %s ,y %s" % (x,y)) + # send coordinates to move 270 根据不同底板机械臂，调整吸泵高度 + self.mc.send_coords([x, y, 140, 179.12, -0.18, 179.46], 30, 0) + time.sleep(7) + print("ntm") + + self.mc.send_coords([x, y, 103, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 m5 + # self.mc.send_coords([x, y, 90, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 pi + # self.mc.send_coords([x, y, 92, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 + time.sleep(6) + + # open pump + if "dev" in self.robot_m5 or "dev" in self.robot_wio: + self.pump_on() + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + 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], 17.22, -32.51, tmp[3], 97, tmp[5]],30) + time.sleep(6) + + + self.mc.send_coords(self.move_coords[color], 30, 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) + + # close pump + if "dev" in self.robot_m5 or "dev" in self.robot_wio: + self.pump_off() + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + self.gpio_status(False) + time.sleep(6) + + self.mc.send_angles(self.move_angles[1], 30) + time.sleep(6) + + # 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 270 + def run(self): + if "dev" in self.robot_wio : + self.mc = MyCobot(self.robot_wio, 115200) + elif "dev" in self.robot_m5: + self.mc = MyCobot(self.robot_m5, 115200) + elif "dev" in self.robot_raspi: + self.mc = MyCobot(self.robot_raspi, 1000000) + if not self.raspi: + self.pub_pump(False, self.Pin) + self.mc.send_angles([-33.31, 2.02, -10.72, -0.08, 95, -54.84], 30) + time.sleep(3) + + # 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 mycobot + 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 mycobot + 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.2):int(self.y1 * 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 + + # detect object + def obj_detect(self, img, goal, kp_img, desc_img, kp_list, desc_list, connection): + i = 0 + MIN_MATCH_COUNT = 5 + # sift = cv2.xfeatures2d.SIFT_create() + + # find the keypoints and descriptors with SIFT + # kp = [] + # des = [] + kp = kp_list + des = desc_list + + # for i in goal: + # kp0, des0 = sift.detectAndCompute(i, None) + # kp.append(kp0) + # des.append(des0) + + # kp1, des1 = sift.detectAndCompute(goal, None) + # kp2, des2 = sift.detectAndCompute(img, None) + kp2, des2 = kp_img, desc_img + + # FLANN parameters + FLANN_INDEX_KDTREE = 0 + index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5) + search_params = dict(checks=50) # or pass empty dictionary + flann = cv2.FlannBasedMatcher(index_params, search_params) + + x, y = 0, 0 + try: + for i in range(len(des)): + matches = flann.knnMatch(des[i], des2, k=2) + # store all the good matches as per Lowe's ratio test. 根据Lowe比率测试存储所有良好匹配项。 + good = [] + for m, n in matches: + if m.distance < 0.7 * n.distance: + good.append(m) + + # When there are enough robust matching point pairs 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时 + if len(good) > MIN_MATCH_COUNT: + + # extract corresponding point pairs from matching 从匹配中提取出对应点对 + # query index of small objects, training index of scenarios 小对象的查询索引，场景的训练索引 + src_pts = np.float32([kp[i][m.queryIdx].pt + for m in good]).reshape(-1, 1, 2) + dst_pts = np.float32([kp2[m.trainIdx].pt + for m in good]).reshape(-1, 1, 2) + + # Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵 + M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, + 5.0) + matchesMask = mask.ravel().tolist() + # Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置 + h, w, d = goal[i].shape + pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1], + [w - 1, 0]]).reshape(-1, 1, 2) + dst = cv2.perspectiveTransform(pts, M) + coord = (dst[0][0] + dst[1][0] + dst[2][0] + + dst[3][0]) / 4.0 + connection.send((DRAW_COORDS, coord)) + # cv2.putText(img, "{}".format(coord), (50, 60), + # fontFace=None, fontScale=1, + # color=(0, 255, 0), lineType=1) + print(format(dst[0][0][0])) + x = (dst[0][0][0] + dst[1][0][0] + dst[2][0][0] + + dst[3][0][0]) / 4.0 + y = (dst[0][0][1] + dst[1][0][1] + dst[2][0][1] + + dst[3][0][1]) / 4.0 + + # bound box 绘制边框 + # img = cv2.polylines(img, [np.int32(dst)], True, 244, 3, cv2.LINE_AA) + connection.send((DRAW_RECT, dst)) + # cv2.polylines(mixture, [np.int32(dst)], True, (0, 255, 0), 2, cv2.LINE_AA) + except Exception as e: + pass + + if x + y > 0: + return x, y + else: + return None + +# The path to save the image folder +def parse_folder(folder): + restore = [] + # path = '' + path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mecharm_270/' + folder + path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mecharm_270/' + folder + + if os.path.exists(path1): + path = path1 + elif os.path.exists(path2): + path = path2 + + # print("path:",path) + for i, j, k in os.walk(path): + for l in k: + restore.append(cv2.imread(folder + '/{}'.format(l))) + return restore + +def compute_keypoints_and_descriptors(sift, images_lists): + kp_list = [] + desc_list = [] + for images in images_lists: + kp_tmp = [] + desc_tmp = [] + for img in images: + kp, desc = sift.detectAndCompute(img, None) + kp_tmp.append(kp) + desc_tmp.append(desc) + kp_list.append(kp_tmp) + desc_list.append(desc_tmp) + + return kp_list, desc_list + +GET_FRAME = 1 +STOP_PROCESSING = 2 +DRAW_COORDS = 3 +DRAW_RECT = 4 +CLEAR_DRAW = 5 +CROP_FRAME = 6 + +def get_frame(connection): + connection.send(GET_FRAME) + frame = connection.recv() + return frame + +def process_transform_frame(frame, x1, y1, x2, y2): + # 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 x1 != x2: + # the cutting ratio here is adjusted according to the actual situation +# frame = frame[int(y2 * 0.2):int(y1 * 1.15), +# int(x1 * 0.7):int(x2 * 1.15)] + return frame + +def process_display_frame(connection): + cap_num = 0 + coord = None + dst = None + x1 = 0 + y1 = 0 + x2 = 0 + y2 = 0 + cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L) + if not cap.isOpened(): + cap.open() + while cv2.waitKey(1) < 0: + _, frame = cap.read() + frame = process_transform_frame(frame, x1, y1, x2, y2) + if connection.poll(): + request = connection.recv() + if request == GET_FRAME: + connection.send(frame) + elif request == CLEAR_DRAW: + coord = None + dst = None + elif type(request) is tuple: + if request[0] == DRAW_COORDS: + coord = request[1] + elif request[0] == DRAW_RECT: + dst = request[1] + elif request[0] == CROP_FRAME: + x1 = request[1] + y1 = request[2] + x2 = request[3] + y2 = request[4] + + if not coord is None: + cv2.putText(frame, "{}".format(coord), (50, 60), fontFace=None, + fontScale=1, color=(0, 255, 0), lineType=1) + if not dst is None: + frame = cv2.polylines(frame, [np.int32(dst)], True, 244, 3, cv2.LINE_AA) + cv2.imshow("figure", frame) + time.sleep(0.04) + connection.send(STOP_PROCESSING) + +def run(): + parent_conn, child_conn = Pipe() + child = Process(target = process_display_frame, args=(child_conn,)) + child.start() + + res_queue = [[], [], [], []] + res_queue[0] = parse_folder('res/red') + res_queue[1] = parse_folder('res/green') + res_queue[2] = parse_folder('res/blue') + res_queue[3] = parse_folder('res/gray') + + + sift = cv2.xfeatures2d.SIFT_create() + kp_list, desc_list = compute_keypoints_and_descriptors(sift, res_queue) + + # init a class of Object_detect + detect = Object_detect() + + # init mycobot + detect.run() + + # _init_ = 20 # + init_num = 0 + nparams = 0 + # num = 0 + # real_sx = real_sy = 0 + while True: + start_time = time.time() + if parent_conn.poll(): + data = parent_conn.recv() + if data == STOP_PROCESSING: + break + # read camera + frame = get_frame(parent_conn) + # 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, + ) + parent_conn.send((CROP_FRAME, + (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 mycobot + 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 + print ("ok") + continue + 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) + print("ok") + continue + + # get detect result + kp_img, desc_img = sift.detectAndCompute(frame, None) + frame = get_frame(parent_conn) + for i, v in enumerate(res_queue): + # HACK: to update frame every time + detect_result = detect.obj_detect(frame, v, kp_img, desc_img, kp_list[i], desc_list[i], parent_conn) + if detect_result: + x, y = detect_result + # calculate real coord between cube and mycobot + real_x, real_y = detect.get_position(x, y) + detect.color = i + detect.pub_marker(real_x / 1000.0, real_y / 1000.0) + detect.decide_move(real_x, real_y, detect.color) + # if num == 5: + # detect.color = i + # detect.pub_marker(real_sx / 5.0 / 1000.0, + # real_sy / 5.0 / 1000.0) + # detect.decide_move(real_sx / 5.0, real_sy / 5.0, + # detect.color) + # num = real_sx = real_sy = 0 + # else: + # num += 1 + # real_sy += real_y + # real_sx += real_x + parent_conn.send(CLEAR_DRAW) + + # cv2.imshow("figure", frame) + time.sleep(0.05) + end_time = time.time() + print("loop_time = ", end_time - start_time) + + # close the window + if cv2.waitKey(1) & 0xFF == ord('q'): + # cap.release() + cv2.destroyAllWindows() + sys.exit() + + child.join() + + +if __name__ == "__main__": + run() + # Object_detect().take_photo() + # Object_detect().cut_photo() diff --git a/mycobot_ai/ai_mycobot_280/res/blue/goal9.jpeg b/mycobot_ai/ai_mycobot_280/res/blue/goal9.jpeg new file mode 100644 index 0000000..3eaf44f Binary files /dev/null and b/mycobot_ai/ai_mycobot_280/res/blue/goal9.jpeg differ diff --git a/mycobot_ai/ai_mycobot_280/res/gray/goal10.jpeg b/mycobot_ai/ai_mycobot_280/res/gray/goal10.jpeg new file mode 100644 index 0000000..df419e2 Binary files /dev/null and b/mycobot_ai/ai_mycobot_280/res/gray/goal10.jpeg differ diff --git a/mycobot_ai/ai_mycobot_280/res/green/goal12.jpeg b/mycobot_ai/ai_mycobot_280/res/green/goal12.jpeg new file mode 100644 index 0000000..19f703f Binary files /dev/null and b/mycobot_ai/ai_mycobot_280/res/green/goal12.jpeg differ diff --git a/mycobot_ai/ai_mycobot_280/res/red/goal8.jpeg b/mycobot_ai/ai_mycobot_280/res/red/goal8.jpeg new file mode 100644 index 0000000..e8dfcbb Binary files /dev/null and b/mycobot_ai/ai_mycobot_280/res/red/goal8.jpeg differ diff --git a/mycobot_ai/ai_mycobot_280/res/red/goal9.jpeg b/mycobot_ai/ai_mycobot_280/res/red/goal9.jpeg new file mode 100644 index 0000000..4ef5940 Binary files /dev/null and b/mycobot_ai/ai_mycobot_280/res/red/goal9.jpeg differ diff --git a/mycobot_ai/ai_mycobot_280/res/takephoto.jpeg b/mycobot_ai/ai_mycobot_280/res/takephoto.jpeg index 4d761e6..348b385 100644 Binary files a/mycobot_ai/ai_mycobot_280/res/takephoto.jpeg and b/mycobot_ai/ai_mycobot_280/res/takephoto.jpeg differ diff --git a/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py b/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py new file mode 100644 index 0000000..0809e89 --- /dev/null +++ b/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py @@ -0,0 +1,479 @@ +# encoding:utf-8 +#!/usr/bin/env python2 + +import cv2 +import numpy as np +import time +import os,sys +import rospy +from visualization_msgs.msg import Marker +from pymycobot.mycobot import MyCobot +from moving_utils import Movement + +IS_CV_4 = cv2.__version__[0] == '4' +__version__ = "1.0" +# Adaptive seeed + + +class Object_detect(Movement): + + def __init__(self, camera_x = 155, camera_y = 10): + # inherit the parent class + super(Object_detect, self).__init__() + # get path of file + dir_path = os.path.dirname(__file__) + self.mc = None + + # 移动角度 + self.move_angles = [ + [-7.11, -6.94, -55.01, -24.16, 0, -15], # init the point + [18.8, -7.91, -54.49, -23.02, -0.79, -14.76], # point to grab + ] + + # 移动坐标 + self.move_coords = [ + [132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # above the red bucket + [238.8, -124.1, 204.3, -169.69, -5.52, -96.52], # green + [115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # blue + [-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # gray + ] + # 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] + self.raspi = False + if "dev" in self.robot_m5: + self.Pin = [2, 5] + elif "dev" in self.robot_wio: + # self.Pin = [20, 21] + self.Pin = [2, 5] + + for i in self.move_coords: + i[2] -= 20 + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + import RPi.GPIO as GPIO + GPIO.setwarnings(False) + self.GPIO = GPIO + GPIO.setmode(GPIO.BCM) + GPIO.setup(20, GPIO.OUT) + GPIO.setup(21, GPIO.OUT) + GPIO.output(20, 1) + GPIO.output(21, 1) + self.raspi = True + if self.raspi: + self.gpio_status(False) + + # 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 + # set color HSV + self.HSV = { + "yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])], + "red": [np.array([0, 43, 46]), np.array([8, 255, 255])], + "green": [np.array([35, 43, 35]), np.array([90, 255, 255])], # [77, 255, 255] + "blue": [np.array([100, 43, 46]), np.array([124, 255, 255])], + "cyan": [np.array([89, 43, 46]), np.array([99, 255, 255])], # np.array([78, 43, 46]), np.array([99, 255, 255]) + } + # 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() + + # 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 = "/joint1" + 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) + + def gpio_status(self, flag): + if flag: + # self.GPIO.output(20, 0) + self.GPIO.output(21, 0) + else: + # self.GPIO.output(20, 1) + self.GPIO.output(21, 1) + + # 开启吸泵 m5 + def pump_on(self): + # 让2号位工作 + # self.mc.set_basic_output(2, 0) + # 让5号位工作 + self.mc.set_basic_output(5, 0) + + # 停止吸泵 m5 + def pump_off(self): + # 让2号位停止工作 + # self.mc.set_basic_output(2, 1) + # 让5号位停止工作 + self.mc.set_basic_output(5, 1) + # Grasping motion + def move(self, x, y, color): + # send Angle to move mycobot + print (color) + self.mc.send_angles(self.move_angles[1], 25) + time.sleep(3) + + # send coordinates to move mycobot + self.mc.send_coords([x, y, 190.6, 179.87, -3.78, -62.75], 25, 1) # usb :rx,ry,rz -173.3, -5.48, -57.9 + time.sleep(3) + + # self.mc.send_coords([x, y, 150, 179.87, -3.78, -62.75], 25, 0) + # time.sleep(3) + + self.mc.send_coords([x, y, 103, 179.87, -3.78, -62.75], 25, 0) + time.sleep(3) + + # open pump + if "dev" in self.robot_m5 or "dev" in self.robot_wio: + self.pump_on() + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + 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, -0.79, tmp[5]],25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76] + time.sleep(2.5) + + 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], 25, 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(3) + + # close pump + if "dev" in self.robot_m5 or "dev" in self.robot_wio: + self.pump_off() + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + self.gpio_status(False) + time.sleep(4) + + if color == 1: + 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_angles(self.move_angles[0], 20) + self.mc.send_angles(self.move_angles[0], 25) + time.sleep(3) + + # 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 mycobot + def run(self): + if "dev" in self.robot_wio : + self.mc = MyCobot(self.robot_wio, 115200) + elif "dev" in self.robot_m5: + self.mc = MyCobot(self.robot_m5, 115200) + elif "dev" in self.robot_raspi: + self.mc = MyCobot(self.robot_raspi, 1000000) + if not self.raspi: + self.pub_pump(False, self.Pin) + self.mc.send_angles([-7.11, -6.94, -55.01, -24.16, 0, -15], 20) + time.sleep(3) + + # 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 mycobot + 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 mycobot + 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 + + # detect cube color + def color_detect(self, img): + # set the arrangement of color'HSV + x = y = 0 + for mycolor, item in self.HSV.items(): + redLower = np.array(item[0]) + redUpper = np.array(item[1]) + # transfrom the img to model of gray + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + # wipe off all color expect color in range + mask = cv2.inRange(hsv, item[0], item[1]) + # a etching operation on a picture to remove edge roughness + erosion = cv2.erode(mask, np.ones((1, 1), 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) + + + # adds pixels to the image + target = cv2.bitwise_and(img, img, mask=dilation) + # the filtered image is transformed into a binary image and placed in binary + ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY) + # get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected + contours, hierarchy = cv2.findContours( + dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + if len(contours) > 0: + # do something about misidentification + 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) + # get the lower left and upper right points of the positioning object + x, y, w, h = cv2.boundingRect(c) + # locate the target by drawing rectangle + cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2) + # calculate the rectangle center + x, y = (x*2+w)/2, (y*2+h)/2 + # calculate the real coordinates of mycobot relative to the target + if mycolor == "red": + self.color = 0 + + elif mycolor == "green": + self.color = 1 + + elif mycolor == "cyan": + self.color = 2 + + else: + self.color = 3 + + + 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) + + if not cap.isOpened(): + cap.open() + # init a class of Object_detect + detect = Object_detect() + # init mycobot + 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 mycobot + 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 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 + ) + print ("ok") + continue + + # get detect result + detect_result = detect.color_detect(frame) + if detect_result is None: + cv2.imshow("figure", frame) + continue + else: + x, y = detect_result + # calculate real coord between cube and mycobot + 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) diff --git a/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_img_folder.py b/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_img_folder.py new file mode 100644 index 0000000..68a1432 --- /dev/null +++ b/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_img_folder.py @@ -0,0 +1,660 @@ +# encoding:utf-8 +#!/usr/bin/env python2 + +from multiprocessing import Process, Pipe +import cv2 +import numpy as np +import time +import os,sys +import rospy +from visualization_msgs.msg import Marker +from pymycobot.mycobot import MyCobot +from moving_utils import Movement + +IS_CV_4 = cv2.__version__[0] == '4' +__version__ = "1.0" # Adaptive seeed + + +class Object_detect(Movement): + + def __init__(self, camera_x = 155, camera_y = 10): + # inherit the parent class + super(Object_detect, self).__init__() + # get path of file + dir_path = os.path.dirname(__file__) + self.mc = None + + # 移动角度 + self.move_angles = [ + [-7.11, -6.94, -55.01, -24.16, 0, -15], # init the point + [18.8, -7.91, -54.49, -23.02, -0.79, -14.76], # point to grab + ] + + # 移动坐标 + self.move_coords = [ + [132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # above the red bucket + [238.8, -124.1, 204.3, -169.69, -5.52, -96.52], # green + [115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # blue + [-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # gray + ] + # 判断连接设备:ttyUSB*为M5，ttyACM*为seeed + self.raspi = False + 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] + if "dev" in self.robot_m5: + self.Pin = [2, 5] + elif "dev" in self.robot_wio: + self.Pin = [2, 5] + for i in self.move_coords: + i[2] -= 20 + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + import RPi.GPIO as GPIO + GPIO.setwarnings(False) + self.GPIO = GPIO + GPIO.setmode(GPIO.BCM) + GPIO.setup(20, GPIO.OUT) + GPIO.setup(21, GPIO.OUT) + GPIO.output(20, 1) + GPIO.output(21, 1) + self.raspi = True + if self.raspi: + self.gpio_status(False) + + # 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 + # load model of img recognition + # self.model_path = os.path.join(dir_path, "frozen_inference_graph.pb") + # self.pbtxt_path = os.path.join(dir_path, "graph.pbtxt") + # self.label_path = os.path.join(dir_path, "labels.json") + # # load class labels + # self.labels = json.load(open(self.label_path)) + + # 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() + + # if IS_CV_4: + # self.net = cv2.dnn.readNetFromTensorflow(self.model_path, self.pbtxt_path) + # else: + # print('Load tensorflow model need the version of opencv is 4.') + # exit(0) + # 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 = "/joint1" + 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 + + self.cache_x = self.cache_y = 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) + + def gpio_status(self, flag): + if flag: + # self.GPIO.output(20, 0) + self.GPIO.output(21, 0) + else: + self.GPIO.output(20, 1) + # self.GPIO.output(21, 1) + + # 开启吸泵 m5 + def pump_on(self): + # 让2号位工作 + # self.mc.set_basic_output(2, 0) + # 让5号位工作 + self.mc.set_basic_output(5, 0) + + # 停止吸泵 m5 + def pump_off(self): + # 让2号位停止工作 + # self.mc.set_basic_output(2, 1) + # 让5号位停止工作 + self.mc.set_basic_output(5, 1) + + # Grasping motion + def move(self, x, y, color): + # send Angle to move mycobot + print (color) + self.mc.send_angles(self.move_angles[1], 25) + time.sleep(3) + + # send coordinates to move mycobot + self.mc.send_coords([x, y, 190.6, 179.87, -3.78, -62.75], 25, 1) # usb :rx,ry,rz -173.3, -5.48, -57.9 + time.sleep(3) + + # self.mc.send_coords([x, y, 150, 179.87, -3.78, -62.75], 25, 0) + # time.sleep(3) + + # self.mc.send_coords([x, y, 105, 179.87, -3.78, -62.75], 25, 0) + self.mc.send_coords([x, y, 103, 179.87, -3.78, -62.75], 25, 0) + + time.sleep(3) + + # open pump + if "dev" in self.robot_m5 or "dev" in self.robot_wio: + self.pump_on() + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + 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, -0.79, tmp[5]],25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76] + time.sleep(5) + + + + self.mc.send_coords(self.move_coords[color], 25, 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(4) + + # close pump + if "dev" in self.robot_m5 or "dev" in self.robot_wio: + self.pump_off() + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + self.gpio_status(False) + time.sleep(4) + + self.mc.send_angles(self.move_angles[0], 25) + time.sleep(3) + + # 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 mycobot + def run(self): + if "dev" in self.robot_wio : + self.mc = MyCobot(self.robot_wio, 115200) + elif "dev" in self.robot_m5: + self.mc = MyCobot(self.robot_m5, 115200) + elif "dev" in self.robot_raspi: + self.mc = MyCobot(self.robot_raspi, 1000000) + if not self.raspi: + self.pub_pump(False, self.Pin) + self.mc.send_angles([-7.11, -6.94, -55.01, -24.16, 0, -15], 20) + time.sleep(3) + + # 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 mycobot + 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 mycobot + 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.2):int(self.y1 * 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 + + # detect object + def obj_detect(self, img, goal, kp_img, desc_img, kp_list, desc_list, connection): + i = 0 + MIN_MATCH_COUNT = 5 + # sift = cv2.xfeatures2d.SIFT_create() + + # find the keypoints and descriptors with SIFT + # kp = [] + # des = [] + kp = kp_list + des = desc_list + + # for i in goal: + # kp0, des0 = sift.detectAndCompute(i, None) + # kp.append(kp0) + # des.append(des0) + + # kp1, des1 = sift.detectAndCompute(goal, None) + # kp2, des2 = sift.detectAndCompute(img, None) + kp2, des2 = kp_img, desc_img + + # FLANN parameters + FLANN_INDEX_KDTREE = 0 + index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5) + search_params = dict(checks=50) # or pass empty dictionary + flann = cv2.FlannBasedMatcher(index_params, search_params) + + x, y = 0, 0 + try: + for i in range(len(des)): + matches = flann.knnMatch(des[i], des2, k=2) + # store all the good matches as per Lowe's ratio test. 根据Lowe比率测试存储所有良好匹配项。 + good = [] + for m, n in matches: + if m.distance < 0.7 * n.distance: + good.append(m) + + # When there are enough robust matching point pairs 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时 + if len(good) > MIN_MATCH_COUNT: + + # extract corresponding point pairs from matching 从匹配中提取出对应点对 + # query index of small objects, training index of scenarios 小对象的查询索引，场景的训练索引 + src_pts = np.float32([kp[i][m.queryIdx].pt + for m in good]).reshape(-1, 1, 2) + dst_pts = np.float32([kp2[m.trainIdx].pt + for m in good]).reshape(-1, 1, 2) + + # Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵 + M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, + 5.0) + matchesMask = mask.ravel().tolist() + # Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置 + h, w, d = goal[i].shape + pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1], + [w - 1, 0]]).reshape(-1, 1, 2) + dst = cv2.perspectiveTransform(pts, M) + coord = (dst[0][0] + dst[1][0] + dst[2][0] + + dst[3][0]) / 4.0 + connection.send((DRAW_COORDS, coord)) + # cv2.putText(img, "{}".format(coord), (50, 60), + # fontFace=None, fontScale=1, + # color=(0, 255, 0), lineType=1) + print(format(dst[0][0][0])) + x = (dst[0][0][0] + dst[1][0][0] + dst[2][0][0] + + dst[3][0][0]) / 4.0 + y = (dst[0][0][1] + dst[1][0][1] + dst[2][0][1] + + dst[3][0][1]) / 4.0 + + # bound box 绘制边框 + # img = cv2.polylines(img, [np.int32(dst)], True, 244, 3, cv2.LINE_AA) + connection.send((DRAW_RECT, dst)) + # cv2.polylines(mixture, [np.int32(dst)], True, (0, 255, 0), 2, cv2.LINE_AA) + except Exception as e: + pass + + if x + y > 0: + return x, y + else: + return None + +# The path to save the image folder +def parse_folder(folder): + restore = [] + path = '' + path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/' + folder + path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/' + folder + if os.path.exists(path1): + path = path1 + elif os.path.exists(path2): + path = path2 + + # print("path:",path) + + for i, j, k in os.walk(path): + for l in k: + restore.append(cv2.imread(folder + '/{}'.format(l))) + return restore + +def compute_keypoints_and_descriptors(sift, images_lists): + kp_list = [] + desc_list = [] + for images in images_lists: + kp_tmp = [] + desc_tmp = [] + for img in images: + kp, desc = sift.detectAndCompute(img, None) + kp_tmp.append(kp) + desc_tmp.append(desc) + kp_list.append(kp_tmp) + desc_list.append(desc_tmp) + + return kp_list, desc_list + +GET_FRAME = 1 +STOP_PROCESSING = 2 +DRAW_COORDS = 3 +DRAW_RECT = 4 +CLEAR_DRAW = 5 +CROP_FRAME = 6 + +def get_frame(connection): + connection.send(GET_FRAME) + frame = connection.recv() + return frame + +def process_transform_frame(frame, x1, y1, x2, y2): + # 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 x1 != x2: + # the cutting ratio here is adjusted according to the actual situation +# frame = frame[int(y2 * 0.2):int(y1 * 1.15), +# int(x1 * 0.7):int(x2 * 1.15)] + return frame + +def process_display_frame(connection): + cap_num = 0 + coord = None + dst = None + x1 = 0 + y1 = 0 + x2 = 0 + y2 = 0 + cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L) + if not cap.isOpened(): + cap.open() + while cv2.waitKey(1) < 0: + _, frame = cap.read() + frame = process_transform_frame(frame, x1, y1, x2, y2) + if connection.poll(): + request = connection.recv() + if request == GET_FRAME: + connection.send(frame) + elif request == CLEAR_DRAW: + coord = None + dst = None + elif type(request) is tuple: + if request[0] == DRAW_COORDS: + coord = request[1] + elif request[0] == DRAW_RECT: + dst = request[1] + elif request[0] == CROP_FRAME: + x1 = request[1] + y1 = request[2] + x2 = request[3] + y2 = request[4] + + if not coord is None: + cv2.putText(frame, "{}".format(coord), (50, 60), fontFace=None, + fontScale=1, color=(0, 255, 0), lineType=1) + if not dst is None: + frame = cv2.polylines(frame, [np.int32(dst)], True, 244, 3, cv2.LINE_AA) + cv2.imshow("figure", frame) + time.sleep(0.04) + connection.send(STOP_PROCESSING) + +def run(): + parent_conn, child_conn = Pipe() + child = Process(target = process_display_frame, args=(child_conn,)) + child.start() + + # Object_detect().take_photo() + # Object_detect().cut_photo() + # goal = Object_detect().distinguist() + + res_queue = [[], [], [], []] + res_queue[0] = parse_folder('res/red') + res_queue[1] = parse_folder('res/green') + res_queue[2] = parse_folder('res/blue') + res_queue[3] = parse_folder('res/gray') + + # res_queue = [] + # res_queue.extend(parse_folder('res/red')) + # res_queue.extend(parse_folder('res/green')) + # res_queue.extend(parse_folder('res/gray')) + # res_queue.extend(parse_folder('res/blue')) + + sift = cv2.xfeatures2d.SIFT_create() + kp_list, desc_list = compute_keypoints_and_descriptors(sift, res_queue) + + # init a class of Object_detect + detect = Object_detect() + # init mycobot + detect.run() + + # _init_ = 20 # + init_num = 0 + nparams = 0 + # num = 0 + # real_sx = real_sy = 0 + while True: + start_time = time.time() + if parent_conn.poll(): + data = parent_conn.recv() + if data == STOP_PROCESSING: + break + # read camera + frame = get_frame(parent_conn) + # 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, + ) + parent_conn.send((CROP_FRAME, + (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 mycobot + 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 + print ("ok") + continue + 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) + print("ok") + continue + + # get detect result + kp_img, desc_img = sift.detectAndCompute(frame, None) + frame = get_frame(parent_conn) + for i, v in enumerate(res_queue): + # HACK: to update frame every time + detect_result = detect.obj_detect(frame, v, kp_img, desc_img, kp_list[i], desc_list[i], parent_conn) + if detect_result: + x, y = detect_result + # calculate real coord between cube and mycobot + real_x, real_y = detect.get_position(x, y) + detect.color = i + detect.pub_marker(real_x / 1000.0, real_y / 1000.0) + detect.decide_move(real_x, real_y, detect.color) + # if num == 5: + # detect.color = i + # detect.pub_marker(real_sx / 5.0 / 1000.0, + # real_sy / 5.0 / 1000.0) + # detect.decide_move(real_sx / 5.0, real_sy / 5.0, + # detect.color) + # num = real_sx = real_sy = 0 + # else: + # num += 1 + # real_sy += real_y + # real_sx += real_x + parent_conn.send(CLEAR_DRAW) + + # cv2.imshow("figure", frame) + time.sleep(0.05) + end_time = time.time() + # print("loop_time = ", end_time - start_time) + + # close the window + if cv2.waitKey(1) & 0xFF == ord('q'): + # cap.release() + cv2.destroyAllWindows() + sys.exit() + child.join() + + +if __name__ == "__main__": + run() + # Object_detect().take_photo() + # Object_detect().cut_photo() diff --git a/mycobot_ai/ai_mycobot_280/scripts/test.py b/mycobot_ai/ai_mycobot_280/scripts/test.py index 816215a..0e4aead 100755 --- a/mycobot_ai/ai_mycobot_280/scripts/test.py +++ b/mycobot_ai/ai_mycobot_280/scripts/test.py @@ -1,14 +1,14 @@ # -*- coding: utf-8 -*- from pymycobot.mycobot import MyCobot from pymycobot.genre import Angle -from pymycobot import PI_PORT, PI_BAUD # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化 + # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化 import time mc = MyCobot("/dev/ttyACM0", 115200) # mc = MyCobot("/dev/ttyUSB0", 115200) # mc = MyCobot("/dev/ttyAMA0", 1000000) -mc.send_angles([0,0,0,0,0,0], 25) +# mc.send_angles([0,0,0,0,0,0], 25) # print(mc.get_angles()) # mc.send_angles([-7.11, -6.94, -55.01, -24.16, 0.0, -15], 30) # time.sleep(4) @@ -42,3 +42,16 @@ mc.send_angles([0,0,0,0,0,0], 25) # print("angles:%s"% mc.get_angles()) # print("coords:%s"% mc.get_coords()) # print("\n") +move_coords = [ + [132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # above the red bucket + [232.5, -124.6, 212.8, -169.94, -5.88, -97.63], # green + [115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # blue + [-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # gray + ] + +# mc.send_coords(move_coords[1],20, 1) +# mc.send_angles([-13.44, -57.3, 0.7, -22.76, -4.74, -6.76], 20) +mc.send_coords([238.8, -124.1, 204.3, -169.69, -5.52, -96.52], 20, 1) +time.sleep(3) +print(mc.get_angles()) +print(mc.get_coords()) \ No newline at end of file diff --git a/mycobot_ai/ai_mypalletizer_260/launch/vision_wio.launch b/mycobot_ai/ai_mypalletizer_260/launch/vision_wio.launch index 51eb78b..5548425 100644 --- a/mycobot_ai/ai_mypalletizer_260/launch/vision_wio.launch +++ b/mycobot_ai/ai_mypalletizer_260/launch/vision_wio.launch @@ -2,8 +2,8 @@ - - + + @@ -14,7 +14,7 @@ - + diff --git a/mycobot_ai/ai_mypalletizer_260/res/blue/goal9.jpeg b/mycobot_ai/ai_mypalletizer_260/res/blue/goal9.jpeg new file mode 100644 index 0000000..a370eb4 Binary files /dev/null and b/mycobot_ai/ai_mypalletizer_260/res/blue/goal9.jpeg differ diff --git a/mycobot_ai/ai_mypalletizer_260/res/gray/goal9.jpeg b/mycobot_ai/ai_mypalletizer_260/res/gray/goal9.jpeg new file mode 100644 index 0000000..8f63e91 Binary files /dev/null and b/mycobot_ai/ai_mypalletizer_260/res/gray/goal9.jpeg differ diff --git a/mycobot_ai/ai_mypalletizer_260/res/green/goal12.jpeg b/mycobot_ai/ai_mypalletizer_260/res/green/goal12.jpeg new file mode 100644 index 0000000..c2949f0 Binary files /dev/null and b/mycobot_ai/ai_mypalletizer_260/res/green/goal12.jpeg differ diff --git a/mycobot_ai/ai_mypalletizer_260/res/green/goal14.jpeg b/mycobot_ai/ai_mypalletizer_260/res/green/goal14.jpeg new file mode 100644 index 0000000..7778ecb Binary files /dev/null and b/mycobot_ai/ai_mypalletizer_260/res/green/goal14.jpeg differ diff --git a/mycobot_ai/ai_mypalletizer_260/res/red/goal8.jpeg b/mycobot_ai/ai_mypalletizer_260/res/red/goal8.jpeg new file mode 100644 index 0000000..abb5652 Binary files /dev/null and b/mycobot_ai/ai_mypalletizer_260/res/red/goal8.jpeg differ diff --git a/mycobot_ai/ai_mypalletizer_260/res/takephoto.jpeg b/mycobot_ai/ai_mypalletizer_260/res/takephoto.jpeg index 1798785..e529be2 100644 Binary files a/mycobot_ai/ai_mypalletizer_260/res/takephoto.jpeg and b/mycobot_ai/ai_mypalletizer_260/res/takephoto.jpeg differ diff --git a/mycobot_ai/ai_mypalletizer_260/scripts/advance_detect_obj_color.py b/mycobot_ai/ai_mypalletizer_260/scripts/advance_detect_obj_color.py new file mode 100644 index 0000000..6d3e902 --- /dev/null +++ b/mycobot_ai/ai_mypalletizer_260/scripts/advance_detect_obj_color.py @@ -0,0 +1,550 @@ +# -*- coding:utf-8 -*- +#!/usr/bin/env python2 +import cv2 +import numpy as np +import time +import os,sys +import rospy +from visualization_msgs.msg import Marker +from pymycobot.mypalletizer import MyPalletizer +from moving_utils import Movement + + +IS_CV_4 = cv2.__version__[0] == '4' +__version__ = "1.0" +# Adaptive seeed + + +class Object_detect(Movement): + + def __init__(self, camera_x = 160, camera_y = 10): + # inherit the parent class + super(Object_detect, self).__init__() + # get path of file + dir_path = os.path.dirname(__file__) + + # declare mypal260 + self.mc = None + + # 移动角度 + self.move_angles = [ + [0, 0, 0, 0], # init the point + [-29.0, 5.88, -4.92, -76.28], # point to grab + [17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab + ] + + # 移动坐标 + self.move_coords = [ + [132.6, -155.6, 211.8, -20.9], # above the red bucket + [232.5, -134.1, 197.7, -45.26], # above the green bucket + [111.6, 159, 221.5, -120], # above the blue bucket + [-15.9, 164.6, 217.5, -119.35], # above the gray bucket + ] + + # 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] + self.raspi = False + if "dev" in self.robot_m5: + self.Pin = [2, 5] + elif "dev" in self.robot_wio: + # self.Pin = [20, 21] + self.Pin = [2, 5] + for i in self.move_coords: + i[2] -= 20 + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + import RPi.GPIO as GPIO + GPIO.setwarnings(False) + self.GPIO = GPIO + GPIO.setmode(GPIO.BCM) + GPIO.setup(20, GPIO.OUT) + GPIO.setup(21, GPIO.OUT) + GPIO.output(20, 1) + GPIO.output(21, 1) + self.raspi = True + if self.raspi: + self.gpio_status(False) + # 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 + + self.HSV = { + "yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])], + "red": [np.array([0, 43, 46]), np.array([8, 255, 255])], + "green": [np.array([70, 100, 100]), np.array([90, 255, 255])], # [77, 255, 255] + "blue": [np.array([100, 43, 46]), np.array([124, 255, 255])], + "cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])], # np.array([78, 43, 46]), np.array([99, 255, 255]) + } + + # set color HSV + # self.HSV = { + # # "yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])], + # "yellow": [np.array([22, 93, 70]), np.array([45, 255, 245])], + # "red": [np.array([0, 43, 46]), np.array([8, 255, 255])], + # # "green": [np.array([35, 43, 46]), np.array([77, 255, 255])], + # "green": [np.array([35, 55, 30]), np.array([88, 255, 255])], + # "blue": [np.array([100, 43, 46]), np.array([124, 255, 255])], + # # "blue": [np.array([-10, 100, 100]), np.array([10, 255, 255])], + # "cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])], + # } + # self.HSV = { + # # "yellow": [np.array([11, 85, 70]), np.array([59, 255, 245])], + # "yellow": [np.array([22, 93, 0]), np.array([45, 255, 245])], + # "red": [np.array([0, 43, 46]), np.array([8, 255, 255])], + # "green": [np.array([35, 43, 35]), np.array([90, 255, 255])], + # # "blue": [np.array([100, 43, 46]), np.array([124, 255, 255])], + # "cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])], + # } + # use to calculate coord between cube and mypal260 + self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0 + # The coordinates of the grab center point relative to the mypal260 + self.camera_x, self.camera_y = camera_x, camera_y + # The coordinates of the cube relative to the mypal260 + 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() + + # 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 = "/joint1" + 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: + # self.GPIO.output(20, 0) + self.GPIO.output(21, 0) + else: + # self.GPIO.output(20, 1) + self.GPIO.output(21, 1) + + # 开启吸泵 m5 + def pump_on(self): + # 让2号位工作 + # self.mc.set_basic_output(2, 0) + # 让5号位工作 + self.mc.set_basic_output(5, 0) + + # 停止吸泵 m5 + def pump_off(self): + # 让2号位停止工作 + # self.mc.set_basic_output(2, 1) + # 让5号位停止工作 + self.mc.set_basic_output(5, 1) + + # Grasping motion + def move(self, x, y, color): + # send Angle to move mypal260 + print(color) + self.mc.send_angles(self.move_angles[0], 20) + time.sleep(3) + + # send coordinates to move mypal260 + self.mc.send_coords([x, y, 160, 0], 20, 0) + time.sleep(1.5) + self.mc.send_coords([x, y, 101, 0], 20, 0) + time.sleep(1.5) + + # open pump + if "dev" in self.robot_m5 or "dev" in self.robot_wio: + self.pump_on() + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + self.gpio_status(True) + time.sleep(1.5) + + self.mc.send_angle(2, 0, 20) + time.sleep(0.3) + self.mc.send_angle(3, -20, 20) + time.sleep(2) + + self.mc.send_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) + time.sleep(3) + + # close pump + if "dev" in self.robot_m5 or "dev" in self.robot_wio: + self.pump_off() + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + self.gpio_status(False) + time.sleep(6) + + if color == 1: + 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.mc.send_angles(self.move_angles[1], 20) + time.sleep(1.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 mypal260 + def run(self): + if "dev" in self.robot_m5: + self.mc = MyPalletizer(self.robot_m5, 115200) + elif "dev" in self.robot_wio: + self.mc = MyPalletizer(self.robot_wio, 115200) + elif "dev" in self.robot_raspi: + self.mc = MyPalletizer(self.robot_raspi, 1000000) + if not self.raspi: + print('start pump') + self.pub_pump(False, self.Pin) + print('end') + self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20) + time.sleep(3) + + # 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 mypal260 + 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 mypal260 + 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.2):int(self.y1*1.15), + # int(self.x1*0.7):int(self.x2*1.15)] + frame = frame[int(self.y2*0.78):int(self.y1*1.1), + int(self.x1*0.86):int(self.x2*1.08)] + return frame + + # detect cube color + def color_detect(self, img): + # set the arrangement of color'HSV + x = y = 0 + for mycolor, item in self.HSV.items(): + # print("mycolor:",mycolor) + redLower = np.array(item[0]) + redUpper = np.array(item[1]) + + # transfrom the img to model of gray + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + # print("hsv",hsv) + + # wipe off all color expect color in range + mask = cv2.inRange(hsv, item[0], item[1]) + + # a etching operation on a picture to remove edge roughness + erosion = cv2.erode(mask, np.ones((1, 1), 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) + + # adds pixels to the image + target = cv2.bitwise_and(img, img, mask=dilation) + + # the filtered image is transformed into a binary image and placed in binary + ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY) + + # get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected + contours, hierarchy = cv2.findContours( + dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + if len(contours) > 0: + # do something about misidentification + 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) + # get the lower left and upper right points of the positioning object + x, y, w, h = cv2.boundingRect(c) + # locate the target by drawing rectangle + cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2) + # calculate the rectangle center + x, y = (x*2+w)/2, (y*2+h)/2 + # calculate the real coordinates of mypal260 relative to the target + if mycolor == "red": + self.color = 0 + break + elif mycolor == "blue": + self.color = 2 + break + elif mycolor == "green": + self.color = 1 + break + elif mycolor == "cyan": + self.color = 2 + break + + elif mycolor == "yellow": + self.color = 3 + break + + # elif mycolor == "green": + # self.color = 1 + + # if mycolor == "red": + # self.color = 0 + # break + # elif mycolor == "green": + # self.color = 1 + # print('green') + # break + # elif mycolor == "cyan": + # self.color = 2 + # break + # elif mycolor == "blue": + # self.color = 2 + # break + # elif mycolor == "yellow": + # self.color = 3 + # break + # if mycolor == "yellow": + + # self.color = 3 + # print(mycolor) + # break + + # elif mycolor == "red": + # self.color = 0 + # print(mycolor) + # break + + # elif mycolor == "cyan" or mycolor == "blue": + # self.color = 2 + # print(mycolor) + # break + + # # elif mycolor == "blue": + # # self.color =2 + # # break + # else: + # self.color = 1 + # print(mycolor) + # # break + + 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) + if not cap.isOpened(): + cap.open() + # init a class of Object_detect + detect = Object_detect() + # init mypal260 + 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 mypal260 + 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 mypal260 + 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) + if detect_result is None: + cv2.imshow("figure", frame) + continue + else: + x, y = detect_result + # calculate real coord between cube and mypal260 + 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() \ No newline at end of file diff --git a/mycobot_ai/ai_mypalletizer_260/scripts/advance_detect_obj_img_folder.py b/mycobot_ai/ai_mypalletizer_260/scripts/advance_detect_obj_img_folder.py new file mode 100644 index 0000000..01a7153 --- /dev/null +++ b/mycobot_ai/ai_mypalletizer_260/scripts/advance_detect_obj_img_folder.py @@ -0,0 +1,628 @@ +#!/usr/bin/env python2 +# encoding:utf-8 +from multiprocessing import Process, Pipe +import cv2 +import numpy as np +import time +import os,sys +import rospy +from visualization_msgs.msg import Marker +from moving_utils import Movement +from pymycobot.mypalletizer import MyPalletizer + +IS_CV_4 = cv2.__version__[0] == '4' +__version__ = "1.0" # Adaptive seeed + + +class Object_detect(Movement): + + def __init__(self, camera_x = 160, camera_y = 10): + # inherit the parent class + super(Object_detect, self).__init__() + # get path of file + dir_path = os.path.dirname(__file__) + + # declare mypal260 + self.mc = None + # 移动角度 + self.move_angles = [ + [0, 0, 0, 0], # init the point + [-29.0, 5.88, -4.92, -76.28], # point to grab + [17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab + ] + + # 移动坐标 + self.move_coords = [ + [132.6, -155.6, 211.8, -20.9], # above the red bucket + [232.5, -134.1, 197.7, -45.26], # above the green bucket + [111.6, 159, 221.5, -120], # above the blue bucket + [-15.9, 164.6, 217.5, -119.35], # above the gray bucket + ] + # 判断连接设备:ttyUSB*为M5，ttyACM*为seeed + self.raspi = False + 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] + if "dev" in self.robot_m5: + self.Pin = [2, 5] + elif "dev" in self.robot_wio: + self.Pin = [2, 5] + for i in self.move_coords: + i[2] -= 20 + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + import RPi.GPIO as GPIO + GPIO.setwarnings(False) + self.GPIO = GPIO + GPIO.setmode(GPIO.BCM) + GPIO.setup(20, GPIO.OUT) + GPIO.setup(21, GPIO.OUT) + GPIO.output(20, 1) + GPIO.output(21, 1) + self.raspi = True + if self.raspi: + self.gpio_status(False) + # 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() + + + # 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 = "/joint1" + 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 + + self.cache_x = self.cache_y = 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: + self.GPIO.output(20, 0) + self.GPIO.output(21, 0) + else: + self.GPIO.output(20, 1) + self.GPIO.output(21, 1) + + # 开启吸泵 m5 + def pump_on(self): + # 让2号位工作 + # self.mc.set_basic_output(2, 0) + # 让5号位工作 + self.mc.set_basic_output(5, 0) + + # 停止吸泵 m5 + def pump_off(self): + # 让2号位停止工作 + # self.mc.set_basic_output(2, 1) + # 让5号位停止工作 + self.mc.set_basic_output(5, 1) + + # Grasping motion + def move(self, x, y, color): + print(color) + # send Angle to move mypal260 + self.mc.send_angles(self.move_angles[0], 20) + time.sleep(3) + + # send coordinates to move mypal260 根据不同底板机械臂，调整吸泵高度 + self.mc.send_coords([x, y, 160, 0], 20, 0) + time.sleep(1.5) + self.mc.send_coords([x, y, 101, 0], 20, 0) + time.sleep(1.5) + + # open pump + if "dev" in self.robot_m5 or "dev" in self.robot_wio: + self.pump_on() + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + self.gpio_status(True) + time.sleep(1.5) + + self.mc.send_angle(2, 0, 20) + time.sleep(0.3) + self.mc.send_angle(3, -18, 20) + time.sleep(2) + + self.mc.send_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) + time.sleep(3) + + # close pump + if "dev" in self.robot_m5 or "dev" in self.robot_wio: + self.pump_off() + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + self.gpio_status(False) + time.sleep(6) + + self.mc.send_angles(self.move_angles[1], 20) + time.sleep(1.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 mypal260 + def run(self): + if "dev" in self.robot_m5: + self.mc = MyPalletizer(self.robot_m5, 115200) + elif "dev" in self.robot_wio: + self.mc = MyPalletizer(self.robot_wio, 115200) + elif "dev" in self.robot_raspi: + self.mc = MyPalletizer(self.robot_raspi, 1000000) + if not self.raspi: + self.pub_pump(False, self.Pin) + self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20) + time.sleep(3) + + # 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 mycobot + 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 mycobot + 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.2):int(self.y1 * 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 + + # detect object + def obj_detect(self, img, goal, kp_img, desc_img, kp_list, desc_list, connection): + i = 0 + MIN_MATCH_COUNT = 5 + # sift = cv2.xfeatures2d.SIFT_create() + + # find the keypoints and descriptors with SIFT + # kp = [] + # des = [] + kp = kp_list + des = desc_list + + # for i in goal: + # kp0, des0 = sift.detectAndCompute(i, None) + # kp.append(kp0) + # des.append(des0) + + # kp1, des1 = sift.detectAndCompute(goal, None) + # kp2, des2 = sift.detectAndCompute(img, None) + kp2, des2 = kp_img, desc_img + + # FLANN parameters + FLANN_INDEX_KDTREE = 0 + index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5) + search_params = dict(checks=50) # or pass empty dictionary + flann = cv2.FlannBasedMatcher(index_params, search_params) + + x, y = 0, 0 + try: + for i in range(len(des)): + matches = flann.knnMatch(des[i], des2, k=2) + # store all the good matches as per Lowe's ratio test. 根据Lowe比率测试存储所有良好匹配项。 + good = [] + for m, n in matches: + if m.distance < 0.7 * n.distance: + good.append(m) + + # When there are enough robust matching point pairs 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时 + if len(good) > MIN_MATCH_COUNT: + + # extract corresponding point pairs from matching 从匹配中提取出对应点对 + # query index of small objects, training index of scenarios 小对象的查询索引，场景的训练索引 + src_pts = np.float32([kp[i][m.queryIdx].pt + for m in good]).reshape(-1, 1, 2) + dst_pts = np.float32([kp2[m.trainIdx].pt + for m in good]).reshape(-1, 1, 2) + + # Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵 + M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, + 5.0) + matchesMask = mask.ravel().tolist() + # Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置 + h, w, d = goal[i].shape + pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1], + [w - 1, 0]]).reshape(-1, 1, 2) + dst = cv2.perspectiveTransform(pts, M) + coord = (dst[0][0] + dst[1][0] + dst[2][0] + + dst[3][0]) / 4.0 + connection.send((DRAW_COORDS, coord)) + # cv2.putText(img, "{}".format(coord), (50, 60), + # fontFace=None, fontScale=1, + # color=(0, 255, 0), lineType=1) + print(format(dst[0][0][0])) + x = (dst[0][0][0] + dst[1][0][0] + dst[2][0][0] + + dst[3][0][0]) / 4.0 + y = (dst[0][0][1] + dst[1][0][1] + dst[2][0][1] + + dst[3][0][1]) / 4.0 + + # bound box 绘制边框 + # img = cv2.polylines(img, [np.int32(dst)], True, 244, 3, cv2.LINE_AA) + connection.send((DRAW_RECT, dst)) + # cv2.polylines(mixture, [np.int32(dst)], True, (0, 255, 0), 2, cv2.LINE_AA) + except Exception as e: + pass + + if x + y > 0: + return x, y + else: + return None + +# The path to save the image folder +def parse_folder(folder): + restore = [] + path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mypalletizer_260/' + folder + path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mypalletizer_260/' + folder + + if os.path.exists(path1): + path = path1 + elif os.path.exists(path2): + path = path2 + + for i, j, k in os.walk(path): + for l in k: + restore.append(cv2.imread(folder + '/{}'.format(l))) + return restore + +def compute_keypoints_and_descriptors(sift, images_lists): + kp_list = [] + desc_list = [] + for images in images_lists: + kp_tmp = [] + desc_tmp = [] + for img in images: + kp, desc = sift.detectAndCompute(img, None) + kp_tmp.append(kp) + desc_tmp.append(desc) + kp_list.append(kp_tmp) + desc_list.append(desc_tmp) + + return kp_list, desc_list + +GET_FRAME = 1 +STOP_PROCESSING = 2 +DRAW_COORDS = 3 +DRAW_RECT = 4 +CLEAR_DRAW = 5 +CROP_FRAME = 6 + +def get_frame(connection): + connection.send(GET_FRAME) + frame = connection.recv() + return frame + +def process_transform_frame(frame, x1, y1, x2, y2): + # 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 x1 != x2: + # the cutting ratio here is adjusted according to the actual situation +# frame = frame[int(y2 * 0.2):int(y1 * 1.15), +# int(x1 * 0.7):int(x2 * 1.15)] + return frame + +def process_display_frame(connection): + cap_num = 0 + coord = None + dst = None + x1 = 0 + y1 = 0 + x2 = 0 + y2 = 0 + cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L) + if not cap.isOpened(): + cap.open() + while cv2.waitKey(1) < 0: + _, frame = cap.read() + frame = process_transform_frame(frame, x1, y1, x2, y2) + if connection.poll(): + request = connection.recv() + if request == GET_FRAME: + connection.send(frame) + elif request == CLEAR_DRAW: + coord = None + dst = None + elif type(request) is tuple: + if request[0] == DRAW_COORDS: + coord = request[1] + elif request[0] == DRAW_RECT: + dst = request[1] + elif request[0] == CROP_FRAME: + x1 = request[1] + y1 = request[2] + x2 = request[3] + y2 = request[4] + + if not coord is None: + cv2.putText(frame, "{}".format(coord), (50, 60), fontFace=None, + fontScale=1, color=(0, 255, 0), lineType=1) + if not dst is None: + frame = cv2.polylines(frame, [np.int32(dst)], True, 244, 3, cv2.LINE_AA) + cv2.imshow("figure", frame) + time.sleep(0.04) + connection.send(STOP_PROCESSING) + +def run(): + parent_conn, child_conn = Pipe() + child = Process(target = process_display_frame, args=(child_conn,)) + child.start() + + res_queue = [[], [], [], []] + res_queue[0] = parse_folder('res/red') + res_queue[1] = parse_folder('res/green') + res_queue[2] = parse_folder('res/blue') + res_queue[3] = parse_folder('res/gray') + + sift = cv2.xfeatures2d.SIFT_create() + kp_list, desc_list = compute_keypoints_and_descriptors(sift, res_queue) + + # init a class of Object_detect + detect = Object_detect() + + # init mycobot + detect.run() + + # _init_ = 20 # + init_num = 0 + nparams = 0 + # num = 0 + # real_sx = real_sy = 0 + while True: + start_time = time.time() + if parent_conn.poll(): + data = parent_conn.recv() + if data == STOP_PROCESSING: + break + # read camera + frame = get_frame(parent_conn) + # 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, + ) + parent_conn.send((CROP_FRAME, + (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 mycobot + 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 + print ("ok") + continue + 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) + print("ok") + continue + + # get detect result + kp_img, desc_img = sift.detectAndCompute(frame, None) + frame = get_frame(parent_conn) + for i, v in enumerate(res_queue): + # HACK: to update frame every time + detect_result = detect.obj_detect(frame, v, kp_img, desc_img, kp_list[i], desc_list[i], parent_conn) + if detect_result: + x, y = detect_result + # calculate real coord between cube and mycobot + real_x, real_y = detect.get_position(x, y) + detect.color = i + detect.pub_marker(real_x / 1000.0, real_y / 1000.0) + detect.decide_move(real_x, real_y, detect.color) + # if num == 5: + # detect.color = i + # detect.pub_marker(real_sx / 5.0 / 1000.0, + # real_sy / 5.0 / 1000.0) + # detect.decide_move(real_sx / 5.0, real_sy / 5.0, + # detect.color) + # num = real_sx = real_sy = 0 + # else: + # num += 1 + # real_sy += real_y + # real_sx += real_x + parent_conn.send(CLEAR_DRAW) + + # cv2.imshow("figure", frame) + time.sleep(0.05) + end_time = time.time() + # print("loop_time = ", end_time - start_time) + + # close the window + if cv2.waitKey(1) & 0xFF == ord('q'): + # cap.release() + cv2.destroyAllWindows() + sys.exit() + + child.join() + + +if __name__ == "__main__": + run() + # Object_detect().take_photo() + # Object_detect().cut_photo() diff --git a/mycobot_ai/ai_mypalletizer_260/scripts/test.py b/mycobot_ai/ai_mypalletizer_260/scripts/test.py index d73de19..1d232e0 100755 --- a/mycobot_ai/ai_mypalletizer_260/scripts/test.py +++ b/mycobot_ai/ai_mypalletizer_260/scripts/test.py @@ -4,7 +4,7 @@ from pymycobot.genre import Angle from pymycobot import PI_PORT, PI_BAUD # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化 import time,os -mc = MyPalletizer(os.popen("ls /dev/ttyUSB*").readline()[:-1], 115200) +# mc = MyPalletizer(os.popen("ls /dev/ttyUSB*").readline()[:-1], 115200) # mc = MyPalletizer("/dev/ttyAMA0", 1000000) # mc.send_angles([-29.0, 5.88, -4.92, -76.28],25) # init the point coords:[155.3, -86.1, 218.4, -47.28] @@ -13,8 +13,8 @@ mc = MyPalletizer(os.popen("ls /dev/ttyUSB*").readline()[:-1], 115200) # mc.send_angles([-47.1, 10.19, -10.1, -76.37],25) # above the red bucket; coords:[127.3, -137.1, 219.2, -29.26] # time.sleep(1.5) -mc.send_angles([0,0,-15,0],25) -time.sleep(2) +# mc.send_angles([0,0,-15,0],25) +# time.sleep(2) # mc.send_coords([141.2, -142.0, 206.2, -26.8],25,1) # above the red bucket # time.sleep(2) @@ -38,4 +38,27 @@ time.sleep(2) # mc.set_servo_calibration(1) # mc.set_servo_calibration(2) # mc.set_servo_calibration(3) -# mc.set_servo_calibration(4) \ No newline at end of file +# mc.set_servo_calibration(4) + +import sys +import numpy as np +import cv2 + +print("Please enter blue:") +blue = input() +print("Please enter green:") +green = input() +print("Please enter red:") +red = input() + +color = np.uint8([[[blue, green, red]]]) +hsv_color = cv2.cvtColor(color, cv2.COLOR_BGR2HSV) + +hue = hsv_color[0][0][0] + +print("Lower bound is :") +print("[" + str(hue - 10) + ", 100, 100]\n") + +print("Upper bound is :"), +print("[" + str(hue + 10) + ", 255, 255]") + diff --git a/mypalletizer_260/mypalletizer_260/config/mypal_260.rviz b/mypalletizer_260/mypalletizer_260/config/mypal_260.rviz index 4e1a0a5..2c84dd7 100644 --- a/mypalletizer_260/mypalletizer_260/config/mypal_260.rviz +++ b/mypalletizer_260/mypalletizer_260/config/mypal_260.rviz @@ -8,8 +8,9 @@ Panels: - /Status1 - /RobotModel1 - /TF1 + - /Marker1 Splitter Ratio: 0.5 - Tree Height: 601 + Tree Height: 607 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -18,7 +19,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.5886790156364441 + Splitter Ratio: 0.588679016 - Class: rviz/Views Expanded: - /Current View1 @@ -29,8 +30,6 @@ Panels: Name: Time SyncMode: 0 SyncSource: "" -Preferences: - PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: @@ -42,7 +41,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.029999999329447746 + Line Width: 0.0299999993 Value: Lines Name: Grid Normal Cell Count: 0 @@ -69,6 +68,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + env: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true link1: Alpha: 1 Show Axes: false @@ -107,6 +111,8 @@ Visualization Manager: All Enabled: true base: Value: true + env: + Value: true link1: Value: true link2: @@ -117,13 +123,15 @@ Visualization Manager: Value: true link5: Value: true - Marker Scale: 0.30000001192092896 + Marker Scale: 0.300000012 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: base: + env: + {} link1: link2: link3: @@ -132,6 +140,14 @@ Visualization Manager: {} Update Interval: 0 Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /cube + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -147,10 +163,7 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -160,33 +173,33 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.2028908729553223 + Distance: 1.20289087 Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 + Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 0 Y: 0 - Z: 0.10758385062217712 + Z: 0.107583851 Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 + Focal Shape Size: 0.0500000007 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.42039862275123596 + Near Clip Distance: 0.00999999978 + Pitch: 0.420398623 Target Frame: Value: Orbit (rviz) - Yaw: 1.1754094362258911 + Yaw: 1.17540944 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 892 + Height: 888 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002e2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001a4000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002e2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006200000003efc0100000002fb0000000800540069006d00650100000000000006200000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000306000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a000002eefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002ee000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001a4000002eefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002ee000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006200000003efc0100000002fb0000000800540069006d00650100000000000006200000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000306000002ee00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -198,201 +211,3 @@ Window Geometry: Width: 1568 X: 144 Y: 41 - -# Panels: -# - Class: rviz/Displays -# Help Height: 78 -# Name: Displays -# Property Tree Widget: -# Expanded: -# - /Global Options1 -# - /Status1 -# - /RobotModel1 -# - /TF1 -# Splitter Ratio: 0.5 -# Tree Height: 775 -# - Class: rviz/Selection -# Name: Selection -# - Class: rviz/Tool Properties -# Expanded: -# - /2D Pose Estimate1 -# - /2D Nav Goal1 -# - /Publish Point1 -# Name: Tool Properties -# Splitter Ratio: 0.588679016 -# - Class: rviz/Views -# Expanded: -# - /Current View1 -# Name: Views -# Splitter Ratio: 0.5 -# - Class: rviz/Time -# Experimental: false -# Name: Time -# SyncMode: 0 -# SyncSource: "" -# Toolbars: -# toolButtonStyle: 2 -# Visualization Manager: -# Class: "" -# Displays: -# - Alpha: 0.5 -# Cell Size: 1 -# Class: rviz/Grid -# Color: 160; 160; 164 -# Enabled: true -# Line Style: -# Line Width: 0.0299999993 -# Value: Lines -# Name: Grid -# Normal Cell Count: 0 -# Offset: -# X: 0 -# Y: 0 -# Z: 0 -# Plane: XY -# Plane Cell Count: 10 -# Reference Frame: -# Value: true -# - Alpha: 1 -# Class: rviz/RobotModel -# Collision Enabled: false -# Enabled: true -# Links: -# All Links Enabled: true -# Expand link Details: false -# Expand Link Details: false -# Expand Tree: false -# Link Tree Style: Links in Alphabetic Order -# base: -# Alpha: 1 -# Show Axes: false -# Show Trail: false -# Value: true -# link1: -# Alpha: 1 -# Show Axes: false -# Show Trail: false -# Value: true -# link2: -# Alpha: 1 -# Show Axes: false -# Show Trail: false -# Value: true -# link3: -# Alpha: 1 -# Show Axes: false -# Show Trail: false -# Value: true -# link4: -# Alpha: 1 -# Show Axes: false -# Show Trail: false -# Value: true -# link5: -# Alpha: 1 -# Show Axes: false -# Show Trail: false -# Value: true - -# Name: RobotModel -# Robot Description: robot_description -# TF Prefix: "" -# Update Interval: 0 -# Value: true -# Visual Enabled: true -# - Class: rviz/TF -# Enabled: true -# Frame Timeout: 15 -# Frames: -# All Enabled: true -# base: -# Value: true -# link1: -# Value: true -# link2: -# Value: true -# link3: -# Value: true -# link4: -# Value: true -# link5: -# Value: true - -# Marker Scale: 0.300000012 -# Name: TF -# Show Arrows: true -# Show Axes: true -# Show Names: true -# Tree: -# base: -# link1: -# link2: -# link3: -# link4: -# link5: -# {} -# Update Interval: 0 -# Value: true -# Enabled: true -# Global Options: -# Background Color: 48; 48; 48 -# Default Light: true -# Fixed Frame: base -# Frame Rate: 30 -# Name: root -# Tools: -# - Class: rviz/Interact -# Hide Inactive Objects: true -# - Class: rviz/MoveCamera -# - Class: rviz/Select -# - Class: rviz/FocusCamera -# - Class: rviz/Measure -# - Class: rviz/SetInitialPose -# Topic: /initialpose -# - Class: rviz/SetGoal -# Topic: /move_base_simple/goal -# - Class: rviz/PublishPoint -# Single click: true -# Topic: /clicked_point -# Value: true -# Views: -# Current: -# Class: rviz/Orbit -# Distance: 1.20289087 -# Enable Stereo Rendering: -# Stereo Eye Separation: 0.0599999987 -# Stereo Focal Distance: 1 -# Swap Stereo Eyes: false -# Value: false -# Focal Point: -# X: 0 -# Y: 0 -# Z: 0.107583851 -# Focal Shape Fixed Size: true -# Focal Shape Size: 0.0500000007 -# Invert Z Axis: false -# Name: Current View -# Near Clip Distance: 0.00999999978 -# Pitch: 0.440398335 -# Target Frame: -# Value: Orbit (rviz) -# Yaw: 0.430389732 -# Saved: ~ -# Window Geometry: -# Displays: -# collapsed: false -# Height: 1056 -# Hide Left Dock: false -# Hide Right Dock: false -# QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 -# Selection: -# collapsed: false -# Time: -# collapsed: false -# Tool Properties: -# collapsed: false -# Views: -# collapsed: false -# Width: 1855 -# X: 65 -# Y: 24 diff --git a/mypalletizer_260/mypalletizer_communication/launch/communication_topic.launch b/mypalletizer_260/mypalletizer_communication/launch/communication_topic.launch index 6cf9d22..3966f37 100644 --- a/mypalletizer_260/mypalletizer_communication/launch/communication_topic.launch +++ b/mypalletizer_260/mypalletizer_communication/launch/communication_topic.launch @@ -1,5 +1,5 @@ - + diff --git a/mypalletizer_260/mypalletizer_communication/scripts/mypal_topics.py b/mypalletizer_260/mypalletizer_communication/scripts/mypal_topics.py index 49deb6e..0655778 100755 --- a/mypalletizer_260/mypalletizer_communication/scripts/mypal_topics.py +++ b/mypalletizer_260/mypalletizer_communication/scripts/mypal_topics.py @@ -67,11 +67,17 @@ class MypalTopics(object): rospy.init_node("mypal_topics") rospy.loginfo("start ...") - port = rospy.get_param("~port", os.popen("ls /dev/ttyUSB*").readline()[:-1]) - print(port) - baud = rospy.get_param("~baud", 115200) - rospy.loginfo("%s,%s" % (port, baud)) - self.mc = MyPalletizer(port, baud) + port_m5 = rospy.get_param("~port", os.popen("ls /dev/ttyUSB*").readline()[:-1]) + # if not port: + port_wio = rospy.get_param("~port", os.popen("ls /dev/ttyACM*").readline()[:-1]) + if "dev" in port_m5: + baud = rospy.get_param("~baud", 115200) + rospy.loginfo("%s,%s" % (port_m5, baud)) + self.mc = MyPalletizer(port_m5, baud) + elif "dev" in port_wio: + baud = rospy.get_param("~baud", 115200) + rospy.loginfo("%s,%s" % (port_wio, baud)) + self.mc = MyPalletizer(port_wio, baud) self.lock = threading.Lock() def start(self):