kenaug74/mycobot_ros
1
0
Fork 0
mirror of https://github.com/elephantrobotics/mycobot_ros.git synced 2026-07-05 19:47:04 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

Integrate aikit code

Browse source
This commit is contained in:
weiquan 2021-11-01 10:41:55 +08:00
parent 1bd5beefed
commit eddef1364f
9 changed files with 195 additions and 212 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

BIN
mycobot_ai/local_photo/img/goal1.jpeg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 5 KiB

BIN
mycobot_ai/local_photo/img/goal2.jpeg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 6.3 KiB

BIN
mycobot_ai/local_photo/img/goal3.jpeg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 4.8 KiB

BIN
mycobot_ai/local_photo/img/goal4.jpeg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 4.4 KiB

BIN
mycobot_ai/local_photo/img/goal5.jpeg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 4.3 KiB

8
mycobot_ai/scripts/ai_windows.py
View file

@ -3,7 +3,8 @@
from tkinter import ttk from tkinter import ttk
from tkinter import * from tkinter import *
import os, time import os
import time
import threading import threading
from multiprocessing import Process from multiprocessing import Process
@ -34,7 +35,7 @@ class Application(object):
# 设置标题 # 设置标题
self.win.title("aikit启动工具") self.win.title("aikit启动工具")
self.win.geometry( self.win.geometry(
"500x300+100+100") # 290 160为窗口大小，+10 +10 定义窗口弹出时的默认展示位置 "550x350+100+100") # 290 160为窗口大小，+10 +10 定义窗口弹出时的默认展示位置
# 打开ros按钮 # 打开ros按钮
self.btn = Button(self.win, text="open ROS", command=self.open_ros) self.btn = Button(self.win, text="open ROS", command=self.open_ros)
self.btn.grid(row=0) self.btn.grid(row=0)
@ -65,7 +66,7 @@ class Application(object):
# self.e2 = Entry(self.win,textvariable=self.v2, width=10) # self.e2 = Entry(self.win,textvariable=self.v2, width=10)
# self.e2.insert(0,0) # self.e2.insert(0,0)
# self.e2.grid(row=3,column=1) # self.e2.grid(row=3,column=1)
self.tips = "1、首先打开ros\n2、选择所要运行的程序点击运行即可" self.tips = "1、首先打开ros,大概需要等待15s\n2、选择所要运行的程序点击运行即可,开启大概需要10秒,可以通过查看终端查看开启情况。\n\n添加新的图像：\n1、点击按钮，等待开启摄像头\n2、选中图像框，按z键拍照\n3、使用鼠标框出需要识别的图像区域\n4、按Enter键提取图像\n5、再次按Enter键保存即可"
self.btn = Button(self.win, text="运行", command=self.start_run) self.btn = Button(self.win, text="运行", command=self.start_run)
self.btn.grid(row=5) self.btn.grid(row=5)
@ -87,6 +88,7 @@ class Application(object):
print(u"开始运行") print(u"开始运行")
one = self.myCombox.get() one = self.myCombox.get()
if one == u"颜色识别": if one == u"颜色识别":
self.run_py = "detect_obj_color.py"
t2 = threading.Thread(target=self.open_py1) t2 = threading.Thread(target=self.open_py1)
t2.setDaemon(True) t2.setDaemon(True)
t2.start() t2.start()

124
mycobot_ai/scripts/detect_encode.py
View file

@ -1,4 +1,4 @@
#encoding: UTF-8 # encoding: UTF-8
#!/usr/bin/env python2 #!/usr/bin/env python2
import cv2 as cv import cv2 as cv
import os import os
@ -13,19 +13,32 @@ pump_y = -55
# x轴偏移量 # x轴偏移量
pump_x = 15 pump_x = 15
class Detect_marker(Movement): class Detect_marker(Movement):
def __init__(self): def __init__(self):
super(Detect_marker, self).__init__() super(Detect_marker, self).__init__()
# set cache of real coord # set cache of real coord
self.cache_x = self.cache_y = 0 self.cache_x = self.cache_y = 0
# which robot # which robot
self.robot = os.popen("ls /dev/ttyUSB*") self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
if "dev" in self.robot: self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
self.Pin = [2,5] self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
else: self.raspi = False
self.Pin = [20,21] if "dev" in self.robot_m5:
self.Pin = [2, 5]
elif "dev" in self.robot_wio:
self.Pin = [20, 21]
for i in self.move_coords:
i[2] -= 20
elif "dev" in self.robot_raspi:
import RPi.GPIO as GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setup(20, GPIO.OUT)
GPIO.setup(21, GPIO.OUT)
self.raspi = True
pump_y -= 5
# Creating a Camera Object # Creating a Camera Object
cap_num = 0 cap_num = 0
self.cap = cv.VideoCapture(cap_num) self.cap = cv.VideoCapture(cap_num)
@ -33,7 +46,8 @@ class Detect_marker(Movement):
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
# Get ArUco marker params. # Get ArUco marker params.
self.aruco_params = cv.aruco.DetectorParameters_create() self.aruco_params = cv.aruco.DetectorParameters_create()
self.calibrationParams = cv.FileStorage("calibrationFileName.xml", cv.FILE_STORAGE_READ) self.calibrationParams = cv.FileStorage(
"calibrationFileName.xml", cv.FILE_STORAGE_READ)
# Get distance coefficient. # Get distance coefficient.
self.dist_coeffs = self.calibrationParams.getNode("distCoeffs").mat() self.dist_coeffs = self.calibrationParams.getNode("distCoeffs").mat()
@ -66,7 +80,6 @@ class Detect_marker(Movement):
self.marker.color.g = 0.3 self.marker.color.g = 0.3
self.marker.color.b = 0.3 self.marker.color.b = 0.3
# marker position initial # marker position initial
self.marker.pose.position.x = 0 self.marker.pose.position.x = 0
self.marker.pose.position.y = 0 self.marker.pose.position.y = 0
@ -78,11 +91,16 @@ class Detect_marker(Movement):
# Grasping motion # Grasping motion
def move(self, x, y): def move(self, x, y):
if self.raspi:
coords = [ coords = [
[135.0, -65.5, 280.1, 178.99, 5.38, -179.9], [145.6, -64.9, 285.2, 179.88, 7.67, -177.06],
[136.1, -141.6, 243.9, 178.99, 5.38, -179.9] [130.1, -155.6, 243.9, 178.99, 5.38, -179.9]
] ]
else:
coords = [
[135.0, -65.5, 280.1, 178.99, 5.38, -179.9],
[136.1, -141.6, 243.9, 178.99, 5.38, -179.9]
]
# publish marker # publish marker
self.marker.header.stamp = rospy.Time.now() self.marker.header.stamp = rospy.Time.now()
@ -93,21 +111,32 @@ class Detect_marker(Movement):
# send coordinates to move mycobot # send coordinates to move mycobot
self.pub_coords(coords[0], 30, 1) self.pub_coords(coords[0], 30, 1)
time.sleep(2) time.sleep(2)
self.pub_coords([coords[0][0]-x, coords[0][1]-y, 240, 178.99, 5.38, -179.9], 25, 1) self.pub_coords([coords[0][0]-x, coords[0][1]-y,
240, 178.99, 5.38, -179.9], 25, 1)
time.sleep(2) time.sleep(2)
self.pub_coords([coords[0][0]-x, coords[0][1]-y, 200, 178.99, 5.38, -179.9], 25, 1) self.pub_coords([coords[0][0]-x, coords[0][1]-y,
200, 178.99, 5.38, -179.9], 25, 1)
time.sleep(2) time.sleep(2)
if "dev" in self.robot: if "dev" in self.robot_m5 or self.raspi:
self.pub_coords([coords[0][0]-x, coords[0][1]-y, 90, 178.99, 5.38, -179.9], 25, 1) self.pub_coords([coords[0][0]-x, coords[0][1]-y,
90, 178.99, 5.38, -179.9], 25, 1)
elif "dev" in self.robot_wio:
self.pub_coords([coords[0][0]-x+20, coords[0][1] -
y-10, 70, 178.99, 5.38, -179.9], 25, 1)
time.sleep(2)
if self.raspi:
self.gpio_status(True)
else: else:
self.pub_coords([coords[0][0]-x+20, coords[0][1]-y-10, 70, 178.99, 5.38, -179.9], 25, 1) self.pub_pump(True, self.Pin)
time.sleep(3.5) time.sleep(1)
self.pub_pump(True,self.Pin)
self.pub_coords(coords[0], 30, 1) self.pub_coords(coords[0], 30, 1)
time.sleep(3) time.sleep(3)
self.pub_coords(coords[1], 30, 1) self.pub_coords(coords[1], 30, 1)
time.sleep(2) time.sleep(2)
self.pub_pump(False,self.Pin) if self.raspi:
self.gpio_status(False)
else:
self.pub_pump(False, self.Pin)
# publish marker # publish marker
self.marker.header.stamp = rospy.Time.now() self.marker.header.stamp = rospy.Time.now()
self.marker.pose.position.x = coords[1][0]/1000.0 self.marker.pose.position.x = coords[1][0]/1000.0
@ -117,12 +146,20 @@ class Detect_marker(Movement):
self.pub_coords(coords[0], 30, 1) self.pub_coords(coords[0], 30, 1)
time.sleep(2) time.sleep(2)
def gpio_status(self, flag):
if flag:
GPIO.output(20, 0)
GPIO.output(21, 0)
else:
GPIO.output(20, 1)
GPIO.output(21, 1)
# decide whether grab cube # decide whether grab cube
def decide_move(self, x, y): def decide_move(self, x, y):
print(x,y) print(x, y)
# detect the cube status move or run # detect the cube status move or run
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
self.cache_x, self.cache_y = x, y self.cache_x, self.cache_y = x, y
return return
else: else:
@ -131,35 +168,37 @@ class Detect_marker(Movement):
# init mycobot # init mycobot
def init_mycobot(self): def init_mycobot(self):
self.pub_pump(False,self.Pin) if self.raspi:
self.gpio_status(False)
else:
self.pub_pump(False, self.Pin)
for _ in range(5): for _ in range(5):
print _ print _
self.pub_coords([135.0, -65.5, 280.1, 178.99, 5.38, -179.9], 20, 1) self.pub_coords([145.6, -64.9, 285.2, 179.88, 7.67, -177.06], 20, 1), 20, 1)
time.sleep(0.5)
time.sleep(0.5)
def run(self): def run(self):
global pump_y, pump_x global pump_y, pump_x
self.init_mycobot() self.init_mycobot()
num = sum_x = sum_y = 0 num=sum_x=sum_y=0
while cv.waitKey(1) < 0: while cv.waitKey(1) < 0:
success, img = self.cap.read() success, img=self.cap.read()
if not success: if not success:
print("It seems that the image cannot be acquired correctly.") print("It seems that the image cannot be acquired correctly.")
break break
# transfrom the img to model of gray # transfrom the img to model of gray
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) gray=cv.cvtColor(img, cv.COLOR_BGR2GRAY)
# Detect ArUco marker. # Detect ArUco marker.
corners, ids, rejectImaPoint = cv.aruco.detectMarkers( corners, ids, rejectImaPoint=cv.aruco.detectMarkers(
gray, self.aruco_dict, parameters=self.aruco_params gray, self.aruco_dict, parameters = self.aruco_params
) )
if len(corners) > 0: if len(corners) > 0:
if ids is not None: if ids is not None:
# get informations of aruco # get informations of aruco
ret = cv.aruco.estimatePoseSingleMarkers( ret=cv.aruco.estimatePoseSingleMarkers(
corners, 0.03, self.camera_matrix, self.dist_coeffs corners, 0.03, self.camera_matrix, self.dist_coeffs
) )
# rvec:rotation offset,tvec:translation deviator # rvec:rotation offset,tvec:translation deviator
@ -167,11 +206,11 @@ class Detect_marker(Movement):
(rvec - tvec).any() (rvec - tvec).any()
xyz = tvec[0, 0, :] xyz = tvec[0, 0, :]
# calculate the coordinates of the aruco relative to the pump # calculate the coordinates of the aruco relative to the pump
xyz = [round(xyz[0]*1000+pump_y, 2), round(xyz[1]*1000+pump_x, 2), round(xyz[2]*1000, 2)] xyz = [round(xyz[0]*1000+pump_y, 2), round(xyz[1]
* 1000+pump_x, 2), round(xyz[2]*1000, 2)]
for i in range(rvec.shape[0]): for i in range(rvec.shape[0]):
# draw the aruco on img # draw the aruco on img
cv.aruco.drawDetectedMarkers(img, corners) cv.aruco.drawDetectedMarkers(img, corners)
cv.aruco.drawAxis( cv.aruco.drawAxis(
img, img,
@ -182,16 +221,19 @@ class Detect_marker(Movement):
0.03, 0.03,
) )
if num < 40 : if num < 40:
if self.raspi:
sum_x -= 30
sum_x += xyz[1] sum_x += xyz[1]
sum_y += xyz[0] sum_y += xyz[0]
num += 1 num += 1
elif num ==40 : elif num == 40:
self.decide_move(sum_x/40.0, sum_y/40.0) self.decide_move(sum_x/40.0, sum_y/40.0)
num = sum_x = sum_y = 0 num = sum_x = sum_y = 0
cv.imshow("encode_image", img) cv.imshow("encode_image", img)
if __name__ == "__main__": if __name__ == "__main__":
detect = Detect_marker() detect = Detect_marker()
detect.run() detect.run()

