def __init__(self, logger=None): # Set logger self.logger = None if logger is not None: self.logger = logger else: logging.config.fileConfig('logging.conf') self.logger = logging.getLogger('track_follower') self.debug = True # Build shared data structures self.logd("__init__()", "Creating shared data structures...") self.manager = Manager() self.bot_loc = self.manager.dict(x=-1, y=-1, theta=0.0, dirty=False) # manage bot loc in track follower self.blobs = self.manager.list() # for communication between vision and planner self.blocks = self.manager.dict() self.zones = self.manager.dict() self.corners = self.manager.list() self.bot_state = self.manager.dict(nav_type=None, action_type=None, naving=False) #nav_type is "micro" or "macro" # Set shared parameters and flags self.bot_state['cv_offsetDetect'] = False self.bot_state['cv_lineTrack'] = True self.bot_state['cv_blobTrack'] = False # Serial interface and command self.logd("__init__()", "Creating SerialInterface process...") self.si = comm.SerialInterface(timeout=1.0) self.si.start() self.sc = comm.SerialCommand(self.si.commands, self.si.responses) # Read waypoints from file self.waypoints = pickler.unpickle_waypoints(waypoints_file) self.logd("__init__()", "Loaded waypoints") #pp.pprint(self.waypoints) # [debug] # TODO Create nodes (waypoints) and edges using graph methods to simplify things self.graph = Graph() self.graph.nodes['start'] = Node('start', Point(self.waypoints['start'][1][0], self.waypoints['start'][1][1]), self.waypoints['start'][2]) self.graph.nodes['alpha'] = Node('alpha', Point(15, 12), 0) # first point to go to self.graph.nodes['land'] = Node('land', Point(39.5, 12), 0) # point near beginning of land self.graph.nodes['beta'] = Node('beta', Point(64, 12), pi/2) # point near one end of land and beginning of ramp self.graph.nodes['east_off'] = Node('east_off', Point(64, 20), pi/2) # point beside start of ramp when we should switch off east sensor self.graph.nodes['celta'] = Node('celta', Point(64, 34), pi) # point beside ramp self.graph.nodes['pickup'] = Node('pickup', Point(58, 34), pi) # point near start of pickup self.graph.nodes['delta'] = Node('delta', Point(12, 34), pi) # point past end of pickup self.graph.nodes['sea'] = Node('sea', Point(12, 33), 3*pi/2) # point near start of sea self.graph.nodes['eps'] = Node('eps', Point(12, 12), 0) # point past end of sea self.graph.edges[('start', 'alpha')] = Edge(self.graph.nodes['start'], self.graph.nodes['alpha']) self.graph.edges[('alpha', 'land')] = Edge(self.graph.nodes['alpha'], self.graph.nodes['land'], props=dict(follow=1)) self.graph.edges[('land', 'beta')] = Edge(self.graph.nodes['land'], self.graph.nodes['beta'], props=dict(follow=1, isDropoff=True, isLand=True)) self.graph.edges[('beta', 'east_off')] = Edge(self.graph.nodes['beta'], self.graph.nodes['east_off'], props=dict(follow=1)) self.graph.edges[('east_off', 'celta')] = Edge(self.graph.nodes['east_off'], self.graph.nodes['celta'], props=dict(follow=1)) self.graph.edges[('celta', 'pickup')] = Edge(self.graph.nodes['celta'], self.graph.nodes['pickup'], props=dict(follow=1)) self.graph.edges[('pickup', 'delta')] = Edge(self.graph.nodes['pickup'], self.graph.nodes['delta'], props=dict(follow=1, isPickup=True)) self.graph.edges[('delta', 'sea')] = Edge(self.graph.nodes['delta'], self.graph.nodes['sea'], props=dict(follow=1)) self.graph.edges[('sea', 'eps')] = Edge(self.graph.nodes['sea'], self.graph.nodes['eps'], props=dict(follow=1, isDropoff=True, isSea=True)) self.graph.edges[('eps', 'alpha')] = Edge(self.graph.nodes['eps'], self.graph.nodes['alpha'], props=dict(follow=1)) # Create a path (track) to follow self.init_path = [self.graph.edges[('start', 'alpha')]] self.path = [ self.graph.edges[('alpha', 'land')], self.graph.edges[('land', 'beta')], self.graph.edges[('beta', 'east_off')], self.graph.edges[('east_off', 'celta')], self.graph.edges[('celta', 'pickup')], self.graph.edges[('pickup', 'delta')], self.graph.edges[('delta', 'sea')], self.graph.edges[('sea', 'eps')], self.graph.edges[('eps', 'alpha')] ] # Report entire path self.logd("__init__()", "Path:") print Node.dumpHeader() for edge in self.path: print edge.fromNode.dump() if edge is not None: print edge.toNode.dump() # Initialize bot at start location self.bot = Bot(self.graph.nodes['start'].loc)
rv = call([config["map_script"], config["map_res"]]) # Change back to original CWD (typically qwe) os.chdir(origCWD) # Check return value of map script to confirm that it worked if rv != config["map_script_success"]: logger.critical("Map script returned {}, call was: {} {}".format(rv, config["map_script"], config["map_res"])) sys.exit(errors["ERROR_MAP_SCRIPT"]) else: logger.info("Map files already exist and the map is of resolution {}".format(config["map_res"])) # Get map, waypoints and map properties course_map = mapper.unpickle_map(config["map_pkl"]) logger.debug("Map unpickled") waypoints = mapper.unpickle_waypoints(config["waypoints_pkl"]) logger.debug("Waypoints unpickled") map_properties = mapper.unpickle_map_prop_vars(config["map_props_pkl"]) logger.debug("Map properties unpickled") # Find start location start_x = waypoints["start"][0][0] * float(nav.env_config["cellsize"]) * 39.3701 # Convert to inches start_y = waypoints["start"][0][1] * float(nav.env_config["cellsize"]) * 39.3701 # Convert to inches start_theta = 0 # In radians # Build shared data structures # Not wrapping them in a mutable container, as it's more complex for other devs # See the following for details: http://goo.gl/SNNAs manager = Manager() bot_loc = manager.dict(x=start_x, y=start_y, theta=start_theta, dirty=False) blobs = manager.list() # for communication between vision and planner