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