9
mycobot_ai/scripts/detect_obj_color.py
View file

@ -1,4 +1,5 @@
# encoding:utf-8 # encoding:utf-8
#!/usr/bin/env python2
from tokenize import Pointfloat from tokenize import Pointfloat
import cv2 import cv2
@ -33,7 +34,7 @@ class Object_detect(Movement):
self.move_coords = [ self.move_coords = [
[120.1, -141.6, 240.9, -173.34, -8.15, -83.11], # above the red bucket [120.1, -141.6, 240.9, -173.34, -8.15, -83.11], # above the red bucket
# above the yello bucket # above the yello bucket
[208.2, -127.8, 260.9, -157.51, -17.5, -71.18], [215.2, -127.8, 260.9, -157.51, -17.5, -71.18],
[209.7, -18.6, 230.4, -168.48, -9.86, -39.38], [209.7, -18.6, 230.4, -168.48, -9.86, -39.38],
[196.9, -64.7, 232.6, -166.66, -9.44, -52.47], [196.9, -64.7, 232.6, -166.66, -9.44, -52.47],
[126.6, -118.1, 305.0, -157.57, -13.72, -75.3], [126.6, -118.1, 305.0, -157.57, -13.72, -75.3],
@ -140,7 +141,7 @@ class Object_detect(Movement):
time.sleep(1.5) time.sleep(1.5)
if "dev" in self.robot_m5 or self.raspi: if "dev" in self.robot_m5 or self.raspi:
self.pub_coords([x, y, 90, -178.9, -1.57, -25.95], 20, 1) self.pub_coords([x, y, 90, -178.9, -1.57, -25.95], 20, 1)
else: elif "dev" in self.robot_wio:
h = 0 h = 0
if 165 < x < 180: if 165 < x < 180:
@ -172,7 +173,6 @@ class Object_detect(Movement):
self.pub_marker( 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.move_coords[4][0]/1000.0, self.move_coords[4][1]/1000.0, self.move_coords[4][2]/1000.0)
print 'down:', self.move_coords[color]
self.pub_coords(self.move_coords[color], 20, 1) self.pub_coords(self.move_coords[color], 20, 1)
self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color] self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color]
[1]/1000.0, self.move_coords[color][2]/1000.0) [1]/1000.0, self.move_coords[color][2]/1000.0)
@ -202,7 +202,8 @@ class Object_detect(Movement):
return return
else: else:
self.cache_x = self.cache_y = 0 self.cache_x = self.cache_y = 0
if "dev" not in self.robot: # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
if "dev" not in self.robot_wio:
if (y < -30 and x > 140) or (x > 150 and y < -10): if (y < -30 and x > 140) or (x > 150 and y < -10):
x -= 10 x -= 10

