def make_frame(t):
context.save()
context.set_source_rgb(*BG_COLOR)
context.rectangle(0, 0, WIDTH, WIDTH)
context.fill()
context.restore()
for r, angle in zip(rs, angles):
# shifted_angle = angle + angle * np.sin(2 * PI * t / DURATION)
shifted_angle = angle + 2 * PI * t / DURATION
shifted_r = max(
0, r + WIDTH / 3 - WIDTH * np.sin(2 * PI *
(t + PHASE_TIME) / DURATION))
x, y = np.round(polar2cartesian(shifted_r, shifted_angle),
2) + WIDTH / 2
# x, y = np.round(polar2cartesian(r, angle), 2) + WIDTH / 2
circle = Circle(context,
x=x,
y=y,
r=CIRCLE_RADIUS,
fill_rgb=(1, 1, 1),
stroke=True,
gradient=None)
circle.draw()
return get_image(surface)
def run(self):
k = 0
#cv2.namedWindow("display", cv2.WINDOW_NORMAL)
while (self.cap.isOpened()):
if random.randint(1, 10 * 1) == 1:
self.object_manager.add(
Circle(random.randint(0, SCREEN_WIDTH), SCREEN_HEIGHT, 0,
-20))
ret, img = self.cap.read()
debug_drawing = np.zeros(img.shape, np.uint8)
display_drawing = np.empty((SCREEN_HEIGHT, SCREEN_WIDTH, 3),
np.uint8)
display_drawing.fill(255)
#import ipdb; ipdb.set_trace()
if k == 82: # init, or R
print "COMMAND REGISTERED. calibrating background"
#self.image_processor.startCalibration()
self.image_processor.calibrate_background()
# Update
try:
if self.image_processor.isCalibrationDone():
self.image_processor.update(img, debug_drawing)
self.object_manager.update()
self.interaction_manager.update(self.image_processor,
self.object_manager)
# Render
self.image_processor.renderDebug(debug_drawing)
self.image_processor.renderDisplay(display_drawing)
self.object_manager.renderDebug(debug_drawing)
self.object_manager.renderDisplay(display_drawing)
else:
self.image_processor.calibrationStep(img, debug_drawing)
self.image_processor.renderCalibrationDisplay(
display_drawing)
#Bug in OpenCV for MacOsX prevents fullscreen: http://code.opencv.org/issues/2846
#cv2.namedWindow("display", cv2.WND_PROP_FULLSCREEN)
#cv2.setWindowProperty("display", cv2.WND_PROP_FULLSCREEN, cv2.cv.CV_WINDOW_FULLSCREEN)
cv2.imshow('debug', debug_drawing)
#scaled = cv2.resize(display_drawing, dsize=(SCREEN_WIDTH, SCREEN_HEIGHT) )
# import ipdb; ipdb.set_trace()
cv2.imshow('display', display_drawing)
except Exception as e:
print traceback.format_exc()
#cv2.imshow('input',img)
k = cv2.waitKey(10)
if k == 27:
break
cv2.destroyAllWindows()
def simulate(robot, m, path, ts):
grad_max = 10
grad_min = -10
dt = 0.1
path_index = 1
values = []
headings = []
values.append(robot.get_pose())
headings.append(robot.get_unit_heading())
for step, t in enumerate(ts):
# calculate gradient
# Add attraction
grad = (path[path_index].val - robot.pose[:-1])
if np.linalg.norm(grad) < 3:
path_index += 1
if path_index == len(path):
break
continue
grad[0] = max(grad_min, min(grad[0], grad_max))
grad[1] = max(grad_min, min(grad[1], grad_max))
unit_grad = grad / np.linalg.norm(grad)
unit_robot = robot.get_unit_heading()
side = unit_grad[1] * unit_robot[0] - unit_grad[0] * unit_robot[1]
angle = np.sign(side) * np.arccos(np.dot(unit_grad, unit_robot))
v = np.sqrt(grad[0]**2 + grad[1]**2)
if angle > np.radians(40):
v = 0
w = max(-3 / 2 * np.pi, min(3 / 2 * np.pi, angle))
new_robot_pose = robot.get_update(v, w, dt)
robot_circle = Circle(new_robot_pose[:-1], robot.b / 2)
intersects = m.get_intersecting_rects(robot_circle)
if len(intersects) > 0:
v = 0
w = np.sign(w) * np.pi / 2
robot.update(v, w, dt)
values.append(robot.get_pose())
headings.append(robot.get_unit_heading())
return np.array(values), np.array(headings)
num_pix = 100
m = get_map(pgm)
# Robot parameters
b = 4.5
robot = Robot(np.array([5, 5, np.radians(90)]), b)
# Generate milestones for kd-tree
np.random.seed(42)
coordinates = np.random.rand(70, 2) * 97 + 1
milestones = []
milestones.append(np.array([5, 5]))
for coord in coordinates:
circle = Circle(coord, robot.b / 2)
if len(m.get_intersecting_rects(circle)) == 0:
milestones.append(coord)
milestones.append(np.array([70, 15]))
milestones.append(np.array([90, 50]))
milestones.append(np.array([30, 95]))
milestones.append(np.array([5, 50]))
milestones = np.array(milestones)
# Generate kd-tree
tree = KdTree(milestones)
tree.init_build()
# Create connection/adjacency matrix for nodes
# Also remove invalid connections
grad_max = 10
grad_min = -10
values = []
headings = []
values.append(robot.get_pose())
headings.append(robot.get_unit_heading())
for step, t in enumerate(ts):
# calculate gradient
# Add attraction
grad = -k_att * (robot.pose[:-1] - end_pos)
# Add repulsion
# Radius check around for intersections
rep_circle = Circle(robot.pose[:-1], rep_radius)
nearby_rects = m.get_intersecting_rects(rep_circle)
for i in nearby_rects:
rect = m.get_rect(i)
dist = np.linalg.norm(robot.pose[:-1] - rect.get_centroid())
grad += -k_rep * (1 / dist - 1 / rho0) * \
(robot.pose[:-1] - rect.get_centroid()) / (dist**3)
grad[0] = max(grad_min, min(grad[0], grad_max))
grad[1] = max(grad_min, min(grad[1], grad_max))
unit_grad = grad / np.linalg.norm(grad)
unit_robot = robot.get_unit_heading()
side = unit_grad[1] * unit_robot[0] - unit_grad[0] * unit_robot[1]
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE or \
event.key == pygame.K_q:
running = False
if event.type == pygame.MOUSEBUTTONDOWN:
if home_page:
home_page = False
game_page = True
for i in range(12):
c = Circle(i)
circle_group.add(c)
elif game_page:
if not clicked:
clicked = True
rotate = False
clicks += 1
click_fx.play()
if event.type == pygame.MOUSEBUTTONUP:
clicked = False
rotate = True
if home_page:
count += 1
def test_circle(self):
c1 = Circle(Point(3, 4), 1)
c2 = Circle(Point(6, 8), 1)
overlap = circles_overlap(c1, c2)
self.assertFalse(overlap)
def test_circle2(self):
c1 = Circle(Point(5, 6), 3)
self.assertEqual(c1.center.x, 5)
self.assertEqual(c1.center.y, 6)
self.assertEqual(c1.radius, 3)
def test_circle(self):
c = Circle(Point(3, 4), 2)
self.assertEqual(c.center.x, 3)
self.assertEqual(c.center.y, 4)
self.assertEqual(c.radius, 2)
elif game_page:
if not clicked:
if not start_rotation:
start_rotation = True
clicked = True
clicks += 1
ball.dtheta *= -1
if len(circle_group) < 4:
circle_count += 1
angle = 45 * (2 * circle_count - 1)
x, y = get_circle_position(angle)
circle = Circle(x, y, circle_count, win)
circle_group.add(circle)
if event.type == pygame.MOUSEBUTTONDOWN:
clicked = False
if home_page:
image, rect, angle = rotate_image(r_msg, r_rect, angle)
win.blit(image, rect)
win.blit(otate_msg, (120, HEIGHT // 2 - 190))
win.blit(dash_msg, (140, HEIGHT // 2 - 140))
ball.alive = True
ball.update(color, True)
tap_to_play.update()