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:
