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
