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

update

Browse source
This commit is contained in:
weijian 2022-12-02 16:45:22 +08:00
parent a7f592229b
commit be8834ded1
10 changed files with 0 additions and 2271 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

BIN
mycobot_280/mycobot_280/scripts/1245.jpg
View file

Binary file not shown.
Side by side

Before

Width:  |  Height:  |  Size: 31 KiB

BIN
mycobot_280/mycobot_280/scripts/3a4.bmp
View file

Binary file not shown.
Side by side

Before

Width:  |  Height:  |  Size: 1.6 MiB

199
mycobot_280/mycobot_280/scripts/calibration_camera.py
View file

@ -1,199 +0,0 @@
#!/usr/bin/python3
# -*- coding: UTF-8 -*-
import os
import numpy as np
import cv2 as cv
import cv2
import matplotlib.pyplot as plt
# 生成一张630*890的全黑图片
# img = np.zeros((630,890,3),np.uint8)
# plt.imshow(img[:,:,::-1])
# while True:
# # plt.show()
# cv2.imshow('img', img)
# key = cv2.waitKey(0)
# if key == ord('q'):
# break
# elif key == ord('s'):
# cv2.imwrite('/home/h/catkin_ws/src/mycobot_ros/mycobot_280/mycobot_280/scripts/123.png', img)
# print('saved')
# cv2.destroyAllWindows()
path = os.path.join(os.path.dirname(__file__), "3a4.bmp")
print(path)
frame = cv2.imread(path)
row, col, nc = frame.shape
width_of_roi = 90
# 这里是对全黑图片做处理，将图片以黑白间隔的形式zh
for j in range(row):
data = frame[j]
for i in range(col):
f = int(i / width_of_roi) % 2 ^ int(j / width_of_roi) % 2
if f:
frame[j][i][0] = 255
frame[j][i][1] = 255
frame[j][i][2] = 255
cv2.imshow("", frame)
cv2.waitKey(0) & 0xFF == ord("q")
cv2.imwrite(os.path.join(os.path.dirname(__file__), "1245.jpg"), frame)
# import os
# import cv2
# import threading
# if_save = False
# # 设置摄像头编号（由于电脑型号不同，分配给USB摄像头的编号也可能不同，一般为0或1）
# cap_num = int(input("Input the camare number:"))
# # 设置所存储的图片名称，设置为1，即表示从1开始累加存储。如：1.jpg,2.jpg,3.jpg......
# name = int(input("Input start name, use number:"))
# cap = cv2.VideoCapture(cap_num)
# dir_path = os.path.dirname(__file__)
# def save():
# global if_save
# while True:
# input("Input any to save a image:")
# if_save = True
# # 开启线程进行摄像头拍摄
# t = threading.Thread(target=save)
# # 设置为异步运行
# t.setDaemon(True)
# t.start()
# while cv2.waitKey(1) != ord("q"):
# _, frame = cap.read()
# if if_save:
# # 设置名称为当前路径下，否则会因为运行环境的原因使得存储位置发生变化
# img_name = os.path.join(dir_path,str(name)+".jpg")
# # 存储图片
# cv2.imwrite(img_name, frame)
# print("Save {} successful.".format(img_name))
# name += 1
# if_save = False
# cv2.imshow("", frame)
# import os
# import glob
# import numpy as np
# import cv2 as cv
# from pprint import pprint
# obj_points = [] # 3d点在现实世界的位置。
# img_points = [] # 2d点在图片中的位置。
# gray = None
# def calibration_camera(row, col, path=None, cap_num=None, saving=False):
# """校准摄像头
# 参数说明:
# row (int): 网格中的行数。
# col (int): 网格中的列数。
# path (string): 存放校准图片的位置。
# cap_num (int): 表示摄像头的编号，一般0或1
# saving (bool): 是否存放相机矩阵和失真系数(.npz).
# """
# # 终止准则/失效准则
# criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# # 准备物体点, 比如 (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
# obj_p = np.zeros((row * col, 3), np.float32)
# obj_p[:, :2] = np.mgrid[0:row, 0:col].T.reshape(-1, 2)
# # 组用于存储来自所有图像的对象点和图像点。
# def _find_grid(img):
# # 使用函数外的参数
# global gray, obj_points, img_points
# # 将图片转换为灰色度图片
# gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
# # 寻找棋盘的角落
# ret, corners = cv.findChessboardCorners(gray, (row, col), None)
# # 如果找到，则添加处理后的2d点和3d点
# if ret == True:
# obj_points.append(obj_p)
# corners2 = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
# img_points.append(corners)
# # 在图片中绘制并展示所寻找到的角
# cv.drawChessboardCorners(img, (row, col), corners2, ret)
# # 要求必须选择使用图片校准或者摄像头实时捕获校准中的一种
# if path and cap_num:
# raise Exception("The parameter `path` and `cap_num` only need one.")
# # 图片校准
# if path:
# # 获取当前路径中的所有图片
# images = glob.glob(os.path.join(path, "*.jpg"))
# pprint(images)
# # 对获取的每张图片进行处理
# for f_name in images:
# # 读取图片
# img = cv.imread(f_name)
# _find_grid(img)
# # 展示图片
# cv.imshow("img", img)
# # 图片展示等待0.5s
# cv.waitKey(500)
# # 摄像头实时捕获校准
# if cap_num:
# # 开启摄像头
# cap = cv.VideoCapture(cap_num)
# while True:
# # 读取摄像头开启后的每帧图片
# _, img = cap.read()
# _find_grid(img)
# cv.imshow("img", img)
# cv.waitKey(500)
# print(len(obj_points))
# if len(obj_points) > 14:
# break
# # 销毁展示窗口
# cv.destroyAllWindows()
# # 通过计算获取的3d点和2d点得出相机矩阵和失真系数
# ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(
# obj_points, img_points, gray.shape[::-1], None, None
# )
# print("ret: {}".format(ret))
# print("matrix:")
# pprint(mtx)
# print("distortion: {}".format(dist))
# # 决定是否存储所计算出的参数
# if saving:
# np.savez(os.path.join(os.path.dirname(__file__), "camera_mtx_dist.npz"), mtx=mtx, dist=dist)
# mean_error = 0
# for i in range(len(obj_points)):
# img_points_2, _ = cv.projectPoints(obj_points[i], rvecs[i], tvecs[i], mtx, dist)
# error = cv.norm(img_points[i], img_points_2, cv.NORM_L2) / len(img_points_2)
# mean_error += error
# print("total error: {}".format(mean_error / len(obj_points)))
# return mtx, dist
# if __name__ == "__main__":
# path = os.path.dirname(__file__)
# mtx, dist = calibration_camera(8, 6, path, saving=True)
# # 设置是否需要测试计算出的参数
# if_test = input("If testing the result (default: no), [yes/no]:")
# if if_test not in ["y", "Y", "yes", "Yes"]:
# exit(0)
# cap_num = int(input("Input camera number:"))
# cap = cv.VideoCapture(cap_num)
# while cv.waitKey(1) != ord("q"):
# _, img = cap.read()
# h, w = img.shape[:2]
# # 相机校准
# dst = cv.undistort(img, mtx, dist)
# cv.imshow("", dst)

