In [2]:
# start in non-inline mode, so we can use interactive plots
%pylab

import sys
import math

#example message
msg = {"t":[0.0,0.5172413793103449,1.0344827586206897,1.5517241379310345,2.0689655172413794,2.586206896551724,3.103448275862069,3.620689655172414,4.137931034482759,4.655172413793103,5.172413793103448,5.689655172413793,6.206896551724138,6.724137931034483,7.241379310344828,7.758620689655173,8.275862068965518,8.793103448275861,9.310344827586206,9.827586206896552,10.344827586206897,10.862068965517242,11.379310344827585,11.896551724137932,12.413793103448276,12.93103448275862,13.448275862068966,13.96551724137931,14.482758620689657,15.0],"x":[[106.08830776237102],[107.33926475594807],[104.05904906311419],[98.19856393861026],[90.92519893100008],[82.88252978512752],[74.4049355200306],[65.62976060445979],[56.5467799334418],[47.02490987314188],[36.87831759331243],[25.923980045026575],[14.057340472853374],[1.3998502440408025],[-11.541860610230104],[-23.791670692529756],[-34.01699405113328],[-40.88848615212464],[-43.54267697929256],[-41.85075189776524],[-36.34530290829045],[-26.77332501664871],[-14.59206405499903],[-1.6560065386536909],[10.06544990509022],[18.78781983588694],[23.18320164166832],[22.582437600241043],[17.076988947814854],[7.5050104352754285]],"u":[0.7853981466618558,0.7056766188861293,0.4270623387075009,0.25351772470480577,0.14742050044710311,0.0872364116163186,0.06162829236217106,0.06564503279703553,0.09747087312167017,0.14846537029070989,0.21585780917590644,0.3012165447297263,0.39800119137095447,0.49745712092029565,0.5891217623229049,0.6605929518552345,0.7016575033219311,0.7058178146974804,0.6748894921376468,0.616163334113353,0.7853979839538575,0.7853981542793778,0.7853981536484227,0.7853982206233072,0.7853981569677946,0.7853981372728716,0.7853982502043825,0.7853981927209015,0.7853981838257337,-0.7170440592343447],"CHANNEL":"NEW_TAPE","y":[[88.57043270326244],[75.68099491101485],[63.153318105753286],[51.60527869632456],[40.890771592345786],[30.74099457163903],[20.951566817978964],[11.427974779191375],[2.1974838325162525],[-6.579559591966094],[-14.626246204353514],[-21.533126045106133],[-26.718236156295962],[-29.455103240841233],[-28.99182877890345],[-24.79127441207638],[-16.84487453415099],[-5.868303923962927],[6.806781034459511],[19.645779991997948],[31.367236047405093],[40.08960613100896],[44.484988027677176],[43.88422470729768],[38.37877654469722],[28.806798513801017],[16.62553751935268],[3.689480036466564],[-8.031976177233513],[-16.75434557951182]]}
x = msg['x']
y = msg['y']

#plot tape and initial uav position
fig, ax = plt.subplots()

errortape = None
errorwall = None
sidetape = None
sidewall = None
curline_text = ax.text(0, 0, 'test')
curline, = ax.plot([0.0, 10.0], [0.0, 10.0], color='m')
curwallleft, = ax.plot([0.0, 10.0], [0.0, 10.0], color='g')
curwallright, = ax.plot([0.0, 10.0], [0.0, 10.0], color='c')
curtrack, = ax.plot([0.0, 10.0], [0.0, 10.0], color='y')
tape, = ax.plot(x, y, '.-')
#uav_x = 105.0
#uav_y = 95.0
uav_x = -5
uav_y = -17.5
uav = ax.scatter(uav_x, uav_y, marker='+', color='r', s=160)
step_size = 1

#setup indices for current line
cur_idx = 0
nxt_idx = cur_idx + 1

#callback for uav movement
def on_key_press(event):
    global uav_x, uav_y
    if event.key == 'left':
        uav_x = uav_x - step_size
        uav.set_offsets((uav_x, uav_y))
    if event.key == 'right':
        uav_x = uav_x + step_size
        uav.set_offsets((uav_x, uav_y))
    if event.key == 'down':
        uav_y = uav_y - step_size
        uav.set_offsets((uav_x, uav_y))
    if event.key == 'up':
        uav_y = uav_y + step_size
        uav.set_offsets((uav_x, uav_y))
    calcCurLine()
    sys.stdout.flush()
    event.canvas.draw()

def calcCurLine():
    global cur_idx, nxt_idx, errortape, errorwall, sidetape, sidewall
    x1 = x[cur_idx][0]
    y1 = y[cur_idx][0]
    x2 = x[nxt_idx][0]
    y2 = y[nxt_idx][0]
    
    #for calcuating the current line
    dx = (x2-x1)
    dy = (y2-y1)
    a = dy / dx;
    b = y1-a*x1;
    xleft = -10000
    xright = 10000
    yleft = xleft*a+b;
    yright = xright*a+b;
    
    #for plotting the current line
    curline.set_xdata([xleft, xright])
    curline.set_ydata([yleft, yright])
    
    #for calculating error to and side of current line of tape
    err_num = abs(dy*uav_x - dx*uav_y - x1*y2 + x2*y1)
    err_den = np.sqrt(dx**2 + dy**2)
    errortape = err_num/err_den
    sidetape = (uav_x - x1)*dy - (uav_y - y1)*dx
    if sidetape > 0:
        sidetape = 'right'
    else:
        sidetape = 'left'
     
    #for calculating the current wall
    angle = math.atan2(dy, dx) * 180 / math.pi
    perpangle = (angle + 90) * math.pi / 180
    xperpleft = x2 + 20*math.cos(perpangle)
    xperpright = x2 - 20*math.cos(perpangle)
    yperpleft = y2 + 20*math.sin(perpangle)
    yperpright = y2 - 20*math.sin(perpangle)
    
    #for calculating the error to and side of current wall
    dx = (xperpright-xperpleft)
    dy = (yperpright-yperpleft)
    err_num = abs(dy*uav_x - dx*uav_y - xperpleft*yperpright + xperpright*yperpleft)
    err_den = np.sqrt(dx**2 + dy**2)
    errorwall = err_num/err_den
    sidewall = (uav_x - xperpleft)*dy - (uav_y - yperpleft)*dx
    if sidewall > 0:
        sidewall = 'behind'
    else:
        sidewall = 'in front'
    
    #for plotting the current wall
    curwallleft.set_xdata([x2, xperpleft])
    curwallleft.set_ydata([y2, yperpleft])
    curwallright.set_xdata([x2, xperpright])
    curwallright.set_ydata([y2, yperpright])
    
    #for plotting the text info
    curline_text.set_text(str(errortape) + "\n" + sidetape + "\n" + str(errorwall) + "\n" + sidewall)
    curline_text.set_x(uav_x + 2)
    curline_text.set_y(uav_y)
    
    #update current waypoint
    if sidewall == 'in front':
        if cur_idx < (len(x)-2):
            cur_idx = nxt_idx
            nxt_idx = cur_idx + 1
    
#setup callback
#fig.canvas.mpl_disconnect(fig.canvas.manager.key_press_handler_id)
fig.canvas.mpl_connect('key_press_event', on_key_press)


Using matplotlib backend: TkAgg
Populating the interactive namespace from numpy and matplotlib
Out[2]:
6

In [ ]: