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