class EduroMaxiReader: def __init__(self, fn): self.runconfig = list(getlogs(fn)) self.run=0 def startrun(self, i, startposition=None): if i<0: i=len(self.runconfig)+i print("selecting run %i of %i"%(i,len(self.runconfig))) self.run=i runnumber, position, run = self.runconfig[i] if startposition: self.startposition=startposition position=startposition else: self.startposition=position self.robot=EduroMaxi(can=DummyCan()) self.robot.localisation = KalmanFilter(pose=position) self.waitforstart=True self.lastcable=False self.readers=[] self.names=[] for type, fn in run: if type=="CANlog": self.canreader=MsgGrouper(MsgDummyInterpreter(Msgparser(fn)), filter_origin=["can"]) else: self.names.append(type) self.readers.append(textlogreader(fn)) def nextrun(self): run=self.run+1 if run >= len(self.runconfig): run=0 self.startrun(run) def update(self): while True: time, can_msgs=self.canreader.next() for id,data in can_msgs: self.robot.updateEnc(id,data) #self.robot.updateEmergencyStop( id, data ) #self.robot.updateCompass(id,data) self.robot.updateButtons(id,data) #self.robot.updateSharps( id,data ) #self.robot.updateBattery( id, data ) self.received=dict() for name, reader in zip(self.names, self.readers): self.received[name]=reader.next() if self.waitforstart: if self.lastcable and not self.robot.startCableIn: self.waitforstart=False break else: self.lastcable=self.robot.startCableIn else: break return time, self.robot.localisation.pose(), self.received def __iter__(self): return self def next(self): return self.update()
self.lastcable=self.robot.startCableIn else: break return time, self.robot.localisation.pose(), self.received def __iter__(self): return self def next(self): return self.update() if __name__ == "__main__": robot=EduroMaxi(can=DummyCan()) robot.localisation = KalmanFilter() for run, start, msgdict in MetaLogReader(sys.argv[1]): time, can_msgs=msgdict["CANlog"] for id,data in can_msgs: robot.updateEnc(id,data) robot.updateEmergencyStop( id, data ) robot.updateCompass(id,data) robot.updateButtons(id,data) robot.updateSharps( id,data ) robot.updateBattery( id, data ) #print(robot.localisation.pose()) if 'RFU620LOG' in msgdict and msgdict['RFU620LOG']: id, scans=msgdict['RFU620LOG'] #print(robot.localisation.pose()) for s in scans: #print(s.id, s.RSSI) pass