class Controller(object): def __init__(self): self.lock = Lock() self.kb = KnowledgeBase() self.location = Location(self.kb, self.lock) self.motion = MotionSubsystem(self.kb, self.lock, self.motion_callback) self.rf = RadioSubsystem(self.kb, self.lock, self.radio_data_callback) self.fsm_state = 'at_beginning' self.sent_packet = np.array([]) self.ack_packet = np.array([]) self.goodput = np.array([]) self.color_values = np.array([]) self.arrived = False def run(self): """ Start the controller. """ self.kb.build_route() self.location.start() self.motion.start() self.rf.start() # time.sleep(0.1) self.fsm() def motion_callback(self, has_arrived): self.arrived = has_arrived def radio_data_callback(self, sent_packet, ack_packet, goodput): """ Radio Subsystem IPC. """ self.sent_packet = np.append(self.sent_packet, sent_packet) self.ack_packet = np.append(self.ack_packet, ack_packet) self.goodput = np.append(self.goodput, goodput) def reset_radio_data(self): """ Reset local radio data storage. """ self.sent_packet = np.array([]) self.ack_packet = np.array([]) self.goodput = np.array([]) def fsm(self): while True: if self.fsm_state == 'at_beginning': before_start = 0 start_node = self.kb.get_start_node() self.motion.set_source_destination(before_start, start_node) self.motion.set_speed(25) self.motion.control_motion_operation('go') while not self.arrived: time.sleep(0.1) next_node = self.kb.get_next_node(start_node) self.kb.set_current_node(start_node) self.kb.set_next_node(next_node) self.kb.set_next_edge((start_node, next_node)) self.fsm_state = 'traversing_edge' continue elif self.fsm_state == 'traversing_edge': if DEBUG: print "traversing_edge" kb_state = self.kb.get_state() print kb_state current_edge = kb_state['next_edge'] next_edge = None last_node = kb_state['current_node'] current_node = None next_node = kb_state['next_node'] self.kb.set_current_edge(current_edge) self.kb.set_next_edge(next_edge) self.kb.set_last_node(last_node) self.kb.set_current_node(current_node) self.arrived = False self.motion.set_source_destination(current_edge[0], current_edge[1]) self.motion.set_speed(45) tic = time.time() self.motion.control_motion_operation('go') while not self.arrived: self.color_values = np.append(self.color_values, self.motion.color_reading()) time.sleep(0.1) toc = time.time() - tic # weight = toc * np.average(self.goodput) # targets = self.count_targets(self.color_values) # if DEBUG: # print "values for edge %s: weight = %0.2f, targets = %d" %(str(current_edge), # weight, # targets) # self.kb.set_edge_values(current_edge, weight, targets) metric_b = toc * np.average(self.goodput) targets = self.count_targets(self.color_values) if DEBUG: print "values for edge %s: targets = %d, metric_b = %f" %(str(current_edge), targets, metric_b) self.kb.set_edge_values(current_edge, targets, metric_b) self.fsm_state = 'at_a_node' continue elif self.fsm_state == 'at_a_node': self.rf.control_radio_operation('pause') kb_state = self.kb.get_state() current_node = kb_state['next_node'] next_node = self.kb.get_next_node(current_node) current_edge = None last_edge = kb_state['current_edge'] next_edge = (current_node, next_node) self.kb.set_current_node(current_node) self.kb.set_next_node(next_node) self.kb.set_current_edge(current_edge) self.kb.set_last_edge(last_edge) self.kb.set_next_edge(next_edge) self.fsm_state = 'traversing_edge' self.reset_radio_data() self.color_values = np.array([]) self.rf.control_radio_operation('continue') continue else: print "controller.fsm() error" print "state == ", state break def count_targets(self, a): a[a!=5] = 0 j = 0 for i in range(len(a)-1): if a[i] == 0 and a[i+1] == 5: j += 1 return j def shutdown(self): self.kb.save_kb() self.motion.join() self.rf.join() time.sleep(0.1) self.location.join() # shut this down last
class Controller(object): def __init__(self): self.lock = Lock() self.kb = KnowledgeBase() self.location = Location(self.kb, self.lock) self.motion = MotionSubsystem(self.kb, self.lock, self.motion_callback) self.rf = RadioSubsystem(self.kb, self.lock, self.radio_data_callback) self.fsm_state = 'at_beginning' self.sent_packet = np.array([]) self.ack_packet = np.array([]) self.goodput = np.array([]) self.color_values = np.array([]) self.arrived = False def run(self): """ Start the controller. """ self.kb.build_route() self.location.start() self.motion.start() self.rf.start() # time.sleep(0.1) self.fsm() def motion_callback(self, has_arrived): self.arrived = has_arrived def radio_data_callback(self, sent_packet, ack_packet, goodput): """ Radio Subsystem IPC. """ self.sent_packet = np.append(self.sent_packet, sent_packet) self.ack_packet = np.append(self.ack_packet, ack_packet) self.goodput = np.append(self.goodput, goodput) def reset_radio_data(self): """ Reset local radio data storage. """ self.sent_packet = np.array([]) self.ack_packet = np.array([]) self.goodput = np.array([]) def fsm(self): while True: if self.fsm_state == 'at_beginning': before_start = 0 start_node = self.kb.get_start_node() self.motion.set_source_destination(before_start, start_node) self.motion.set_speed(25) self.motion.control_motion_operation('go') while not self.arrived: time.sleep(0.1) next_node = self.kb.get_next_node(start_node) self.kb.set_current_node(start_node) self.kb.set_next_node(next_node) self.kb.set_next_edge((start_node, next_node)) self.fsm_state = 'traversing_edge' continue elif self.fsm_state == 'traversing_edge': if DEBUG: print "traversing_edge" kb_state = self.kb.get_state() print kb_state current_edge = kb_state['next_edge'] next_edge = None last_node = kb_state['current_node'] current_node = None next_node = kb_state['next_node'] self.kb.set_current_edge(current_edge) self.kb.set_next_edge(next_edge) self.kb.set_last_node(last_node) self.kb.set_current_node(current_node) self.arrived = False self.motion.set_source_destination(current_edge[0], current_edge[1]) self.motion.set_speed(45) tic = time.time() self.motion.control_motion_operation('go') while not self.arrived: self.color_values = np.append(self.color_values, self.motion.color_reading()) time.sleep(0.1) toc = time.time() - tic # weight = toc * np.average(self.goodput) # targets = self.count_targets(self.color_values) # if DEBUG: # print "values for edge %s: weight = %0.2f, targets = %d" %(str(current_edge), # weight, # targets) # self.kb.set_edge_values(current_edge, weight, targets) metric_b = toc * np.average(self.goodput) targets = self.count_targets(self.color_values) if DEBUG: print "values for edge %s: targets = %d, metric_b = %f" % ( str(current_edge), targets, metric_b) self.kb.set_edge_values(current_edge, targets, metric_b) self.fsm_state = 'at_a_node' continue elif self.fsm_state == 'at_a_node': self.rf.control_radio_operation('pause') kb_state = self.kb.get_state() current_node = kb_state['next_node'] next_node = self.kb.get_next_node(current_node) current_edge = None last_edge = kb_state['current_edge'] next_edge = (current_node, next_node) self.kb.set_current_node(current_node) self.kb.set_next_node(next_node) self.kb.set_current_edge(current_edge) self.kb.set_last_edge(last_edge) self.kb.set_next_edge(next_edge) self.fsm_state = 'traversing_edge' self.reset_radio_data() self.color_values = np.array([]) self.rf.control_radio_operation('continue') continue else: print "controller.fsm() error" print "state == ", state break def count_targets(self, a): a[a != 5] = 0 j = 0 for i in range(len(a) - 1): if a[i] == 0 and a[i + 1] == 5: j += 1 return j def shutdown(self): self.kb.save_kb() self.motion.join() self.rf.join() time.sleep(0.1) self.location.join() # shut this down last
class Controller(object): def __init__(self): self.kb = KnowledgeBase() self.lock = threading.Lock() self.location = Location(self.kb, self.lock) self.motion = SimpleMotion(self.kb, self.lock) self.rf = RadioSubsystem(self.kb, self.lock, self.radio_data_callback) self.fsm_state = 'at_beginning' self.sent_packet = np.array([]) self.ack_packet = np.array([]) self.goodput = np.array([]) def run(self): """ Start the controller. """ self.kb.build_route() self.location.start() self.rf.start() while True: self.fsm() def radio_data_callback(self, sent_packet, ack_packet, goodput): self.sent_packet = np.append(self.sent_packet, sent_packet) self.ack_packet = np.append(self.ack_packet, ack_packet) self.goodput = np.append(self.goodput, goodput) def reset_radio_data(self): self.sent_packet = np.array([]) self.ack_packet = np.array([]) self.goodput = np.array([]) def fsm(self): if self.fsm_state == 'at_beginning': print "\n\n\n" print "at_beginning" start_node = self.kb.get_start_node() self.motion.move_until_location(start_node, speed = 25) while not self.kb.get_state()['current_location'] == start_node: time.sleep(0.2) next_node = self.kb.get_next_node(start_node) # self.lock.acquire() self.kb.set_current_node(start_node) self.kb.set_next_node(next_node) self.kb.set_next_edge((start_node, next_node)) # self.lock.release() self.fsm_state = 'traversing_edge' return elif self.fsm_state == 'traversing_edge': print "\n\n\n" print "traversing_edge" kb_state = self.kb.get_state() current_edge = kb_state['next_edge'] next_edge = None last_node = kb_state['current_node'] current_node = None next_node = kb_state['next_node'] # self.lock.acquire() self.kb.set_current_edge(current_edge) self.kb.set_next_edge(next_edge) self.kb.set_last_node(last_node) self.kb.set_current_node(current_node) # self.lock.release() tic = time.time() # this is not threaded!!!!! self.motion.move_from_here_to_there(last_node, next_node, speed = 45) # print "self.kb.get_state()['current_location'] = ", self.kb.get_state()['current_location'] # print "next_node = ", next_node # print "self.kb.get_state()['current_location'] == next_node : ", self.kb.get_state()['current_location'] == next_node print "traversing_eddge, current_node = ", current_node print "traversing_edge, next_node = ", next_node while not self.kb.get_state()['current_location'] == next_node: # color = motion.get_color_reading() print "\n\n\n\n" print "Blah blah blah" # print "Detected color: ", color print "\n\n\n\n" time.sleep(0.01) toc = time.time() - tic weight = toc * np.average(self.goodput) print "weight value for edge %s = %0.2f" %(str(current_edge), weight) self.kb.set_edge_weight(current_edge, weight) # print self.goodput # print "average goodput for edge %s = %0.2f" %(str(current_edge), np.average(self.goodput)) self.fsm_state = 'at_a_node' return elif self.fsm_state == 'at_a_node': print "\n\n\n" print "at_a_node" self.rf.control_radio_operation('pause') kb_state = self.kb.get_state() current_node = kb_state['next_node'] next_node = self.kb.get_next_node(current_node) print "at_a_node, current_node = ", current_node print "at_a_node, next_node = ", next_node current_edge = None last_edge = kb_state['current_edge'] next_edge = (current_node, next_node) # self.lock.acquire() self.kb.set_current_node(current_node) self.kb.set_next_node(next_node) self.kb.set_current_edge(current_edge) self.kb.set_last_edge(last_edge) self.kb.set_next_edge(next_edge) # self.lock.release() self.fsm_state = 'traversing_edge' self.rf.control_radio_operation('continue') self.reset_radio_data() return else: if DEBUG: print "controller.fsm() error" print "state == ", state else: pass def shutdown(self): self.kb.save_kb() # pprint.pprint(self.kb.get_state()) # print "\n\n\n\n\n" # self.kb.route_debug() # print "\n\n\n\n\n" self.motion.shutdown() self.rf.join() time.sleep(0.1) self.location.join() # shut this down last