def end_door(Chest):
while True:
t1 = cv2.getTickCount() # 时间计算
handling = Chest.imgs
cv2.imshow('handling', handling)
cv2.waitKey(1)
border = cv2.copyMakeBorder(handling,
12,
12,
16,
16,
borderType=cv2.BORDER_CONSTANT,
value=(255, 255, 255)) # 扩展白边，防止边界无法识别
handling = cv2.resize(border, (DIM[0], DIM[1]),
interpolation=cv2.INTER_CUBIC) # 将图片缩放
frame_gauss = cv2.GaussianBlur(handling, (21, 21), 0) # 高斯模糊
frame_hsv = cv2.cvtColor(frame_gauss, cv2.COLOR_BGR2HSV) # 将图片转换到HSV空间
frame_door_yellow = cv2.inRange(
frame_hsv, color_range['yellow_door'][0],
color_range['yellow_door'][1]) # 对原图像和掩模(颜色的字典)进行位运算
frame_door_black = cv2.inRange(
frame_hsv, color_range['black_door'][0],
color_range['black_door'][1]) # 对原图像和掩模(颜色的字典)进行位运算
frame_door = cv2.add(frame_door_yellow, frame_door_black)
open_pic = cv2.morphologyEx(frame_door, cv2.MORPH_OPEN,
np.ones((5, 5), np.uint8)) # 开运算 去噪点
closed_pic = cv2.morphologyEx(open_pic, cv2.MORPH_CLOSE,
np.ones((50, 50), np.uint8)) # 闭运算 封闭连接
(image, contours,
hierarchy) = cv2.findContours(closed_pic, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE) # 找出轮廓
areaMaxContour, area_max = getAreaMaxContour1(contours) # 找出最大轮廓
percent = round(100 * area_max / (DIM[0] * DIM[1]), 2) # 最大轮廓的百分比
# 根据比例得到是否前进的信息
if percent > 5: #检测到横杆
print(percent, "%")
print("有障碍 等待 contours len：", len(contours))
time.sleep(0.01)
else:
print(percent)
print("执行快走04-快走9步")
action_append("fastForward04")
print('Finish End Door')
break
def across_landmine_track_2(distance, step_num, landmine_right_movecont,
landmine_left_movecont):
"""return step_num,landmine_right_movecont,landmine_left_movecont
"""
if abs(distance) <= landmine_max_distance:
#if step_number % 6 == 0 and step_number != 0 :
# print("turn000R")
# action_append("turn000R")
print("Forwalk02")
action_append("Forwalk02") #向前进
action_append("turn000R")
step_num += 1
elif distance < -landmine_max_distance:
if abs(landmine_left_movecont - landmine_right_movecont) >= 12:
print("test")
landmine_right_movecont, landmine_left_movecont = landmine_devition_monitor(
landmine_right_movecont, landmine_left_movecont)
else:
landmine_left_movecont += 1
print("Left02move")
action_append("Left02move") #向左迈一步
elif distance > landmine_max_distance:
if abs(landmine_left_movecont - landmine_right_movecont) >= 12:
print("test")
landmine_right_movecont, landmine_left_movecont = landmine_devition_monitor(
landmine_right_movecont, landmine_left_movecont)
else:
landmine_right_movecont += 1
print("Right02move")
action_append("Right02move") #向右迈一步
return step_num, landmine_right_movecont, landmine_left_movecont
def Ball():
action_append("fastForward04")
action_append("fastForward04")
for i in range(8):
action_append("turn001L")
while len(action_list) != 0:
time.sleep(0.1)
def flap(ChestOrg_img):
org_img_copy = np.rot90(ChestOrg_img)[400:640, 80:400].copy()
handling = org_img_copy.copy()
frame_gauss = cv2.GaussianBlur(handling, (21, 21), 0) # 高斯模糊
frame_hsv = cv2.cvtColor(frame_gauss, cv2.COLOR_BGR2HSV) # 将图片转换到HSV空间
#h,s,v = cv2.split(frame_hsv)
#cv2.imshow("s space", s)
#cv2.waitKey(1)
frame_flap = cv2.inRange(frame_hsv, color_dist['flap']['Lower'],
color_dist['flap']['Upper'])
open_pic = cv2.morphologyEx(frame_flap, cv2.MORPH_OPEN,
np.ones((5, 5), np.uint8)) # 开运算 去噪点
closed_pic = cv2.morphologyEx(open_pic, cv2.MORPH_CLOSE,
np.ones((50, 50), np.uint8)) # 闭运算 封闭连接
contours, _ = cv2.findContours(closed_pic, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE) # 找出轮廓
areaMaxContour, area_max = getAreaMaxContour1(contours) # 找出最大轮廓
percent = round(100 * area_max / (DIM[0] * DIM[1]), 2) # 最大轮廓的百分比
print(percent, "%")
if areaMaxContour is not None:
rect = cv2.minAreaRect(areaMaxContour) # 矩形框选
box = np.int0(cv2.boxPoints(rect)) # 点的坐标
cv2.drawContours(handling, [box], 0, (153, 200, 0), 2) # 将最小外接矩形画在图上
cv2.imshow("flap test", handling)
cv2.waitKey(1)
if percent > 12:
print("roll")
#action_append("RollRail")
event_state = "finish_event"
else:
action_append("Forwalk00")
return event_state
def start_door(Chest):
waited = 0
event_step_startdoor = 0
while True:
t1 = cv2.getTickCount() # 时间计算
# cv2.imshow("ChestOrg_img", Chest.imgs)#[0])
#cv2.waitKey(1)
org_img_copy = np.rot90(Chest.imgs).copy()
handling = org_img_copy.copy()[100:250]
# cv2.imshow("HeadOrg_img", Head.imgs)
# cv2.waitKey(1)
# org_img_copy = Head.imgs.copy()
# handling = org_img_copy.copy()[150:350, :]
# handling = cv2.imread("D:\\akejian\\runningrobot\\videos\\3.PNG")
# handling = handling[150:350, :]
border = cv2.copyMakeBorder(handling,
12,
12,
16,
16,
borderType=cv2.BORDER_CONSTANT,
value=(255, 255, 255)) # 扩展白边，防止边界无法识别
handling = cv2.resize(border, (DIM[0], DIM[1]),
interpolation=cv2.INTER_CUBIC) # 将图片缩放
cv2.imshow("handling", handling)
cv2.waitKey(1)
frame_gauss = cv2.GaussianBlur(handling, (21, 21), 0) # 高斯模糊
frame_hsv = cv2.cvtColor(frame_gauss, cv2.COLOR_BGR2HSV) # 将图片转换到HSV空间
frame_door_yellow = cv2.inRange(
frame_hsv, color_range['yellow_door'][0],
color_range['yellow_door'][1]) # 对原图像和掩模(颜色的字典)进行位运算
frame_door_black = cv2.inRange(
frame_hsv, color_range['black_door_new'][0],
color_range['black_door_new'][1]) # 对原图像和掩模(颜色的字典)进行位运算
#frame_door = cv2.add(frame_door_yellow, frame_door_black)
open_pic_yellow = cv2.morphologyEx(frame_door_yellow, cv2.MORPH_OPEN,
np.ones((5, 5),
np.uint8)) # 开运算 去噪点
closed_pic_yellow = cv2.morphologyEx(open_pic_yellow, cv2.MORPH_CLOSE,
np.ones((50, 50),
np.uint8)) # 闭运算 封闭连接
(image_yellow, contours_yellow,
hierarchy_yellow) = cv2.findContours(closed_pic_yellow,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE) # 找出轮廓
areaMaxContour_yellow, area_max_yellow = getAreaMaxContour1(
contours_yellow) # 找出最大轮廓
percent_yellow = round(100 * area_max_yellow / (DIM[0] * DIM[1]),
2) # 最大轮廓的百分比
open_pic_black = cv2.morphologyEx(frame_door_black, cv2.MORPH_OPEN,
np.ones((5, 5), np.uint8)) # 开运算 去噪点
closed_pic_black = cv2.morphologyEx(open_pic_black, cv2.MORPH_CLOSE,
np.ones((50, 50),
np.uint8)) # 闭运算 封闭连接
(image_black, contours_black,
hierarchy_black) = cv2.findContours(closed_pic_black,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE) # 找出轮廓
areaMaxContour_black, area_max_black = getAreaMaxContour1(
contours_black) # 找出最大轮廓
percent_black = round(100 * area_max_black / (DIM[0] * DIM[1]),
2) # 最大轮廓的百分比
cv2.imshow("image_yellow", image_yellow)
#cv2.imshow("image_black", image_black)
cv2.waitKey(1)
# 根据比例得到是否前进的信息
if event_step_startdoor == 1:
if percent_yellow < 2:
event_step_startdoor = 2
break
else:
event_step_startdoor = 0
action_append("Back2Run")
while len(action_list) != 0:
time.sleep(0.1)
else:
if percent_yellow > 2: #检测到横杆
print(percent_yellow, "%")
print("有障碍 等待")
waited = 1
time.sleep(0.01)
else:
print(percent_yellow, "%")
if waited == 1:
print("执行快走05-快走12步")
action_append("fastForward06")
while len(action_list) != 0:
time.sleep(0.1)
event_step_startdoor = 1
cv2.destroyAllWindows()
else:
print("继续等吧，如果还没走过去横杆就下来了怎么办")
def baffle(Chest, resize_width, resize_height, baffleturn):
global baffle_step
ori_width = int(3 * 160) # 原始图像640x480
ori_height = int(4 * 160)
# cv2.imshow('frame2',frame2)
OrgFrame1 = Chest.imgs.copy()
# OrgFrame1 = cv2.imread('0.jpg')
# cv2.imshow('read',OrgFrame1)
(h, w) = OrgFrame1.shape[:2]
center = (w // 2, h // 2)
R = cv2.getRotationMatrix2D(center, 90, 1.0)
OrgFrame = cv2.warpAffine(OrgFrame1, R, (h, w))
cv2.imshow("Rotated by -90 Degrees", OrgFrame)
frame_Chest = cv2.resize(OrgFrame, (resize_width, resize_height),
interpolation=cv2.INTER_LINEAR)
cv2.imshow('init', frame_Chest)
center = []
# 开始处理图像
hsv = cv2.cvtColor(frame_Chest, cv2.COLOR_BGR2HSV) # rgv转hsv
hsv = cv2.GaussianBlur(hsv, (3, 3), 0) # 转完高斯滤波
# cv2.imshow('hsv', hsv)
Imask = cv2.inRange(hsv, color_dist_lza['flap']['Lower'],
color_dist_lza['flap']['Upper'])
# Imask = cv2.inRange(hsv, color_dist_lza['blue']['Lower'], color_dist_lza['blue']['Upper']) # 提取颜色，此处为蓝
# cv2.imshow('blue', Imask)
Imask = cv2.erode(Imask, None, iterations=2) # 腐蚀
Imask = cv2.dilate(Imask, np.ones((3, 3), np.uint8), iterations=2) # 膨胀
cv2.imshow('color', Imask)
cv2.waitKey(1)
_, cnts, hieracy = cv2.findContours(Imask, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_TC89_L1) # 找出所有轮廓
cnt_large = getAreaMaxContour2_lza(cnts, area=1000) # 找出最大轮廓
if cnt_large is None:
print('no blue')
action_append('Forwalk01') # 向前走
print('Forwalk01')
return
cnt_large_area = math.fabs(cv2.contourArea(cnt_large)) # 计算最大轮廓面积
print('the largest area : ', cnt_large_area)
if cnt_large_area > 4000: # 判断进入挡板关卡，执行下面命令
print('get ready for baffle')
rect = cv2.minAreaRect(cnt_large) # 最小外接矩形，存在
# cv2.imshow('lunkuo', rect)
box = np.int0(cv2.boxPoints(rect)) # 最小外接矩形的四个顶点
box[0, 0] = int(leMap_lza(box[0, 0], 0, resize_width, 0,
ori_width)) # 缩放图片，转换四个点的八个坐标值
box[0, 1] = int(leMap_lza(box[0, 1], 0, resize_height, 0, ori_height))
box[1, 0] = int(leMap_lza(box[1, 0], 0, resize_width, 0, ori_width))
box[1, 1] = int(leMap_lza(box[1, 1], 0, resize_height, 0, ori_height))
box[2, 0] = int(leMap_lza(box[2, 0], 0, resize_width, 0, ori_width))
box[2, 1] = int(leMap_lza(box[2, 1], 0, resize_height, 0, ori_height))
box[3, 0] = int(leMap_lza(box[3, 0], 0, resize_width, 0, ori_width))
box[3, 1] = int(leMap_lza(box[3, 1], 0, resize_height, 0, ori_height))
pt1_x, pt1_y = box[0, 0], box[0, 1]
pt3_x, pt3_y = box[2, 0], box[2, 1]
center_x = int((pt1_x + pt3_x) / 2)
center_y = int((pt1_y + pt3_y) / 2)
center.append([center_x, center_y]) # 矩形上边界中点添加到center列表里
cv2.drawContours(OrgFrame, [box], -1, [0, 0, 255, 255], 3)
cv2.circle(OrgFrame, (center_x, center_y), 10, (0, 0, 255), -1) # 画出中心
# 求得大矩形的旋转角度，if条件是为了判断长的一条边的旋转角度，因为box存储的点的顺序不确定\
if math.sqrt(
math.pow(box[3, 1] - box[0, 1], 2) +
math.pow(box[3, 0] - box[0, 0], 2)) > math.sqrt(
math.pow(box[3, 1] - box[2, 1], 2) +
math.pow(box[3, 0] - box[2, 0], 2)):
baffle_angle = -math.atan(
(box[3, 1] - box[0, 1]) /
(box[3, 0] - box[0, 0])) * 180.0 / math.pi
else:
baffle_angle = -math.atan(
(box[3, 1] - box[2, 1]) /
(box[3, 0] - box[2, 0])) * 180.0 / math.pi # 负号是因为坐标原点的问题
# 判断方向是否正直
print('angle: ', baffle_angle)
if baffle_angle > 5:
action_append('turn001L') # 转向
print('turn left')
return
elif baffle_angle < -5:
action_append('turn001R') # 转向
print('turn right')
return
else:
print('direction correct')
# 以外接矩形的中点横坐标来校准位置
print('center_x = ')
print(center_x)
if center_x > 288:
action_append('Right02move')
print('move right')
return
elif center_x < 268:
action_append('Left02move')
print('move left')
return
else:
print('location correct')
# 位置已校准，方向已对准，开始前进准备翻滚
# 获取roi区域图像处理判断面积
baffle_area_flag = False
frame2 = frame_Chest[80:240, 120:320] # 我的roi
cv2.imshow('roi', frame2)
center = []
# 开始处理图像
hsv2 = cv2.cvtColor(frame2, cv2.COLOR_BGR2HSV) # rgv转hsv
hsv2 = cv2.GaussianBlur(hsv2, (3, 3), 0) # 转完高斯滤波
# cv2.imshow('hsv2', hsv2)
Imask2 = cv2.inRange(hsv2, color_dist_lza['flap']['Lower'],
color_dist_lza['flap']['Upper']) # 提取颜色，此处为蓝
# cv2.imshow('blue2', Imask2)
Imask2 = cv2.erode(Imask2, None, iterations=2) # 腐蚀
Imask2 = cv2.dilate(Imask2, np.ones((3, 3), np.uint8),
iterations=2) # 膨胀
cv2.imshow('color2', Imask2)
_, cnts2, hieracy = cv2.findContours(
Imask2, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_TC89_L1) # 找出所有轮廓
cnt_large2 = getAreaMaxContour2_lza(cnts2, area=1000) # 找出最大轮廓
if cnt_large2 is None:
print('no blue')
action_append('Forwalk01') # 向前走
print('Forwalk01')
return
cnt_large_area2 = math.fabs(cv2.contourArea(cnt_large2)) # 计算最大轮廓面积
print('the largest area2 : ', cnt_large_area2)
if cnt_large_area2 > 14000: # 判断roi区域做出动作
action_append('Forwalk01')
baffle_area_flag = True
action_append('RollRail')
print('roll now ！')
if baffleturn:
action_append('turn004L')
action_append('turn004L')
action_append('Left3move')
action_append('turn004L')
action_append('Left3move')
else:
action_append('turn004R')
action_append('turn004R')
action_append('turn004R')
action_append('Right3move')
baffle_step = 1
else:
action_append('Forwalk01')
print('go forwalk')
cv2.putText(OrgFrame, "area:" + str(cnt_large_area),
(10, OrgFrame.shape[0] - 35), cv2.FONT_HERSHEY_SIMPLEX,
0.65, (0, 0, 255), 2)
cv2.putText(OrgFrame, "are_flag:" + str(baffle_area_flag),
(10, OrgFrame.shape[0] - 55), cv2.FONT_HERSHEY_SIMPLEX,
0.65, (0, 0, 255), 2)
cv2.putText(OrgFrame, "angle:" + str(baffle_angle),
(10, OrgFrame.shape[0] - 75), cv2.FONT_HERSHEY_SIMPLEX,
0.65, (0, 0, 255), 2)
cv2.putText(OrgFrame, "center_x:" + str(center_x),
(10, OrgFrame.shape[0] - 95), cv2.FONT_HERSHEY_SIMPLEX,
0.65, (0, 0, 255), 2)
cv2.imshow('OrgFrame', OrgFrame)
else:
action_append('Forwalk01') # 向前走
print('Forwalk01')
def turn():
for i in range(10):
action_append("turn001L")
while len(action_list) != 0:
time.sleep(0.1)
def bridge(Chest):
event_step_bridge = 0
counter_step = 0
while True:
img = undistort_chest(Chest.imgs)
img = img[50:430, 50:400].copy()
img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
cv2.imshow("img", img)
cv2.waitKey(1)
height = img.shape[0]
width = img.shape[1]
blank_img = np.zeros((height, width, 1), np.uint8)
h, s, v = cv2.split(img)
thresh_value, thresh_img = cv2.threshold(s, 110, 255,
cv2.THRESH_BINARY)
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (10, 10))
thresh = cv2.morphologyEx(thresh_img, cv2.MORPH_OPEN, kernel)
cv2.imshow("thresh", thresh)
_, contours, hierarchy = cv2.findContours(thresh, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
length = len(contours)
list1 = []
for i in range(length):
cnt = contours[i]
epsilon = 0.02 * cv2.arcLength(cnt, True)
approx = cv2.approxPolyDP(cnt, 20, True)
cv2.polylines(blank_img, [approx], 1, (255, 0, 0), 1, cv2.LINE_4)
if event_step_bridge == 0:
if (cv2.isContourConvex(approx)):
list1.append(approx)
flag_hole = True
else:
action_append("Forwalk01")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
else:
list1.append(approx)
flag_hole = True
if flag_hole == True:
area_max_contour, contour_area_max = getAreaMaxContour1(list1)
M = cv2.moments(area_max_contour)
cX = int(M["m10"] / (M["m00"] + 0.0001))
cY = int(M["m01"] / (M["m00"] + 0.0001))
midpoint = [cX, cY]
cv2.polylines(blank_img, [area_max_contour], 1, (255, 0, 255), 1,
cv2.LINE_4)
cv2.circle(blank_img, (cX, cY), 6, (255, 255, 255), -1)
cv2.imshow("blank_img", blank_img)
print(cX, cY)
else:
action_append("Forwalk01")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
cv2.waitKey(1)
if event_step_bridge == 0:
print("event_step_bridge =", event_step_bridge)
x_dis = abs(cX - 150)
print("cX =", cX)
if cX > 150:
if x_dis > 50:
print("Forwalk02")
action_append("Forwalk02")
while len(action_list) != 0:
time.sleep(0.1)
elif x_dis > 30:
print("Forwalk01")
action_append("Forwalk01")
while len(action_list) != 0:
time.sleep(0.1)
elif x_dis > 20:
print("Forwalk00")
action_append("Forwalk00")
while len(action_list) != 0:
time.sleep(0.1)
# else:
# print("Back1Run")
# action_append("Back1Run")
y_dis = abs(cY - 190)
print("cY =", cY)
if cY > 190:
if y_dis > 50:
if cX > 140:
print("Right3move")
action_append("Right3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
elif y_dis > 20:
if cX > 300:
print("Right3move")
action_append("Right3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
else:
if cY != 0 and cX <= 170:
event_step_bridge = 1
print("event_step_bridge =", event_step_bridge)
elif cY < 190 and cY != 0:
if y_dis > 50:
if cX > 140:
print("Left3move")
action_append("Left3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
elif y_dis > 20:
if cX > 300:
print("Left3move")
action_append("Left3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
else:
if cX <= 170:
event_step_bridge = 1
if cX == 0 and cY == 0:
action_append("Forwalk00")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
if event_step_bridge == 1:
print("event_step_bridge =", event_step_bridge)
lines = cv2.HoughLines(blank_img, 1, 3.14159 / 180, 30, None, 0, 0)
cv2.polylines(blank_img, [area_max_contour], 1, (255, 0, 255), 1,
cv2.LINE_4)
k = np.zeros(2, dtype=np.float32)
bb = np.zeros(2, dtype=np.float32)
#cnt=-1
large = 10
small = 10
if lines is not None:
for i in range(0, len(lines)):
rho = lines[i][0][0]
theta = lines[i][0][1]
if (theta > np.pi / 2):
if ((theta - np.pi / 2) < large):
large = theta - np.pi / 2
t_l = np.pi / 2 + large
a = math.cos(t_l)
b = math.sin(t_l)
x0 = a * rho
y0 = b * rho
pt1 = (int(x0 + 1000 * (-b)), int(y0 + 1000 * (a)))
pt2 = (int(x0 - 1000 * (-b)), int(y0 - 1000 * (a)))
k[0] = (pt2[1] - pt1[1]) / (pt2[0] - pt1[0])
bb[0] = float((pt1[1] * pt2[0] - pt2[1] * pt1[0]) /
(pt2[0] - pt1[0]))
else:
if ((np.pi / 2 - theta) < small):
small = np.pi / 2 - theta
t_s = np.pi / 2 - small
a = math.cos(t_s)
b = math.sin(t_s)
x0 = a * rho
y0 = b * rho
pt3 = (int(x0 + 1000 * (-b)), int(y0 + 1000 * (a)))
pt4 = (int(x0 - 1000 * (-b)), int(y0 - 1000 * (a)))
k[1] = (pt4[1] - pt3[1]) / (pt4[0] - pt3[0] +
0.0001)
bb[1] = float((pt3[1] * pt4[0] - pt4[1] * pt3[0]) /
(pt4[0] - pt3[0] + 0.0001))
cv2.line(img, pt1, pt2, (0, 255, 0), 1)
cv2.line(img, pt3, pt4, (0, 255, 0), 1)
cv2.imshow("frame", img)
#cv2.waitKey(1)
left_x_start = k[0] * 590 + bb[0] + 50
left_x_end = k[0] * 50 + bb[0] + 50
left_line = [left_x_end, left_x_start]
right_x_start = k[1] * 590 + bb[1] + 50
right_x_end = k[1] * 50 + bb[1] + 50
right_line = [right_x_end, right_x_start]
dx_l, deg_l, posi_left = Calculate_position_bridge(left_line)
print('left', dx_l, deg_l)
dx_r, deg_r, posi_right = Calculate_position_bridge(right_line)
print('right', dx_r, deg_r)
if posi_left == False and posi_right == False:
print("Right3move")
action_append("Right3move")
while len(action_list) != 0:
time.sleep(0.1)
elif posi_left == True and posi_right == True:
print("Left3move")
action_append("Left3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
if dx_l <= 8 or dx_r >= 12:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
elif dx_r <= 8 or dx_l >= 12:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
deg_avg = (deg_l + deg_r) / 2
if deg_avg > 5:
print("turn000L")
action_append("turn000L")
while len(action_list) != 0:
time.sleep(0.1)
elif deg_avg < -5:
print("turn000R")
action_append("turn000R")
while len(action_list) != 0:
time.sleep(0.1)
else:
event_step_bridge = 2
if event_step_bridge == 2:
print("event_step_bridge =", event_step_bridge)
blank_img = np.rot90(blank_img).copy()
#lines = cv2.HoughLines(blank_img,1,3.14159/180,30,None,0,0)
lines = cv2.HoughLinesP(blank_img,
1.0,
np.pi / 180,
100,
minLineLength=10,
maxLineGap=15)
if lines is None:
action_append("Forwalk01")
continue
final_image, Final_line, good = group_lines_and_draw_bridge(
blank_img, lines, 'Right')
if Final_line is None:
action_append("Forwalk01")
continue
# cv2.imshow("final_image", final_image)
# cv2.waitKey(1)
dx_line, deg_line, posi = Calculate_position_bridge(Final_line)
print(dx_line, deg_line)
if posi == True:
if dx_line <= 8:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
elif dx_line >= 12:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
else:
if dx_line <= 8:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
elif dx_line >= 12:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
if deg_line > 5:
for i in range(int(deg_line / 5) + 1):
print("turn000L")
action_append("turn000L")
while len(action_list) != 0:
time.sleep(0.1)
elif deg_line < -5:
for i in range(int(abs(deg_line) / 5) + 1):
print("turn000R")
action_append("turn000R")
while len(action_list) != 0:
time.sleep(0.1)
if dx_line >= 8 and dx_line <= 10:
if counter_step > 6:
action_append("fastForward04")
while len(action_list) != 0:
time.sleep(0.1)
event_step_bridge = 3
else:
print("Forwalk02")
action_append("Forwalk02")
while len(action_list) != 0:
time.sleep(0.1)
counter_step = counter_step + 1
if event_step_bridge == 3:
print('bridge finish')
break
def landmine_devition_monitor(landmine_right_movecont, landmine_left_movecont):
if landmine_left_movecont - landmine_right_movecont >= 12:
print("Right02move")
action_append("Right02move") #向右迈一步
action_append("Right02move") #向右迈一步
action_append("Right02move") #向右迈一步
landmine_right_movecont += 3
elif landmine_right_movecont - landmine_left_movecont >= 12:
print("Left02move")
action_append("Left02move") #向左迈一步
action_append("Left02move") #向左迈一步
action_append("Left02move") #向左迈一步
landmine_left_movecont += 3
return landmine_right_movecont, landmine_left_movecont
def landmine_lyu(Chest):
global landmine_end_flag_judge
global landmine_max_distance
landmine_max_distance = 150 #############
landmine_start_flag = False
landmine_end_flag_judge = False
landmine_right_movecont = 0
landmine_left_movecont = 0
landmine_end_flag = 0
step_number = 0
landmine_judge_loop_cont = 0
landmine_pass_loop_cont = 0
# lanemine_correcion(Head)
#开始地雷关卡开始判断
while (True):
# time.sleep(1)
if len(action_list) == 0:
landmine_judge_loop_cont += 1
origin_img = np.rot90(Chest.imgs).copy()
# cv2.imshow("origin", origin_img)
# cv2.waitKey(1)
img_test = origin_img.copy()
roi = img_test[410:600, 0:480] #取感兴趣区域
cv2.imshow("roi", roi)
cv2.waitKey(1)
roi_shape = roi.shape
#颜色阈值
color_high = np.array([95, 120, 55])
color_low = np.array([65, 44, 20])
# color_high = 50
# color_low = 0
#转到灰度空间
# roi_gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
roi_hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
# cv2.imshow("roi_hsv",roi_hsv)
#颜色筛选
# roi_mask = cv2.inRange(roi_gray, color_low, color_high)
roi_mask = cv2.inRange(roi_hsv, color_low, color_high)
cv2.imshow("roi_mask_inrange", roi_mask)
##腐蚀
#roi_mask = cv2.erode(roi_mask, np.ones((3,3)), iterations=1)
#cv2.imshow("roi_mask_erode",roi_mask)
#膨胀
roi_mask = cv2.dilate(roi_mask, np.ones((3, 3)), iterations=1)
# cv2.imshow("roi_mask",roi_mask)
#找轮廓
_, contours, _ = cv2.findContours(roi_mask, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
#image = cv2.drawContours(roi, contours, -1,(255,0,0),1 )
# cv2.imshow("contours",image)
#设置最大最小面积
min_size = 500
max_size = 4500
delete_list = []
#print(len(contours))
#筛选轮廓
for i in range(len(contours)):
shape = detectrectangle(contours[i]) #检测轮廓形状
#print(shape)
#轮廓可视化
# M = cv2.moments(contours[i])
# cX = int(M["m10"] / (M["m00"] + 0.001))
# cY = int(M["m01"] / (M["m00"] + 0.001))
contours[i] = contours[i].astype("int")
# cv2.drawContours(roi, [contours[i]], -1, (0, 255, 0), -1)
# cv2.putText(roi, shape, (cX, cY), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
# cv2.imshow("shapename",roi)
area = cv2.contourArea(contours[i]) #计算轮廓面积
#print(area)
#rect = cv2.minAreaRect(contours[i])#最小外接矩形
#area = rect[1][0] * rect[1][1]
#print(area)
#box = cv2.boxPoints(rect) # 获取最小外接矩形的4个顶点坐标(ps: cv2.boxPoints(rect) for OpenCV 3.x)
#box = np.int0(box)
#cv2.drawContours(roi, [box], 0, (0, 0, 255), 1) #画出来
#cv2.imshow("hehe",roi)
#筛选矩形且大小在范围内的轮廓
if shape == "rectangle" or shape == "pentagon" or shape == "circle":
shape_flag = True
else:
shape_flag = False
if shape_flag != True or (area < min_size) or (area >
max_size):
#print(i)
delete_list.append(i)
contours = delete_contours(contours, delete_list)
#print(len(contours)) #看看识别成功的轮廓数量
#判断符合情况的轮廓数量
if len(contours) >= 1:
landmine_start_flag = True
else:
landmine_start_flag = False
#for i in contours:
#print(cv2.arcLength(i,True))
#展示识别成功的轮廓
# image = cv2.drawContours(roi, contours, -1,(0,0,255),1 )
# cv2.imshow("contours5",image)
print(landmine_start_flag)
if landmine_start_flag == True:
print("start passing process")
break
else:
print("Forwalk02")
action_append("Forwalk02")
if landmine_judge_loop_cont == 4: ### 这个地方可能要改，取决于上个关卡结束至地雷关卡开始的距离（看看差了几个Forwalk02）
print("landmine judge failed, start passing process")
landmine_start_flag = True
break
#开始进行过地雷关卡
while (landmine_start_flag):
# time.sleep(2)
if len(action_list) == 0:
landmine_pass_loop_cont += 1
# if step_number % 2 == 0 and step_number != 0:
# print("turn000R")
# action_append("turn000R")
landmine_center = []
origin_img = Chest.imgs.copy()
origin_img = np.rot90(origin_img)
# cv2.imshow("origin", origin_img)
img_test = origin_img.copy()
roi = img_test[410:600, 0:480] #取感兴趣区域要近一些从脚底开始
cv2.imshow("roi", roi)
# cv2.waitKey(1)
roi_shape = roi.shape
###颜色阈值 根据实际情况调整
color_high = np.array([95, 120, 55])
color_low = np.array([65, 44, 20])
# color_high = 50
# color_low = 0
#转到hsv空间
# roi_gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
roi_hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
#cv2.imshow("roi_hsv",roi_hsv)
#颜色筛选
roi_mask = cv2.inRange(roi_hsv, color_low, color_high)
cv2.imshow("roi_mask_inrange", roi_mask)
##腐蚀
#roi_mask = cv2.erode(roi_mask, np.ones((3,3)), iterations=1)
#cv2.imshow("roi_mask_erode",roi_mask)
#膨胀
roi_mask = cv2.dilate(roi_mask, np.ones((3, 3)), iterations=1)
#cv2.imshow("roi_mask",roi_mask)
#找轮廓
_, contours, _ = cv2.findContours(roi_mask, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
image = cv2.drawContours(roi, contours, -1, (255, 0, 0), 1)
cv2.imshow("contours", image)
cv2.waitKey(1)
###设置最大最小面积 最大面积和最小面积都要稍微大一些，根据实际情况调整
min_size = 500
max_size = 4500
delete_list = []
shape_flag = False
#筛选轮廓
if len(contours) != 0:
for i in range(len(contours)):
shape = detectrectangle(contours[i]) #检测轮廓形状
#print(shape)
#轮廓可视化
# M = cv2.moments(contours[i])
# cX = int(M["m10"] / (M["m00"] + 0.001))
# cY = int(M["m01"] / (M["m00"] + 0.001))
contours[i] = contours[i].astype("int")
# cv2.drawContours(roi, [contours[i]], -1, (0, 255, 0), -1)
# cv2.putText(roi, shape, (cX, cY), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
#cv2.imshow("shapename",roi)
area = cv2.contourArea(contours[i]) #计算轮廓面积
#print(area)
#rect = cv2.minAreaRect(contours[i])#最小外接矩形
#area = rect[1][0] * rect[1][1]
#print(area)
#box = cv2.boxPoints(rect) # 获取最小外接矩形的4个顶点坐标(ps: cv2.boxPoints(rect) for OpenCV 3.x)
#box = np.int0(box)
#cv2.drawContours(roi, [box], 0, (0, 0, 255), 1) #画出来
#cv2.imshow("hehe",roi)
#筛选矩形且大小在范围内的轮廓
if shape == "rectangle" or shape == "circle" or shape == "pentagon":
shape_flag = True
else:
shape_flag = False
if shape_flag != True or (area < min_size) or (area >
max_size):
delete_list.append(i)
contours = delete_contours(contours, delete_list)
contour_num = len(contours) #筛选后的轮廓数量
print("the contour_num is" + str(contour_num))
#print(len(contours)) #看看识别成功的轮廓数量
for i in contours:
rect = cv2.minAreaRect(i) #最小外接矩形
box = cv2.boxPoints(rect)
box = np.int0(box) # 画出来
#cv2.drawContours(roi, [box], 0, (255, 0, 0), 1)
landmine_center.append((int(rect[0][0]), int(rect[0][1])))
print("landmine center is " + str(landmine_center))
#画点
# for i in landmine_center:
# cv2.circle(roi, i, 1, (0,0,255), 4)
#cv2.imshow("min_rectangle",roi)
if contour_num == 0:
distance = 500 #随便设的数值，只要够独一无二就行
if landmine_end_flag == 2:
print("begin end")
break
elif step_number > 8:
landmine_end_flag += 1
print(landmine_end_flag)
step_number, landmine_right_movecont, landmine_left_movecont = across_landmine_track_1(
distance, step_number, landmine_right_movecont,
landmine_left_movecont)
else:
step_number, landmine_right_movecont, landmine_left_movecont = across_landmine_track_1(
distance, step_number, landmine_right_movecont,
landmine_left_movecont)
elif contour_num == 1:
landmine_end_flag = 0
distance = landmine_center[0][0] - 240
step_number, landmine_right_movecont, landmine_left_movecont = across_landmine_track_1(
distance, step_number, landmine_right_movecont,
landmine_left_movecont)
elif contour_num == 2:
landmine_end_flag = 0
center_point_x_between_twolandmine = (
landmine_center[0][0] + landmine_center[1][0]) / 2
distance = center_point_x_between_twolandmine - 240
step_number, landmine_right_movecont, landmine_right_movecont = across_landmine_track_2(
distance, step_number, landmine_right_movecont,
landmine_left_movecont)
else:
landmine_end_flag = 0
first_large_y = 0
first_large_x = 0
seconde_large_y = 0
seconde_large_x = 0
for i in landmine_center:
if i[1] > first_large_y:
first_large_y = i[1]
first_large_x = i[0]
if i[1] > seconde_large_y and i[0] != first_large_x:
seconde_large_y = i[1]
seconde_large_x = i[0]
center_point_x_between_twolandmine = (first_large_x +
seconde_large_x) / 2
distance = center_point_x_between_twolandmine
step_number, landmine_right_movecont, landmine_right_movecont = across_landmine_track_2(
distance, step_number, landmine_right_movecont,
landmine_right_movecont)
else:
if landmine_end_flag == 2:
print("begin end")
break
elif step_number > 8:
landmine_end_flag += 1
print(landmine_end_flag)
print("Forwalk02")
action_append("Forwalk02")
action_append("turn000R")
step_number += 1
else:
print("Forwalk02")
action_append("Forwalk02")
action_append("turn000R")
step_number += 1
#停止关卡判断
if landmine_end_flag == 2 and step_number == 15:
movement_sum = landmine_left_movecont - landmine_right_movecont
if movement_sum > 0:
landmine_right_movecont += 1
print("Right02move")
action_append("Right02move")
elif movement_sum < 0:
landmine_left_movecont += 1
print("Left02move")
action_append("Left02move")
elif movement_sum == 0:
print("Robot Debug: Pass!")
landmine_end_flag_judge = True
break
def door(Head):
event_step_door = 0
while True:
door_left = 120
door_right = 520
blobs = Head.imgs[int(0):int(200),
int(door_left):int(door_right)].copy()
cv2.imshow("Head", blobs)
cv2.waitKey(1)
hsv = cv2.cvtColor(blobs, cv2.COLOR_BGR2HSV)
hsv = cv2.GaussianBlur(hsv, (3, 3), 0)
Imask = cv2.inRange(hsv, color_dist['blue']['Lower'],
color_dist['blue']['Upper'])
Imask = cv2.erode(Imask, None, iterations=2)
Imask = cv2.dilate(Imask, np.ones((3, 3), np.uint8), iterations=2)
_, cnts, hierarchy = cv2.findContours(Imask, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_TC89_L1)
if cnts == None:
action_append("Back1Run")
else:
cnt_sum = getSumContour(cnts, area=300)
if event_step_door == 1:
if cnt_sum is None:
for i in range(10):
action_append("turn001L")
while len(action_list) != 0:
time.sleep(0.1)
break
else:
if cnt_sum is not None:
rect = cv2.minAreaRect(cnt_sum)
box = np.int0(cv2.boxPoints(rect))
box_width = abs(box[0, 0] - box[2, 0]) / 2
center_x = (box[0, 0] + box[2, 0]) / 2 + door_left
print("center_x ", center_x)
cv2.drawContours(blobs, [box], -1, (0, 0, 255), 5)
cv2.imshow('door sum', blobs)
cv2.waitKey(1)
print("boxwidth", box_width)
x_dis = center_x - 320
if box_width > 50:
if x_dis > 50:
action_append("Right3move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
elif x_dis > 30:
action_append("Right02move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
elif x_dis > -50 and x_dis < -30:
action_append("Left02move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
elif x_dis < -50:
action_append("Left3move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
else:
action_append("Forwalk02") #Finish
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
continue
if box_width < 30:
action_append("Back1Run")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
else:
action_append("Forwalk01")
action_append("Forwalk01")
action_append("Forwalk01")
action_append("Forwalk01")
action_append("Forwalk01")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
event_step_door = 1
def afterbaffle_straight(resize_width, resize_height):
global baffle_step
global frame
#ret, frame = cap_chest.read()
# cv2.imshow('frame2',frame2)
OrgFrame1 = frame
# OrgFrame1 = cv2.imread('0.jpg')
# cv2.imshow('read',OrgFrame1)
(h, w) = OrgFrame1.shape[:2]
center = (w // 2, h // 2)
R = cv2.getRotationMatrix2D(center, 90, 1.0)
OrgFrame = cv2.warpAffine(OrgFrame1, R, (h, w))
cv2.imshow("after Rotated by -90 Degrees", OrgFrame)
frame = cv2.resize(OrgFrame, (resize_width, resize_height),
interpolation=cv2.INTER_LINEAR)
cv2.imshow('after init', frame)
center = []
# 开始处理图像
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # rgv转hsv
hsv = cv2.GaussianBlur(hsv, (3, 3), 0) # 转完高斯滤波
cv2.imshow('after hsv', hsv)
Imask = cv2.inRange(hsv, color_dist['flap']['Lower'],
color_dist['flap']['Upper'])
# Imask = cv2.inRange(hsv, color_dist['blue']['Lower'], color_dist['blue']['Upper']) # 提取颜色，此处为蓝
cv2.imshow('after blue', Imask)
Imask = cv2.erode(Imask, None, iterations=2) # 腐蚀
Imask = cv2.dilate(Imask, np.ones((3, 3), np.uint8), iterations=2) # 膨胀
cv2.imshow('after color', Imask)
_, cnts, hieracy = cv2.findContours(Imask, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_TC89_L1) # 找出所有轮廓
cnt_large = getAreaMaxContour2(cnts, area=500) # 找出最大轮廓
if cnt_large is None:
print('no blue')
action_append('Forwalk01') # 向前走
print('Forwalk4cm')
return
rect = cv2.minAreaRect(cnt_large) # 最小外接矩形，存在
box = np.int0(cv2.boxPoints(rect)) # 最小外接矩形的四个顶点
box[0, 0] = int(leMap(box[0, 0], 0, resize_width, 0,
ori_width)) # 缩放图片，转换四个点的八个坐标值
box[0, 1] = int(leMap(box[0, 1], 0, resize_height, 0, ori_height))
box[1, 0] = int(leMap(box[1, 0], 0, resize_width, 0, ori_width))
box[1, 1] = int(leMap(box[1, 1], 0, resize_height, 0, ori_height))
box[2, 0] = int(leMap(box[2, 0], 0, resize_width, 0, ori_width))
box[2, 1] = int(leMap(box[2, 1], 0, resize_height, 0, ori_height))
box[3, 0] = int(leMap(box[3, 0], 0, resize_width, 0, ori_width))
box[3, 1] = int(leMap(box[3, 1], 0, resize_height, 0, ori_height))
pt1_x, pt1_y = box[0, 0], box[0, 1]
pt3_x, pt3_y = box[2, 0], box[2, 1]
center_x = int((pt1_x + pt3_x) / 2)
center_y = int((pt1_y + pt3_y) / 2)
center.append([center_x, center_y]) # 矩形上边界中点添加到center列表里
cv2.drawContours(OrgFrame, [box], -1, [0, 0, 255, 255], 3)
cv2.circle(OrgFrame, (center_x, center_y), 10, (0, 0, 255), -1) # 画出中心
# 求得大矩形的旋转角度，if条件是为了判断长的一条边的旋转角度，因为box存储的点的顺序不确定\
if math.sqrt(
math.pow(box[3, 1] - box[0, 1], 2) +
math.pow(box[3, 0] - box[0, 0], 2)) > math.sqrt(
math.pow(box[3, 1] - box[2, 1], 2) +
math.pow(box[3, 0] - box[2, 0], 2)):
baffle_angle = -math.atan((box[3, 1] - box[0, 1]) /
(box[3, 0] - box[0, 0])) * 180.0 / math.pi
else:
baffle_angle = -math.atan(
(box[3, 1] - box[2, 1]) /
(box[3, 0] - box[2, 0])) * 180.0 / math.pi # 负号是因为坐标原点的问题
# 判断方向是否正直
print('angle: ', baffle_angle)
if baffle_angle > 5:
action_append('turn001L') # 转向
print('turn left')
return
elif baffle_angle < -5:
action_append('turn001R') # 转向
print('turn right')
return
else:
# 以外接矩形的中点横坐标来校准位置
print('center_x = ')
print(center_x)
if center_x > 288:
action_append('Right02move')
print('move right')
return
elif center_x < 268:
action_append('Left02move')
print('move left')
return
else:
action_append('Forwalk02')
action_append('Forwalk01')
for i in range(9):
action_append('turn004R')
baffle_step = 2
print('finish')
def step(Chest):
while True:
org_img_copy = np.rot90(Chest.imgs)
border = cv2.copyMakeBorder(org_img_copy,
12,
12,
16,
16,
borderType=cv2.BORDER_CONSTANT,
value=(255, 255, 255)) # 扩展白边，防止边界无法识别
handling = cv2.resize(border, (DIM[0], DIM[1]),
interpolation=cv2.INTER_CUBIC) # 将图片缩放
frame_gauss = cv2.GaussianBlur(handling, (21, 21), 0) # 高斯模糊
frame_hsv = cv2.cvtColor(frame_gauss, cv2.COLOR_BGR2HSV) # 将图片转换到HSV空间
frame_door_rgb = cv2.inRange(
frame_hsv, color_range['red blue green'][0],
color_range['red blue green'][1]) # 对原图像和掩模(颜色的字典)进行位运算
open_pic = cv2.morphologyEx(frame_door_rgb, cv2.MORPH_OPEN,
np.ones((5, 5), np.uint8)) # 开运算 去噪点
closed_pic = cv2.morphologyEx(open_pic, cv2.MORPH_CLOSE,
np.ones((50, 50), np.uint8)) # 闭运算 封闭连接
contours = cv2.findContours(closed_pic, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE) # 找出轮廓
areaMaxContour, area_max = getAreaMaxContour1(contours) # 找出最大轮廓
M = cv2.moments(areaMaxContour)
cX = int(M["m10"] / (M["m00"] + 0.0001))
cY = int(M["m01"] / (M["m00"] + 0.0001))
cv2.drawContours(closed_pic, contours, -1, (0, 255, 0), 2)
cv2.imshow("image", closed_pic)
cv2.waitKey(1)
percent = round(100 * area_max / (DIM[0] * DIM[1]), 2) # 最大轮廓的百分比
if cY > 190:
if y_dis > 50:
if cX > 140:
print("Right3move")
action_append("Right3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
elif y_dis > 20:
if cX > 300:
print("Right3move")
action_append("Right3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
else:
if cY != 0 and cX <= 170:
event_step_bridge = 1
print("event_step_bridge =", event_step_bridge)
elif cY < 190:
if y_dis > 50 and cY != 0:
if cX > 140:
print("Left3move")
action_append("Left3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
elif y_dis > 20 and cY != 0:
if cX > 300:
print("Left3move")
action_append("Left3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
else:
if cY != 0 and cX <= 170:
event_step_bridge = 1
if cX == 0 and cY == 0:
action_append("Forwalk02")
# 根据比例得到是否前进的信息
if percent > 15: #检测到横杆
print(percent, "%")
print("快走", len(contours))
action_append("fastForward04")
action_append("UpBridge")
action_append("DownBridge")
break
else:
print(percent, "%")
action_append("fastForward04")
def holeb(Chest):
counter_forwalk = 0
event_step = 0
while True:
if event_step == 0:
frame = undistort_chest(Chest.imgs)
img = frame[30:450, 50:400]
img_test = img.copy()
fsrc = np.array(img, dtype=np.float32) / 255.0
(b, g, r) = cv2.split(fsrc)
#lv
#gray = 2 * g - b - r
gray = 2*b - g- r
(minVal, maxVal, minLoc, maxLoc) = cv2.minMaxLoc(gray)
hist = cv2.calcHist([gray], [0], None, [256], [minVal, maxVal])
gray_u8 = np.array((gray - minVal) / (maxVal - minVal) * 255, dtype=np.uint8)
(t, thresh_img) = cv2.threshold(gray_u8, -1.0, 255, cv2.THRESH_OTSU+cv2.THRESH_BINARY_INV)
cv2.imshow("th", thresh_img)
cv2.imshow("th", thresh_img)
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (15, 15))
thresh = cv2.morphologyEx(thresh_img, cv2.MORPH_OPEN, kernel)
# edges = cv2.Canny(thresh,20,100,apertureSize = 3,L2gradient=0)
_, contours, _ = cv2.findContours(thresh, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
cv2.drawContours(img, contours, -1, (0, 255, 0), 2)
#cv2.imshow('c',img)
#cv2.waitKey(1)
length = len(contours)
list1 = []
flag_hole = False
for i in range(length):
cnt = contours[i]
#epsilon = 0.02*cv2.arcLength(cnt,True)
approx = cv2.approxPolyDP(cnt, 15, True)
if (({len(approx) == 4 or len(approx) == 5})
and (cv2.isContourConvex(approx))):
list1.append(approx)
flag_hole = True
if flag_hole == True:
area_max_contour, _ = getAreaMaxContour1(list1)
M = cv2.moments(area_max_contour)
cX = int(M["m10"] / (M["m00"] + 0.0001))
cY = int(M["m01"] / (M["m00"] + 0.0001))
# midpoint=[cX,cY]
print("cX is {} and cY is {}".format(cX, cY))
cv2.circle(img_test, (cX, cY), 6, 1, -1)
cv2.drawContours(img_test, area_max_contour, -1, (0, 0, 255),
2)
cv2.imshow("hole test", img_test)
cv2.waitKey(1)
x_dis = abs(cX - 150)
y_dis = abs(cY - 190)
# y_right = abs(300 - cY)
# y_left = abs(220 - cY)
print('x_dis =', x_dis)
if cX > 150:
if x_dis > 50:
action_append("Forwalk02")
action_append("turn000R")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
elif x_dis > 30:
action_append("Forwalk01")
action_append("turn000R")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
elif x_dis > 20:
action_append("Forwalk00")
action_append("turn000R")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
else:
event_step = 1
elif cX > 130 and cX < 170:
event_step = 1
else:
action_append("Back2Run")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
if cY > 190:
if y_dis > 50:
if cX > 140:
print("Right3move")
action_append("Right3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
elif y_dis > 20:
if cX > 300:
print("Right3move")
action_append("Right3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Right02move")
action_append("Right02move")
while len(action_list) != 0:
time.sleep(0.1)
else:
if cY != 0 and cX <= 170:
event_step_bridge = 1
print("event_step_bridge =", event_step_bridge)
elif cY < 190:
if y_dis > 50 and cY != 0:
if cX > 140:
print("Left3move")
action_append("Left3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
elif y_dis > 20 and cY != 0:
if cX > 300:
print("Left3move")
action_append("Left3move")
while len(action_list) != 0:
time.sleep(0.1)
else:
print("Left02move")
action_append("Left02move")
while len(action_list) != 0:
time.sleep(0.1)
else:
if cY != 0 and cX <= 170:
event_step_bridge = 1
# cv2.circle(img_test, (cX, cY), 6, 1, -1)
# cv2.drawContours(img_test, area_max_contour, -1, (0, 0, 255), 2)
# cv2.imshow("hole test", img_test)
# cv2.waitKey(1)
else:
action_append("Right02move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
if event_step == 1:
action_append("turn001R")
action_append("turn001R")
action_append("Right3move")
action_append("Right3move")
action_append("Right3move")
#action_append("Right3move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
event_step = 2
if event_step == 2:
ROI_image = np.rot90(Chest.imgs).copy()[250:550, :]
cv2.imshow("Chest_img", ROI_image)
cv2.waitKey(1)
Canny_img = cv2.Canny(ROI_image, 15, 150)
# cv2.imshow("Canny_img",Canny_img)
# cv2.waitKey(1)
#膨胀加粗边缘
dilate = cv2.dilate(Canny_img,
np.ones((2, 2), np.uint8),
iterations=1)
cv2.imshow("dilate", dilate)
cv2.waitKey(1)
print('event2')
Lines = cv2.HoughLinesP(dilate,
1.0,
np.pi / 180,
100,
minLineLength=50,
maxLineGap=10)
final_image, right_side, _ = group_lines_and_draw(
ROI_image, Lines, 'Right')
if right_side is None:
action_append("Forwalk01")
action_append("turn001R")
continue
dx, deg = Calculate_position(right_side)
print('right', dx, deg)
if dx > 10:
# print("Right3move")
action_append("Right3move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
event_step = 3
else:
event_step = 3
if event_step == 3:
ROI_image = np.rot90(Chest.imgs).copy()[250:550, :]
cv2.imshow("Chest_img", ROI_image)
#cv2.waitKey(1)
Canny_img = cv2.Canny(ROI_image, 15, 150)
# cv2.imshow("Canny_img",Canny_img)
# cv2.waitKey(1)
#膨胀加粗边缘
dilate = cv2.dilate(Canny_img,
np.ones((2, 2), np.uint8),
iterations=1)
#cv2.imshow("dilate",dilate)
#cv2.waitKey(1)
print('event3')
Lines = cv2.HoughLinesP(dilate,
1.0,
np.pi / 180,
100,
minLineLength=50,
maxLineGap=10)
final_image, right_side, _ = group_lines_and_draw(
ROI_image, Lines, 'Right')
if right_side is None:
action_append("Forwalk01")
action_append("turn001R")
continue
dx, deg = Calculate_position(right_side)
print('right', dx, deg)
cv2.imshow("image line", final_image)
cv2.waitKey(1)
if deg >= 7:
print("turn001L")
action_append("turn001L")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
elif deg <= -7:
print("turn001R")
action_append("turn001R")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
if dx >= 12:
print("Right02move")
action_append("Right02move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
elif dx <= 8:
print("Left02move")
action_append("Left02move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
if -7 < deg < 7 and 8 < dx < 12:
print("go straight")
action_append("Forwalk02")
action_append("turn000R")
action_append("Forwalk02")
action_append("turn000R")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
counter_forwalk = counter_forwalk + 1
if counter_forwalk == 2:
event_step = 4
if event_step == 4:
ROI_image = np.rot90(Chest.imgs).copy()[250:550, :]
cv2.imshow("Chest_img", ROI_image)
cv2.waitKey(1)
Canny_img = cv2.Canny(ROI_image, 15, 150)
# cv2.imshow("Canny_img",Canny_img)
# cv2.waitKey(1)
#膨胀加粗边缘
dilate = cv2.dilate(Canny_img,
np.ones((2, 2), np.uint8),
iterations=1)
#cv2.imshow("dilate",dilate)
cv2.waitKey(1)
print('event_step == 4')
Lines = cv2.HoughLinesP(dilate,
1.0,
np.pi / 180,
100,
minLineLength=50,
maxLineGap=10)
final_image, right_side, _ = group_lines_and_draw(
ROI_image, Lines, 'Right')
if right_side is None:
action_append("Forwalk01")
action_append("turn001R")
continue
dx, deg = Calculate_position(right_side)
print('right', dx, deg)
cv2.imshow("image line", final_image)
#cv2.waitKey(1)
if deg >= 7:
print("turn001L")
action_append("turn001L")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
elif deg <= -7:
print("turn001R")
action_append("turn001R")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
if dx >= 12:
print("Right02move")
action_append("Right02move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
elif dx <= 8:
print("Left02move")
action_append("Left02move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
if -7 < deg < 7 and 8 < dx < 12:
print("go left")
action_append("Left3move")
action_append("Left3move")
action_append("Left3move")
action_append("HeadTurnMM")
while len(action_list) != 0:
time.sleep(0.1)
event_step = 5
if event_step == 5:
print('Finish Hole')
cv2.destroyAllWindows()
break
if __name__ == '__main__':
###########读取视频流
Chest = LoadStreams(0)
Head = LoadStreams(2)
# 通信请求线程
th2 = threading.Thread(target=CMD_transfer)
th2.setDaemon(True) # 设置为后台线程，这里默认是False，设置为True之后则主线程不用等待子线程
th2.start()
while len(CMDcontrol.action_list) > 0:
print("等待启动")
time.sleep(1)
action_append("HeadTurnMM")
m = StateMachine()
m.add_state("Trans", Event_transitions)
m.add_state("start_door", start_door_state)
m.add_state("turn", turn_state)
m.add_state("holeb", holeb_state)
m.add_state("landmine", landmine_state)
m.add_state("flap", flap_state)
m.add_state("bridge", bridge_state)
m.add_state("step", step_state)
m.add_state("door", door_state)
m.add_state("ball", ball_state)
m.add_state("End_door", End_door_state, end_state=1) # 添加最终状态
def Stage(Chest, Head):
stage_clear = 0
num = 0
red_num = 0
action = 0
while (True):
while stage_clear == 0:
time.sleep(1)
org_img = Head.imgs.copy()
# r_w = 640
# r_h = 480
# border = cv2.copyMakeBorder(org_img,12,12,16,16,borderType=cv2.BORDER_CONSTANT,value=(255, 255, 255)) # 扩展白边，防止边界无法识别
#cv2.imwrite('border.jpg', border)
cropImg = org_img
#cv2.imwrite('de.jpg',cropImg) #写入图像路径
frame_gauss = cv2.GaussianBlur(cropImg, (3, 3), 0) # 高斯模糊
frame_hsv = cv2.cvtColor(frame_gauss,
cv2.COLOR_BGR2HSV) # 将图片转换到HSV空间
#h, s, v = cv2.split(frame_hsv) # 分离出各个HSV通道
#v = cv2.equalizeHist(v) # 直方图化
#Frame = cv2.merge((h, s, v)) # 合并三个通道
#对图片采样
# low = [0, 0, 0]
# for i in range(0, 400):
# for n in range(0, 55):
# low = low + frame_hsv[170 + n, 120 + i]
# low = low/22000 #求采样结果的均值
# low = low * 0.8 #根据均值算图片中目标区域的色彩阈值，low乘以的数需根据目标区域与周围区域颜色的差别大小调整
# high = low * 2.4
#frame_door1 = cv2.inRange(frame_hsv, low,
#high) # 对原图像和掩模(颜色的字典)进行位运算
frame_door1 = cv2.inRange(
frame_hsv, color_range['blue_floor'][0],
color_range['blue_floor'][1]) # 对原图像和掩模(颜色的字典)进行位运算
frame_door2 = cv2.inRange(
frame_hsv, color_range['green_floor'][0],
color_range['green_floor'][1]) # 对原图像和掩模(颜色的字典)进行位运算
frame_door3 = cv2.inRange(
frame_hsv, color_range['red_floor'][0],
color_range['red_floor'][1]) # 对原图像和掩模(颜色的字典)进行位运算
#frame_door = cv2.add(frame_door1, frame_door2)
#cv2.imwrite('door.jpg', frame_door1)
opened = cv2.morphologyEx(frame_door1, cv2.MORPH_OPEN,
np.ones((3, 3), np.uint8)) # 开运算 去噪点
closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE,
np.ones((10, 10), np.uint8)) # 闭运算 封闭连接
#cv2.imwrite('closed.jpg', closed)
# print(closed)
opened2 = cv2.morphologyEx(frame_door2, cv2.MORPH_OPEN,
np.ones((3, 3), np.uint8)) # 开运算 去噪点
closed2 = cv2.morphologyEx(opened2, cv2.MORPH_CLOSE,
np.ones((10, 10), np.uint8)) # 闭运算 封闭连接
opened3 = cv2.morphologyEx(frame_door3, cv2.MORPH_OPEN,
np.ones((3, 3), np.uint8)) # 开运算 去噪点
closed3 = cv2.morphologyEx(opened3, cv2.MORPH_CLOSE,
np.ones((10, 10), np.uint8)) # 闭运算 封闭连接
_, contours, hierarchy = cv2.findContours(
closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) # 找出轮廓
areaMaxContour, area_max = getAreaMaxContour(contours) # 找出最大轮廓
cv2.drawContours(cropImg, areaMaxContour, -1, (0, 0, 255), 3)
if areaMaxContour is not None:
rect = cv2.minAreaRect(areaMaxContour) #最小外接矩形
box = np.int0(cv2.boxPoints(rect)) #最小外接矩形的四个顶点
cv2.drawContours(cropImg, [box], 0, (255, 0, 0), 5)
else:
print('too far')
red_num = red_num + 1
area_max = 0
_, contours2, hierarchy2 = cv2.findContours(
closed2, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) # 找出轮廓
areaMaxContour2, area_max2 = getAreaMaxContour(contours2) # 找出最大轮廓
cv2.drawContours(cropImg, areaMaxContour2, -1, (0, 0, 255), 3)
if areaMaxContour2 is not None:
rect2 = cv2.minAreaRect(areaMaxContour2) #最小外接矩形
box2 = np.int0(cv2.boxPoints(rect2)) #最小外接矩形的四个顶点
cv2.drawContours(cropImg, [box2], 0, (255, 0, 0), 5)
else:
action_append('Forwalk01')
print('too far')
area_max2 = 0
action = 1
_, contours3, hierarchy3 = cv2.findContours(
closed3, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) # 找出轮廓
areaMaxContour3, area_max3 = getAreaMaxContour(contours3) # 找出最大轮廓
cv2.drawContours(cropImg, areaMaxContour3, -1, (0, 0, 255), 3)
if areaMaxContour3 is not None:
rect3 = cv2.minAreaRect(areaMaxContour3) #最小外接矩形
box3 = np.int0(cv2.boxPoints(rect3)) #最小外接矩形的四个顶点
cv2.drawContours(cropImg, [box3], 0, (255, 0, 0), 5)
else:
action_append('Forwalk01')
print('too far')
area_max3 = 0
action = 1
#print(areaMaxContour)
cv2.imshow('contour', org_img)
cv2.waitKey(1)
percent = round(100 * (area_max + area_max2 + area_max3) /
(640 * 480), 2) # 最大轮廓的百分比
# if box3 is not None:
# x_central = (box3[0,0] + box3[2,0])/2
# else:
# print('no red floor')
# print(percent)
#print(box[0,0])
if action == 1:
x_central = stagePosition(Head)
else:
action_append('Forwalk01')
x_central = (box3[0, 0] + box3[2, 0]) / 2
print(x_central)
forward = 0
left = 0
right = 0
if x_central < 250:
left = 1
elif x_central > 270:
right = 1
else:
forward = 1
if right == 1:
action_append('Right02move')
action_append('Right02move')
elif left == 1:
action_append('Left02move')
action_append('Left02move')
else:
action_append('Forwalk02')
if red_num == 5:
num = 1
else:
pass
if percent > 7:
close_center = 0
center_num = 0
while ((close_center == 0) & (center_num < 6)):
x_central = stagePosition(Head)
if x_central < 250:
action_append('Left02move')
center_num = center_num + 1
elif x_central > 270:
action_append('Right02move')
center_num = center_num + 1
else:
close_center = 1
close_forward = 0
while (close_forward == 0):
action_append('Forwalk02')
x_central = stagePosition(Head)
if x_central < 250:
action_append('Left02move')
elif x_central > 270:
action_append('Right02move')
else:
pass
y_central = stagePosition_y(Head)
if y_central >= 330:
close_forward = 1
print('y_central:', y_central)
print('stand by ready')
else:
action_append('Forwalk02')
action_append('Forwalk02')
action_append('shanglouti_v1')
time.sleep(1)
stage_position = stagePosition(Head)
if (stage_position > 340) & (stage_position < 380):
action_append('Right02move')
elif (stage_position >= 380) & (stage_position < 420):
action_append('Right02move')
action_append('Right02move')
elif stage_position >= 420:
action_append('Right02move')
action_append('Right02move')
action_append('Right02move')
elif (stage_position < 300) & (stage_position > 260):
action_append('Left02move')
elif (stage_position <= 260) & (stage_position > 220):
action_append('Left02move')
action_append('Left02move')
elif stage_position <= 220:
action_append('Left02move')
action_append('Left02move')
action_append('Left02move')
else:
pass
action_append('Forwalk02')
action_append('shanglouti_v1')
time.sleep(1)
stage_position = stagePosition(Head)
if (stage_position > 340) & (stage_position < 380):
action_append('Right02move')
elif (stage_position >= 380) & (stage_position < 420):
action_append('Right02move')
action_append('Right02move')
elif stage_position >= 420:
action_append('Right02move')
action_append('Right02move')
action_append('Right02move')
elif (stage_position < 300) & (stage_position > 260):
action_append('Left02move')
elif (stage_position <= 260) & (stage_position > 220):
action_append('Left02move')
action_append('Left02move')
elif stage_position <= 220:
action_append('Left02move')
action_append('Left02move')
action_append('Left02move')
else:
pass
action_append('Forwalk02')
action_append('shanglouti_v1')
time.sleep(1)
action_append('Forwalk00')
action_append('Forwalk00')
action_append('xialouti')
time.sleep(1)
action_append('turn004L')
action_append('xialouti')
time.sleep(1)
action_append('turn004L')
action_append('Left02move')
action_append('Left02move')
action_append('turn004L')
# stage_position = stagePosition_Chest(Chest)
# if (stage_position < 220) & (stage_position > 190):
# action_append('Left02move')
# elif stage_position <= 190:
# action_append('Left02move')
# action_append('Left02move')
# elif (stage_position > 260) & (stage_position < 290):
# action_append('Right02move')
# elif stage_position >= 290:
# action_append('Right02move')
# action_append('Right02move')
# else:
# pass
action_append('Forwalk02')
action_append('Forwalk02')
action_append('Forwalk02')
action_append('Forwalk02')
action_append('Forwalk02')
stage_clear = 1
elif num == 1:
close_center = 0
center_num = 0
while ((close_center == 0) & (center_num < 6)):
x_central = stagePosition(Head)
if x_central < 250:
action_append('Left02move')
center_num = center_num + 1
elif x_central > 270:
action_append('Right02move')
center_num = center_num + 1
else:
close_center = 1
close_forward = 0
while (close_forward == 0):
action_append('Forwalk02')
x_central = stagePosition(Head)
if x_central < 250:
action_append('Left02move')
elif x_central > 270:
action_append('Right02move')
else:
pass
y_central = stagePosition_y(Head)
if y_central >= 330:
close_forward = 1
print('y_central:', y_central)
print('stand by ready')
else:
action_append('Forwalk02')
action_append('Forwalk02')
action_append('shanglouti_v1')
time.sleep(1)
stage_position = stagePosition(Head)
if (stage_position > 340) & (stage_position < 380):
action_append('Right02move')
elif (stage_position >= 380) & (stage_position < 420):
action_append('Right02move')
action_append('Right02move')
elif stage_position >= 420:
action_append('Right02move')
action_append('Right02move')
action_append('Right02move')
elif (stage_position < 300) & (stage_position > 260):
action_append('Left02move')
elif (stage_position <= 260) & (stage_position > 220):
action_append('Left02move')
action_append('Left02move')
elif stage_position <= 220:
action_append('Left02move')
action_append('Left02move')
action_append('Left02move')
else:
pass
action_append('Forwalk02')
action_append('shanglouti_v1')
time.sleep(1)
stage_position = stagePosition(Head)
if (stage_position > 340) & (stage_position < 380):
action_append('Right02move')
elif (stage_position >= 380) & (stage_position < 420):
action_append('Right02move')
action_append('Right02move')
elif stage_position >= 420:
action_append('Right02move')
action_append('Right02move')
action_append('Right02move')
elif (stage_position < 300) & (stage_position > 260):
action_append('Left02move')
elif (stage_position <= 260) & (stage_position > 220):
action_append('Left02move')
action_append('Left02move')
elif stage_position <= 220:
action_append('Left02move')
action_append('Left02move')
action_append('Left02move')
else:
pass
action_append('Forwalk02')
action_append('shanglouti_v1')
time.sleep(1)
action_append('Forwalk00')
action_append('Forwalk00')
action_append('xialouti')
time.sleep(1)
action_append('turn004L')
action_append('xialouti')
time.sleep(1)
action_append('turn004L')
action_append('Left02move')
action_append('Left02move')
action_append('turn004L')
# stage_position = stagePosition_Chest(Chest)
# if (stage_position < 220) & (stage_position > 190):
# action_append('Left02move')
# elif stage_position <= 190:
# action_append('Left02move')
# action_append('Left02move')
# elif (stage_position > 260) & (stage_position < 290):
# action_append('Right02move')
# elif stage_position >= 290:
# action_append('Right02move')
# action_append('Right02move')
# else:
# pass
action_append('Forwalk02')
action_append('Forwalk02')
action_append('Forwalk02')
action_append('Forwalk02')
action_append('Forwalk02')
stage_clear = 1
else:
action_append('Forwalk02')
num = num + 1
print('stage clear')
break
