def __init__(self, figureName=None, color_map=CMap_color, clearFigure=True):
""" ToVibes visitor object
Args:
max (float): bound of the windows centered in (0,0)
figureName (string): If None create a new figure else select the existing one
BW (bool): Use black and white color map
"""
SepVisitor.__init__(self)
self.figureName = str(figureName)
if figureName is not None:
vibes.selectFigure(figureName)
if clearFigure is True:
vibes.clearFigure()
self.color_map = color_map
for r in range(nbsRobots):
m[r][0] = Interval(position[i][r][1]).inflate(PositionIncertitude) #x
m[r][1] = Interval(position[i][r][2]).inflate(PositionIncertitude) #y
t = m[i][0][0]
#SIVIA test for Robots secure area whith incertitude
pdcRobots = IncertitudeRobots(m,r**2)
# SIVIA test for Trail
sizeKernelErosion = enemySpeed*(t-t_old)/(1000*echellePixel)
print(sizeKernelErosion)
imgTrail = computeTrail(imgOut, enemySpeed)
imgIntegralTrail = cv2.integral(imgTrail)
pdcTrail = ImageToBoxes(imgIntegralTrail,i0,j0,echellePixel)
vibes.clearFigure('Robotique')
boatBoxesNP = SIVIA(X0, pdcGascogne, pdcTrail, pdcRobots, epsilon)
#Draw of the robots:
for m_ in m:
vibes.drawCircle(m_[0].mid(), m_[1].mid(), 0.2, '[k]')
vibes.drawArrow([-15, -15], [-15, -10], 1, 'w[w]')
vibes.drawArrow([-15, -15], [-10, -15], 1, 'w[w]')
#Creation of the output image:
imgT=np.zeros((j_max, i_max),dtype="uint8")
imgOut=cv2.fillPoly(imgT,np.array( boatBoxesNP),(1,1,1))
t_old = t
def main(stdscr):
# Clear screen
stdscr.clear()
stdscr.keypad(1)
begin_x = 20; begin_y = 7
height = 5; width = 40
win = curses.newwin(height, width, begin_y, begin_x)
stdscr.addstr(0,10,"Hit 'space' to quit")
stdscr.refresh()
cpt = 0
key = ''
x = Interval(1,1.5)
y = Interval(2,3)
dth = Interval.HALF_PI.mid()/32.
th = Interval(0).inflate(2*dth)
Interval.__str__ = print2
fout = open("test.txt", "a")
fout.write("#================ RUN ===============\n")
while key != ord(' '):
key = stdscr.getch()
lower_key = chr(key).lower()
if lower_key == 'z' :
y += 0.5 if curses.ascii.isupper(key) else 0.1
elif lower_key == 's' :
y -= 0.5 if curses.ascii.isupper(key) else 0.1
elif lower_key == 'd' :
x += 0.5 if curses.ascii.isupper(key) else 0.1
elif lower_key == 'q' :
x -= 0.5 if curses.ascii.isupper(key) else 0.1
elif lower_key == 'a':
th += dth/8. if curses.ascii.isupper(key) else dth
elif lower_key == 'e':
th -= dth/8. if curses.ascii.isupper(key) else dth
elif key == curses.KEY_NPAGE:
th = th.lb() + (Interval(0,th.diam()-dth) | Interval(0))
elif key == curses.KEY_PPAGE:
th = th.lb() + (Interval(0,th.diam()+dth) | Interval(0))
elif key == curses.KEY_RIGHT:
x = x.lb() + (Interval(0,x.diam()+1) | Interval(0))
elif key == curses.KEY_LEFT:
x = x.lb() + (Interval(0,x.diam()-1) | Interval(0))
elif key == curses.KEY_UP:
y = y.lb() + (Interval(0,y.diam()+1) | Interval(0))
elif key == curses.KEY_DOWN:
y = y.lb() + (Interval(0,y.diam()-1) | Interval(0))
#
# # if curses.ascii.ascii(key) == "a":
#
# if key == curses.KEY_UP or key == 566:
# y += 0.5 if key==566 else 0.1
# elif key == curses.KEY_DOWN or key == 525:
# y -= 0.5 if key==525 else 0.1
# elif key == curses.KEY_RIGHT or key == 560:
# x += 0.5 if key==560 else 0.1
# elif key == curses.KEY_LEFT or key == 545:
# x -= 0.5 if key==545 else 0.1
# elif key == ord('a') or key == 1:
# th += 0.1 if key==1 else 0.05
# elif key == ord('e') or key == 5:
# th -= 0.1 if key==5 else 0.05
#
# elif key == ord('A'):
# th = th.lb() + (Interval(0,th.diam()-0.1) | Interval(0))
# elif key == ord('E'):
# th = th.lb() + (Interval(0,th.diam()+0.1) | Interval(0))
# elif key == ord('z'):
# x = x.lb() + (Interval(0,x.diam()+1) | Interval(0))
# elif key == ord('s'):
# x = x.lb() + (Interval(0,x.diam()-1) | Interval(0))
# elif key == ord('d'):
# y = y.lb() + (Interval(0,y.diam()+1) | Interval(0))
# elif key == ord('q'):
# y = y.lb() + (Interval(0,y.diam()-1) | Interval(0))
elif key == ord('R'):
x = Interval(1,1.5)
y = Interval(2,3)
th = Interval(0).inflate(1)
xx,yy,thh = Catan2(x,y,Interval(th))
# print(event.Key, event.Ascii, th, thh)
# print(x, y, th )
vibes.clearFigure()
vibes.drawBox(x[0], x[1], y[0], y[1], 'r')
vibes.drawPie((0,0), (1e-5,5), th*180./math.pi, "r")
vibes.drawPie((0,0), (1e-5,5), thh*180./math.pi, "b")
vibes.drawBox(xx[0], xx[1], yy[0], yy[1], 'b')
# if key in actions:
# x, y, th = actions[key]( x,y,th )
# fct = actions.get(key, lambda x,y,th:x,y,th)
stdscr.clear()
stdscr.addstr(1, 10, "Pressed %s %d"%(lower_key, key))
stdscr.addstr(1,25,str(curses.ascii.ctrl(key)))
stdscr.addstr(1,40,str(curses.ascii.ascii(key)))
stdscr.addstr(1,40,str(curses.ascii.alt(key)))
stdscr.addstr(3, 20, "x : %s\t\t%s"%(x,xx))
stdscr.addstr(4, 20, "y : %s\t\t%s"%(y,yy))
stdscr.addstr(5, 20, "theta : %s\t\t%s"%(th, thh))
if key == ord("t"):
fout.write("%s %s %s -> %s %s %s\n"%(x,y,th, xx, yy, thh))
stdscr.refresh()
curses.endwin()
fout.write("#===================== END ===============\n");
# m= [[cx1 , cy1 ],[cx2, cy2],[cx3, cy3]]
# pdc = test3(m)
# #vibes.clearFigure()
# SIVIA(X0, pdc, 0.5)
#
# for m_ in m:
# vibes.drawCircle(m_[0].mid(), m_[1].mid(), 0.2, '[k]')
# vibes.drawArrow([-15, -15], [-15, -10], 1, 'w[w]')
# vibes.drawArrow([-15, -15], [-10, -15], 1, 'w[w]')
for t in range(10):
time1 = time.time()
m= [[cx1 , cy1 ],[cx2, cy2],[cx3, cy3]]
pdc = (m,100,True)
vibes.clearFigure()
box = pyIbex.SIVIAtest(X0,m,100,1,True)
print(type(box),len(box))
for m_ in m:
vibes.drawCircle(m_[0].mid(), m_[1].mid(), 0.5, '[k]')
vibes.drawArrow([-15, -15], [-15, -10], 1, 'w[w]')
vibes.drawArrow([-15, -15], [-10, -15], 1, 'w[w]')
vibes.drawBoxesUnion(listBoxToDraw(box),'[r]')
cx1,cy1,v1,p1 = subMove(cx1,cy1,v1,p1,u1,h)
cx2,cy2,v2,p2 = subMove(cx2,cy2,v2,p2,u2,h)
cx3,cy3,v3,p3 = subMove(cx3,cy3,v3,p3,u3,h)
print(time.time()-time1)
time.sleep(3)
vibes.endDrawing()
def main(stdscr):
# Clear screen
stdscr.clear()
stdscr.keypad(1)
begin_x = 20
begin_y = 7
height = 5
width = 40
win = curses.newwin(height, width, begin_y, begin_x)
stdscr.addstr(0, 10, "Hit 'space' to quit")
stdscr.refresh()
cpt = 0
key = ''
x = Interval(1, 1.5)
y = Interval(2, 3)
dth = Interval.HALF_PI.mid() / 32.
th = Interval(0).inflate(2 * dth)
Interval.__str__ = print2
fout = open("test.txt", "a")
fout.write("#================ RUN ===============\n")
while key != ord(' '):
key = stdscr.getch()
lower_key = chr(key).lower()
if lower_key == 'z':
y += 0.5 if curses.ascii.isupper(key) else 0.1
elif lower_key == 's':
y -= 0.5 if curses.ascii.isupper(key) else 0.1
elif lower_key == 'd':
x += 0.5 if curses.ascii.isupper(key) else 0.1
elif lower_key == 'q':
x -= 0.5 if curses.ascii.isupper(key) else 0.1
elif lower_key == 'a':
th += dth / 8. if curses.ascii.isupper(key) else dth
elif lower_key == 'e':
th -= dth / 8. if curses.ascii.isupper(key) else dth
elif key == curses.KEY_NPAGE:
th = th.lb() + (Interval(0, th.diam() - dth) | Interval(0))
elif key == curses.KEY_PPAGE:
th = th.lb() + (Interval(0, th.diam() + dth) | Interval(0))
elif key == curses.KEY_RIGHT:
x = x.lb() + (Interval(0, x.diam() + 1) | Interval(0))
elif key == curses.KEY_LEFT:
x = x.lb() + (Interval(0, x.diam() - 1) | Interval(0))
elif key == curses.KEY_UP:
y = y.lb() + (Interval(0, y.diam() + 1) | Interval(0))
elif key == curses.KEY_DOWN:
y = y.lb() + (Interval(0, y.diam() - 1) | Interval(0))
#
# # if curses.ascii.ascii(key) == "a":
#
# if key == curses.KEY_UP or key == 566:
# y += 0.5 if key==566 else 0.1
# elif key == curses.KEY_DOWN or key == 525:
# y -= 0.5 if key==525 else 0.1
# elif key == curses.KEY_RIGHT or key == 560:
# x += 0.5 if key==560 else 0.1
# elif key == curses.KEY_LEFT or key == 545:
# x -= 0.5 if key==545 else 0.1
# elif key == ord('a') or key == 1:
# th += 0.1 if key==1 else 0.05
# elif key == ord('e') or key == 5:
# th -= 0.1 if key==5 else 0.05
#
# elif key == ord('A'):
# th = th.lb() + (Interval(0,th.diam()-0.1) | Interval(0))
# elif key == ord('E'):
# th = th.lb() + (Interval(0,th.diam()+0.1) | Interval(0))
# elif key == ord('z'):
# x = x.lb() + (Interval(0,x.diam()+1) | Interval(0))
# elif key == ord('s'):
# x = x.lb() + (Interval(0,x.diam()-1) | Interval(0))
# elif key == ord('d'):
# y = y.lb() + (Interval(0,y.diam()+1) | Interval(0))
# elif key == ord('q'):
# y = y.lb() + (Interval(0,y.diam()-1) | Interval(0))
elif key == ord('R'):
x = Interval(1, 1.5)
y = Interval(2, 3)
th = Interval(0).inflate(1)
xx, yy, thh = Catan2(x, y, Interval(th))
# print(event.Key, event.Ascii, th, thh)
# print(x, y, th )
vibes.clearFigure()
vibes.drawBox(x[0], x[1], y[0], y[1], 'r')
vibes.drawPie((0, 0), (1e-5, 5), th * 180. / math.pi, "r")
vibes.drawPie((0, 0), (1e-5, 5), thh * 180. / math.pi, "b")
vibes.drawBox(xx[0], xx[1], yy[0], yy[1], 'b')
# if key in actions:
# x, y, th = actions[key]( x,y,th )
# fct = actions.get(key, lambda x,y,th:x,y,th)
stdscr.clear()
stdscr.addstr(1, 10, "Pressed %s %d" % (lower_key, key))
stdscr.addstr(1, 25, str(curses.ascii.ctrl(key)))
stdscr.addstr(1, 40, str(curses.ascii.ascii(key)))
stdscr.addstr(1, 40, str(curses.ascii.alt(key)))
stdscr.addstr(3, 20, "x : %s\t\t%s" % (x, xx))
stdscr.addstr(4, 20, "y : %s\t\t%s" % (y, yy))
stdscr.addstr(5, 20, "theta : %s\t\t%s" % (th, thh))
if key == ord("t"):
fout.write("%s %s %s -> %s %s %s\n" % (x, y, th, xx, yy, thh))
stdscr.refresh()
curses.endwin()
fout.write("#===================== END ===============\n")