def show_frame(v, p, robot):
"""Show the current probability and the current robot"""
f = Frame.Frame()
for l in landmarks:
f.add_circle(pos=(l[1], l[0]), color=Color.Color("blue"), radius=3)
for r in p:
poly = Shapes.arrow_head_polygon((r.x, r.y), r.orientation, scale=0.5)
f.add_polygons([poly], color=Color.Color("red"), alpha=0.2)
poly = Shapes.arrow_head_polygon((robot.x, robot.y),
robot.orientation,
scale=0.5)
f.add_polygons([poly], color=Color.Color("black"), alpha=0.9)
v.add_frame(f)
def show_frame(v,p,robot):
"""Show the current probability and the current robot"""
f = Frame.Frame()
for l in landmarks:
f.add_circle(pos=(l[1],l[0]),
color=Color.Color("blue"),
radius=3)
for r in p:
poly = Shapes.arrow_head_polygon((r.x, r.y),
r.orientation,
scale=0.5)
f.add_polygons([poly],
color=Color.Color("red"),
alpha=0.2)
poly = Shapes.arrow_head_polygon((robot.x, robot.y),
robot.orientation,
scale=0.5
)
f.add_polygons([poly],
color=Color.Color("black"),
alpha=0.9)
v.add_frame(f)
def draw_robot(self, draw_path=True, text_info=''):
"""Create a new frame and draw a representation of the robot in it"""
x,y,phi = self.state.get_pose()
if self.viewer:
f = Frame.Frame()
if draw_path:
f.add_lines(color='black',
alpha=0.5,
lines=[self.path[:]],
linewidth=0.02)
# Draw start and goal
f.add_circle(self.init,
color='green',
alpha=0.5,
radius=0.05)
for g in self.goals:
f.add_circle(g,
color='blue',
alpha=0.5,
radius=0.05)
# Our robot representation is built of an arrow head
# with two wheels.
poly = Shapes.arrow_head_polygon((x, y),
phi,
scale=0.02)
f.add_polygons([poly],
color="red",
alpha=0.5)
wheels = Shapes.rectangle_pair((x,y),
5.,2.,7.,
angle=phi,
scale=0.02)
f.add_polygons(wheels,
color="black",
alpha=0.5)
# Add some anotation
f.add_text(pos=(-0.5,-0.5), size=0.1, text="Time=%.2fs"%self.time,color="darkgreen")
dy = -0.6
for i,s in enumerate(text_info.split('\n')):
f.add_text(pos=(-0.5,dy-0.1*i), size=0.07,
text=s,color="darkgreen")
self.viewer.add_frame(f)
time.sleep(0.01)
def draw_robot(self, draw_path=True, text_info=''):
"""Create a new frame and draw a representation of the robot in it"""
x,y,phi = self.state.get_pose()
phi_d = self.get_dir_to_goal()
if self.viewer:
f = Frame.Frame()
if draw_path:
f.add_lines(color=Color.Color("black"),
lines=[self.path[:]],
linewidth=0.01)
# Draw start and goal
for p,color in ( (self.init, "green"),
(self.goal, "blue") ):
if p:
f.add_circle(p,
color=color,
alpha=0.5,
radius=0.05)
# Our robot representation is built of an arrow head
# with two wheels.
poly = Shapes.arrow_head_polygon((x, y),
phi,
scale=0.02)
f.add_polygons([poly],
color="red",
alpha=0.5)
wheels = Shapes.rectangle_pair((x,y),
5.,2.,7.,
angle=phi,
scale=0.02)
f.add_polygons(wheels,
color="black",
alpha=0.5)
# Add some anotation
f.add_text(pos=(-0.5,-0.5), size=0.1, text="Time=%.2fs"%self.time,color="darkgreen")
dy = -0.6
for i,s in enumerate(text_info.split('\n')):
f.add_text(pos=(-0.5,dy-0.1*i), size=0.07,
text=s,color="darkgreen")
self.viewer.add_frame(f)
time.sleep(0.01)
def draw_robot(self, draw_path=True):
"""Create a new frame and draw a representation of the robot in it"""
if self.viewer:
f = Frame.Frame()
if draw_path:
f.add_lines(color=Color.Color("black"),
lines=[self.path[:]],
linewidth=0.01)
# Draw start and goal
for p,color in ( (self.init, "green"),
(self.goal, "blue") ):
if p:
f.add_circle(p,
color=color,
alpha=0.5,
radius=0.05)
# Our robot representation is built of an arrow head
# with two wheels.
poly = Shapes.arrow_head_polygon((self.x, self.y),
self.phi,
scale=0.02)
f.add_polygons([poly],
color=Color.Color("red"),
alpha=0.5)
wheels = Shapes.rectangle_pair((self.x,self.y),
5.,2.,7.,
angle=self.phi,
scale=0.02)
f.add_polygons(wheels,
color=Color.Color("black"),
alpha=0.5)
# Add some anotation
f.add_text(pos=(-0.5,-0.5),
size=0.1,
text="Time=%.2fs"%(self.time),
color=Color.Color("darkgreen"))
self.viewer.add_frame(f)
time.sleep(0.01)
def draw_robot(self, draw_path=True):
"""Create a new frame and draw a representation of the robot in it"""
if self.viewer:
f = Frame.Frame()
if draw_path:
f.add_lines(color=Color.Color("black"),
lines=[self.path[:]],
linewidth=0.01)
# Draw start and goal
for p, color in ((self.init, "green"), (self.goal, "blue")):
if p:
f.add_circle(p, color=color, alpha=0.5, radius=0.05)
# Our robot representation is built of an arrow head
# with two wheels.
poly = Shapes.arrow_head_polygon((self.x, self.y),
self.phi,
scale=0.02)
f.add_polygons([poly], color=Color.Color("red"), alpha=0.5)
wheels = Shapes.rectangle_pair((self.x, self.y),
5.,
2.,
7.,
angle=self.phi,
scale=0.02)
f.add_polygons(wheels, color=Color.Color("black"), alpha=0.5)
# Add some anotation
f.add_text(pos=(-0.5, -0.5),
size=0.1,
text="Time=%.2fs" % (self.time),
color=Color.Color("darkgreen"))
self.viewer.add_frame(f)
time.sleep(0.01)
import Euv.Color as Color
import Euv.Shapes as Shapes
import time
import math
N = 200
v = Euv.Viewer(
size=(800, 600), view_port_center=(0, 0), view_port_width=800, max_num_frames=N, recording=True, flip_y=True
)
for i in range(N):
f = Frame.Frame()
x = 400 * 1.0 * (i - N / 2) / N
n = 40
for j in range(n):
f.add_circle(pos=(x + j * 5, 20 * math.sin(2 * math.pi * (i + j) / n)), color=Color.Color("red"), radius=3)
f.add_lines(color=Color.Color("midnightblue"), lines=[[(x + 5, 105), (x + 35, 105)]])
f.add_polygons(
color=Color.Color("magenta3"),
polygons=[[(x, 100), (x + 10, 100), (x + 5, 110)], [(x + 30, 100), (x + 40, 100), (x + 35, 110)]],
)
arw = Shapes.arrow_head_polygon((x, -25), scale=10)
rect = Shapes.rotated_rectangle((x, 25), width=50, height=10, angle=1.0 * i / N * math.pi * 2)
f.add_polygons(polygons=[arw, rect], color="cyan", alpha=0.5)
f.add_text(pos=(250, 200), face="Sans", size=20, text="Frame %d" % i, color=Color.Color("darkgreen"))
if v.user_break():
break
v.add_frame(f)
time.sleep(1.0 / 40)
v.wait()
n = 40
for j in range(n):
f.add_circle(pos=(x+j*5,20*math.sin(2*math.pi*(i+j)/n)),
color=Color.Color("red"),
radius=3)
f.add_lines(color=Color.Color("midnightblue"),
lines=[[(x+5,105),(x+35,105)]])
f.add_polygons(color=Color.Color("magenta3"),
polygons = [[(x,100),
(x+10,100),
(x+5,110)],
[(x+30,100),
(x+40,100),
(x+35,110)],
])
arw = Shapes.arrow_head_polygon((x,-25),
scale=10)
rect = Shapes.rotated_rectangle((x,-150),
width=50,
height=10,
angle=-1.0*i/N * math.pi*2,
)
wheels = Shapes.rectangle_pair((x,-25),
5.,2.,7.,
left_angle = 1.0*i/N * math.pi*2,
scale=10)
f.add_polygons(polygons = [arw,
rect],
color="cyan",
alpha=0.5)
f.add_polygons(polygons = wheels,
color="black",