BIN
mycobot_280/mycobot_280/scripts/mtx_dist.npz
View file

Binary file not shown.

68
mycobot_280/mycobot_280/scripts/test.py
View file

@ -1,68 +0,0 @@
#!/usr/bin/python
# -*- coding: UTF-8 -*-
from pymycobot.mycobot import MyCobot
import time
import threading
import os
print(os.path.join(os.path.dirname(__file__)))
mc = MyCobot('/dev/ttyUSB0', 115200)
mc.set_tool_reference([-50,0,0,0,0,0])
mc.set_end_type(1)
init_angles = [0, 0.52, -85.69, 0.0, 89.82, 0.08]
start_angles = [18.64, 0.52, -85.69, 0.0, 89.82, 0.08]
end_angles = [-18.56, 0.52, -85.69, 0.0, 89.82, 0.08]
ang = []
def wait_time(t):
global ang
for i in range(t*10+1):
time.sleep(0.1)
ang1 = mc.get_angles()
ang.append(ang1)
coord = mc.get_coords()
# print('ange-------->', ang)
print('coord-------->', coord)
def get_ang():
i = 0
print('55555555555')
ang_list = None
while True:
for j in range(i,len(ang)):
i+=1
# print('1111111111111')
# if ang_list != ang:
# ang_list = ang
print('----------', ang[j])
t = threading.Thread(target=get_ang)
t.setDaemon(True)
t.start()
def move():
try:
mc.send_angles(init_angles, 5)
time.sleep(0.1)
# for _ in range(50):
# mc.send_angles(start_angles, 5)
# wait_time(6)
# mc.send_angles(end_angles, 5)
# wait_time(6)
except Exception as e:
print(e)
while True:
t = wait_time(1)
print('dddd', t)
move()

164
mycobot_ai/ai_mecharm_270/scripts/color_test.py
View file

@ -1,164 +0,0 @@
# ecoding=utf-8
import cv2
import numpy as np
from imutils import contours
import math
'''HSV中的颜色空间
https://blog.csdn.net/wsp_1138886114/article/details/80660014
'''
color_dist = {'red': {'Lower': np.array([0, 120, 120]), 'Upper': np.array([6, 255, 255])},
# 'blue': {'Lower': np.array([100, 80, 46]), 'Upper': np.array([124, 255, 255])},
# 'blue':{'Lower': np.array([100,43,46]),'Upper': np.array([124,255,255])},
'blue':{'Lower': np.array([100,43,46]),'Upper': np.array([124,255,255])},
"cyan": {'Lower': np.array([78, 43, 46]),'Upper': np.array([99, 255, 255])},
'green': {'Lower': np.array([35, 43, 35]), 'Upper': np.array([90, 255, 255])},
'yellow': {"Lower": np.array([22, 93, 0]), "Upper": np.array([45, 255, 255])},
# 'orange': {"Lower": np.array([0, 100, 45]), "Upper":np.array([255, 250, 255])},
'orange': {"Lower": np.array([11, 43, 46]), "Upper":np.array([25, 255, 255])},
}
cap = cv2.VideoCapture(0)
# cap = cv2.VideoCapture("src/test_1_2.mp4")
cap.set(3,420)
cap.set(4,360)
cv2.namedWindow('camera', cv2.WINDOW_AUTOSIZE)
# 内核
kernel = np.ones((5, 5), np.uint8)
while cap.isOpened():
ret, frame = cap.read()
if ret:
if frame is not None:
gs_frame = cv2.GaussianBlur(frame, (5, 5), 0)# 高斯模糊
hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV) # 转化成HSV图像
erode_hsv = cv2.erode(hsv, None, iterations=2) # 腐蚀 作用是粗的变细
for _color in color_dist:
_font_x_pos = 0
_font_y_pos = 0
## 颜色阈值识别
inRange_hsv = cv2.inRange(erode_hsv, color_dist[_color]['Lower'], color_dist[_color]['Upper'])
# 膨胀操作
dilation = cv2.dilate(inRange_hsv, kernel, iterations=1)
# 闭操作
closing = cv2.morphologyEx(dilation, cv2.MORPH_CLOSE, kernel)
# 边缘检测
edges = cv2.Canny(closing, 10, 20)
# 检测物体边框
cnts, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
# 判断轮廓数量也就是判断是否寻找到轮廓，如果没有找到轮廓就不继续进行操作
if len(cnts) > 0:
for cnt in cnts:
if cv2.contourArea(cnt) >1500:
peri = cv2.arcLength(cnt,True)
#print(peri)
# 用于获得轮廓的近似值，使用cv2.drawCountors进行画图操作 ，
# 参数说明：cnt为输入的轮廓值， epsilon为阈值T，通常使用轮廓的周长作为阈值，True表示的是轮廓是闭合的
'''
cv2.approxPolyDP
@:param
cnt:
epsilon :算法参数
True：表示是否闭合
'''
approx = cv2.approxPolyDP(cnt,0.02*peri,True)
# print(len(approx))
# 提取拐点
'''
返回列表元素，列表中的元素代表一个边沿信息。
'''
objCor = len(approx)
if objCor ==3:
'''https://blog.51cto.com/hiszm/5201991'''
objectType = "Triangle"
mm = cv2.moments(cnt)
cx = int(mm['m10']/mm['m00'])
cy = int(mm['m01']/mm['m00'])
_font_x_pos = cx
_font_y_pos = cy
cv2.circle(frame,(cx,cy),3,(0,0,255),-1)
cv2.drawContours(frame, [cnt], 0, (0, 0, 255), 3)
print("三角形的坐标",cx,cy)
# 检测出是矩形
elif objCor == 4:
# 可旋转矩形，即最小的外包矩形
rect= cv2.minAreaRect(cnt)
'''
@:param
函数 cv2.minAreaRect() 返回一个Box2D结构 rect：（最小外接矩形的中心（x，y），（宽度，高度），旋转角度）。
分别对应于返回值：(rect[0][0], rect[0][1]), (rect[1][0], rect[1][1]), rect[2]
'''
# 中心点坐标
pos_x = int(rect[0][0])
pos_y = int(rect[0][1])
#print("position",pos_x,pos_y)
# 旋转角度
theta = np.round(cv2.minAreaRect(cnt)[2],2)
box = cv2.boxPoints(rect)
box = np.int0(box)
print("物体的坐标为",(pos_x,pos_y),"旋转角度为",theta)
# 判断长方形还是正方形
_font_x_pos = box[0][0]
_font_y_pos = box[0][1]
_W = math.sqrt(math.pow((box[0][0] - box[1][0]), 2) + math.pow((box[0][1] - box[1][1]), 2))
_H = math.sqrt(math.pow((box[0][0] - box[3][0]), 2) + math.pow((box[0][1] - box[3][1]), 2))
# 长宽比
aspRatio = _W/float(_H)
if aspRatio >0.98 and aspRatio <1.03:
objectType= "Square"#正方形
else:
objectType="Rectangle"#长方形
_pos = (pos_x,pos_y)
cv2.circle(frame,_pos,1,(0,0,255),2)#绘制中心点
#cv2.putText(frame, 'teheta = ' + str(theta), (int(_font_x_pos),int(_font_y_pos+20)), cv2.FONT_HERSHEY_COMPLEX_SMALL,0.8, (0, 255, 0) )
cv2.drawContours(frame,[box],0,(0,0,255),2)
# 圆形
elif objCor>4:
objectType= "Circles"
'''
void minEnclosingCircle(InputArray points, Point2f& center, float& radius)
@:param
points：输入信息，可以为包含点的容器(vector)或是Mat。
center：包覆圆形的圆心。
radius：包覆圆形的半径。
'''
(x,y),radius = cv2.minEnclosingCircle(cnt)
center = (int(x),int(y))
radius = int(radius)
_font_x_pos = center[0]
_font_y_pos = center[1]
print("物体的圆心为:",(_font_x_pos, _font_y_pos),"半径为",radius)
cv2.circle(frame,center,1,(255,0,0),2)#绘制中心点
cv2.circle(frame,center,radius,(255,0,0),2)
else:
objectType="None"
# objectType 物体形状
print("颜色类别：",_color)
cv2.putText(frame, str(_color + objectType), (int(_font_x_pos),int(_font_y_pos)), cv2.FONT_HERSHEY_COMPLEX_SMALL,1, (0, 255, 0) )
cv2.imshow('camera', frame)
# 存储识别结果视频
# out.write(frame)
cv2.waitKey(1)
if cv2.waitKey(10) & 0xFF == 27:
break
else:
print("无画面")
break
else:
print("无法读取摄像头！")
break
cap.release()
# out.release()
cv2.waitKey(0)
cv2.destroyAllWindows()

