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

Optimize aikit code

Browse source
This commit is contained in:
weiquan 2021-10-27 09:32:05 +08:00
parent c723b2bc0f
commit c213331a31
2 changed files with 257 additions and 222 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

64
mycobot_ai/scripts/detect_obj_color.py
View file

@ -15,6 +15,7 @@ IS_CV_4 = cv2.__version__[0] == '4'
__version__ = "1.0" __version__ = "1.0"
# Adaptive seeed # Adaptive seeed
class Object_detect(Movement): class Object_detect(Movement):
def __init__(self, camera_x=150, camera_y=-10): def __init__(self, camera_x=150, camera_y=-10):
@ -31,13 +32,14 @@ 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
[228.2, -127.8, 260.9, -157.51, -17.5, -71.18], # above the yello bucket # above the yello bucket
[208.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],
] ]
# which robot # which robot
self.robot = os.popen("ls /dev/ttyUSB*") self.robot = os.popen("ls /dev/ttyUSB*").readline()[:-1]
if "dev" in self.robot: if "dev" in self.robot:
self.Pin = [2, 5] self.Pin = [2, 5]
else: else:
@ -88,7 +90,6 @@ class Object_detect(Movement):
self.marker.color.g = 1.0 self.marker.color.g = 1.0
self.marker.color.r = 1.0 self.marker.color.r = 1.0
# 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
@ -140,33 +141,38 @@ class Object_detect(Movement):
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)
self.pub_marker(self.move_coords[2][0]/1000.0, self.move_coords[2][1]/1000.0, self.move_coords[2][2]/1000.0) self.pub_marker(
self.move_coords[2][0]/1000.0, self.move_coords[2][1]/1000.0, self.move_coords[2][2]/1000.0)
self.pub_angles(self.move_angles[1], 20) self.pub_angles(self.move_angles[1], 20)
time.sleep(1.5) time.sleep(1.5)
self.pub_marker(self.move_coords[3][0]/1000.0, self.move_coords[3][1]/1000.0, self.move_coords[3][2]/1000.0) self.pub_marker(
self.move_coords[3][0]/1000.0, self.move_coords[3][1]/1000.0, self.move_coords[3][2]/1000.0)
self.pub_angles(self.move_angles[0], 20) self.pub_angles(self.move_angles[0], 20)
time.sleep(2) time.sleep(2)
self.pub_marker(self.move_coords[4][0]/1000.0, self.move_coords[4][1]/1000.0, self.move_coords[4][2]/1000.0) self.pub_marker(
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] 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][1]/1000.0, self.move_coords[color][2]/1000.0) self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color]
[1]/1000.0, self.move_coords[color][2]/1000.0)
time.sleep(2) time.sleep(2)
# close pump # close pump
self.pub_pump(False, self.Pin) self.pub_pump(False, self.Pin)
if color == 1: if color == 1:
self.pub_marker(self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02) self.pub_marker(
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.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.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.pub_angles(self.move_angles[0], 20)
time.sleep(3) time.sleep(3)
# decide whether grab cube # decide whether grab cube
def decide_move(self, x, y, color):
def decide_move(self, x, y, color):
print(x, y, self.cache_x, self.cache_y) print(x, y, self.cache_x, self.cache_y)
# detect the cube status move or run # detect the cube status move or run
@ -186,12 +192,18 @@ class Object_detect(Movement):
x -= 10 x -= 10
y += 10 y += 10
print x, y print x, y
else:
if x > 160:
y += 10
elif y < -20:
x -= 10
y += 10
self.move(x, y, color) self.move(x, y, color)
# init mycobot # init mycobot
def run(self): def run(self):
for _ in range(10): 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)
@ -209,7 +221,8 @@ class Object_detect(Movement):
lineType=cv2.FONT_HERSHEY_COMPLEX, lineType=cv2.FONT_HERSHEY_COMPLEX,
) )
# add text on rectangle # add text on rectangle
cv2.putText(img,"({},{})".format(x,y),(x,y),cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,) cv2.putText(img, "({},{})".format(x, y), (x, y),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,)
# get points of two aruco # get points of two aruco
def get_calculate_params(self, img): def get_calculate_params(self, img):
@ -231,9 +244,11 @@ class Object_detect(Movement):
return None return None
x1 = x2 = y1 = y2 = 0 x1 = x2 = y1 = y2 = 0
point_11, point_21, point_31, point_41 = corners[0][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) 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] 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) 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 x1, x2, y1, y2
return None return None
@ -260,14 +275,17 @@ class Object_detect(Movement):
Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times. 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. If two ARuco values have been calculated, clip the video.
""" """
def transform_frame(self, frame): def transform_frame(self, frame):
# enlarge the image by 1.5 times # enlarge the image by 1.5 times
fx = 1.5 fx = 1.5
fy = 1.5 fy = 1.5
frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy, interpolation=cv2.INTER_CUBIC) frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy,
interpolation=cv2.INTER_CUBIC)
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), int(self.x1*0.7):int(self.x2*1.15)] frame = frame[int(self.y2*0.2):int(self.y1*1.15),
int(self.x1*0.7):int(self.x2*1.15)]
return frame return frame
# detect cube color # detect cube color
@ -284,7 +302,8 @@ class Object_detect(Movement):
# a etching operation on a picture to remove edge roughness # a etching operation on a picture to remove edge roughness
erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2) 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 # 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) dilation = cv2.dilate(erosion, np.ones(
(1, 1), np.uint8), iterations=2)
# adds pixels to the image # adds pixels to the image
target = cv2.bitwise_and(img, img, mask=dilation) target = cv2.bitwise_and(img, img, mask=dilation)
# the filtered image is transformed into a binary image and placed in binary # the filtered image is transformed into a binary image and placed in binary
@ -327,8 +346,6 @@ class Object_detect(Movement):
return None return None
if __name__ == "__main__": if __name__ == "__main__":
# open the camera # open the camera
cap_num = 0 cap_num = 0
@ -351,7 +368,6 @@ if __name__ == "__main__":
# 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
@ -402,7 +418,8 @@ if __name__ == "__main__":
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
@ -427,6 +444,3 @@ if __name__ == "__main__":
real_sx += real_x real_sx += real_x
cv2.imshow("figure", frame) cv2.imshow("figure", frame)

