diff --git a/mycobot_ai/mechArm_270/CMakeLists.txt b/mycobot_ai/mechArm_270/CMakeLists.txt index a597ed9..cdbf847 100644 --- a/mycobot_ai/mechArm_270/CMakeLists.txt +++ b/mycobot_ai/mechArm_270/CMakeLists.txt @@ -9,6 +9,7 @@ project(mechArm_270) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS mecharm_pi + mecharm ) ## System dependencies are found with CMake's conventions diff --git a/mycobot_ai/mechArm_270/launch/vision_m5.launch b/mycobot_ai/mechArm_270/launch/vision_m5.launch new file mode 100644 index 0000000..4805952 --- /dev/null +++ b/mycobot_ai/mechArm_270/launch/vision_m5.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + ["joint_states"] + + + + + + + + + + + + + + diff --git a/mycobot_ai/mechArm_270/launch/vision.launch b/mycobot_ai/mechArm_270/launch/vision_pi.launch similarity index 66% rename from mycobot_ai/mechArm_270/launch/vision.launch rename to mycobot_ai/mechArm_270/launch/vision_pi.launch index a01ed45..df576e0 100644 --- a/mycobot_ai/mechArm_270/launch/vision.launch +++ b/mycobot_ai/mechArm_270/launch/vision_pi.launch @@ -2,8 +2,8 @@ - - + + @@ -13,14 +13,14 @@ ["joint_states"] - - + + - + diff --git a/mycobot_ai/mechArm_270/launch/vision_wio.launch b/mycobot_ai/mechArm_270/launch/vision_wio.launch index 654f540..51eb78b 100644 --- a/mycobot_ai/mechArm_270/launch/vision_wio.launch +++ b/mycobot_ai/mechArm_270/launch/vision_wio.launch @@ -3,7 +3,7 @@ - + @@ -20,7 +20,7 @@ - + diff --git a/mycobot_ai/mechArm_270/package.xml b/mycobot_ai/mechArm_270/package.xml index 31da1fc..fb15204 100644 --- a/mycobot_ai/mechArm_270/package.xml +++ b/mycobot_ai/mechArm_270/package.xml @@ -42,7 +42,8 @@ - + @@ -53,6 +54,9 @@ mecharm_pi mecharm_pi + mecharm + mecharm + mecharm diff --git a/mycobot_ai/mechArm_270/res/blue/goal3.jpeg b/mycobot_ai/mechArm_270/res/blue/goal3.jpeg new file mode 100644 index 0000000..7af2ae1 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/blue/goal3.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/blue/goal4.jpeg b/mycobot_ai/mechArm_270/res/blue/goal4.jpeg new file mode 100644 index 0000000..e499bbb Binary files /dev/null and b/mycobot_ai/mechArm_270/res/blue/goal4.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/blue/goal5.jpeg b/mycobot_ai/mechArm_270/res/blue/goal5.jpeg new file mode 100644 index 0000000..c6061be Binary files /dev/null and b/mycobot_ai/mechArm_270/res/blue/goal5.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/blue/goal6.jpeg b/mycobot_ai/mechArm_270/res/blue/goal6.jpeg new file mode 100644 index 0000000..037a4a1 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/blue/goal6.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/blue/goal7.jpeg b/mycobot_ai/mechArm_270/res/blue/goal7.jpeg new file mode 100644 index 0000000..d7e1970 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/blue/goal7.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/blue/goal8.jpeg b/mycobot_ai/mechArm_270/res/blue/goal8.jpeg new file mode 100644 index 0000000..a49591f Binary files /dev/null and b/mycobot_ai/mechArm_270/res/blue/goal8.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/gray/goal3.jpeg b/mycobot_ai/mechArm_270/res/gray/goal3.jpeg new file mode 100644 index 0000000..1d3ddcb Binary files /dev/null and b/mycobot_ai/mechArm_270/res/gray/goal3.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/gray/goal4.jpeg b/mycobot_ai/mechArm_270/res/gray/goal4.jpeg new file mode 100644 index 0000000..e10f0f4 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/gray/goal4.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/gray/goal5.jpeg b/mycobot_ai/mechArm_270/res/gray/goal5.jpeg new file mode 100644 index 0000000..5750a65 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/gray/goal5.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/gray/goal6.jpeg b/mycobot_ai/mechArm_270/res/gray/goal6.jpeg new file mode 100644 index 0000000..32c5343 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/gray/goal6.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/gray/goal7.jpeg b/mycobot_ai/mechArm_270/res/gray/goal7.jpeg new file mode 100644 index 0000000..519d153 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/gray/goal7.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/gray/goal8.jpeg b/mycobot_ai/mechArm_270/res/gray/goal8.jpeg new file mode 100644 index 0000000..8ef7c4a Binary files /dev/null and b/mycobot_ai/mechArm_270/res/gray/goal8.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/green/goal10.jpeg b/mycobot_ai/mechArm_270/res/green/goal10.jpeg new file mode 100644 index 0000000..e63eb35 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/green/goal10.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/green/goal11.jpeg b/mycobot_ai/mechArm_270/res/green/goal11.jpeg new file mode 100644 index 0000000..b1242b0 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/green/goal11.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/green/goal5.jpeg b/mycobot_ai/mechArm_270/res/green/goal5.jpeg new file mode 100644 index 0000000..b4e7578 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/green/goal5.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/green/goal6.jpeg b/mycobot_ai/mechArm_270/res/green/goal6.jpeg new file mode 100644 index 0000000..fe5bff1 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/green/goal6.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/green/goal7.jpeg b/mycobot_ai/mechArm_270/res/green/goal7.jpeg new file mode 100644 index 0000000..0353ba3 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/green/goal7.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/green/goal8.jpeg b/mycobot_ai/mechArm_270/res/green/goal8.jpeg new file mode 100644 index 0000000..241727e Binary files /dev/null and b/mycobot_ai/mechArm_270/res/green/goal8.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/green/goal9.jpeg b/mycobot_ai/mechArm_270/res/green/goal9.jpeg new file mode 100644 index 0000000..2946c5b Binary files /dev/null and b/mycobot_ai/mechArm_270/res/green/goal9.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/red/goal2.jpeg b/mycobot_ai/mechArm_270/res/red/goal2.jpeg new file mode 100644 index 0000000..10787b9 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/red/goal2.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/red/goal3.jpeg b/mycobot_ai/mechArm_270/res/red/goal3.jpeg new file mode 100644 index 0000000..519460f Binary files /dev/null and b/mycobot_ai/mechArm_270/res/red/goal3.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/red/goal4.jpeg b/mycobot_ai/mechArm_270/res/red/goal4.jpeg new file mode 100644 index 0000000..11f77b3 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/red/goal4.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/red/goal5.jpeg b/mycobot_ai/mechArm_270/res/red/goal5.jpeg new file mode 100644 index 0000000..1ffa851 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/red/goal5.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/red/goal6.jpeg b/mycobot_ai/mechArm_270/res/red/goal6.jpeg new file mode 100644 index 0000000..4a4f9a5 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/red/goal6.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/red/goal7.jpeg b/mycobot_ai/mechArm_270/res/red/goal7.jpeg new file mode 100644 index 0000000..ca20e77 Binary files /dev/null and b/mycobot_ai/mechArm_270/res/red/goal7.jpeg differ diff --git a/mycobot_ai/mechArm_270/res/takephoto.jpeg b/mycobot_ai/mechArm_270/res/takephoto.jpeg index 2a29a1e..1798785 100644 Binary files a/mycobot_ai/mechArm_270/res/takephoto.jpeg and b/mycobot_ai/mechArm_270/res/takephoto.jpeg differ diff --git a/mycobot_ai/mechArm_270/scripts/add_img.py b/mycobot_ai/mechArm_270/scripts/add_img.py index 4d38ec6..6ff25b9 100755 --- a/mycobot_ai/mechArm_270/scripts/add_img.py +++ b/mycobot_ai/mechArm_270/scripts/add_img.py @@ -49,19 +49,19 @@ def take_photo(): def cut_photo(): - path_red = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/red' + path_red = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/myPalletizer_260/res/red' for i, j, k in os.walk(path_red): file_len_red = len(k) - path_gray = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/gray' + path_gray = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/myPalletizer_260/res/gray' for i, j, k in os.walk(path_gray): file_len_gray = len(k) - path_green = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/green' + path_green = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/myPalletizer_260/res/green' for i, j, k in os.walk(path_green): file_len_green = len(k) - path_blue = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/blue' + path_blue = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/myPalletizer_260/res/blue' for i, j, k in os.walk(path_blue): file_len_blue = len(k) print("请截取要识别的部分") @@ -95,19 +95,19 @@ def cut_photo(): cv2.imshow('crop', crop) # 选择红桶文件夹 if kw == 'red': - cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/red/goal{}.jpeg'.format(str(file_len_red + 1)),crop) + cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/myPalletizer_260/res/red/goal{}.jpeg'.format(str(file_len_red + 1)),crop) print('Saved') # 选择灰桶文件夹 elif kw == 'gray': - cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/gray/goal{}.jpeg'.format(str(file_len_gray+1)),crop) + cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/myPalletizer_260/res/gray/goal{}.jpeg'.format(str(file_len_gray+1)),crop) print('Saved') # 选择绿桶文件夹 elif kw == 'green': - cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/green/goal{}.jpeg'.format(str(file_len_green+1)),crop) + cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/myPalletizer_260/res/green/goal{}.jpeg'.format(str(file_len_green+1)),crop) print('Saved') # 选择蓝桶文件夹 elif kw == 'blue': - cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/blue/goal{}.jpeg'.format(str(file_len_blue+1)),crop) + cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/myPalletizer_260/res/blue/goal{}.jpeg'.format(str(file_len_blue+1)),crop) print('Saved') # 退出 diff --git a/mycobot_ai/mechArm_270/scripts/detect_obj_img.py b/mycobot_ai/mechArm_270/scripts/combine_detect_obj_color.py old mode 100755 new mode 100644 similarity index 51% rename from mycobot_ai/mechArm_270/scripts/detect_obj_img.py rename to mycobot_ai/mechArm_270/scripts/combine_detect_obj_color.py index 47bc937..cf95c53 --- a/mycobot_ai/mechArm_270/scripts/detect_obj_img.py +++ b/mycobot_ai/mechArm_270/scripts/combine_detect_obj_color.py @@ -1,49 +1,50 @@ -# encoding:utf-8 +# -*- coding:utf-8 -*- #!/usr/bin/env python2 - +from operator import imod from tokenize import Pointfloat import cv2 import numpy as np import time import json -import os +import os,sys import rospy -from visualization_msgs.msg import Marker -from PIL import Image -from threading import Thread -import tkFileDialog as filedialog -import Tkinter as tk +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 +__version__ = "1.0" +# Adaptive seeed class Object_detect(Movement): - def __init__(self, camera_x=150, camera_y=-10): + + 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 = [ - [-7.11, -6.94, -55.01, -24.16, 0, 15], # init the point - [-1.14, -10.63, -87.8, 9.05, -3.07, 15], # point to grab + [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 = [ - [120.1, -141.6, 240.9, -173.34, -8.15, -110.11], # above the red bucket - # above the yello bucket - #[208.2, -127.8, 260.9, -157.51, -17.5, -71.18], - [205.6, -130.5, 263.0, -150.99, -0.07, -107.35], - [209.7, -18.6, 230.4, -168.48, -9.86, -39.38], - [196.9, -64.7, 232.6, -166.66, -9.44, -52.47], - [126.6, -118.1, 305.0, -157.57, -13.72, -75.3], + [141.2, -142.0, 210, -26.8], # above the red bucket + [234.3, -120, 210, -48.77], # above the green bucket + [100.9, 159.3, 248.6, -124.27], # above the blue bucket + [-17.6, 161.6, 238.4, -152.31], # above the gray bucket ] - # 判断连接设备:ttyUSB*为M5，ttyACM*为seeed + + # 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] @@ -57,37 +58,35 @@ class Object_detect(Movement): i[2] -= 20 elif "dev" in self.robot_raspi or "dev" in self.robot_jes: import RPi.GPIO as GPIO - self.GPIO = 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) - self.Pin = [2, 5] # 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 + # 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 mypal260 self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0 - # The coordinates of the grab center point relative to the mycobot + # 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 mycobot + # 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 @@ -96,11 +95,6 @@ class Object_detect(Movement): # 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) @@ -126,11 +120,7 @@ class Object_detect(Movement): 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 @@ -139,6 +129,7 @@ class Object_detect(Movement): 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) @@ -146,80 +137,69 @@ class Object_detect(Movement): 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 - self.pub_angles(self.move_angles[0], 20) - time.sleep(1.5) - self.pub_angles(self.move_angles[1], 20) - time.sleep(1.5) - self.pub_angles(self.move_angles[2], 20) - time.sleep(1.5) - # send coordinates to move mycobot - self.pub_coords([x, y, 165, -178.9, -1.57, -66], 20, 1) - time.sleep(1.5) - # 根据不同底板机械臂，调整吸泵高度 - if "dev" in self.robot_m5: - # m5 and jetson nano - self.pub_coords([x, y, 90, -178.9, -1.57, -66], 25, 1) - elif "dev" in self.robot_wio: - h = 0 - if 165 < x < 180: - h = 10 - elif x > 180: - h = 20 - elif x < 135: - h = -20 - self.pub_coords([x, y, 31.9+h, -178.9, -1, -66], 20, 1) - elif "dev" in self.robot_jes: - h = 0 - if x<130: - h=15 - self.pub_coords([x, y, 90-h, -178.9, -1.57, -66], 25, 1) - time.sleep(1.5) - # open pump - if self.raspi: - self.gpio_status(True) - else: - self.pub_pump(True, self.Pin) - time.sleep(0.5) - self.pub_angles(self.move_angles[2], 20) + # send Angle to move mypal260 + print(color) + self.mc.send_angles(self.move_angles[0], 20) time.sleep(3) - self.pub_marker( - self.move_coords[2][0]/1000.0, self.move_coords[2][1]/1000.0, self.move_coords[2][2]/1000.0) - self.pub_angles(self.move_angles[1], 20) + # 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, 90, 0], 20, 0) time.sleep(1.5) - self.pub_marker( - self.move_coords[3][0]/1000.0, self.move_coords[3][1]/1000.0, self.move_coords[3][2]/1000.0) - self.pub_angles(self.move_angles[0], 20) + # open pump + if "dev" in self.robot_m5: + self.pump_on() + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: + self.gpio_status(True) time.sleep(1.5) - self.pub_marker( - self.move_coords[4][0]/1000.0, self.move_coords[4][1]/1000.0, self.move_coords[4][2]/1000.0) - print self.move_coords[color] - self.pub_coords(self.move_coords[color], 20, 1) + + self.mc.send_angle(2, 0, 20) + time.sleep(0.3) + self.mc.send_angle(3, -15, 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(2) + time.sleep(3) + # close pump - if self.raspi: + if "dev" in self.robot_m5: + self.pump_off() + elif "dev" in self.robot_raspi or "dev" in self.robot_jes: self.gpio_status(False) - else: - self.pub_pump(False, self.Pin) - time.sleep(1) + 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.pub_angles(self.move_angles[0], 20) - time.sleep(3) + + 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 @@ -229,43 +209,23 @@ class Object_detect(Movement): else: self.cache_x = self.cache_y = 0 # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动 - if "dev" in self.robot_wio: - if (y < -30 and x > 140) or (x > 150 and y < -10): - x -= 10 - y += 10 - elif y > -10: - y += 10 - elif x > 170: - x -= 10 - y += 10 - elif "dev" in self.robot_m5: - y += 10 - x -= 15 - if y < -20: - y += 5 - # print x,y - elif "dev" in self.robot_jes: - if y<0: - x+=5 - y+=3 - y+=10 - print x,y self.move(x, y, color) - # init mycobot + # init mypal260 def run(self): + if "dev" in self.robot_m5: + self.mc = MyPalletizer(self.robot_m5, 115200) + elif "dev" in self.robot_raspi: + self.mc = MyPalletizer(self.robot_raspi, 1000000) if not self.raspi: self.pub_pump(False, self.Pin) - for _ in range(5): - self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -15], 20) - print(_) - time.sleep(0.5) + 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( + cv2.rectangle( img, (x - 20, y - 20), (x + 20, y + 20), @@ -313,13 +273,13 @@ class Object_detect(Movement): self.y2 = int(y2) print(self.x1, self.y1, self.x2, self.y2) - # set parameters to calculate the coords between cube and mycobot + # 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 mycobot + # 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) @@ -328,7 +288,6 @@ class Object_detect(Movement): 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 @@ -341,160 +300,88 @@ class Object_detect(Movement): 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 + # 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]) - def obj_detect(self, img, goal): - # rows, cols = frame.shape[:-1] - # Resize image and swap BGR to RGB. - # blob = cv2.dnn.blobFromImage( - # frame, - # size=(300, 300), - # mean=(0, 0, 0), - # swapRB=True, - # crop=False, - # ) + # transfrom the img to model of gray + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + # print("hsv",hsv) - # Detecting. - # self.net.setInput(blob) - # out = self.net.forward() - # x, y = 0, 0 + # wipe off all color expect color in range + mask = cv2.inRange(hsv, item[0], item[1]) - # Processing result. - # for detection in out[0, 0, :, :]: - # score = float(detection[2]) - # if score > 0.3: - # class_id = detection[1] - # left = detection[3] * cols - # top = detection[4] * rows - # right = detection[5] * cols - # bottom = detection[6] * rows - # if abs(right + bottom - left - top) > 380: - # continue - # x, y = (left + right) / 2.0, (top + bottom) / 2.0 - # cv2.rectangle( - # frame, - # (int(left), int(top)), - # (int(right), int(bottom)), - # (0, 230, 0), - # thickness=2, - # ) - # cv2.putText( - # frame, - # "{}: {}%".format(self.id_class_name(class_id),round(score * 100, 2)), - # (int(left), int(top) - 10), - # cv2.FONT_HERSHEY_COMPLEX_SMALL, - # 1, - # (243, 0, 0), - # 2, - # ) - i = 0 - MIN_MATCH_COUNT = 10 - sift = cv2.xfeatures2d.SIFT_create() + # a etching operation on a picture to remove edge roughness + erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2) - # find the keypoints and descriptors with SIFT - kp = [] - des = [] + # 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) - 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) + # adds pixels to the image + target = cv2.bitwise_and(img, img, mask=dilation) - # 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) + # the filtered image is transformed into a binary image and placed in binary + ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY) - 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) + # 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) - # When there are enough robust matching point pairs 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时 - if len(good) > MIN_MATCH_COUNT: + 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 + elif mycolor == "green": + self.color = 1 + elif mycolor == "cyan": + self.color = 2 + else: + self.color = 3 - # 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) - ccoord = (dst[0][0]+dst[1][0]+dst[2][0]+dst[3][0])/4.0 - cv2.putText(img, "{}".format(ccoord), (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) - # cv2.polylines(mixture, [np.int32(dst)], True, (0, 255, 0), 2, cv2.LINE_AA) - except Exception as e: - pass - - # else: - # if(len(good) < MIN_MATCH_COUNT): - - # i += 1 - # if(i % 10 == 0): - # print("Not enough matches are found - %d/%d" % - # (len(good), MIN_MATCH_COUNT)) - - # matchesMask = None - if x+y > 0: + if abs(x) + abs(y) > 0: return x, y else: return None +if __name__ == "__main__": -def run(): - - # Object_detect().take_photo() - # Object_detect().cut_photo() - # goal = Object_detect().distinguist() - goal = [] - path = os.getcwd()+'/local_photo/img' - - for i, j, k in os.walk(path): - for l in k: - goal.append(cv2.imread('local_photo/img/{}'.format(l))) - + # 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 + # init mypal260 detect.run() - _init_ = 20 # + _init_ = 20 init_num = 0 nparams = 0 num = 0 @@ -504,10 +391,10 @@ def run(): _, 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: @@ -534,7 +421,7 @@ def run(): init_num += 1 continue - # calculate params of the coords between cube and mycobot + # calculate params of the coords between cube and mypal260 if nparams < 10: if detect.get_calculate_params(frame) is None: cv2.imshow("figure", frame) @@ -551,27 +438,28 @@ def run(): continue elif nparams == 10: nparams += 1 - # calculate and set params of calculating real coord between cube and mycobot + # 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" + print("ok") continue + # get detect result - detect_result = detect.obj_detect(frame, goal) + 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 + # calculate real coord between cube and mypal260 real_x, real_y = detect.get_position(x, y) - if num == 5: - 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) + 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: @@ -581,8 +469,8 @@ def run(): cv2.imshow("figure", frame) - -if __name__ == "__main__": - run() - # Object_detect().take_photo() - # Object_detect().cut_photo() + # 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/mechArm_270/scripts/combine_detect_obj_img_folder_opt.py b/mycobot_ai/mechArm_270/scripts/combine_detect_obj_img_folder_opt.py new file mode 100755 index 0000000..c40ffbe --- /dev/null +++ b/mycobot_ai/mechArm_270/scripts/combine_detect_obj_img_folder_opt.py @@ -0,0 +1,667 @@ +#!/usr/bin/env python2 +# encoding:utf-8 +from multiprocessing import Process, Pipe +from cgi import parse +from difflib import restore +# import queue +from sys import path +from tokenize import Pointfloat +from turtle import color +# from typing_extensions import Self +import cv2 +import numpy as np +import time +import json +import os,sys +import rospy +from visualization_msgs.msg import Marker +from PIL import Image +from threading import Thread +import tkFileDialog as filedialog +import Tkinter as tk +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 = [ + [141.2, -142.0, 210, -26.8], # above the red bucket + [234.3, -120, 210, -48.77], # above the green bucket + [100.9, 159.3, 248.6, -124.27], # above the blue bucket + [-17.6, 161.6, 238.4, -152.31], # above the gray bucket + ] + + # 判断连接设备:ttyUSB*为M5，ttyACM*为seeed + self.raspi = False + # import RPi.GPIO as GPIO + # self.GPIO = GPIO + # GPIO.setwarnings(False) + # GPIO.setmode(GPIO.BCM) + # GPIO.setup(20, GPIO.OUT) + # GPIO.setup(21, GPIO.OUT) + + # self.gpio_status(False) + # self.Pin = [2, 5] + 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] + 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) + # 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 + 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, 90, 0], 20, 0) + time.sleep(1.5) + + # open pump + if "dev" in self.robot_m5: + 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, -15, 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: + 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_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/myPalletizer_260/' + folder + path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_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() + + # 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/mechArm_270/scripts/moving_utils.py b/mycobot_ai/mechArm_270/scripts/moving_utils.py index 1b6a651..d23ff8f 100755 --- a/mycobot_ai/mechArm_270/scripts/moving_utils.py +++ b/mycobot_ai/mechArm_270/scripts/moving_utils.py @@ -24,8 +24,8 @@ class Movement(object): self.coords.y = item[1] self.coords.z = item[2] self.coords.rx = item[3] - self.coords.ry = item[4] - self.coords.rz = item[5] + # self.coords.ry = item[4] + # self.coords.rz = item[5] self.coords.speed = sp self.coords.model = m self.coord_pub.publish(self.coords) @@ -36,8 +36,8 @@ class Movement(object): self.angles.joint_2 = item[1] self.angles.joint_3 = item[2] self.angles.joint_4 = item[3] - self.angles.joint_5 = item[4] - self.angles.joint_6 = item[5] + # self.angles.joint_5 = item[4] + # self.angles.joint_6 = item[5] self.angles.speed = sp self.angle_pub.publish(self.angles) diff --git a/mycobot_ai/mechArm_270/scripts/detect_obj_color.py b/mycobot_ai/mechArm_270/scripts/pi_detect_obj_color.py old mode 100755 new mode 100644 similarity index 84% rename from mycobot_ai/mechArm_270/scripts/detect_obj_color.py rename to mycobot_ai/mechArm_270/scripts/pi_detect_obj_color.py index 26032a4..cc9124e --- a/mycobot_ai/mechArm_270/scripts/detect_obj_color.py +++ b/mycobot_ai/mechArm_270/scripts/pi_detect_obj_color.py @@ -1,17 +1,18 @@ -# encoding:utf-8 +# -*- coding:utf-8 -*- #!/usr/bin/env python2 - +from operator import imod from tokenize import Pointfloat import cv2 import numpy as np import time import json -import os +import os,sys import rospy -from visualization_msgs.msg import Marker - +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 @@ -19,27 +20,30 @@ __version__ = "1.0" class Object_detect(Movement): - def __init__(self, camera_x=150, camera_y=-10): + 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 = [ - [-7.11, -6.94, -55.01, -24.16, 0, 15], # init the point - [5, -10.63, -87.8, 9.05, -3.07, 15], # point to grab + [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 = [ - [120.1, -141.6, 240.9, -173.34, -8.15, -110.11], # above the red bucket - # above the yello bucket - #[215.2, -127.8, 260.9, -157.51, -17.5, -71.18], - [210.6, -130.5, 263.0, -150.99, -0.07, -107.35], - [209.7, -18.6, 230.4, -168.48, -9.86, -39.38], - [196.9, -64.7, 232.6, -166.66, -9.44, -52.47], - [126.6, -118.1, 305.0, -157.57, -13.72, -75.3], + [141.2, -142.0, 210, -26.8], # above the red bucket + [234.3, -120, 210, -48.77], # above the green bucket + [100.9, 159.3, 248.6, -124.27], # above the blue bucket + [-17.6, 161.6, 238.4, -152.31], # 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] @@ -80,11 +84,11 @@ class Object_detect(Movement): "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 mycobot + # 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 mycobot + # 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 mycobot + # 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 @@ -137,65 +141,54 @@ class Object_detect(Movement): # Grasping motion def move(self, x, y, color): - # send Angle to move mycobot - print color - self.pub_angles(self.move_angles[0], 20) + # 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.pub_angles(self.move_angles[1], 20) + self.mc.send_coords([x, y, 90, 0], 20, 0) time.sleep(1.5) - self.pub_angles(self.move_angles[2], 20) - time.sleep(1.5) - # send coordinates to move mycobot - self.pub_coords([x, y, 165, -178.9, -1.57, -25.95], 20, 1) - time.sleep(1.5) - - self.pub_coords([x, y, 80, -178.9, -1.57, -25.95], 20, 1) - - time.sleep(1) + # open pump if self.raspi: self.gpio_status(True) else: self.pub_pump(True, self.Pin) - time.sleep(0.5) - self.pub_angles(self.move_angles[2], 20) - time.sleep(3) - 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.pub_angles(self.move_angles[1], 20) time.sleep(1.5) - self.pub_marker( - self.move_coords[3][0]/1000.0, self.move_coords[3][1]/1000.0, self.move_coords[3][2]/1000.0) - self.pub_angles(self.move_angles[0], 20) + self.mc.send_angle(2, 0, 20) + time.sleep(0.3) + self.mc.send_angle(3, -15, 20) time.sleep(2) - self.pub_marker( - self.move_coords[4][0]/1000.0, self.move_coords[4][1]/1000.0, self.move_coords[4][2]/1000.0) - self.pub_coords(self.move_coords[color], 20, 1) + 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(2) + time.sleep(3) + # close pump if self.raspi: self.gpio_status(False) else: self.pub_pump(False, self.Pin) - time.sleep(1) + 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.pub_angles(self.move_angles[0], 20) - time.sleep(3) + + # self.pub_angles(self.move_angles[0], 20) + 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 @@ -204,24 +197,20 @@ class Object_detect(Movement): else: self.cache_x = self.cache_y = 0 # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动 - - self.move(x,y, color) - # init mycobot + # init mypal260 def run(self): + self.mc = MyPalletizer("/dev/ttyAMA0",1000000) # ok if not self.raspi: self.pub_pump(False, self.Pin) - for _ in range(5): - self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -15], 20) - print(_) - time.sleep(0.5) + self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20) # ok + time.sleep(3) # draw aruco - def draw_marker(self, img, x, y): # draw rectangle on img - cv2.rectangle( + cv2.rectangle( img, (x - 20, y - 20), (x + 20, y + 20), @@ -269,13 +258,13 @@ class Object_detect(Movement): self.y2 = int(y2) print(self.x1, self.y1, self.x2, self.y2) - # set parameters to calculate the coords between cube and mycobot + # 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 mycobot + # 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) @@ -284,7 +273,6 @@ class Object_detect(Movement): 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 @@ -302,21 +290,30 @@ class Object_detect(Movement): # 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) @@ -341,33 +338,35 @@ class Object_detect(Movement): 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 == "yellow": - self.color = 1 - elif mycolor == "red": + # calculate the real coordinates of mypal260 relative to the target + + if mycolor == "red": self.color = 0 - else: + 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 + # init mypal260 detect.run() - _init_ = 20 # + _init_ = 20 init_num = 0 nparams = 0 num = 0 @@ -377,10 +376,10 @@ if __name__ == "__main__": _, 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: @@ -407,7 +406,7 @@ if __name__ == "__main__": init_num += 1 continue - # calculate params of the coords between cube and mycobot + # calculate params of the coords between cube and mypal260 if nparams < 10: if detect.get_calculate_params(frame) is None: cv2.imshow("figure", frame) @@ -424,14 +423,14 @@ if __name__ == "__main__": continue elif nparams == 10: nparams += 1 - # calculate and set params of calculating real coord between cube and mycobot + # 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" + print("ok") continue # get detect result @@ -441,7 +440,7 @@ if __name__ == "__main__": continue else: x, y = detect_result - # calculate real coord between cube and mycobot + # 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) @@ -454,3 +453,9 @@ if __name__ == "__main__": 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/mechArm_270/scripts/detect_obj_img_folder_opt.py b/mycobot_ai/mechArm_270/scripts/pi_detect_obj_img_folder_opt.py similarity index 87% rename from mycobot_ai/mechArm_270/scripts/detect_obj_img_folder_opt.py rename to mycobot_ai/mechArm_270/scripts/pi_detect_obj_img_folder_opt.py index 1ed9909..971082f 100644 --- a/mycobot_ai/mechArm_270/scripts/detect_obj_img_folder_opt.py +++ b/mycobot_ai/mechArm_270/scripts/pi_detect_obj_img_folder_opt.py @@ -22,6 +22,7 @@ from threading import Thread import tkFileDialog as filedialog import Tkinter as tk from moving_utils import Movement +from pymycobot.mypalletizer import MyPalletizer IS_CV_4 = cv2.__version__[0] == '4' __version__ = "1.0" # Adaptive seeed @@ -29,28 +30,27 @@ __version__ = "1.0" # Adaptive seeed class Object_detect(Movement): - def __init__(self, camera_x=150, camera_y=-10): + 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 = [ - [-26.11, -6.94, -55.01, -24.16, 0, 15], # init the point - [-1.14, 0.63, -87.8, 9.05, -3.07, 15], # point to grab + [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 = [ - [120.1, -141.6, 240.9, -173.34, -8.15, -110.11], # above the red bucket - # above the yello bucket - #[208.2, -127.8, 260.9, -157.51, -17.5, -71.18], - [205.6, -130.5, 263.0, -150.99, -0.07, -107.35], # above the green bucket - [-20.0, 176.7, 242.6, -166.66, -9.44, -52.47], # above the gray bucket - [104.9, 176.7, 242.6, -166.66, -9.44, -52.47], # above the blue bucket - [126.6, -118.1, 305.0, -157.57, -13.72, -75.3], + [141.2, -142.0, 210, -26.8], # above the red bucket + [234.3, -120, 210, -48.77], # above the green bucket + [100.9, 159.3, 248.6, -124.27], # above the blue bucket + [-17.6, 161.6, 238.4, -152.31], # above the gray bucket ] # 判断连接设备:ttyUSB*为M5，ttyACM*为seeed @@ -97,6 +97,7 @@ class Object_detect(Movement): # 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) @@ -125,7 +126,6 @@ class Object_detect(Movement): 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 @@ -144,56 +144,40 @@ class Object_detect(Movement): # Grasping motion def move(self, x, y, color): - # send Angle to move mycobot - self.pub_angles(self.move_angles[0], 20) + # 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.pub_angles(self.move_angles[1], 20) - time.sleep(1.5) - self.pub_angles(self.move_angles[2], 20) - time.sleep(1.5) - # send coordinates to move mycobot - self.pub_coords([x, y, 165, -178.9, -1.57, -66], 20, 1) - time.sleep(1.5) - # 根据不同底板机械臂，调整吸泵高度 - self.pub_coords([x, y, 90, -178.9, -1.57, -66], 25, 1) + self.mc.send_coords([x, y, 90, 0], 20, 0) time.sleep(1.5) + # open pump self.gpio_status(True) - - time.sleep(0.5) - self.pub_angles(self.move_angles[2], 20) - time.sleep(3) - self.pub_marker(self.move_coords[2][0] / 1000.0, - self.move_coords[2][1] / 1000.0, - self.move_coords[2][2] / 1000.0) - - self.pub_angles(self.move_angles[1], 20) time.sleep(1.5) - self.pub_marker(self.move_coords[3][0] / 1000.0, - self.move_coords[3][1] / 1000.0, - self.move_coords[3][2] / 1000.0) - self.pub_angles(self.move_angles[0], 20) - time.sleep(1.5) - self.pub_marker(self.move_coords[4][0] / 1000.0, - self.move_coords[4][1] / 1000.0, - self.move_coords[4][2] / 1000.0) + self.mc.send_angle(2, 0, 20) + time.sleep(0.3) + self.mc.send_angle(3, -15, 20) + time.sleep(2) print(self.move_coords[color]) - self.pub_coords(self.move_coords[color], 20, 1) + + 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(4) + time.sleep(3) + # close pump self.gpio_status(False) - - time.sleep(1) - self.pub_marker(self.move_coords[color][0] / 1000.0 + 0.04, - self.move_coords[color][1] / 1000.0 - 0.02) - self.pub_angles(self.move_angles[0], 20) time.sleep(3) + 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) @@ -209,13 +193,14 @@ class Object_detect(Movement): # init mycobot def run(self): - for _ in range(5): - self.pub_angles([-26.11, -6.94, -55.01, -24.16, 0, -15], 20) - print(_) - time.sleep(0.5) + self.mc = MyPalletizer("/dev/ttyAMA0",1000000) # ok + if not self.raspi: + self.pub_pump(False, self.Pin) + + self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20) # ok + time.sleep(3) # draw aruco - def draw_marker(self, img, x, y): # draw rectangle on img cv2.rectangle( @@ -244,6 +229,7 @@ class Object_detect(Movement): # 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. @@ -389,15 +375,6 @@ class Object_detect(Movement): except Exception as e: pass - # else: - # if(len(good) < MIN_MATCH_COUNT): - - # i += 1 - # if(i % 10 == 0): - # print("Not enough matches are found - %d/%d" % - # (len(good), MIN_MATCH_COUNT)) - - # matchesMask = None if x + y > 0: return x, y else: @@ -406,13 +383,12 @@ class Object_detect(Movement): # The path to save the image folder def parse_folder(folder): restore = [] - path = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/' + folder + path = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/myPalletizer_260/' + folder 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 = [] @@ -428,7 +404,6 @@ def compute_keypoints_and_descriptors(sift, images_lists): return kp_list, desc_list - GET_FRAME = 1 STOP_PROCESSING = 2 DRAW_COORDS = 3 @@ -436,13 +411,11 @@ 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 @@ -457,7 +430,6 @@ def process_transform_frame(frame, x1, y1, x2, y2): # int(x1 * 0.7):int(x2 * 1.15)] return frame - def process_display_frame(connection): cap_num = 0 coord = None @@ -502,8 +474,7 @@ def process_display_frame(connection): def run(): parent_conn, child_conn = Pipe() - - child = Process(target=process_display_frame, args=(child_conn,)) + child = Process(target = process_display_frame, args=(child_conn,)) child.start() # Object_detect().take_photo() @@ -513,8 +484,8 @@ def run(): res_queue = [[], [], [], []] res_queue[0] = parse_folder('res/red') res_queue[1] = parse_folder('res/green') - res_queue[2] = parse_folder('res/gray') - res_queue[3] = parse_folder('res/blue') + res_queue[2] = parse_folder('res/blue') + res_queue[3] = parse_folder('res/gray') # res_queue = [] # res_queue.extend(parse_folder('res/red')) @@ -527,6 +498,7 @@ def run(): # init a class of Object_detect detect = Object_detect() + # init mycobot detect.run() @@ -607,9 +579,7 @@ def run(): 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 @@ -639,8 +609,14 @@ def run(): end_time = time.time() print("loop_time = ", end_time - start_time) - child.join() + # close the window + if cv2.waitKey(1) & 0xFF == ord('q'): + cap.release() + cv2.destroyAllWindows() + sys.exit() + child.join() + if __name__ == "__main__": run() diff --git a/mycobot_ai/mechArm_270/scripts/test.py b/mycobot_ai/mechArm_270/scripts/test.py index 8f974fb..d73de19 100755 --- a/mycobot_ai/mechArm_270/scripts/test.py +++ b/mycobot_ai/mechArm_270/scripts/test.py @@ -1,104 +1,41 @@ # -*- coding: utf-8 -*- - -# from fileinput import filename -# from genericpath import isfile -# import os -# from sys import path -# import cv2 -# from PIL import Image - - -# # #count=0 -# for file in dirs: -# pic_dir=os.path.join(path,file) # res中子文件夹的路径 -# print(pic_dir) -# for i in os.listdir(pic_dir): -# imgdir=os.path.join(pic_dir,i) -# print(imgdir) -# for i in os.listdir(pic_dir): -# image_dir=os.path.join(pic_dir,i) #res中每个子文件夹中图片的路径 -# img1 = cv2.imread(image_dir) # 读取res中每个子文件夹中的图片 -# #count+=1 -# print(image_dir)#输出图片的路径 -#print(img1)#输出图片 -#print(count)#图片个数 -# dir_path = os.path.dirname(__file__) -# print(dir_path) -# # path1 = os.path.split(os.path.realpath(__file__))[0] -# # print(path1) -# path3=os.path.join(os.path.split(dir_path)[0]+'/res/') - -# print(path3) -# #for file in os.listdir(path3): -# pic_dir=os.path.join(path3+'red') -# # print(pic_dir) -# for i in os.listdir(pic_dir): -# img=os.path.join(pic_dir,i) -# print(img) -# print(os.getcwd()+'/mycobot_ai/res/red') - -# list1 = [[1, 2, 3], [4, 5, 6], [7, 8, 9], [10, 11, 12]] -# print(list1[0][0]) -# for i, v in enumerate(list1): -# print(i) - -# res = [] -# for i in list1: -# res.append(i) -# print(res) - -# # for i in list1: -# # print(i) - -# color=i -# print(color) -# for color in range(0,4): -# print(color) - -from pymycobot.mycobot import MyCobot +from pymycobot.mypalletizer import MyPalletizer from pymycobot.genre import Angle from pymycobot import PI_PORT, PI_BAUD # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化 -import time +import time,os -# mc = MyCobot("/dev/ttyACM0", 115200) +mc = MyPalletizer(os.popen("ls /dev/ttyUSB*").readline()[:-1], 115200) -mc = MyCobot("/dev/ttyAMA0", 1000000) -# mc.send_angles([0,0,0,0,0,0], 20) +# 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] +# time.sleep(1.5) +# 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) -# move_coords = [ -# [120.1, -141.6, 240.9, -173.34, -8.15, -110.11], # above the red bucket -# # above the green bucket -# #[208.2, -127.8, 260.9, -157.51, -17.5, -71.18], -# [205.6, -130.5, 263.0, -150.99, -0.07, -107.35], -# [209.7, -18.6, 230.4, -168.48, -9.86, -39.38], -# [196.9, -64.7, 232.6, -166.66, -9.44, -52.47], -# [126.6, -118.1, 305.0, -157.57, -13.72, -75.3], -# ] -# mc.send_coords([126.6, -118.1, 305.0, -157.57, -13.72, -75.3],20,1) +# mc.send_coords([141.2, -142.0, 206.2, -26.8],25,1) # above the red bucket # time.sleep(2) -# mc.send_coords([104.9, 176.7, 242.6, -166.66, -9.44, -52.47],20,1) # above the blue bucket +# mc.send_coords([234.3, -120, 210, -48.77],25,1) # above the green bucket # time.sleep(2) -# mc.send_coords([-20.0, 176.7, 242.6, -166.66, -9.44, -52.47],20,1) # abobe the gray bucket -# time.sleep(2) -# mc.send_coords([120.1,151.6,250.0,-173.34,-8.15,-110.11],20,1) -# time.sleep(2) -# mc.send_coords([104.9, 176.7, 242.6, -166.66, -9.44, -52.47],20,1) +# mc.send_coords([100.9, 159.3, 248.6, -124.27],20,1) # above the blue bucket +# time.sleep(3) +# mc.send_coords([-17.6, 161.6, 238.4, -152.31],20,1) # above the gray bucket +# time.sleep(3) + +# mc.send_angle(3,0,25) +# print(mc.get_angles()) +# print(mc.get_coords()) + +# while True: +# print("angles:%s"%mc.get_angles()) +# print("coords:%s"%mc.get_coords()) +# print("\n") -mc.send_angles([-26.11, -6.94, -55.01, -24.16, 0, 15],20) # mc.release_all_servos() -time.sleep(3) -print(mc.get_angles()) -# mc.send_angles([-1.14, 3.63, -87.8, 9.05, -3.07, 15],20) -time.sleep(2) -# print(mc.get_angles()) -# mc.send_angles([17.4, -10.1, -87.27, 5.8, -2.02, 15],20) -# time.sleep(2) -# print(mc.get_angles()) - -# mc.release_servo(6) -mc.release_all_servos() -# mc.set_servo_calibration(6) -# if os.listdir(path,filename='blue'): -# pass \ No newline at end of file +# 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