480
mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py
View file

@ -1,480 +0,0 @@
# encoding:utf-8
#!/usr/bin/env python2
import cv2
import numpy as np
import time
import os,sys
import rospy
from visualization_msgs.msg import Marker
from pymycobot.mycobot import MyCobot
from moving_utils import Movement
IS_CV_4 = cv2.__version__[0] == '4'
__version__ = "1.0"
# Adaptive seeed
class Object_detect(Movement):
def __init__(self, camera_x = 155, camera_y = 10):
# inherit the parent class
super(Object_detect, self).__init__()
# get path of file
dir_path = os.path.dirname(__file__)
self.mc = None
# 移动角度
self.move_angles = [
[-7.11, -6.94, -55.01, -24.16, 0, -15], # init the point
[18.8, -7.91, -54.49, -23.02, -0.79, -14.76], # point to grab
]
# 移动坐标
self.move_coords = [
[132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # above the red bucket
[238.8, -124.1, 204.3, -169.69, -5.52, -96.52], # green
[115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # blue
[-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # gray
]
# which robot: USB* is m5; ACM* is wio; AMA* is raspi
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
self.raspi = False
if "dev" in self.robot_m5:
self.Pin = [2, 5]
elif "dev" in self.robot_wio:
# self.Pin = [20, 21]
self.Pin = [2, 5]
for i in self.move_coords:
i[2] -= 20
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
import RPi.GPIO as GPIO
GPIO.setwarnings(False)
self.GPIO = GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setup(20, GPIO.OUT)
GPIO.setup(21, GPIO.OUT)
GPIO.output(20, 1)
GPIO.output(21, 1)
self.raspi = True
if self.raspi:
self.gpio_status(False)
# choose place to set cube
self.color = 0
# parameters to calculate camera clipping parameters
self.x1 = self.x2 = self.y1 = self.y2 = 0
# set cache of real coord
self.cache_x = self.cache_y = 0
# set color HSV
self.HSV = {
"yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])],
"red": [np.array([0, 43, 46]), np.array([8, 255, 255])],
"green": [np.array([35, 43, 35]), np.array([90, 255, 255])], # [77, 255, 255]
"blue": [np.array([100, 43, 46]), np.array([124, 255, 255])],
"cyan": [np.array([89, 43, 46]), np.array([99, 255, 255])], # np.array([78, 43, 46]), np.array([99, 255, 255])
}
# use to calculate coord between cube and mycobot
self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
# The coordinates of the grab center point relative to the mycobot
self.camera_x, self.camera_y = camera_x, camera_y
# The coordinates of the cube relative to the mycobot
self.c_x, self.c_y = 0, 0
# The ratio of pixels to actual values
self.ratio = 0
# Get ArUco marker dict that can be detected.
self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
# Get ArUco marker params.
self.aruco_params = cv2.aruco.DetectorParameters_create()
# init a node and a publisher
rospy.init_node("marker", anonymous=True)
self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
# init a Marker
self.marker = Marker()
self.marker.header.frame_id = "/joint1"
self.marker.ns = "cube"
self.marker.type = self.marker.CUBE
self.marker.action = self.marker.ADD
self.marker.scale.x = 0.04
self.marker.scale.y = 0.04
self.marker.scale.z = 0.04
self.marker.color.a = 1.0
self.marker.color.g = 1.0
self.marker.color.r = 1.0
# marker position initial
self.marker.pose.position.x = 0
self.marker.pose.position.y = 0
self.marker.pose.position.z = 0.03
self.marker.pose.orientation.x = 0
self.marker.pose.orientation.y = 0
self.marker.pose.orientation.z = 0
self.marker.pose.orientation.w = 1.0
# publish marker
def pub_marker(self, x, y, z=0.03):
self.marker.header.stamp = rospy.Time.now()
self.marker.pose.position.x = x
self.marker.pose.position.y = y
self.marker.pose.position.z = z
self.marker.color.g = self.color
self.pub.publish(self.marker)
def gpio_status(self, flag):
if flag:
# self.GPIO.output(20, 0)
self.GPIO.output(21, 0)
else:
# self.GPIO.output(20, 1)
self.GPIO.output(21, 1)
# 开启吸泵 m5
def pump_on(self):
# 让2号位工作
# self.mc.set_basic_output(2, 0)
# 让5号位工作
self.mc.set_basic_output(5, 0)
# 停止吸泵 m5
def pump_off(self):
# 让2号位停止工作
# self.mc.set_basic_output(2, 1)
# 让5号位停止工作
self.mc.set_basic_output(5, 1)
# Grasping motion
def move(self, x, y, color):
# send Angle to move mycobot
print (color)
self.mc.send_angles(self.move_angles[1], 25)
time.sleep(3)
# send coordinates to move mycobot
self.mc.send_coords([x, y, 190.6, 179.87, -3.78, -62.75], 25, 1) # usb :rx,ry,rz -173.3, -5.48, -57.9
time.sleep(3)
# self.mc.send_coords([x, y, 150, 179.87, -3.78, -62.75], 25, 0)
# time.sleep(3)
self.mc.send_coords([x, y, 103, 179.87, -3.78, -62.75], 25, 0)
time.sleep(3)
# open pump
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
self.pump_on()
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
self.gpio_status(True)
time.sleep(1.5)
tmp = []
while True:
if not tmp:
tmp = self.mc.get_angles()
else:
break
time.sleep(0.5)
# print(tmp)
self.mc.send_angles([tmp[0], -0.71, -54.49, -23.02, -0.79, tmp[5]],25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76]
time.sleep(2.5)
self.pub_marker(
self.move_coords[2][0]/1000.0, self.move_coords[2][1]/1000.0, self.move_coords[2][2]/1000.0)
self.mc.send_coords(self.move_coords[color], 25, 1)
self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color]
[1]/1000.0, self.move_coords[color][2]/1000.0)
time.sleep(3)
# close pump
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
self.pump_off()
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
self.gpio_status(False)
time.sleep(4)
if color == 1:
self.pub_marker(
self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02)
elif color == 0:
self.pub_marker(
self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0)
# self.pub_angles(self.move_angles[0], 20)
self.mc.send_angles(self.move_angles[0], 25)
time.sleep(3)
# decide whether grab cube
def decide_move(self, x, y, color):
print(x, y, self.cache_x, self.cache_y)
# detect the cube status move or run
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
self.cache_x, self.cache_y = x, y
return
else:
self.cache_x = self.cache_y = 0
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
self.move(x, y, color)
# init mycobot
def run(self):
if "dev" in self.robot_wio :
self.mc = MyCobot(self.robot_wio, 115200)
elif "dev" in self.robot_m5:
self.mc = MyCobot(self.robot_m5, 115200)
elif "dev" in self.robot_raspi:
self.mc = MyCobot(self.robot_raspi, 1000000)
if not self.raspi:
self.pub_pump(False, self.Pin)
self.mc.send_angles([-7.11, -6.94, -55.01, -24.16, 0, -15], 20)
time.sleep(3)
# draw aruco
def draw_marker(self, img, x, y):
# draw rectangle on img
cv2.rectangle(
img,
(x - 20, y - 20),
(x + 20, y + 20),
(0, 255, 0),
thickness=2,
lineType=cv2.FONT_HERSHEY_COMPLEX,
)
# add text on rectangle
cv2.putText(img, "({},{})".format(x, y), (x, y),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,)
# get points of two aruco
def get_calculate_params(self, img):
# Convert the image to a gray image
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Detect ArUco marker.
corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
gray, self.aruco_dict, parameters=self.aruco_params
)
"""
Two Arucos must be present in the picture and in the same order.
There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
Determine the center of the aruco by the four corners of the aruco.
"""
if len(corners) > 0:
if ids is not None:
if len(corners) <= 1 or ids[0] == 1:
return None
x1 = x2 = y1 = y2 = 0
point_11, point_21, point_31, point_41 = corners[0][0]
x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int(
(point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0)
point_1, point_2, point_3, point_4 = corners[1][0]
x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int(
(point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0)
return x1, x2, y1, y2
return None
# set camera clipping parameters
def set_cut_params(self, x1, y1, x2, y2):
self.x1 = int(x1)
self.y1 = int(y1)
self.x2 = int(x2)
self.y2 = int(y2)
print(self.x1, self.y1, self.x2, self.y2)
# set parameters to calculate the coords between cube and mycobot
def set_params(self, c_x, c_y, ratio):
self.c_x = c_x
self.c_y = c_y
self.ratio = 220.0/ratio
# calculate the coords between cube and mycobot
def get_position(self, x, y):
return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y)
"""
Calibrate the camera according to the calibration parameters.
Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
If two ARuco values have been calculated, clip the video.
"""
def transform_frame(self, frame):
# enlarge the image by 1.5 times
fx = 1.5
fy = 1.5
frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy,
interpolation=cv2.INTER_CUBIC)
if self.x1 != self.x2:
# the cutting ratio here is adjusted according to the actual situation
frame = frame[int(self.y2*0.78):int(self.y1*1.1),
int(self.x1*0.86):int(self.x2*1.08)]
return frame
# detect cube color
def color_detect(self, img):
# set the arrangement of color'HSV
x = y = 0
gs_img = cv2.GaussianBlur(img, (3, 3), 0) # 高斯模糊
# transfrom the img to model of gray
hsv = cv2.cvtColor(gs_img, cv2.COLOR_BGR2HSV)
for mycolor, item in self.HSV.items():
redLower = np.array(item[0])
redUpper = np.array(item[1])
# wipe off all color expect color in range
mask = cv2.inRange(hsv, item[0], item[1])
# a etching operation on a picture to remove edge roughness
erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2)
# the image for expansion operation, its role is to deepen the color depth in the picture
dilation = cv2.dilate(erosion, np.ones(
(1, 1), np.uint8), iterations=2)
# adds pixels to the image
target = cv2.bitwise_and(img, img, mask=dilation)
# the filtered image is transformed into a binary image and placed in binary
ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY)
# get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected
contours, hierarchy = cv2.findContours(
dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if len(contours) > 0:
# do something about misidentification
boxes = [
box
for box in [cv2.boundingRect(c) for c in contours]
if min(img.shape[0], img.shape[1]) / 10
< min(box[2], box[3])
< min(img.shape[0], img.shape[1]) / 1
]
if boxes:
for box in boxes:
x, y, w, h = box
# find the largest object that fits the requirements
c = max(contours, key=cv2.contourArea)
# get the lower left and upper right points of the positioning object
x, y, w, h = cv2.boundingRect(c)
# locate the target by drawing rectangle
cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2)
# calculate the rectangle center
x, y = (x*2+w)/2, (y*2+h)/2
# calculate the real coordinates of mycobot relative to the target
if mycolor == "red":
self.color = 0
elif mycolor == "green":
self.color = 1
elif mycolor == "cyan" or mycolor == "blue":
self.color = 2
else:
self.color = 3
if abs(x) + abs(y) > 0:
return x, y
else:
return None
if __name__ == "__main__":
# open the camera
cap_num = 0
cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
if not cap.isOpened():
cap.open()
# init a class of Object_detect
detect = Object_detect()
# init mycobot
detect.run()
_init_ = 20 #
init_num = 0
nparams = 0
num = 0
real_sx = real_sy = 0
while cv2.waitKey(1) < 0:
# read camera
_, frame = cap.read()
# deal img
frame = detect.transform_frame(frame)
if _init_ > 0:
_init_ -= 1
continue
# calculate the parameters of camera clipping
if init_num < 20:
if detect.get_calculate_params(frame) is None:
cv2.imshow("figure", frame)
continue
else:
x1, x2, y1, y2 = detect.get_calculate_params(frame)
detect.draw_marker(frame, x1, y1)
detect.draw_marker(frame, x2, y2)
detect.sum_x1 += x1
detect.sum_x2 += x2
detect.sum_y1 += y1
detect.sum_y2 += y2
init_num += 1
continue
elif init_num == 20:
detect.set_cut_params(
(detect.sum_x1)/20.0,
(detect.sum_y1)/20.0,
(detect.sum_x2)/20.0,
(detect.sum_y2)/20.0,
)
detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
init_num += 1
continue
# calculate params of the coords between cube and mycobot
if nparams < 10:
if detect.get_calculate_params(frame) is None:
cv2.imshow("figure", frame)
continue
else:
x1, x2, y1, y2 = detect.get_calculate_params(frame)
detect.draw_marker(frame, x1, y1)
detect.draw_marker(frame, x2, y2)
detect.sum_x1 += x1
detect.sum_x2 += x2
detect.sum_y1 += y1
detect.sum_y2 += y2
nparams += 1
continue
elif nparams == 10:
nparams += 1
# calculate and set params of calculating real coord between cube and mycobot
detect.set_params(
(detect.sum_x1+detect.sum_x2)/20.0,
(detect.sum_y1+detect.sum_y2)/20.0,
abs(detect.sum_x1-detect.sum_x2)/10.0 +
abs(detect.sum_y1-detect.sum_y2)/10.0
)
print ("ok")
continue
# get detect result
detect_result = detect.color_detect(frame)
if detect_result is None:
cv2.imshow("figure", frame)
continue
else:
x, y = detect_result
# calculate real coord between cube and mycobot
real_x, real_y = detect.get_position(x, y)
if num == 20:
detect.pub_marker(real_sx/20.0/1000.0, real_sy/20.0/1000.0)
detect.decide_move(real_sx/20.0, real_sy/20.0, detect.color)
num = real_sx = real_sy = 0
else:
num += 1
real_sy += real_y
real_sx += real_x
cv2.imshow("figure", frame)

