def __init__ (self, logs):
self.sols, self.rnodes = [], []
self.utime = None
self.nodes = []
self.index = {}
self.node_utime = []
self._Ns = 2
self._rows, self._cols, self._data = [],[],[]
self._vector = []
self.x = None
self.cov = None
self.nlogs = len (logs)
self.lcmlogs = logs
# find server node ids for range events---client log is 0th
rfactors = [isam2_f_pose_pose_partial_t.decode (e.data) for e in logs[0] if e.channel=='F_RANGE2D']
#rfactors = [isam2_f_glc_t.decode (e.data) for e in logs[0] if e.channel=='F_DS2D']
logs[0].seek (0)
self.range_ids = [f.node_id2 for f in rfactors]
#self.range_ids = [f.node_id[1] for f in rfactors]
self.rid = 0
self.ids = {ii:[] for ii in range (self.nlogs)}
self.events = [l.read_next_event () for l in self.lcmlogs]
self.lid = self.nlogs-1 # active log id
from perls.lcmtypes.perllcm import isam2_f_glc_t
if __name__ == '__main__':
if len(sys.argv) < 4:
print 'usage: %s <est_ceif.pkl> <est_deif.pkl> <lcmlog-client-gm>' % sys.argv[
0]
sys.exit(0)
with open(sys.argv[1], 'rb') as f:
estc = pickle.load(f)
with open(sys.argv[2], 'rb') as f:
estd = pickle.load(f)
logc = lcm.EventLog(sys.argv[3])
f_owtt = [
isam2_f_pose_pose_partial_t.decode(e.data) for e in logc
if e.channel == 'F_RANGE2D'
]
t_ds = np.asarray([f.utime for f in f_owtt])
#f_ds = [isam2_f_glc_t.decode (e.data) for e in logc if e.channel == 'F_DS2D']
#t_ds = np.asarray ([f.utime for f in f_ds])
# plot paths
fig_path = plt.figure()
ax_path = fig_path.add_subplot(111)
estc.plot(ax_path, 'b.-', label='CEIF')
estd.plot(ax_path, 'r.-', label='DEIF')
ax_path.set_xlabel('east [m]')
ax_path.set_ylabel('north [m]')
import sys
import lcm
from perls.lcmtypes.perllcm import isam2_f_prior_t
from perls.lcmtypes.perllcm import isam2_f_pose_pose_t
from perls.lcmtypes.perllcm import isam2_f_pose_pose_partial_t
if __name__ == '__main__':
log = lcm.EventLog (sys.argv[1])
for e in log:
if e.channel=='F_PRIOR2D':
f = isam2_f_prior_t.decode (e.data)
if f.node_id < 1e5:
print f.utime, f.node_id
elif e.channel=='F_ODO2D':
f = isam2_f_pose_pose_t.decode (e.data)
if f.node_id1 < 1e5:
print f.utime, f.node_id1
if f.node_id2 < 1e5:
print f.utime, f.node_id2
elif e.channel=='F_RANGE2D':
f = isam2_f_pose_pose_partial_t.decode (e.data)
if f.node_id1 < 1e5:
print f.utime, f.node_id1
if f.node_id2 < 1e5:
print f.utime, f.node_id2
sys.exit (0)
from perls.lcmtypes.perllcm import isam2_f_pose_pose_partial_t
from perls.lcmtypes.perllcm import isam2_f_glc_t
if __name__ == '__main__':
if len (sys.argv) < 4:
print 'usage: %s <est_ceif.pkl> <est_deif.pkl> <lcmlog-client-gm>' % sys.argv[0]
sys.exit (0)
with open (sys.argv[1], 'rb') as f:
estc = pickle.load (f)
with open (sys.argv[2], 'rb') as f:
estd = pickle.load (f)
logc = lcm.EventLog (sys.argv[3])
f_owtt = [isam2_f_pose_pose_partial_t.decode (e.data) for e in logc if e.channel == 'F_RANGE2D']
t_ds = np.asarray ([f.utime for f in f_owtt])
#f_ds = [isam2_f_glc_t.decode (e.data) for e in logc if e.channel == 'F_DS2D']
#t_ds = np.asarray ([f.utime for f in f_ds])
# plot paths
fig_path = plt.figure ()
ax_path = fig_path.add_subplot (111)
estc.plot (ax_path, 'b.-', label='CEIF')
estd.plot (ax_path, 'r.-', label='DEIF')
ax_path.set_xlabel ('east [m]')
ax_path.set_ylabel ('north [m]')
ax_path.legend ()
def run (self, log):
"""
basic idea is to process all other logs up to the next tol node id,
then process client through the range measurement, then lather rinse
and repeat until all range measurements have been reached.
"""
if self.nlogs==1: logname = 'lcmlog-decentralized'
else: logname = 'lcmlog-centralized'
est_log = lcm.EventLog (logname, 'w', overwrite=True)
li,e = self.next_event ()
while not e is None:
if e.channel=='F_PRIOR2D':
f = isam2_f_prior_t.decode (e.data)
self.add_prior_factor (f,li)
if li==0 and f.utime > self.utime and f.node_id > 1e5:
self.publish (est_log)
self.utime = f.utime
elif e.channel=='F_ODO2D':
f = isam2_f_pose_pose_t.decode (e.data)
self.add_odo_factor (f,li)
if li==0 and f.utime > self.utime and f.node_id1 > 1e5:
self.publish (est_log)
self.utime = f.utime
#if li==0 and f.node_id2 < 1e5:
# self.solve ()
# sol = Centralized_solution (self.ids, self.nodes, self.x, self.cov)
# self.sols.append (sol)
elif e.channel=='F_DS2D' and self.nlogs==1:
f = isam2_f_glc_t.decode (e.data)
self.add_ds_factor (f, li)
if li==0 and f.utime > self.utime:
self.publish (est_log)
self.utime = f.utime
self.solve ()
sol = Centralized_solution (self.ids, self.nodes, self.x, self.cov)
self.sols.append (sol)
elif e.channel=='F_RANGE2D':
f = isam2_f_pose_pose_partial_t.decode (e.data)
if li==0 and f.utime > self.utime:
self.publish (est_log)
self.utime = f.utime
self.add_range_factor (f)
self.rnodes.append ((f.node_id1, f.node_id2))
#self.solve ()
#sol = Centralized_solution (self.ids, self.nodes, self.x, self.cov)
#self.sols.append (sol)
li,e = self.next_event ()
# save list of solutions
with open ('solutions.pkl', 'wb') as f:
pickle.dump (self.sols, f)
with open ('rangenodes.pkl', 'wb') as f:
pickle.dump (self.rnodes, f)
# recover state at conclusion
if len (self.nodes) < 6.8e3:
print '{Centralized} recovering mean AND cov'
self.solve (recover_cov=True)
else:
print '{Centralized} recovering mean NOT cov'
self.solve (recover_cov=False)
print '{Centralized} added %d nodes' % (len (self.nodes))
def next_event (self):
"""
first return all events in server log up until next tol, then return
all client events through range measurement.
"""
#utime = [e.timestamp for e in self.events]
##ii = np.argmin (utime)
#iisort = np.argsort (utime)
##not1,not2 = False,False
##if 1103 in self.nodes:
## not1 = True
## print 'finished 28'
##if 10094 in self.nodes:
## not2 = True
## print 'finished 31'
#ii = iisort[0]
##if ii==1 and not1:
## if not2:
## ii = 0
## else:
## ii = iisort[1]
##if ii==2 and not2:
## if not1:
## ii = 0
## else:
## ii = iisort[1]
#e = self.events[ii]
#ne = self.lcmlogs[ii].read_next_event ()
#while not ne is None and (ne.channel=='REQUEST_STATE' or ne.channel=='CMD'):
# ne = self.lcmlogs[ii].read_next_event ()
#self.events[ii] = ne
#if not ne is None: return ii,e
#else: return 0,None
ii = self.lid
e = self.events[ii]
ne = self.lcmlogs[ii].read_next_event ()
while not ne is None and (ne.channel=='REQUEST_STATE' or ne.channel=='CMD'):
ne = self.lcmlogs[ii].read_next_event ()
self.events[ii] = ne
if e is None: return 0,e
#if self.rid > len (self.range_ids)-1: return 0,None
if self.lid==0:
if e.channel=='F_RANGE2D':
f = isam2_f_pose_pose_partial_t.decode (e.data)
if f.node_id2==self.range_ids[self.rid]:
self.rid+=1
self.lid=self.nlogs-1
else:
if e.channel=='F_PRIOR2D':
f = isam2_f_prior_t.decode (e.data)
if f.node_id==self.range_ids[self.rid]:
self.lid=0
elif e.channel=='F_ODO2D':
f = isam2_f_pose_pose_t.decode (e.data)
if f.node_id2==self.range_ids[self.rid]:
self.lid=0
return ii, e
