import lcm
from perls.lcmtypes.perllcm import isam2_f_prior_t
from perls.lcmtypes.perllcm import isam2_f_pose_pose_t
if __name__ == '__main__':
if len (sys.argv) < 2:
print 'usage: %s <lcmlog-graph-manager>' % sys.argv[0]
sys.exit (0)
ilog = lcm.EventLog (sys.argv[1])
olog = lcm.EventLog ('lcmlog-graph-manager-cooked', 'w', overwrite=True)
DTOL = 2
for e in ilog:
if e.channel=='F_PRIOR2D':
f = isam2_f_prior_t.decode (e.data)
if f.node_id < 1e5:
f.node_id += DTOL
olog.write_event (e.timestamp, e.channel, f.encode ())
continue
elif e.channel=='F_ODO2D':
f = isam2_f_pose_pose_t.decode (e.data)
if f.node_id1 < 1e5:
f.node_id1 += DTOL
if f.node_id2 < 1e5:
f.node_id2 += DTOL
olog.write_event (e.timestamp, e.channel, f.encode ())
continue
olog.write_event (e.timestamp, e.channel, e.data)
sys.exit (0)
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))
import lcm
from perls.lcmtypes.perllcm import isam2_f_prior_t
from perls.lcmtypes.perllcm import isam2_f_pose_pose_t
if __name__ == '__main__':
if len(sys.argv) < 2:
print 'usage: %s <lcmlog-graph-manager>' % sys.argv[0]
sys.exit(0)
ilog = lcm.EventLog(sys.argv[1])
olog = lcm.EventLog('lcmlog-graph-manager-cooked', 'w', overwrite=True)
DTOL = 2
for e in ilog:
if e.channel == 'F_PRIOR2D':
f = isam2_f_prior_t.decode(e.data)
if f.node_id < 1e5:
f.node_id += DTOL
olog.write_event(e.timestamp, e.channel, f.encode())
continue
elif e.channel == 'F_ODO2D':
f = isam2_f_pose_pose_t.decode(e.data)
if f.node_id1 < 1e5:
f.node_id1 += DTOL
if f.node_id2 < 1e5:
f.node_id2 += DTOL
olog.write_event(e.timestamp, e.channel, f.encode())
continue
olog.write_event(e.timestamp, e.channel, e.data)
sys.exit(0)
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
