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 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 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 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