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