#!/usr/bin/python3 # coding=utf8 import sys sys.path.append('/home/pi/TurboPi/') import time import HiwonderSDK.PID as PID import HiwonderSDK.Misc as Misc import HiwonderSDK.mecanum as mecanum import cv2 import time import math import Camera import numpy as np import yaml_handle #stores all functionalities needed to traverse a maze path class Traverser: def __init__(self,maze,path, start_row, start_col): self.maze = maze self.car = mecanum.MecanumChassis() self.path = path self.directions = [0,1,2,3] self.orient = 0 self.row_index = start_row self.col_index = start_col self.isRunning = True self.car_en = False self.line_centerx = -1 self.img_centerx = 320 self.swerve_pid = PID.PID(P=0.001, I=0.00001, D=0.000001) self.target_color = ('red',) self.camera = Camera.Camera() self.camera.camera_open(correction=True) # adjust this variable to change the amount of time the robot moves in the forward direction self.forward_timer = None #centers the car along the red line def move(self,move_time): start_time = time.time() while (start_time+float(move_time)>time.time()): if self.isRunning: img = self.camera.frame if img is not None: frame = img.copy() Frame = self.run(frame) frame_resize = cv2.resize(Frame, (320, 240)) cv2.imshow('frame', frame_resize) if self.line_centerx > 0: if abs(self.line_centerx-self.img_centerx) < 10: self.line_centerx = self.img_centerx self.swerve_pid.SetPoint = self.img_centerx self.swerve_pid.update(self.line_centerx) angle = -self.swerve_pid.output self.car.set_velocity(30, 90, angle) self.car_en = True else: if self.car_en: self.car_en = False self.car.set_velocity(0,90,0) time.sleep(0.01) #process images for line following def run(self,img): size = (640,480) lab_data = yaml_handle.get_yaml_data(yaml_handle.lab_file_path) roi = [ # [ROI, weight] (240, 280, 0, 640, 0.1), (340, 380, 0, 640, 0.3), (430, 460, 0, 640, 0.6) ] roi_h1 = roi[0][0] roi_h2 = roi[1][0] - roi[0][0] roi_h3 = roi[2][0] - roi[1][0] roi_h_list = [roi_h1, roi_h2, roi_h3] img_copy = img.copy() img_h, img_w = img.shape[:2] if not self.isRunning or self.target_color == (): return img frame_resize = cv2.resize(img_copy, size, interpolation=cv2.INTER_NEAREST) frame_gb = cv2.GaussianBlur(frame_resize, (3, 3), 3) centroid_x_sum = 0 weight_sum = 0 center_ = [] n = 0 for r in roi: roi_h = roi_h_list[n] n += 1 blobs = frame_gb[r[0]:r[1], r[2]:r[3]] frame_lab = cv2.cvtColor(blobs, cv2.COLOR_BGR2LAB) area_max = 0 areaMaxContour = 0 for i in self.target_color: if i in lab_data: detect_color = i frame_mask = cv2.inRange(frame_lab, (lab_data[i]['min'][0], lab_data[i]['min'][1], lab_data[i]['min'][2]), (lab_data[i]['max'][0], lab_data[i]['max'][1], lab_data[i]['max'][2])) eroded = cv2.erode(frame_mask, cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))) dilated = cv2.dilate(eroded, cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))) cnts = cv2.findContours(dilated , cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_TC89_L1)[-2] cnt_large, area = self.getAreaMaxContour(cnts) if cnt_large is not None: rect = cv2.minAreaRect(cnt_large) box = np.int0(cv2.boxPoints(rect)) for i in range(4): box[i, 1] = box[i, 1] + (n - 1)*roi_h + roi[0][0] box[i, 1] = int(Misc.map(box[i, 1], 0, size[1], 0, img_h)) for i in range(4): box[i, 0] = int(Misc.map(box[i, 0], 0, size[0], 0, img_w)) cv2.drawContours(img, [box], -1, (0,0,255,255), 2) pt1_x, pt1_y = box[0, 0], box[0, 1] pt3_x, pt3_y = box[2, 0], box[2, 1] center_x, center_y = (pt1_x + pt3_x) / 2, (pt1_y + pt3_y) / 2 cv2.circle(img, (int(center_x), int(center_y)), 5, (0,0,255), -1) center_.append([center_x, center_y]) centroid_x_sum += center_x * r[4] weight_sum += r[4] if weight_sum != 0: cv2.circle(img, (self.line_centerx, int(center_y)), 10, (0,255,255), -1) self.line_centerx = int(centroid_x_sum / weight_sum) else: self.line_centerx = -1 return img def getAreaMaxContour(self,contours): contour_area_temp = 0 contour_area_max = 0 area_max_contour = None for c in contours: contour_area_temp = math.fabs(cv2.contourArea(c)) if contour_area_temp > contour_area_max: contour_area_max = contour_area_temp if contour_area_temp >= 5: area_max_contour = c return area_max_contour, contour_area_max #initialize the maze environment def init_env(self): #set all spaces as non-path spaces for space in self.path: self.maze[space[0]][space[1]] = 2 def print_maze(self): for i in range(len(self.maze)-1,-1,-1): print(self.maze[i]) #define movements and turns def move_forward(self): print('moving forward') self.move(self.forward_timer) def turn_left(self): print('turning left') self.car.set_velocity(0,185,-0.3) time.sleep(0.9) def turn_right(self): print('turning right') self.car.set_velocity(0,0,0.30) time.sleep(0.75) #define index update for accessing neighboring spaces def find_space(self,state_direction): #direction of state to check is in front of robot curr_row = self.row_index curr_col = self.col_index if(state_direction==self.directions[0]): print('computing forward space index') #if robot is objectively facing forward if (self.orient==self.directions[0]): print('forward index is forward') curr_row += 1 #if robot is objectively facing left elif (self.orient==self.directions[1]): print('forward index is to the left') curr_col -= 1 #if robot is objectively facing right elif (self.orient==self.directions[2]): print('forward index is to the right') curr_col += 1 #if robot is objectively facing backward elif (self.orient==self.directions[3]): print('forward index is behind') curr_row -= 1 #direction of state to check is left of robot elif(state_direction==self.directions[1]): print('computing left space index') if (self.orient==self.directions[0]): print('left index is to the left') curr_col -= 1 elif (self.orient==self.directions[1]): print('left index is behind') curr_row -= 1 elif (self.orient==self.directions[2]): print('left index is forward') curr_row += 1 elif (self.orient==self.directions[3]): print('left index is to the right') curr_col += 1 elif(state_direction==self.directions[2]): print('computing right space index') if (self.orient==self.directions[0]): print('right index is to the right') curr_col += 1 elif (self.orient==self.directions[1]): print('right index is forward') curr_row += 1 elif (self.orient==self.directions[2]): print('right index is behind') curr_row -= 1 elif (self.orient==self.directions[3]): print('right index is to the left') curr_col -= 1 else: print('cannot compute index; invalid direction supplied') return curr_row,curr_col #update the orientation of the robot def update_orient(self,direction): #if going left if (direction==self.directions[1]): print('updating orientation for left turn') #if orientation was originally forward if (self.orient==self.directions[0]): print('orientation is now left') self.orient=self.directions[1] #if orientation was originally left elif (self.orient==self.directions[1]): print('orientation is now backward') self.orient=self.directions[3] #if orientation was originally right elif (self.orient==self.directions[2]): print('orientation is now forward') self.orient=self.directions[0] #if orientation was originally backward elif (self.orient==self.directions[3]): print('orientation is now right') self.orient=self.directions[2] #if going right elif (direction==self.directions[2]): print('updating orientation for right turn') if (self.orient==self.directions[0]): print('orientation is now right') self.orient=self.directions[2] elif (self.orient==self.directions[1]): print('orientation is now forward') self.orient=self.directions[0] elif (self.orient==self.directions[2]): print('orientation is now backward') self.orient=self.directions[3] elif (self.orient==self.directions[3]): print('orientation is now left') self.orient=self.directions[1] else: print('invalid direction to orient towards') def follow_path(self): #go through the path, until the space you arrive at is the last one next_direction = 0 self.car.set_velocity(0,90,0) while(next_direction!=-1): next_direction = -1 print('current space:', self.row_index, ', ', self.col_index) self.maze[self.row_index][self.col_index] = 0 for direction in self.directions[:-1]: #check where next path space is, starting with forward space next_row,next_col = self.find_space(direction) if (next_row in [0,1,2,3,4,5,6,7,8] and next_col in [0,1,2,3,4,5,6,7,8]): if(self.maze[next_row][next_col]==2): self.row_index,self.col_index = next_row,next_col next_direction = direction break #move to next path space if(next_direction==self.directions[0]): self.move_forward() elif(next_direction==self.directions[1]): self.turn_left() self.move_forward() self.update_orient(next_direction) elif(next_direction==self.directions[2]): self.turn_right() self.move_forward() self.update_orient(next_direction) self.arrived = True self.isRunning = False self.camera.camera_close() time.sleep(1) self.car.set_velocity(30,90,0) time.sleep(2) self.car.set_velocity(0,90,0) time.sleep(1) print('You have now arrived at your destination!')