555
mycobot_ai/ai_mycobot_280/scripts/aikit_img.py
View file

@ -1,555 +0,0 @@
# 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
from PIL import Image
from threading import Thread
from pymycobot.mypalletizer import MyPalletizer
IS_CV_4 = cv2.__version__[0] == '4'
__version__ = "1.0" # Adaptive seeed
class Object_detect():
def __init__(self, camera_x = 160, camera_y = 10):
# inherit the parent class
super(Object_detect, self).__init__()
# declare mypal260
self.mc = None
# 移动角度
self.move_angles = [
[0, 0, 0, 0], # init the point
[-29.0, 5.88, -4.92, -76.28], # point to grab
[17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab
]
# 移动坐标
self.move_coords = [
[132.6, -155.6, 211.8, -20.9], # above the red bucket
[232.5, -134.1, 197.7, -45.26], # above the green bucket
[111.6, 159, 221.5, -120], # above the blue bucket
[-15.9, 164.6, 217.5, -119.35], # above the gray bucket
]
# choose place to set cube
self.color = 0
# parameters to calculate camera clipping parameters
self.x1 = self.x2 = self.y1 = self.y2 = 0
# set cache of real coord
self.cache_x = self.cache_y = 0
# use to calculate coord between cube and mycobot
self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
# The coordinates of the grab center point relative to the mycobot
self.camera_x, self.camera_y = camera_x, camera_y
# The coordinates of the cube relative to the mycobot
self.c_x, self.c_y = 0, 0
# The ratio of pixels to actual values
self.ratio = 0
# Get ArUco marker dict that can be detected.
self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
# Get ArUco marker params.
self.aruco_params = cv2.aruco.DetectorParameters_create()
# 开启吸泵 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, 110, 0], 20, 0)
time.sleep(1.5)
# open pump
self.pump_on()
time.sleep(1.5)
self.mc.send_angle(2, 0, 20)
time.sleep(0.3)
self.mc.send_angle(3, -18, 20)
time.sleep(2)
self.mc.send_coords(self.move_coords[color], 20, 1)
time.sleep(3)
# close pump
self.pump_off()
time.sleep(6)
self.mc.send_angles(self.move_angles[1], 20)
time.sleep(1.5)
self.mc.send_angles([-30, 0, 0, 0], 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减小,向后方移动
print('start...')
self.move(x, y, color)
print('end....')
# init mypal260
def run(self):
self.mc = MyPalletizer("COM9", 115200)
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/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mypalletizer_260/' + folder
path2 = r'D:/BaiduSyncdisk/PythonProject/OpenCV' + folder
# if os.path.exists(path1):
# path = path1
# elif os.path.exists(path2):
path = path1
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 = 1
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(1)
while cv2.waitKey(1) < 0:
_, frame = cap.read()
frame = process_transform_frame(frame, x1, y1, x2, y2)
if connection.poll():
request = connection.recv()
if request == GET_FRAME:
connection.send(frame)
elif request == CLEAR_DRAW:
coord = None
dst = None
elif type(request) is tuple:
if request[0] == DRAW_COORDS:
coord = request[1]
elif request[0] == DRAW_RECT:
dst = request[1]
elif request[0] == CROP_FRAME:
x1 = request[1]
y1 = request[2]
x2 = request[3]
y2 = request[4]
if not coord is None:
cv2.putText(frame, "{}".format(coord), (50, 60), fontFace=None,
fontScale=1, color=(0, 255, 0), lineType=1)
if not dst is None:
frame = cv2.polylines(frame, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
cv2.imshow("figure", frame)
time.sleep(0.04)
connection.send(STOP_PROCESSING)
def run():
parent_conn, child_conn = Pipe()
child = Process(target = process_display_frame, args=(child_conn,))
child.start()
res_queue = [[], [], [], []]
res_queue[0] = parse_folder('res/red')
res_queue[1] = parse_folder('res/green')
res_queue[2] = parse_folder('res/blue')
res_queue[3] = parse_folder('res/gray')
sift = cv2.xfeatures2d.SIFT_create()
kp_list, desc_list = compute_keypoints_and_descriptors(sift, res_queue)
# init a class of Object_detect
detect = Object_detect()
# init mycobot
detect.run()
# _init_ = 20 #
init_num = 0
nparams = 0
# num = 0
# real_sx = real_sy = 0
while True:
start_time = time.time()
if parent_conn.poll():
data = parent_conn.recv()
if data == STOP_PROCESSING:
break
# read camera
frame = get_frame(parent_conn)
# deal img
#frame = detect.transform_frame(frame)
# if _init_ > 0:
# _init_ -= 1
# continue
# calculate the parameters of camera clipping
if init_num < 20:
if detect.get_calculate_params(frame) is None:
# cv2.imshow("figure", frame)
continue
else:
x1, x2, y1, y2 = detect.get_calculate_params(frame)
detect.draw_marker(frame, x1, y1)
detect.draw_marker(frame, x2, y2)
detect.sum_x1 += x1
detect.sum_x2 += x2
detect.sum_y1 += y1
detect.sum_y2 += y2
init_num += 1
continue
elif init_num == 20:
detect.set_cut_params(
(detect.sum_x1) / 20.0,
(detect.sum_y1) / 20.0,
(detect.sum_x2) / 20.0,
(detect.sum_y2) / 20.0,
)
parent_conn.send((CROP_FRAME,
(detect.sum_x1) / 20.0,
(detect.sum_y1) / 20.0,
(detect.sum_x2) / 20.0,
(detect.sum_y2) / 20.0))
detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
init_num += 1
continue
# calculate params of the coords between cube and mycobot
if nparams < 10:
if detect.get_calculate_params(frame) is None:
# cv2.imshow("figure", frame)
continue
else:
x1, x2, y1, y2 = detect.get_calculate_params(frame)
detect.draw_marker(frame, x1, y1)
detect.draw_marker(frame, x2, y2)
detect.sum_x1 += x1
detect.sum_x2 += x2
detect.sum_y1 += y1
detect.sum_y2 += y2
nparams += 1
print ("ok")
continue
elif nparams == 10:
nparams += 1
# calculate and set params of calculating real coord between cube and mycobot
detect.set_params((detect.sum_x1 + detect.sum_x2) / 20.0,
(detect.sum_y1 + detect.sum_y2) / 20.0,
abs(detect.sum_x1 - detect.sum_x2) / 10.0 +
abs(detect.sum_y1 - detect.sum_y2) / 10.0)
print("ok")
continue
# get detect result
kp_img, desc_img = sift.detectAndCompute(frame, None)
frame = get_frame(parent_conn)
for i, v in enumerate(res_queue):
# HACK: to update frame every time
detect_result = detect.obj_detect(frame, v, kp_img, desc_img, kp_list[i], desc_list[i], parent_conn)
if detect_result:
x, y = detect_result
# calculate real coord between cube and mycobot
real_x, real_y = detect.get_position(x, y)
detect.color = i
detect.pub_marker(real_x / 1000.0, real_y / 1000.0)
detect.decide_move(real_x, real_y, detect.color)
# if num == 5:
# detect.color = i
# detect.pub_marker(real_sx / 5.0 / 1000.0,
# real_sy / 5.0 / 1000.0)
# detect.decide_move(real_sx / 5.0, real_sy / 5.0,
# detect.color)
# num = real_sx = real_sy = 0
# else:
# num += 1
# real_sy += real_y
# real_sx += real_x
parent_conn.send(CLEAR_DRAW)
# cv2.imshow("figure", frame)
time.sleep(0.05)
end_time = time.time()
# print("loop_time = ", end_time - start_time)
# close the window
if cv2.waitKey(1) & 0xFF == ord('q'):
# cap.release()
cv2.destroyAllWindows()
sys.exit()
child.join()
if __name__ == "__main__":
run()
# Object_detect().take_photo()
# Object_detect().cut_photo()

248
mycobot_ai/ai_mycobot_280/scripts/encode_test.py
View file

@ -1,248 +0,0 @@
# encoding: UTF-8
#!/usr/bin/env python2
import cv2 as cv
import os
import numpy as np
import time
import rospy
from visualization_msgs.msg import Marker
from moving_utils import Movement
# y轴偏移量
pump_y = -55
# x轴偏移量
pump_x = 15
class Detect_marker(Movement):
def __init__(self):
super(Detect_marker, self).__init__()
# set cache of real coord
self.cache_x = self.cache_y = 0
# which robot
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
self.raspi = False
if "dev" in self.robot_m5:
self.Pin = [2, 5]
elif "dev" in self.robot_wio:
self.Pin = [20, 21]
# 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
self.GPIO = GPIO
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(20, GPIO.OUT)
GPIO.setup(21, GPIO.OUT)
self.raspi = True
if self.raspi:
self.gpio_status(False)
else:
self.pub_pump(False, self.Pin)
# Creating a Camera Object
cap_num = 0
self.cap = cv.VideoCapture(cap_num, cv.CAP_V4L)
# Get ArUco marker dict that can be detected.
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
# Get ArUco marker params.
self.aruco_params = cv.aruco.DetectorParameters_create()
self.calibrationParams = cv.FileStorage(
"calibrationFileName.xml", cv.FILE_STORAGE_READ)
# Get distance coefficient.
self.dist_coeffs = self.calibrationParams.getNode("distCoeffs").mat()
height = self.cap.get(4)
focal_length = width = self.cap.get(3)
center = [width / 2, height / 2]
# Calculate the camera matrix.
self.camera_matrix = np.array(
[
[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1],
],
dtype=np.float32,
)
# init a node and a publisher
rospy.init_node("encode_marker", anonymous=True)
self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
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
self.marker.color.r = 0.3
self.marker.color.g = 0.3
self.marker.color.b = 0.3
# marker position initial
self.marker.pose.position.x = 0
self.marker.pose.position.y = 0
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
# Grasping motion
def move(self, x, y):
if self.raspi:
coords = [
[145.6, -64.9, 285.2, 179.88, 7.67, 179],
[130.1, -155.6, 243.9, 178.99, 5.38, -179.9]
]
else:
coords = [
[135.0, -65.5, 280.1, 178.99, 5.38, -179.9],
[136.1, -141.6, 243.9, 178.99, 5.38, -179.9]
]
# publish marker
self.marker.header.stamp = rospy.Time.now()
self.marker.pose.position.x = (coords[0][0]-x)/1000.0
self.marker.pose.position.y = (coords[0][1]-y)/1000.0
self.pub.publish(self.marker)
# send coordinates to move mycobot
self.pub_coords(coords[0], 30, 1)
time.sleep(2)
self.pub_coords([coords[0][0]-x, coords[0][1]-y,
240, 178.99, 5.38, -179.9], 25, 1)
time.sleep(2)
self.pub_coords([coords[0][0]-x, coords[0][1]-y,
200, 178.99, 5.38, -179.9], 25, 1)
time.sleep(2)
if "dev" in self.robot_m5 or self.raspi:
self.pub_coords([coords[0][0]-x, coords[0][1]-y,
90, 178.99, 5.38, -179.9], 25, 1)
elif "dev" in self.robot_wio:
self.pub_coords([coords[0][0]-x+20, coords[0][1] -
y-10, 70, 178.99, 5.38, -179.9], 25, 1)
time.sleep(2)
if self.raspi:
self.gpio_status(True)
else:
self.pub_pump(True, self.Pin)
time.sleep(1)
self.pub_coords(coords[0], 30, 1)
time.sleep(3)
self.pub_coords(coords[1], 30, 1)
time.sleep(2)
if self.raspi:
self.gpio_status(False)
else:
self.pub_pump(False, self.Pin)
# publish marker
time.sleep(1)
self.marker.header.stamp = rospy.Time.now()
self.marker.pose.position.x = coords[1][0]/1000.0
self.marker.pose.position.y = coords[1][1]/1000.0
self.pub.publish(self.marker)
self.pub_coords(coords[0], 30, 1)
time.sleep(2)
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)
# decide whether grab cube
def decide_move(self, x, y):
print(x, y)
# detect the cube status move or run
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
self.cache_x, self.cache_y = x, y
return
else:
self.cache_x = self.cache_y = 0
if "dev" in self.robot_jes:
if x > -20:
y += 10
if y > -25:
x -= 5
x += 10
self.move(x, y)
# init mycobot
def init_mycobot(self):
for _ in range(5):
print(_)
self.pub_coords([145.6, -64.9, 285.2, 179.88, 7.67, 179], 20, 1)
time.sleep(0.5)
def run(self):
global pump_y, pump_x
self.init_mycobot()
num = sum_x = sum_y = 0
while cv.waitKey(1) < 0:
success, img = self.cap.read()
if not success:
print("It seems that the image cannot be acquired correctly.")
break
# transfrom the img to model of gray
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
# Detect ArUco marker.
corners, ids, rejectImaPoint = cv.aruco.detectMarkers(
gray, self.aruco_dict, parameters=self.aruco_params
)
font = cv.FONT_HERSHEY_SIMPLEX
if len(corners) > 0:
if ids is not None:
# get informations of aruco
ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.03, self.camera_matrix, self.dist_coeffs
)
# rvec:rotation offset,tvec:translation deviator
(rvec, tvec) = (ret[0], ret[1])
(rvec - tvec).any()
xyz = tvec[0, 0, :]
# calculate the coordinates of the aruco relative to the pump
xyz = [round(xyz[0]*1000+pump_y, 2), round(xyz[1]
* 1000+pump_x, 2), round(xyz[2]*1000, 2)]
cv.putText(img, "Id: " + str(ids[0][0]), (0, 40), font, 0.6, (0, 255, 0), 2, cv.LINE_AA)
for i in range(rvec.shape[0]):
# draw the aruco on img
cv.aruco.drawDetectedMarkers(img, corners)
cv.aruco.drawAxis(
img,
self.camera_matrix,
self.dist_coeffs,
rvec[i, :, :],
tvec[i, :, :],
0.03,
)
if num < 40:
if self.raspi:
sum_x -= 30
sum_x += xyz[1]
sum_y += xyz[0]
num += 1
elif num == 40:
self.decide_move(sum_x/40.0, sum_y/40.0)
num = sum_x = sum_y = 0
cv.imshow("encode_image", img)
if __name__ == "__main__":
detect = Detect_marker()
detect.run()

557
mycobot_ai/ai_mypalletizer_260/scripts/aikit_img.py
View file

@ -1,557 +0,0 @@
# encoding=utf-8
#!/usr/bin/env python2
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
from PIL import Image
from threading import Thread
from pymycobot.mypalletizer import MyPalletizer
IS_CV_4 = cv2.__version__[0] == '4'
__version__ = "1.0" # Adaptive seeed
__metaclass__ = type
class Object_detect():
def __init__(self, camera_x = 160, camera_y = 10):
# inherit the parent class
super(Object_detect, self).__init__()
# declare mypal260
self.mc = None
# 移动角度
self.move_angles = [
[0, 0, 0, 0], # init the point
[-29.0, 5.88, -4.92, -76.28], # point to grab
[17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab
]
# 移动坐标
self.move_coords = [
[132.6, -155.6, 211.8, -20.9], # above the red bucket
[232.5, -134.1, 197.7, -45.26], # above the green bucket
[111.6, 159, 221.5, -120], # above the blue bucket
[-15.9, 164.6, 217.5, -119.35], # above the gray bucket
]
# choose place to set cube
self.color = 0
# parameters to calculate camera clipping parameters
self.x1 = self.x2 = self.y1 = self.y2 = 0
# set cache of real coord
self.cache_x = self.cache_y = 0
# use to calculate coord between cube and mycobot
self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
# The coordinates of the grab center point relative to the mycobot
self.camera_x, self.camera_y = camera_x, camera_y
# The coordinates of the cube relative to the mycobot
self.c_x, self.c_y = 0, 0
# The ratio of pixels to actual values
self.ratio = 0
# Get ArUco marker dict that can be detected.
self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
# Get ArUco marker params.
self.aruco_params = cv2.aruco.DetectorParameters_create()
# 开启吸泵 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, 110, 0], 20, 0)
time.sleep(1.5)
# open pump
self.pump_on()
time.sleep(1.5)
self.mc.send_angle(2, 0, 20)
time.sleep(0.3)
self.mc.send_angle(3, -18, 20)
time.sleep(2)
self.mc.send_coords(self.move_coords[color], 20, 1)
time.sleep(3)
# close pump
self.pump_off()
time.sleep(6)
self.mc.send_angles(self.move_angles[1], 20)
time.sleep(1.5)
self.mc.send_angles([-30, 0, 0, 0], 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减小,向后方移动
print('start...')
self.move(x, y, color)
print('end....')
# init mypal260
def run(self):
self.mc = MyPalletizer("/dev/ttyUSB0", 115200)
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/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mypalletizer_260/' + folder
path2 = r'D:/BaiduSyncdisk/PythonProject/OpenCV' + folder
# if os.path.exists(path1):
# path = path1
# elif os.path.exists(path2):
path = path1
for i, j, k in os.walk(path):
for l in k:
restore.append(cv2.imread(folder + '/{}'.format(l)))
return restore
def compute_keypoints_and_descriptors(sift, images_lists):
kp_list = []
desc_list = []
for images in images_lists:
kp_tmp = []
desc_tmp = []
for img in images:
kp, desc = sift.detectAndCompute(img, None)
kp_tmp.append(kp)
desc_tmp.append(desc)
kp_list.append(kp_tmp)
desc_list.append(desc_tmp)
return kp_list, desc_list
GET_FRAME = 1
STOP_PROCESSING = 2
DRAW_COORDS = 3
DRAW_RECT = 4
CLEAR_DRAW = 5
CROP_FRAME = 6
def get_frame(connection):
connection.send(GET_FRAME)
frame = connection.recv()
return frame
def process_transform_frame(frame, x1, y1, x2, y2):
# enlarge the image by 1.5 times
fx = 1.5
fy = 1.5
frame = cv2.resize(frame, (0, 0),
fx=fx,
fy=fy,
interpolation=cv2.INTER_CUBIC)
# if x1 != x2:
# the cutting ratio here is adjusted according to the actual situation
# frame = frame[int(y2 * 0.2):int(y1 * 1.15),
# int(x1 * 0.7):int(x2 * 1.15)]
return frame
def process_display_frame(connection):
cap_num = 0
coord = None
dst = None
x1 = 0
y1 = 0
x2 = 0
y2 = 0
cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
if not cap.isOpened():
cap.open()
while cv2.waitKey(1) < 0:
_, frame = cap.read()
frame = process_transform_frame(frame, x1, y1, x2, y2)
if connection.poll():
request = connection.recv()
if request == GET_FRAME:
connection.send(frame)
elif request == CLEAR_DRAW:
coord = None
dst = None
elif type(request) is tuple:
if request[0] == DRAW_COORDS:
coord = request[1]
elif request[0] == DRAW_RECT:
dst = request[1]
elif request[0] == CROP_FRAME:
x1 = request[1]
y1 = request[2]
x2 = request[3]
y2 = request[4]
if not coord is None:
cv2.putText(frame, "{}".format(coord), (50, 60), fontFace=None,
fontScale=1, color=(0, 255, 0), lineType=1)
if not dst is None:
frame = cv2.polylines(frame, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
cv2.imshow("figure", frame)
time.sleep(0.04)
connection.send(STOP_PROCESSING)
def run():
parent_conn, child_conn = Pipe()
child = Process(target = process_display_frame, args=(child_conn,))
child.start()
res_queue = [[], [], [], []]
res_queue[0] = parse_folder('res/red')
res_queue[1] = parse_folder('res/green')
res_queue[2] = parse_folder('res/blue')
res_queue[3] = parse_folder('res/gray')
sift = cv2.xfeatures2d.SIFT_create()
kp_list, desc_list = compute_keypoints_and_descriptors(sift, res_queue)
# init a class of Object_detect
detect = Object_detect()
# init mycobot
detect.run()
# _init_ = 20 #
init_num = 0
nparams = 0
# num = 0
# real_sx = real_sy = 0
while True:
start_time = time.time()
if parent_conn.poll():
data = parent_conn.recv()
if data == STOP_PROCESSING:
break
# read camera
frame = get_frame(parent_conn)
# deal img
#frame = detect.transform_frame(frame)
# if _init_ > 0:
# _init_ -= 1
# continue
# calculate the parameters of camera clipping
if init_num < 20:
if detect.get_calculate_params(frame) is None:
# cv2.imshow("figure", frame)
continue
else:
x1, x2, y1, y2 = detect.get_calculate_params(frame)
detect.draw_marker(frame, x1, y1)
detect.draw_marker(frame, x2, y2)
detect.sum_x1 += x1
detect.sum_x2 += x2
detect.sum_y1 += y1
detect.sum_y2 += y2
init_num += 1
continue
elif init_num == 20:
detect.set_cut_params(
(detect.sum_x1) / 20.0,
(detect.sum_y1) / 20.0,
(detect.sum_x2) / 20.0,
(detect.sum_y2) / 20.0,
)
parent_conn.send((CROP_FRAME,
(detect.sum_x1) / 20.0,
(detect.sum_y1) / 20.0,
(detect.sum_x2) / 20.0,
(detect.sum_y2) / 20.0))
detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
init_num += 1
continue
# calculate params of the coords between cube and mycobot
if nparams < 10:
if detect.get_calculate_params(frame) is None:
# cv2.imshow("figure", frame)
continue
else:
x1, x2, y1, y2 = detect.get_calculate_params(frame)
detect.draw_marker(frame, x1, y1)
detect.draw_marker(frame, x2, y2)
detect.sum_x1 += x1
detect.sum_x2 += x2
detect.sum_y1 += y1
detect.sum_y2 += y2
nparams += 1
print ("ok")
continue
elif nparams == 10:
nparams += 1
# calculate and set params of calculating real coord between cube and mycobot
detect.set_params((detect.sum_x1 + detect.sum_x2) / 20.0,
(detect.sum_y1 + detect.sum_y2) / 20.0,
abs(detect.sum_x1 - detect.sum_x2) / 10.0 +
abs(detect.sum_y1 - detect.sum_y2) / 10.0)
print("ok")
continue
# get detect result
kp_img, desc_img = sift.detectAndCompute(frame, None)
frame = get_frame(parent_conn)
for i, v in enumerate(res_queue):
# HACK: to update frame every time
detect_result = detect.obj_detect(frame, v, kp_img, desc_img, kp_list[i], desc_list[i], parent_conn)
if detect_result:
x, y = detect_result
# calculate real coord between cube and mycobot
real_x, real_y = detect.get_position(x, y)
detect.color = i
# detect.pub_marker(real_x / 1000.0, real_y / 1000.0)
detect.decide_move(real_x, real_y, detect.color)
# if num == 5:
# detect.color = i
# detect.pub_marker(real_sx / 5.0 / 1000.0,
# real_sy / 5.0 / 1000.0)
# detect.decide_move(real_sx / 5.0, real_sy / 5.0,
# detect.color)
# num = real_sx = real_sy = 0
# else:
# num += 1
# real_sy += real_y
# real_sx += real_x
parent_conn.send(CLEAR_DRAW)
# cv2.imshow("figure", frame)
time.sleep(0.05)
end_time = time.time()
# print("loop_time = ", end_time - start_time)
# close the window
if cv2.waitKey(1) & 0xFF == ord('q'):
# cap.release()
cv2.destroyAllWindows()
sys.exit()
child.join()
if __name__ == "__main__":
run()
# Object_detect().take_photo()
# Object_detect().cut_photo()