67
mycobot_ai/scripts/detect_obj_img.py
View file

@ -17,6 +17,7 @@ from moving_utils import Movement
IS_CV_4 = cv2.__version__[0] == '4' IS_CV_4 = cv2.__version__[0] == '4'
__version__ = "1.0" # Adaptive seeed __version__ = "1.0" # Adaptive seeed
class Object_detect(Movement): class Object_detect(Movement):
def __init__(self, camera_x=150, camera_y=-10): def __init__(self, camera_x=150, camera_y=-10):
# inherit the parent class # inherit the parent class
@ -32,13 +33,13 @@ 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
[228.2, -127.8, 260.9, -157.51, -17.5, -71.18], # above the yello bucket [208.2, -127.8, 260.9, -157.51, -17.5, -71.18], # above the yello bucket
[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],
] ]
# 判断连接设备:ttyUSB*为M5，ttyACM*为seeed # 判断连接设备:ttyUSB*为M5，ttyACM*为seeed
self.robot = os.popen("ls /dev/ttyUSB*") self.robot = os.popen("ls /dev/ttyUSB*").readline()[:-1]
if "dev" in self.robot: if "dev" in self.robot:
self.Pin = [2, 5] self.Pin = [2, 5]
else: else:
@ -92,7 +93,6 @@ class Object_detect(Movement):
self.marker.color.g = 1.0 self.marker.color.g = 1.0
self.marker.color.r = 1.0 self.marker.color.r = 1.0
# 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
@ -104,6 +104,7 @@ class Object_detect(Movement):
self.cache_x = self.cache_y = 0 self.cache_x = self.cache_y = 0
# publish marker # publish marker
def pub_marker(self, x, y, z=0.03): def pub_marker(self, x, y, z=0.03):
self.marker.header.stamp = rospy.Time.now() self.marker.header.stamp = rospy.Time.now()
self.marker.pose.position.x = x self.marker.pose.position.x = x
@ -144,31 +145,37 @@ class Object_detect(Movement):
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)
self.pub_marker(self.move_coords[2][0]/1000.0, self.move_coords[2][1]/1000.0, self.move_coords[2][2]/1000.0) self.pub_marker(
self.move_coords[2][0]/1000.0, self.move_coords[2][1]/1000.0, self.move_coords[2][2]/1000.0)
self.pub_angles(self.move_angles[1], 20) self.pub_angles(self.move_angles[1], 20)
time.sleep(1.5) time.sleep(1.5)
self.pub_marker(self.move_coords[3][0]/1000.0, self.move_coords[3][1]/1000.0, self.move_coords[3][2]/1000.0) self.pub_marker(
self.move_coords[3][0]/1000.0, self.move_coords[3][1]/1000.0, self.move_coords[3][2]/1000.0)
self.pub_angles(self.move_angles[0], 20) self.pub_angles(self.move_angles[0], 20)
time.sleep(1.5) time.sleep(1.5)
self.pub_marker(self.move_coords[4][0]/1000.0, self.move_coords[4][1]/1000.0, self.move_coords[4][2]/1000.0) self.pub_marker(
self.move_coords[4][0]/1000.0, self.move_coords[4][1]/1000.0, self.move_coords[4][2]/1000.0)
self.pub_coords(self.move_coords[color], 20, 1) self.pub_coords(self.move_coords[color], 20, 1)
self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color][1]/1000.0, self.move_coords[color][2]/1000.0) self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color]
[1]/1000.0, self.move_coords[color][2]/1000.0)
time.sleep(2) time.sleep(2)
# close pump # close pump
self.pub_pump(False, self. self.pub_pump(False, self.
Pin) Pin)
if color == 1: if color == 1:
self.pub_marker(self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02) self.pub_marker(
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.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.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.pub_angles(self.move_angles[0], 20)
time.sleep(3) time.sleep(3)
# decide whether grab cube # decide whether grab cube
def decide_move(self, x, y, color): def decide_move(self, x, y, color):
print(x, y, self.cache_x, self.cache_y) print(x, y, self.cache_x, self.cache_y)
# detect the cube status move or run # detect the cube status move or run
@ -186,6 +193,11 @@ Pin)
elif x > 170: elif x > 170:
x -= 10 x -= 10
y += 10 y += 10
else:
y += 10
x -= 5
if y < -20:
y += 5
# print x,y # print x,y
self.move(x, y, color) self.move(x, y, color)
@ -209,7 +221,8 @@ Pin)
lineType=cv2.FONT_HERSHEY_COMPLEX, lineType=cv2.FONT_HERSHEY_COMPLEX,
) )
# add text on rectangle # add text on rectangle
cv2.putText(img,"({},{})".format(x,y),(x,y),cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,) cv2.putText(img, "({},{})".format(x, y), (x, y),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,)
# get points of two aruco # get points of two aruco
def get_calculate_params(self, img): def get_calculate_params(self, img):
@ -231,9 +244,11 @@ Pin)
return None return None
x1 = x2 = y1 = y2 = 0 x1 = x2 = y1 = y2 = 0
point_11, point_21, point_31, point_41 = corners[0][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) 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] 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) 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 x1, x2, y1, y2
return None return None
@ -260,14 +275,17 @@ Pin)
Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times. 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. If two ARuco values have been calculated, clip the video.
""" """
def transform_frame(self, frame): def transform_frame(self, frame):
# enlarge the image by 1.5 times # enlarge the image by 1.5 times
fx = 1.5 fx = 1.5
fy = 1.5 fy = 1.5
frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy, interpolation=cv2.INTER_CUBIC) frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy,
interpolation=cv2.INTER_CUBIC)
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), int(self.x1*0.7):int(self.x2*1.15)] frame = frame[int(self.y2*0.2):int(self.y1*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
@ -275,9 +293,8 @@ Pin)
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):
# rows, cols = frame.shape[:-1] # rows, cols = frame.shape[:-1]
# Resize image and swap BGR to RGB. # Resize image and swap BGR to RGB.
@ -364,7 +381,8 @@ Pin)
[kp2[m.trainIdx].pt for m in good]).reshape(-1, 1, 2) [kp2[m.trainIdx].pt for m in good]).reshape(-1, 1, 2)
# Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵 # Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0) M, mask = cv2.findHomography(
src_pts, dst_pts, cv2.RANSAC, 5.0)
matchesMask = mask.ravel().tolist() matchesMask = mask.ravel().tolist()
# Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置 # Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置
h, w, d = goal[i].shape h, w, d = goal[i].shape
@ -375,8 +393,10 @@ Pin)
cv2.putText(img, "{}".format(ccoord), (50, 60), fontFace=None, cv2.putText(img, "{}".format(ccoord), (50, 60), fontFace=None,
fontScale=1, color=(0, 255, 0), lineType=1) fontScale=1, color=(0, 255, 0), lineType=1)
print(format(dst[0][0][0])) 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 x = (dst[0][0][0]+dst[1][0][0] +
y = (dst[0][0][1]+dst[1][0][1]+dst[2][0][1]+dst[3][0][1])/4.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 绘制边框 # bound box 绘制边框
img = cv2.polylines( img = cv2.polylines(
@ -398,6 +418,7 @@ Pin)
return x, y return x, y
else: else:
return None return None
def take_photo(self): def take_photo(self):
# 提醒用户操作字典 # 提醒用户操作字典
print("*********************************************") print("*********************************************")
@ -486,14 +507,14 @@ Pin)
return goal return goal
def run(stop): def run():
# Object_detect().take_photo() # Object_detect().take_photo()
# Object_detect().cut_photo() # Object_detect().cut_photo()
# goal = Object_detect().distinguist() # goal = Object_detect().distinguist()
goal = [] goal = []
path = os.getcwd()+'/local_photo/img' path = os.getcwd()+'/local_photo/img'
print path
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)))
@ -596,7 +617,7 @@ def run(stop):
if __name__ == "__main__": if __name__ == "__main__":
run(0) run()
# Object_detect().take_photo() # Object_detect().take_photo()
# Object_detect().cut_photo() # Object_detect().cut_photo()