def createRobot(logName, runNumber=0): metaLog = open(logName) for line in metaLog: if line.startswith("CANlog:"): runNumber -= 1 if runNumber <= 0: logName = logName[:logName.find("meta")] + line.split( )[1].split("/")[-1] break can = CAN(ReplyLogInputsOnly(logName), skipInit=True) robot = EduroMaxi(can, replyLog=logName, metaLog=metaLog) robot.localisation = KalmanFilter() #SimpleOdometry() robot.attachLaser(remission=True) robot.attachCamera() return robot
def createRobot( logName, runNumber=0 ): metaLog = open(logName) for line in metaLog: if line.startswith("CANlog:"): runNumber -= 1 if runNumber <= 0: logName = logName[:logName.find("meta")]+line.split()[1].split("/")[-1] break can = CAN( ReplyLogInputsOnly( logName ), skipInit = True ) robot = EduroMaxi( can, replyLog=logName, metaLog=metaLog) robot.localisation = KalmanFilter() #SimpleOdometry() robot.attachLaser( remission=True ) robot.attachCamera() return robot
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))
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.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() 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: