def __init__(self, pitch, width=640, height=480): self.width = width self.height = height self.pitch = pitch self.new_polygon = True self.polygon = self.polygons = [] self.points = [] self.camera = Camera(pitch=pitch) keys = [ 'outline', 'Zone_0', 'Zone_1', 'Zone_2', 'Zone_3', 'pitch_inner', "Goal_0", "Goal_1" ] self.data = self.drawing = {} # Create keys for key in keys: self.data[key] = [] self.drawing[key] = [] self.color = RED
def __init__(self): """ initializes the thread without starting to capture the data """ super().__init__() self.active = True self.driver = Driver.instance() self.camera = Camera.instance() # define directories and file paths date_str = datetime.today().strftime("%Y-%m-%d-%H-%M-%S") self.log_dir = f"{const.Storage.DATA}/{date_str}" self.img_dir = f"{self.log_dir}/img/" self.log_path = f"{self.log_dir}/log.csv" self.img_extension = "npy" # ensure that the necessary directories exist os.mkdir(self.log_dir) os.mkdir(self.img_dir) assert os.path.isdir(self.log_dir), "data directory could not be created" assert os.path.isdir(self.img_dir), "image directory could not be created"
def follow_road(self): """ drives autonomously without explicit instructions by propagating the camera input through a convolutional neural network that predicts a steering angle NOTE this mode has no particular destination and therfore does not terminate unless the driver is stopped explicitly """ steering_net = SteeringNet() steering_net.load(const.Storage.DEFAULT_STEERING_MODEL) self.start_driving() with Camera.instance() as camera: while self.recorder_thread is not None: image = camera.image predicted_angle = steering_net.predict(image, self.angle) self.angle = predicted_angle time.sleep(0.25) self.stop_driving()
def run(self): """ starts capturing the data (image, angle, previous angle) Raises: OSError: in case the log file cannot be created OSError: in case the log file cannot be opened after being created """ with Camera.instance() as camera: try: # create log file and write headers with open(self.log_path, "w+") as log: writer = csv.writer(log) writer.writerow(["image", "angle", "previous_angle"]) except OSError: raise OSError("The log file could not be created.") previous_angle = 0.0 while self.active: if camera.image is None: continue # skip loop if no image provided # save image img_filename = datetime.today().strftime("%H-%M-%S-%f") + "." + self.img_extension np.save(self.img_dir + img_filename, camera.image) try: # write data to csv file with open(self.log_path, "a") as log: writer = csv.writer(log) angle = str(round(self.driver.angle, 3)) previous_angle = str(previous_angle) writer.writerow([img_filename, angle, previous_angle]) except OSError: raise OSError("The log file could not be opened.") previous_angle = angle # update previous angle for next loop time.sleep(self.CAPTURE_INTERVAL)
drivers.init() drivers.LED4.off() print("Self-identifying...") identity = None with open("identity.dat", "r") as file: identity = file.readlines()[0].strip() print(identity, "online. Pray to Lafayette Official God.\nWaiting for start signal") while not os.path.isfile("go"): pass os.remove("go") print("Go time!") camera = Camera() mod = VisionModule(width=640, height=480) collecting = False drivers.move(drivers.RobotState(drivers.DRIVE, -in_to_cm(6))) def sweep(): cube = turn_to_block() if cube != None: drivers.move(drivers.RobotState(drivers.DRIVE, -cube.dist / 3)) cube = turn_to_block() if cube != None: drivers.move(drivers.RobotState(drivers.DRIVE, -cube.dist / 5)) drivers.move(drivers.RobotState(drivers.TURN, math.pi)) drivers.move(drivers.RobotState(drivers.DRIVE, 10))
def __init__(self, pitch, color_settings, our_name, robot_details, enable_gui=False, pc_name=None, robots_on_pitch=list(), goal=None): """ Entry point for the SDP system. Params: [int] pitch 0 - main pitch, 1 - secondary pitch [int or string] color_settings [0, small, 1, big] - 0 or small for pitch color settings with small numbers (previously - pitch 0) 1 or big - pitch color settings with big numbers (previously - pitch 1) [string] colour The colour of our teams plates [string] our_name our name [int] video_port port number for the camera [boolean] enable_gui Does not draw the normal image to leave space for GUI. Also forces the trackers to return circle contour positons in addition to robot details. [string] pc_name Name of the PC to load the files from (BUT NOT SAVE TO). Will default to local machine if not specified. """ pitch = int(pitch) assert pitch in [0, 1] if goal: assert goal in ["left", "right"] self.pitch = pitch self.target_goal = goal self.color_settings = color_settings self.calibration = Configuration.read_calibration( pc_name, create_if_missing=True) self.video_settings = Configuration.read_video_config( pc_name, create_if_missing=True) # Set up camera for frames self.camera = Camera(pitch) self.camera.start_capture() self.frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center() # Set up vision self.trackers = list() self.world_objects = dict() # Get machine-specific calibration self.enable_gui = enable_gui self.gui = None # Initialize robots self.robots = [] # Initialize ball states - which robot had the ball previously. self.ball_median_size = 5 self.ball_index = 0 self.ball_states = [None] * self.ball_median_size self.BALL_HOLDING_AREA_SCALE = 0.1 for r_name in robot_details.keys(): role = 'ally' if robot_details[r_name]['main_colour'] != robot_details[our_name]['main_colour']: role = 'enemy' elif r_name == our_name: role = 'group9' print "[WRAPPER] Setting %s to %s." % (r_name, role) self.robots.append(RobotInstance(r_name, robot_details[r_name]['main_colour'], robot_details[r_name]['side_colour'], robot_details[r_name]['offset_angle'], role, r_name in robots_on_pitch)) # Draw various things on the image self.draw_direction = True self.draw_robot = True self.draw_contours = True self.draw_ball = True self.vision = Vision( pitch=pitch, frame_shape=self.frame.shape, frame_center=center_point, calibration=self.calibration, robots=self.robots, return_circle_contours=enable_gui, trackers_out=self.trackers) # Used for latency calculations self.t0 = time() self.delta_t = 0 if self.enable_gui: self.gui = GUI(self.pitch, self.color_settings, self.calibration, self) from threading import Thread from gui.common import MainWindow from gui.status_control import StatusUI from gui.main import MainUI self.status_window = None def create_windows(): self.main_window = MainUI(self) return [ self.main_window ] Thread(name="Tkinter UI", target=MainWindow.create_and_show, args=[create_windows]).start() # Set up preprocessing and postprocessing # self.postprocessing = Postprocessing() self.preprocessing = Preprocessing() self.side = our_name self.frameQueue = [] self.video = None
class VisionWrapper: """ Class that handles vision """ ENEMY_SCALE = 1. GROUP9_SCALE = 1. ALLY_SCALE = 1. def __init__(self, pitch, color_settings, our_name, robot_details, enable_gui=False, pc_name=None, robots_on_pitch=list(), goal=None): """ Entry point for the SDP system. Params: [int] pitch 0 - main pitch, 1 - secondary pitch [int or string] color_settings [0, small, 1, big] - 0 or small for pitch color settings with small numbers (previously - pitch 0) 1 or big - pitch color settings with big numbers (previously - pitch 1) [string] colour The colour of our teams plates [string] our_name our name [int] video_port port number for the camera [boolean] enable_gui Does not draw the normal image to leave space for GUI. Also forces the trackers to return circle contour positons in addition to robot details. [string] pc_name Name of the PC to load the files from (BUT NOT SAVE TO). Will default to local machine if not specified. """ pitch = int(pitch) assert pitch in [0, 1] if goal: assert goal in ["left", "right"] self.pitch = pitch self.target_goal = goal self.color_settings = color_settings self.calibration = Configuration.read_calibration( pc_name, create_if_missing=True) self.video_settings = Configuration.read_video_config( pc_name, create_if_missing=True) # Set up camera for frames self.camera = Camera(pitch) self.camera.start_capture() self.frame = self.camera.get_frame() center_point = self.camera.get_adjusted_center() # Set up vision self.trackers = list() self.world_objects = dict() # Get machine-specific calibration self.enable_gui = enable_gui self.gui = None # Initialize robots self.robots = [] # Initialize ball states - which robot had the ball previously. self.ball_median_size = 5 self.ball_index = 0 self.ball_states = [None] * self.ball_median_size self.BALL_HOLDING_AREA_SCALE = 0.1 for r_name in robot_details.keys(): role = 'ally' if robot_details[r_name]['main_colour'] != robot_details[our_name]['main_colour']: role = 'enemy' elif r_name == our_name: role = 'group9' print "[WRAPPER] Setting %s to %s." % (r_name, role) self.robots.append(RobotInstance(r_name, robot_details[r_name]['main_colour'], robot_details[r_name]['side_colour'], robot_details[r_name]['offset_angle'], role, r_name in robots_on_pitch)) # Draw various things on the image self.draw_direction = True self.draw_robot = True self.draw_contours = True self.draw_ball = True self.vision = Vision( pitch=pitch, frame_shape=self.frame.shape, frame_center=center_point, calibration=self.calibration, robots=self.robots, return_circle_contours=enable_gui, trackers_out=self.trackers) # Used for latency calculations self.t0 = time() self.delta_t = 0 if self.enable_gui: self.gui = GUI(self.pitch, self.color_settings, self.calibration, self) from threading import Thread from gui.common import MainWindow from gui.status_control import StatusUI from gui.main import MainUI self.status_window = None def create_windows(): self.main_window = MainUI(self) return [ self.main_window ] Thread(name="Tkinter UI", target=MainWindow.create_and_show, args=[create_windows]).start() # Set up preprocessing and postprocessing # self.postprocessing = Postprocessing() self.preprocessing = Preprocessing() self.side = our_name self.frameQueue = [] self.video = None def get_robots_raw(self): # Filter robots that have no position return [[r.name, r.visible, r.position, r.heading, RobotType.UNKNOWN] for r in self.robots] def get_robot_position(self, robot_name): return filter(lambda r: r.name == robot_name, self.robots)[0].position def get_robot_headings(self): headings = dict() for r in self.robots: headings[r.name] = r.heading return headings def get_circle_position(self, robot_name): for r in self.robots: if r.name == robot_name: return r.side_x, r.side_y def get_all_robots(self): robots = dict() for r in self.robots: robots[r.name] = self.get_robot_position(r.name) return robots def get_ball_position(self): """ :return: Actual ball position or predicted ball position if the robot was near it. Might return None. """ # TODO: call methods here to get robot region if new one will be used. if not self.world_objects['ball'][2]: r_name, _ = self._mode(self.ball_states) if r_name: for r in self.robots: if r.name == r_name: x, y = r.predicted_ball_pos self.world_objects['ball'] = (x, y, 2) if self.world_objects['ball'][2] and not \ (self.world_objects['ball'][0] == 42 and self.world_objects['ball'][1] == 42): return self.world_objects['ball'] else: return None def get_pitch_dimensions(self): from util.tools import get_pitch_size return get_pitch_size() def get_robot_direction(self, robot_name): return filter(lambda r: r.name == robot_name, self.robots)[0] def do_we_have_ball(self, robot_name): return self.is_ball_in_range(robot_name, scale=self.BALL_HOLDING_AREA_SCALE) def is_ball_in_range(self, robot_name, scale=1.): """ returns True if ball is in the grabbing zone. :param robot_name: robot name :param scale: Zone can be scaled, to accommodate for different robots :return: boolean """ ball = self.get_ball_position() if ball and ball[2]: for r in self.robots: if r.name == robot_name and r.visible: if r.role == 'enemy': if r.is_point_in_grabbing_zone(ball[0], ball[1], role=r.role, scale=self.ENEMY_SCALE, circular=False): return True elif r.role == 'group9': if r.is_point_in_grabbing_zone(ball[0], ball[1], role=r.role, scale=self.GROUP9_SCALE, circular=False): return True else: if r.is_point_in_grabbing_zone(ball[0], ball[1], role=r.role, scale=self.ALLY_SCALE, circular=False): return True break return False def is_ball_on_other(self, robot_name, zone, scale=1.): """ returns True if ball is in the side or bottom zone. :param robot_name: robot name :param zone: ["left", "right", "bottom"] :param scale: Zone can be scaled, to accommodate for different robots :return: boolean """ ball = self.get_ball_position() if ball and ball[2]: for r in self.robots: if r.name == robot_name: if r.is_point_in_other_zone(ball[0], ball[1], zone, role=r.role, scale=scale): return True break return False def change_drawing(self, key): """ Toggles drawing of contours, heading directions, robot positions and ball tracker Usage: * 'b' for ball tracker * 'n' for contours * 'j' for robot_tracker * 'i' for heading direction :param key: keypress :return: nothing """ if key == ord('b'): self.gui.draw_ball = not self.gui.draw_ball elif key == ord('n'): self.gui.draw_contours = not self.gui.draw_contours # THIS WAS CRASHING EVERYTHING # elif key == ord('j'): # self.gui.draw_robot = not self.gui.draw_robot elif key == ord('i'): self.gui.draw_direction = not self.gui.draw_direction def get_circle_contours(self): """ Careful! Does not return x and y values. Call minimum bounding circles if proper circle locations are required. Or look at dem fish http://docs.opencv.org/2.4/doc/tutorials/imgproc/shapedescriptors/find_contours/find_contours.html :return: [Contours] """ return self.world_objects.get('circles') def get_frame_size(self): height, width, channels = self.frame.shape return width, height def get_latency_seconds(self): return self.delta_t def _mode(self, array): """ :param array: some array :return: m, c the first mode found and the number of occurences """ m = max(array, key = array.count) return m, array.count(m) def start_video(self, title): if self.video is not None: try: self.video.release() except Exception, e: print "[WARNING] Error releasing previous video:", e self.video = cv2.VideoWriter('recordings/' + "test" + '.mpeg', -1, int(self.camera.capture.get(cv2.CAP_PROP_FPS)), (int(self.camera.capture.get(3)), int(self.camera.capture.get(4))))
class Configure(object): def __init__(self, pitch, width=640, height=480): self.width = width self.height = height self.pitch = pitch self.new_polygon = True self.polygon = self.polygons = [] self.points = [] self.camera = Camera(pitch=pitch) keys = [ 'outline', 'Zone_0', 'Zone_1', 'Zone_2', 'Zone_3', 'pitch_inner', "Goal_0", "Goal_1" ] self.data = self.drawing = {} # Create keys for key in keys: self.data[key] = [] self.drawing[key] = [] self.color = RED def run(self, camera=False): frame = cv2.namedWindow(FRAME_NAME) # Set callback cv2.setMouseCallback(FRAME_NAME, self.draw) if camera: self.image = self.camera.get_raw_frame() else: self.image = cv2.imread('00000001.jpg') self.image = self.camera.fix_radial_distortion(self.image) self.image = self.camera.fix_perspective(self.image) # Get various data about the image from the user self.get_pitch_outline() self.get_zone('Zone_0', 'draw LEFT Defender') self.get_zone('Zone_1', 'draw RIGHT Defender') self.get_zone('Goal_0', 'Point in the center of the left goal') self.get_zone('Goal_1', 'Point in the cneter of the right goal') #self.get_goal('Zone_0') #self.get_goal('Zone_3') print 'Press any key to finish.' cv2.waitKey(0) cv2.destroyAllWindows() # Write out the data # self.dump('calibrations/calibrate.json', self.data) tools.save_croppings(pitch=self.pitch, data=self.data) def reshape(self): return np.array(self.data[self.drawing], np.int32).reshape((-1, 1, 2)) def draw_poly(self, points): cv2.polylines(self.image, [points], True, self.color) cv2.imshow(FRAME_NAME, self.image) def get_zone(self, key, message): print '%s. %s' % (message, "Continue by pressing q") self.drawing, k = key, True while k != ord('q'): cv2.imshow(FRAME_NAME, self.image) k = cv2.waitKey(100) & 0xFF self.draw_poly(self.reshape()) def get_pitch_outline(self): """ Let user select points that corespond to the pitch outline. End selection by pressing 'q'. Result is masked and cropped. """ self.get_zone( 'outline', 'Draw the outline of the pitch. Contine by pressing \'q\'') # Setup black mask to remove overflows self.image = tools.mask_pitch(self.image, self.data[self.drawing]) # Get crop size based on points size = tools.find_crop_coordinates(self.image, self.data[self.drawing]) # Crop #self.image = self.image[size[2]:size[3], size[0]:size[1]] cv2.imshow(FRAME_NAME, self.image) def draw(self, event, x, y, flags, param): """ Callback for events """ if event == cv2.EVENT_LBUTTONDOWN: color = self.color cv2.circle(self.image, (x - 1, y - 1), 2, color, -1) self.data[self.drawing].append((x, y)) def get_goal(self, zone): """ Returns the top and bottom corner of the goal in zone. """ coords = self.data[zone] reverse = int(zone[-1]) % 2 goal_coords = sorted(coords, reverse=reverse)[:2] if goal_coords[0][1] > goal_coords[1][1]: topCorner = goal_coords[1] bottomCorner = goal_coords[0] else: topCorner = goal_coords[0] bottomCorner = goal_coords[1] self.data[zone + '_goal'] = [topCorner, bottomCorner]
import cv2 from vision import Camera import argparse parser = argparse.ArgumentParser() parser.add_argument("pitch", help="[0] Main pitch, [1] Secondary pitch") args = parser.parse_args() pitch_number = int(args.pitch) capture = Camera(pitch=pitch_number) while True: frame = capture.get_frame() cv2.imwrite("currBg_" + str(pitch_number) + ".png", frame) #cv2.imwrite("../currBg_" + str(pitch_number) + ".png", frame) cv2.imshow('Background view', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break print "done"
Save perspetive point fixes """ tools.save_data(self.pitch, self.points, 'calibrations/perspective.json') if __name__ == '__main__': import argparse parser = argparse.ArgumentParser() parser.add_argument("pitch", help="[0] Main pitch, [1] Secondary pitch") args = parser.parse_args() pitch_number = int(args.pitch) WINDOW_NAME = 'Perspective Calibration' cam = Camera(pitch=pitch_number) perspective = Perspective(pitch_number) cv2.namedWindow(WINDOW_NAME) cv2.setMouseCallback(WINDOW_NAME, perspective.add_point) while (1): frame = cam.get_raw_frame() frame = cam.fix_radial_distortion(frame) frame = perspective.draw_points(frame) cv2.imshow(WINDOW_NAME, frame) if cv2.waitKey(20) & 0xFF == 27: break
with open("identity.dat", "r") as file: identity = file.readlines()[0].strip() print(identity, "online!") # Figure out where I'm headed goal = [-1, 1] with open("goal.dat", "r") as file: nums = file.readlines[0].strip().split(" ") goal = [float(x) for x in nums] print("Targeting block at", goal) # System initialization drivers.init() camera = Camera() mod = VisionModule(width=640, height=480) # State pose = ROBOT_ORIGIN_POSES[identity] time = 0 iterations = 0 done = False epoch = time.time() # Parker robots have a simple routine if identity in PARKER_ROBOTS: drivers.move(drivers.RobotState(drivers.DRIVE, in_to_cm(ROBOT_AXIAL + 2))) sleep(3) drivers.move(drivers.RobotState(drivers.DRIVE, -in_to_cm(ROBOT_AXIAL + 3))) done = True