Esempio n. 1
0
 def startRun(self, fn, run, keeppostion=False):
     self.dirname, self.filename = os.path.split(fn)
     print("starting log %s run %i"%(fn,run))
     self.loggenerator=EduroMaxiReader(fn)
     self.loggenerator.startrun(run)
     self.root.title("%s %s run %i"%(self.dirname,self.filename, self.loggenerator.run+1))
     self.data=list(self.loggenerator)
     if not keeppostion:
         self.position=0
     self.posScale.set(self.position)
     self.showData(self.position)
     self.posScale.configure(to_= len(self.data)-1)
     self.view.reset()
     self.camera.clear()
Esempio n. 2
0
class Logplayer(Frame):
    def __init__(self, root, logfn, viewer, analyzator=None, logdir=None, **kwargs):
        self.root=root
        self.logdir = logdir
        self.analyzator = analyzator
        if not self.logdir:
            self.logdir=os.path.join(os.path.dirname(os.path.dirname(__file__)),'log')
        Frame.__init__(self, **kwargs)
        self.view=viewer(self, x=800, y=600, onLeft=self.clickL, onRight=self.clickR)
        self.createWidgets()
        self.bind_all("<Left>", self.prev)
        self.bind_all("<Right>", self.next)

        self.view.redraw()
        self.root.update()
        if not logfn:
            logfn = self.logSelectDialog()
        self.dirname, self.filename = os.path.split(logfn)
        self.startRun(logfn,0)
        self.view.redraw()
        self.root.after(10,self.view.zoomExtens)
        self.running=False

    def logSelectDialog(self):
            self.root.update()
            timestamps, dirs, fns = zip(*sorted(listdir(self.logdir), reverse=True))
            o=SelcectDialog(self.root, "select log", timestamps)
            self.root.wait_window(o.top)
            index=timestamps.index(o.option)
            return os.path.join(dirs[index], fns[index])

    def createWidgets(self):
        self.controlFrame=Frame(self)
        self.posScale = Scale(self.controlFrame, orient='horizontal',
                           length=210,
                           from_=0, tickinterval=0,
                           command=self.pos_callback,
                           showvalue=0,sliderlength=10, resolution=1)
        #self.posScale.bind('<Button-1>',self.pos_start_drag)
        #self.posScale.bind('<ButtonRelease-1>', self.pos_set)

        self.playB=Button(self.controlFrame, text="play", command=self.playToggle)
        self.nextB=Button(self.controlFrame, text="next", command=self.nextRun)
        self.prevB=Button(self.controlFrame, text="prev", command=self.prevRun)
        self.camera=tkimg(self, width=640, height=512)
        self.controlFrame.grid(row=0,column=1, sticky="nw")
        self.view.grid(row=1, column=1, sticky="nsew")
        self.grid_columnconfigure(1,weight=1)
        self.grid_rowconfigure(1,weight=1)
        self.camera.grid(row = 1, column = 2, sticky = 'ne')
        self.playB.grid(row=0, column=1)
        self.prevB.grid(row=0, column=2)
        self.nextB.grid(row=0, column=3)
        self.posScale.grid(row=0, column=0)


    def playToggle(self, *args):
        if self.running:
            self.root.after_cancel(self.cycleId)
        else:
            self.cycleId=self.root.after(20, self.showrun)
        self.running=not self.running

    def nextRun(self, *args):
        self.changeRun(1)

    def prevRun(self, *args):
        self.changeRun(-1)

    def changeRun(self, step):
        if self.running:
            self.root.after_cancel(self.cycleId)
            self.running=False
        run=self.loggenerator.run+step
        if run >= len(self.loggenerator.runconfig) or run<0:
            timestamps, dirs, fns = zip(*sorted(listdir(self.logdir)))
            index=fns.index(self.filename)+step
            if 0<=index<len(fns):
                run = 0 if step==1 else -1
                self.startRun(os.path.join(dirs[index], fns[index]), run)
        else:
            self.startRun(os.path.join(self.dirname, self.filename), run)




    def restart(self, keeppostion=True, startposition=None):
        self.loggenerator.startrun(self.loggenerator.run, startposition=startposition)
        self.data=list(self.loggenerator)
        if not keeppostion:
            self.position=0
        self.posScale.set(self.position)
        self.showData(self.position)

    def startRun(self, fn, run, keeppostion=False):
        self.dirname, self.filename = os.path.split(fn)
        print("starting log %s run %i"%(fn,run))
        self.loggenerator=EduroMaxiReader(fn)
        self.loggenerator.startrun(run)
        self.root.title("%s %s run %i"%(self.dirname,self.filename, self.loggenerator.run+1))
        self.data=list(self.loggenerator)
        if not keeppostion:
            self.position=0
        self.posScale.set(self.position)
        self.showData(self.position)
        self.posScale.configure(to_= len(self.data)-1)
        self.view.reset()
        self.camera.clear()

    def showData(self,position):
        self.position=position
        self.posScale.set(position)
        time, pose, msgs = self.data[position]
        self.view.MsgListener(time, pose, msgs)
        #if self.analyzator:
        #    self.analyzator(time, pose, msgs)
        self.view.robot.update(*pose)
        self.view.redraw_lasers()
        self.view.robot.redraw(self.view)
        if msgs['CAMERALOG']:
            _, shot = msgs['CAMERALOG']
            if shot:
                shot = os.path.join(self.dirname, shot.split('/')[-1])
                self.camera.showImg(shot)

    def clickL(self, x, y):
        #self.loggenerator.correction=Position(self.loggenerator.correction.x+x-self.view.robot.x, self.loggenerator.correction.y+y-self.view.robot.y, self.loggenerator.correction.a)
        #print(self.loggenerator.correction)
        A=self.data[self.position][1]
        S=self.data[0][1]
        #self.restart(keeppostion=True, startposition=Position(S.x+x-A.x, S.y+y-A.y, S.a))
        print("left", x,y)

    def clickR(self, x, y):
        #try to modify start position so that actual position heading is in direction of this click

        #actual position
        A=self.data[self.position][1]
        #start position
        S=self.data[0][1]
        #polar vector from actual to start
        dist=((A.x-S.x)**2+(A.y-S.y)**2)**0.5
        if S.x==A.x:#when x distance is 0 then angle +-90 (when y dist is 0 then it doesnt matter
            beta=-pi/2 if S.y>=A.y else pi/2
        else:
            beta=atan((A.y-S.y)/(A.x-S.x))
        #angle difference from actual position to click
        #TODO solve +-90 same as above
        delta=A.a-atan((A.y-y)/(A.x-x))

        newStart = Position(A.x+cos(beta+delta)*dist, A.y+sin(beta+delta)*dist, S.a-delta)
        #self.restart(keeppostion=True, startposition=newStart)
        print("right", x, y)

    def showrun(self):
        pos=self.position+1
        if pos<len(self.data):
            self.showData(pos)
            self.cycleId=self.root.after(20, self.showrun)
        else:
            self.running=False

    def next(self, event):
        pos=self.position+1
        if pos<len(self.data):
            self.posScale.set(pos)

    def prev(self, event):
        pos=self.position-1
        if pos>=0:
            self.posScale.set(pos)

    def pos_callback(self, pos, *args):
        if not self.running:
            self.showData(int(pos))