266
mycobot_ai/scripts/detect_obj_img.py
View file

@ -1,4 +1,5 @@
# encoding:utf-8 # encoding:utf-8
#!/usr/bin/env python2
from tokenize import Pointfloat from tokenize import Pointfloat
import cv2 import cv2
@ -24,28 +25,38 @@ class Object_detect(Movement):
super(Object_detect, self).__init__() super(Object_detect, self).__init__()
# get path of file # get path of file
dir_path = os.path.dirname(__file__) dir_path = os.path.dirname(__file__)
# 移动角度 # 移动角度
self.move_angles = [ self.move_angles = [
[-7.11, -6.94, -55.01, -24.16, 0, -38.84], # init the point [-7.11, -6.94, -55.01, -24.16, 0, -38.84], # init the point
[-1.14, -10.63, -87.8, 9.05, -3.07, -37.7], # point to grab [-1.14, -10.63, -87.8, 9.05, -3.07, -37.7], # point to grab
[17.4, -10.1, -87.27, 5.8, -2.02, -37.7], # point to grab [17.4, -10.1, -87.27, 5.8, -2.02, -37.7], # point to grab
] ]
# 移动坐标 # 移动坐标
self.move_coords = [ self.move_coords = [
[120.1, -141.6, 240.9, -173.34, -8.15, -83.11], # above the red bucket [120.1, -141.6, 240.9, -173.34, -8.15, -83.11], # above the red bucket
[208.2, -127.8, 260.9, -157.51, -17.5, -71.18], # above the yello bucket # above the yello bucket
[209.7, -18.6, 230.4, -168.48, -9.86, -39.38], [208.2, -127.8, 260.9, -157.51, -17.5, -71.18],
[196.9, -64.7, 232.6, -166.66, -9.44, -52.47], [209.7, -18.6, 230.4, -168.48, -9.86, -39.38],
[126.6, -118.1, 305.0, -157.57, -13.72, -75.3], [196.9, -64.7, 232.6, -166.66, -9.44, -52.47],
[126.6, -118.1, 305.0, -157.57, -13.72, -75.3],
] ]
# 判断连接设备:ttyUSB*为M5，ttyACM*为seeed # 判断连接设备:ttyUSB*为M5，ttyACM*为seeed
self.robot = os.popen("ls /dev/ttyUSB*").readline()[:-1] self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
if "dev" in self.robot: self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
self.raspi = False
if "dev" in self.robot_m5:
self.Pin = [2, 5] self.Pin = [2, 5]
else: elif "dev" in self.robot_wio:
self.Pin = [20, 21] self.Pin = [20, 21]
for i in self.move_coords: for i in self.move_coords:
i[2] -= 20 i[2] -= 20
elif "dev" in self.robot_raspi:
import RPi.GPIO as GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setup(20, GPIO.OUT)
GPIO.setup(21, GPIO.OUT)
self.raspi = True
# choose place to set cube # choose place to set cube
self.color = 0 self.color = 0
@ -113,6 +124,14 @@ class Object_detect(Movement):
self.marker.color.g = self.color self.marker.color.g = self.color
self.pub.publish(self.marker) self.pub.publish(self.marker)
def gpio_status(self, flag):
if flag:
GPIO.output(20, 0)
GPIO.output(21, 0)
else:
GPIO.output(20, 1)
GPIO.output(21, 1)
# Grasping motion # Grasping motion
def move(self, x, y, color): def move(self, x, y, color):
# send Angle to move mycobot # send Angle to move mycobot
@ -125,11 +144,10 @@ class Object_detect(Movement):
# send coordinates to move mycobot # send coordinates to move mycobot
self.pub_coords([x, y, 165, -178.9, -1.57, -25.95], 20, 1) self.pub_coords([x, y, 165, -178.9, -1.57, -25.95], 20, 1)
time.sleep(1.5) time.sleep(1.5)
# 根据不同底板机械臂，调整吸泵高度
if "dev" in self.robot: if "dev" in self.robot_m5:
self.pub_coords([x, y, 90, -178.9, -1.57, -25.95], 20, 1) self.pub_coords([x, y, 90, -178.9, -1.57, -25.95], 20, 1)
else: elif "dev" in self.robot_wio:
h = 0 h = 0
if 165 < x < 180: if 165 < x < 180:
h = 10 h = 10
@ -137,11 +155,13 @@ class Object_detect(Movement):
h = 20 h = 20
elif x < 135: elif x < 135:
h = -20 h = -20
# print 'down_1:',[x, y, 31.9+h, -178.9, -1, -25.95]
self.pub_coords([x, y, 31.9+h, -178.9, -1, -25.95], 20, 1) self.pub_coords([x, y, 31.9+h, -178.9, -1, -25.95], 20, 1)
time.sleep(1.5) time.sleep(1.5)
# open pump # open pump
self.pub_pump(True, self.Pin) if self.raspi:
self.gpio_status(True)
else:
self.pub_pump(True, self.Pin)
time.sleep(0.5) time.sleep(0.5)
self.pub_angles(self.move_angles[2], 20) self.pub_angles(self.move_angles[2], 20)
time.sleep(3) time.sleep(3)
@ -163,14 +183,16 @@ class Object_detect(Movement):
[1]/1000.0, self.move_coords[color][2]/1000.0) [1]/1000.0, self.move_coords[color][2]/1000.0)
time.sleep(2) time.sleep(2)
# close pump # close pump
self.pub_pump(False, self. if self.raspi:
Pin) self.gpio_status(False)
else:
self.pub_pump(False, self.Pin)
if color == 1: if color == 1:
self.pub_marker( self.pub_marker(
self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02) self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02)
elif color == 0: elif color == 0:
self.pub_marker( self.pub_marker(
self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0) 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.pub_angles(self.move_angles[0], 20)
time.sleep(3) time.sleep(3)
@ -184,7 +206,8 @@ Pin)
return return
else: else:
self.cache_x = self.cache_y = 0 self.cache_x = self.cache_y = 0
if "dev" not in self.robot: # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
if "dev" not in self.robot_wio:
if (y < -30 and x > 140) or (x > 150 and y < -10): if (y < -30 and x > 140) or (x > 150 and y < -10):
x -= 10 x -= 10
y += 10 y += 10
@ -193,7 +216,7 @@ Pin)
elif x > 170: elif x > 170:
x -= 10 x -= 10
y += 10 y += 10
else: elif "dev" not in self.robot_m5:
y += 10 y += 10
x -= 5 x -= 5
if y < -20: if y < -20:
@ -203,13 +226,17 @@ Pin)
# init mycobot # init mycobot
def run(self): def run(self):
if self.raspi:
self.gpio_status(False)
else:
self.pub_pump(False, self.Pin)
for _ in range(5): for _ in range(5):
self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -38.84], 20) self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -38.84], 20)
print(_) print(_)
time.sleep(0.5) time.sleep(0.5)
self.pub_pump(False, self.Pin)
# draw aruco # draw aruco
def draw_marker(self, img, x, y): def draw_marker(self, img, x, y):
# draw rectangle on img # draw rectangle on img
cv2.rectangle( cv2.rectangle(
@ -285,14 +312,14 @@ Pin)
if self.x1 != self.x2: if self.x1 != self.x2:
# the cutting ratio here is adjusted according to the actual situation # the cutting ratio here is adjusted according to the actual situation
frame = frame[int(self.y2*0.2):int(self.y1*1.15), frame = frame[int(self.y2*0.2):int(self.y1*1.15),
int(self.x1*0.7):int(self.x2*1.15)] int(self.x1*0.7):int(self.x2*1.15)]
return frame return frame
# according the class_id to get object name # according the class_id to get object name
def id_class_name(self, class_id): def id_class_name(self, class_id):
for key, value in self.labels.items(): for key, value in self.labels.items():
if class_id == int(key): if class_id == int(key):
return value return value
# detect object # detect object
def obj_detect(self, img, goal): def obj_detect(self, img, goal):
@ -405,107 +432,20 @@ Pin)
except Exception as e: except Exception as e:
pass pass
else: # else:
if(len(good) < MIN_MATCH_COUNT): # if(len(good) < MIN_MATCH_COUNT):
i += 1 # i += 1
if(i % 10 == 0): # if(i % 10 == 0):
print("Not enough matches are found - %d/%d" % # print("Not enough matches are found - %d/%d" %
(len(good), MIN_MATCH_COUNT)) # (len(good), MIN_MATCH_COUNT))
matchesMask = None # matchesMask = None
if x+y > 0: if x+y > 0:
return x, y return x, y
else: else:
return None return None
def take_photo(self):
# 提醒用户操作字典
print("*********************************************")
print("* 热键(请在摄像头的窗口使用)： *")
print("* z: 拍摄图片 *")
print("* q: 退出 *")
print("*********************************************")
# 创建/使用local_photo文件夹
class_name = "local_photo"
if(os.path.exists("local_photo")):
pass
else:
os.mkdir(class_name)
# 设置特定值
index = 'takephoto'
cap = cv2.VideoCapture(0)
while True:
# 读入每一帧
ret, frame = cap.read()
cv2.imshow("capture", frame)
# 存储
input = cv2.waitKey(1) & 0xFF
# 拍照
if input == ord('z'):
cv2.imwrite("%s/%s.jpeg" % (class_name, index),
cv2.resize(frame, (600, 480), interpolation=cv2.INTER_AREA))
break
# 退出
if input == ord('q'):
break
# 关闭窗口
cap.release()
cv2.destroyAllWindows()
def cut_photo(self):
path = os.getcwd()+'/local_photo/img'
print path
for i,j,k in os.walk(path):
file_len = len(k)
print("请截取要识别的部分")
# root = tk.Tk()
# root.withdraw()
# temp1=filedialog.askopenfilename(parent=root) #rgb
# temp2=Image.open(temp1,mode='r')
# temp2= cv.cvtColor(np.asarray(temp2),cv.COLOR_RGB2BGR)
# cut = np.array(temp2)
cut = cv2.imread(r"local_photo/takephoto.jpeg")
cv2.imshow('original', cut)
# C:\Users\Elephant\Desktop\pymycobot+opencv\local_photo/takephoto.jpeg
# 选择ROI
roi = cv2.selectROI(windowName="original", img=cut,
showCrosshair=False, fromCenter=False)
x, y, w, h = roi
print(roi)
# 显示ROI并保存图片
if roi != (0, 0, 0, 0):
crop = cut[y:y+h, x:x+w]
cv2.imshow('crop', crop)
cv2.imwrite('local_photo/img/goal{}.jpeg'.format(str(file_len+1)), crop)
print('Saved!')
# 退出
cv2.waitKey(0)
cv2.destroyAllWindows()
def distinguist(self):
print("请选择要识别的物体图片")
root = tk.Tk() # 显式创建根窗体
root.withdraw() # 将根窗体隐藏
file = filedialog.askopenfilename(parent=root)
load = Image.open(file, mode='r')
load = cv.cvtColor(np.asarray(load), cv.COLOR_RGB2BGR)
goal = np.array(load)
return goal
def run(): def run():
@ -515,7 +455,7 @@ def run():
goal = [] goal = []
path = os.getcwd()+'/local_photo/img' path = os.getcwd()+'/local_photo/img'
for i,j,k in os.walk(path): for i, j, k in os.walk(path):
for l in k: for l in k:
goal.append(cv2.imread('local_photo/img/{}'.format(l))) goal.append(cv2.imread('local_photo/img/{}'.format(l)))
cap_num = 0 cap_num = 0
@ -526,78 +466,77 @@ def run():
detect = Object_detect() detect = Object_detect()
# init mycobot # init mycobot
detect.run() detect.run()
_init_ = 20 # _init_ = 20 #
init_num = 0 init_num = 0
nparams = 0 nparams = 0
num = 0 num = 0
real_sx = real_sy = 0 real_sx = real_sy = 0
while cv2.waitKey(1) < 0: while cv2.waitKey(1) < 0:
# read camera # read camera
_,frame = cap.read() _, frame = cap.read()
# deal img # deal img
frame = detect.transform_frame(frame) frame = detect.transform_frame(frame)
if _init_ > 0: if _init_ > 0:
_init_-=1 _init_ -= 1
continue continue
# calculate the parameters of camera clipping # calculate the parameters of camera clipping
if init_num < 20: if init_num < 20:
if detect.get_calculate_params(frame) is None: if detect.get_calculate_params(frame) is None:
cv2.imshow("figure",frame) cv2.imshow("figure", frame)
continue continue
else: else:
x1,x2,y1,y2 = detect.get_calculate_params(frame) x1, x2, y1, y2 = detect.get_calculate_params(frame)
detect.draw_marker(frame,x1,y1) detect.draw_marker(frame, x1, y1)
detect.draw_marker(frame,x2,y2) detect.draw_marker(frame, x2, y2)
detect.sum_x1+=x1 detect.sum_x1 += x1
detect.sum_x2+=x2 detect.sum_x2 += x2
detect.sum_y1+=y1 detect.sum_y1 += y1
detect.sum_y2+=y2 detect.sum_y2 += y2
init_num+=1 init_num += 1
continue continue
elif init_num==20: elif init_num == 20:
detect.set_cut_params( detect.set_cut_params(
(detect.sum_x1)/20.0, (detect.sum_x1)/20.0,
(detect.sum_y1)/20.0, (detect.sum_y1)/20.0,
(detect.sum_x2)/20.0, (detect.sum_x2)/20.0,
(detect.sum_y2)/20.0, (detect.sum_y2)/20.0,
) )
detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0 detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
init_num+=1 init_num += 1
continue continue
# calculate params of the coords between cube and mycobot # calculate params of the coords between cube and mycobot
if nparams < 10: if nparams < 10:
if detect.get_calculate_params(frame) is None: if detect.get_calculate_params(frame) is None:
cv2.imshow("figure",frame) cv2.imshow("figure", frame)
continue continue
else: else:
x1,x2,y1,y2 = detect.get_calculate_params(frame) x1, x2, y1, y2 = detect.get_calculate_params(frame)
detect.draw_marker(frame,x1,y1) detect.draw_marker(frame, x1, y1)
detect.draw_marker(frame,x2,y2) detect.draw_marker(frame, x2, y2)
detect.sum_x1+=x1 detect.sum_x1 += x1
detect.sum_x2+=x2 detect.sum_x2 += x2
detect.sum_y1+=y1 detect.sum_y1 += y1
detect.sum_y2+=y2 detect.sum_y2 += y2
nparams+=1 nparams += 1
continue continue
elif nparams==10: elif nparams == 10:
nparams+=1 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 mycobot
detect.set_params( detect.set_params(
(detect.sum_x1+detect.sum_x2)/20.0, (detect.sum_x1+detect.sum_x2)/20.0,
(detect.sum_y1+detect.sum_y2)/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 abs(detect.sum_x1-detect.sum_x2)/10.0 +
abs(detect.sum_y1-detect.sum_y2)/10.0
) )
print "ok" print "ok"
continue continue
# get detect result # get detect result
detect_result = detect.obj_detect(frame,goal) detect_result = detect.obj_detect(frame, goal)
if detect_result is None: if detect_result is None:
cv2.imshow("figure",frame) cv2.imshow("figure", frame)
continue continue
else: else:
x, y = detect_result x, y = detect_result
@ -613,11 +552,10 @@ def run():
real_sy += real_y real_sy += real_y
real_sx += real_x real_sx += real_x
cv2.imshow("figure",frame) cv2.imshow("figure", frame)
if __name__ == "__main__": if __name__ == "__main__":
run() run()
# Object_detect().take_photo() # Object_detect().take_photo()
# Object_detect().cut_photo() # Object_detect().cut_photo()