class LocMission(Thread):
"""
- manages a measurement publisher
- executes a configurable zigzag mission
"""
def __init__(self, map, req):
Thread.__init__(self)
#system('svctrl --start wifi_sensor > /dev/null')
self.gps_reader = _GPS_Reader(map['gps'])
self.measure_reader = _MeasureReader(map['net'])
self.combined_publisher = _CombinedPublisher(map['pub_server'], self.gps_reader, self.measure_reader)
self.map = map
self.req = req
self.factory = ICARUS_MissionFactory()
self.client = ICARUS_SynClient(map['ctrl'], map['state'])
def run(self):
self.gps_reader.start()
self.measure_reader.start()
self.combined_publisher.start()
for x, y in self._zigzag_gen(0.0, 0.0, 0.1, 0.1, 10):
self.client.execute(self.factory.move_xy(x, y))
self.combined_publisher.term()
self.measure_reader.term()
self.gps_reader.term()
self.req.status = MissionMessage.DONE
self.map['pub_server'].send(self.req.SerializeToString())
def _zigzag_gen(self, x, y, width, height, breaks):
for i in range(breaks):
yield x + width / 2.0, y
y += (height / breaks) / 2.0
yield x - width / 2.0, y
y += (height / breaks) / 2.0
yield width / 2.0, height
gps_reader = GPS_Reader(sm['gps'])
gps_reader.start()
_client = ICARUS_Client(sm['ctrl'])
i = ICARUS_MissionFactory()
def request(item):
try:
_client.execute(item)
except Exception, e:
print e
#ctrl = array([0.0, 0.0])
inf = Interface(1, '/dev/ttyACM0')
while True:
sleep(0.1)
msg = inf.receive()
if msg:
if 1:#msg.type == POS:
gps_nrf = array(gpos.unpack(msg.data))
gps_pos = array([gps_reader.data.lat, gps_reader.data.lon])
print gps_pos, gps_nrf
gps_target = array(gps_nrf)
vec = gps_meters_offset(gps_pos, gps_target)
ctrl = step(vec)
print gps_pos, vec, norm(vec), ctrl
request(i.move_xy(ctrl[0], ctrl[1]))
#ctrl = tam_ctrl.Controller()
#import new
#ctrl = new.Controller()
#ctrl = tobi.Controller()
if 1:
#try:
while True:
# read inputs:
imms_reader.event.wait()
imms_reader.event.clear()
rssi = imms_reader.data.rssi
x = mon_reader.data.x
y = mon_reader.data.y
# call decision function:
x_dest, y_dest, change = ctrl.decide(x, y, rssi, time())
# log decision:
print x, y, rssi, x_dest, y_dest, change
#sleep(15)
# update uav setpoint:
if change:
request(i.move_xy(x_dest, y_dest))
#except:
# pass