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)
Out[2]:
In [ ]: