def __init__(self, port, filename, waitOnStart=False):
self.conn = RxConnector(port, 'UDP')
self.log = open(filename, 'w')
header = 'rcv_time,rcv,msg_id,tmp_id,snd_time,lat,lon,elevation,accuracy'
self.log.write(header)
self.wait = waitOnStart
Process.__init__(self)
class Imu(Sensor):
def __init__(self, port, filename):
self.conn = RxConnector(port, 'UDP')
self.log = open(filename, 'wb')
Sensor.__init__(self)
def run(self):
header = 'time,ax,ay,az,gx,gy,gz,mx,my,mz'
#header = 'time,ax,ay,az,gx,gy,gz,heading,heading_valid'
self.log.write(header)
loglist = []
logcounter = 0
while True:
news = self.conn.recv(36, 1.1)
assert len(news) == 36, "IMU connection stopped"
rcv_time = '\n{:9.3f}'.format(time.time() - self.startTime)
loglist += [rcv_time + parseMessage(news)]
logcounter += 1
if logcounter > 100:
self.log.write(''.join(loglist))
loglist = []
logcounter = 0
def clean(self):
self.log.close()
self.conn.__exit__()
class DsrcLog(Process):
def __init__(self, port, filename, waitOnStart=False):
self.conn = RxConnector(port, 'UDP')
self.log = open(filename, 'w')
header = 'rcv_time,rcv,msg_id,tmp_id,snd_time,lat,lon,elevation,accuracy'
self.log.write(header)
self.wait = waitOnStart
Process.__init__(self)
def start(self, referenceTime):
self.startTime = referenceTime
if self.wait:
self.conn.recv(1024) # wait until you receive first message
Process.start(self)
def run(self):
while True:
news = self.conn.recv(1024, .2)
assert news!='', "DSRC connection stopped"
bsm, extra = parsemessage(news)
rcv_time = '\n{:9.3f},'.format(time.time() - self.startTime)
self.log.write(rcv_time + ','.join((str(mm) for mm in bsm)) )
def __enter__(self): return self
def __exit__(self, errtype, errval, traceback):
self.log.close()
self.conn.__exit__(errtype, errval, traceback)
Process.terminate(self)
def __init__(self, port):
self.conn = RxConnector(port, 'UDP')
self.me_tracker = None
self.me_time = 0
self.me_count = 0
self.starting = True
self.trackers = []
self.counts = []
self.times = []
self.ids = []
self.counts = []
def __init__(self, port, filename):
self.conn = RxConnector(port, 'UDP')
self.log = open(filename, 'wb')
Sensor.__init__(self)
class V2V():
def __init__(self, port):
self.conn = RxConnector(port, 'UDP')
self.me_tracker = None
self.me_time = 0
self.me_count = 0
self.starting = True
self.trackers = []
self.counts = []
self.times = []
self.ids = []
self.counts = []
def __enter__(self):
return self
def __exit__(self, errtype=None, errval=None, traceback=None):
self.conn.__exit__()
def read(self):
n_others = len(self.ids)
keep_other = [count < self.me_count + 50 for count in self.counts]
self.trackers = [self.trackers[k] for k in range(n_others) if keep_other[k]]
self.ids = [self.ids[k] for k in range(n_others) if keep_other[k]]
self.times = [self.times[k] for k in range(n_others) if keep_other[k]]
self.counts = [self.counts[k] for k in range(n_others) if keep_other[k]]
# read all currently available messages
while True:
news = self.conn.recv(1024, .0001)
if news=='': break
bsm = parsemessage(news)
lat = bsm[4]
lon = bsm[5]
if lat > 90 or lon > 180: continue # gps not active yet
coord = utm.from_latlon(lat,lon)[:2]
if bsm[0] == 0:
# your own message
# first check if this is a new message or a repeat of previous
if bsm[3] != self.me_time:
self.me_time = bsm[3]
if self.starting:
self.me_tracker = GpsTracker(coord)
self.starting = False
else:
self.me_tracker.predict()
self.me_tracker.update(coord)
self.me_count += 1
else:
otherID = bsm[2]
othertime = bsm[3]
included = False
for k in range(len(self.trackers)):
if self.ids[k] == otherID:
included = True
time_diff = othertime - self.times[k]
if time_diff < -59000: time_diff += 60000 # correct for minute
if time_diff > 0:
self.times[k] = othertime
tracker = self.trackers[k]
# might need to predict more than once
# because a message was skipped
for tk in range(0,time_diff,200):
tracker.predict()
tracker.update(coord)
self.counts[k] = self.me_count
if not included: # new vehicle detected
self.ids.append(otherID)
self.trackers.append(GpsTracker(coord))
self.times.append(othertime)
self.counts.append(self.me_count)
assert len(self.counts) == len(self.trackers)
assert len(self.counts) == len(self.times)
# return current estimates
me_return = list(self.me_tracker.pos) + list(self.me_tracker.speed)
other_return = [(
list(self.trackers[k].pos) + list(self.trackers[k].speed),
self.ids[k]) for k in range(len(self.trackers))]
return me_return, other_return
def __init__(self, port):
Process.__init__(self)
self.conn = RxConnector(port, 'UDP')
self.queue = Queue()
class DSRC(Process):
def __init__(self, port):
Process.__init__(self)
self.conn = RxConnector(port, 'UDP')
self.queue = Queue()
def __enter__(self):
return self.queue
def run(self):
my_last_time = 0
my_last_msg = None
times = []
msgs = []
ids = []
updated = []
while True:
news = self.conn.recv(1024, .2)
assert news != '', "DSRC connection stopped"
rcv, msg_id, tmp_id, snd_time, lat, lon, elevation, accuracy,\
extra = parsemessage(news)
lat *= 10.**-7
lon *= 10.**-7
if lat < 90 and lon < 180:
x, y = utm.from_latlon(lat, lon)[:2]
if rcv == 0:
if snd_time > my_last_time or snd_time < my_last_time - 50:
my_last_msg = (x, y, extra)
my_last_time = snd_time
# send current messages
output = [
(1, vid, msg[0], msg[1], msg[2])
for vid, msg, update in zip(ids, msgs, updated)
if update
]
output.append((0, 0, my_last_msg[0], my_last_msg[1],
my_last_msg[2]))
self.queue.add(output)
else:
found = False
for idx, current_id in enumerate(ids):
if current_id == tmp_id:
if snd_time > times[
idx] or snd_time < times[idx] - 50:
msgs[idx] = (x, y, extra)
times[idx] = snd_time
updated[idx] = True
found = True
break
if not found:
times.append(snd_time)
ids.append(msg_id)
msgs.append((x, y, extra))
updated[idx].append(True)
def terminate(self):
self.conn.__exit__()
self.queue.close()
super(DSRC, self).terminate()
def __exit__(self, errtype=None, errval=None, traceback=None):
self.terminate()