def check_paddle_collission(self, paddle):
if not self.is_active:
return 0
if check_intersection(self, paddle):
self.is_active = 0
return 1
return 0
def handle_collission_bullet(self, bullet):
if not self.is_active:
return 0
if check_intersection(bullet, self):
bullet.set_active(0)
is_destroyed = self.dec_health()
return is_destroyed
else:
return 0
def _powerup_and_paddle(self):
for powerup in self.__powerups:
if not powerup.check_active():
continue
if check_intersection(powerup, self.__paddle):
if powerup.get_type() == POWERUP_EXPAND_PADDLE:
self.__paddle.change_width(INC_PADDLE_WIDTH)
elif powerup.get_type() == POWERUP_SHRINK_PADDLE:
self.__paddle.change_width(-1 * SHRINK_PADDLE_WIDTH)
elif powerup.get_type() == POWERUP_FAST_BALL:
for ball in self.__balls:
ball.inc_speed(2)
elif powerup.get_type() == POWERUP_BALL_MULITIPLIER:
new_balls = []
for ball in self.__balls:
if ball.check_active():
new_balls.append(self.divide_ball(ball))
for new_ball in new_balls:
self.__balls.append(new_ball)
elif powerup.get_type() == POWERUP_THRU_BALL:
for ball in self.__balls:
ball.set_type(THRU_BALL)
elif powerup.get_type() == POWERUP_GRAB_BALL:
for ball in self.__balls:
ball.set_grabbable()
elif powerup.get_type() == POWERUP_SHOOTING_BULLET:
self.__paddle.set_have_bullet_powerup(1)
self.__paddle.set_powerup_start_time(self.__ticks)
elif powerup.get_type() == POWERUP_FIREBALL:
for ball in self.__balls:
ball.set_fire()
else:
continue
powerup.deactivate()
def obstacle_update(self, data):
self.obstacles = [ [ (point.x, point.y) for point in polygon.points ] for polygon in data.polygons
if not utils.check_intersection(self.path, self.obstacles):
self.call_path_planner()
def call_path_planner(self, first_plan=False):
rospy.logwarn("Waiting for Service")
rospy.wait_for_service("rrt_planner_service")
try:
response = PlannerResponse()
request = PlannerRequest()
request.start.point=#[ , ] Set initial points here
request.goal.point=#[ , ]
#Obstacle to be given in theis format:
# request.obstacle_list.polygons=[
# PointArray([ Point_xy([8, 5]), Point_xy([7,8]), Point_xy([2,9]), Point_xy([3,5]) ]),
# PointArray([ Point_xy([3,3]), Point_xy([3,5]), Point_xy([5,5]), Point_xy([5,3]) ])
# ]
response = self.plan_path(request)
try response.ack:
self.path = [(pt.point[0],pt.point[1]) for pt in response.path.points]
except Exception as e:
print("Failed to compute path!")
print(e)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def main():
rospy.init_node("manager", anonymous=True)
def __obstacle_update(self, data):
self.obstacles = [[(point.x, point.y) for point in polygon.points] for polygon in data.polygons]
if len(self.path) < 2 or not utils.check_intersection(self.path, self.obstacles):
self.__call_path_planner(self.current_goal_point)
def __call__(self,
start_point,
goal_point,
obstacle_list,
animation=False):
"""Plans path from start to goal avoiding obstacles.
Args:
start_point: tuple with start point coordinates.
end_point: tuple with end point coordinates.
obstacle_list: list of obstacles which themselves are list of points
animation: flag for showing planning visualization (default False)
Returns:
A list of points representing the path determined from
start to goal while avoiding obstacles.
An list containing just the start point means path could not be planned.
"""
# Initialize start and goal nodes
start = Node.from_coordinates(start_point)
goal_node = Node.from_coordinates(goal_point)
# Initialize node_list with start
node_list = [start]
# Calculate distances between start and goal
del_x, del_y = start.x - goal_node.x, start.y - goal_node.y
distance_to_goal = math.sqrt(del_x**2 + del_y**2)
# Loop to keep expanding the tree towards goal if there is no direct connection
if check_intersection([start_point, goal_point], obstacle_list):
while True:
# Sample random point in specified area
rnd_point = sampler(self.sample_area, goal_point,
self.goal_sample_rate)
# Find nearest node to the sampled point
distance_list = [
(node.x - rnd_point[0])**2 + (node.y - rnd_point[1])**2
for node in node_list
]
nearest_node_index = min(xrange(len(distance_list)),
key=distance_list.__getitem__)
nearest_node = node_list[nearest_node_index]
# Create new point in the direction of sampled point
theta = math.atan2(rnd_point[1] - nearest_node.y,
rnd_point[0] - nearest_node.x)
new_point = nearest_node.x + self.expand_dis*math.cos(theta), \
nearest_node.y + self.expand_dis*math.sin(theta)
# Check whether new point is inside an obstacles
for obstacle in obstacle_list:
if Point(new_point).within(Polygon(obstacle)):
new_point = float('nan'), float('nan')
continue
# Expand tree
if math.isnan(new_point[0]):
continue
else:
new_node = Node.from_coordinates(new_point)
new_node.parent = nearest_node
node_list.append(new_node)
# Check if goal has been reached or if there is direct connection to goal
del_x, del_y = new_node.x - goal_node.x, new_node.y - goal_node.y
distance_to_goal = math.sqrt(del_x**2 + del_y**2)
if distance_to_goal < self.expand_dis or not check_intersection(\
[new_node.to_tuple(), goal_node.to_tuple()], obstacle_list):
goal_node.parent = node_list[-1]
node_list.append(goal_node)
print("Goal reached!")
break
else:
goal_node.parent = start
node_list = [start, goal_node]
# Construct path by traversing backwards through the tree
path = []
last_node = node_list[-1]
while node_list[node_list.index(last_node)].parent is not None:
node = node_list[node_list.index(last_node)]
path.append(node.to_tuple())
last_node = node.parent
path.append(start.to_tuple())
if animation == True:
RRT.visualize_tree(node_list, obstacle_list)
return list(reversed(path)), node_list
def check_ufo_collission(self, ufo):
if not check_intersection(self, ufo):
return 0
os.system('aplay -q ./sounds/paddle_bounce.wav& 2>/dev/null')
(u_x, u_y, u_width, u_height) = ufo.get_box()
old_x = self.x - self.vx
old_y = self.y - self.vy
if self.vx == 0:
if self.vy > 0:
# Down
self.y = u_y - 1
self.vy *= -1
is_destroyed = ufo.dec_health()
return is_destroyed
else:
# up
self.y = u_y + u_height
self.vy *= -1
is_destroyed = ufo.dec_health()
return is_destroyed
if self.vy == 0:
if self.vx > 0:
# right
self.x = u_x - 1
self.vx *= -1
is_destroyed = ufo.dec_health()
return is_destroyed
else:
# left
self.x = u_x + u_width
self.vx *= -1
is_destroyed = ufo.dec_health()
return is_destroyed
if self.vx < 0 and self.vy < 0:
# top left movement
if old_x >= u_x + u_width:
# right wall collission
self.vx *= -1
self.x = u_x + u_width
is_destroyed = ufo.dec_health()
return is_destroyed
else:
# bottom wall collision
self.vy *= -1
self.y = u_y + u_height
is_destroyed = ufo.dec_health()
return is_destroyed
if self.vx > 0 and self.vy < 0:
# top right movement
if old_x < u_x:
# left wall collission
self.vx *= -1
self.x = u_x - 1
is_destroyed = ufo.dec_health()
return is_destroyed
else:
# bottom wall collision
self.vy *= -1
self.y = u_y + u_height
is_destroyed = ufo.dec_health()
return is_destroyed
if self.vx < 0 and self.vy > 0:
# bottom left movement
if old_x >= u_x + u_width:
# right wall collission
self.vx *= -1
self.x = u_x + u_width
is_destroyed = ufo.dec_health()
return is_destroyed
else:
# top wall collision
self.vy *= -1
self.y = u_y - 1
is_destroyed = ufo.dec_health()
return is_destroyed
if self.vx > 0 and self.vy > 0:
# top right movement
if old_x < u_x:
# left wall collission
self.vx *= -1
self.x = u_x - 1
is_destroyed = ufo.dec_health()
return is_destroyed
else:
# top wall collision
self.vy *= -1
self.y = u_y - 1
is_destroyed = ufo.dec_health()
return is_destroyed
return 0
def _obstacle_update(self, data): self.obstacles = [ [ (point.x, point.y) for point in polygon.points ] for polygon in data.polygons ] if not utils.check_intersection(self.path, self.obstacles): self._call_path_planner()
def handle_collission(self, ball, bricks=None):
if not self.is_active:
return 0
b_x, b_y, b_width, b_height = ball.get_box()
if b_x >= self.x + self.width or b_x + b_width <= self.x:
return 0
if b_y >= self.y + self.height + self.y_fall or b_y + b_height <= self.y + self.y_fall:
return 0
if not check_intersection(ball, self):
return 0
else:
self.contact = 1
# colliding
if ball.get_type() == THRU_BALL:
is_destroyed = self.dec_health(ball.get_type())
return is_destroyed
b_vx, b_vy = ball.get_velocity()
old_x = b_x - b_vx
old_y = b_y - b_vy
is_destroyed = 0
if b_vx == 0:
if b_vy > 0:
ball.set_y(self.y + self.y_fall - 1)
ball.change_velocity(1, -1)
is_destroyed = self.dec_health(ball.get_type())
else:
ball.set_y(self.y + self.y_fall + self.height)
ball.change_velocity(1, -1)
is_destroyed = self.dec_health(ball.get_type())
if b_vy == 0:
if b_vx > 0:
ball.set_x(self.x - 1)
ball.change_velocity(-1, 1)
is_destroyed = self.dec_health(ball.get_type())
else:
ball.set_x(self.x + self.width)
ball.change_velocity(-1, 1)
is_destroyed = self.dec_health(ball.get_type())
if b_vx < 0 and b_vy < 0:
# top left movement
if old_x >= self.x + self.width:
# right wall collission
ball.change_velocity(-1, 1)
ball.set_x(self.x + self.width)
is_destroyed = self.dec_health(ball.get_type())
else:
# bottom wall collision
ball.change_velocity(1, -1)
ball.set_y(self.y + self.y_fall + self.height)
is_destroyed = self.dec_health(ball.get_type())
if b_vx > 0 and b_vy < 0:
# top right movement
if old_x < self.x:
# left wall collission
ball.change_velocity(-1, 1)
ball.set_x(self.x - 1)
is_destroyed = self.dec_health(ball.get_type())
else:
# bottom wall collision
ball.change_velocity(1, -1)
ball.set_y(self.y + self.y_fall + self.height)
is_destroyed = self.dec_health(ball.get_type())
if b_vx < 0 and b_vy > 0:
# bottom left movement
if old_x >= self.x + self.width:
# right wall collission
ball.change_velocity(-1, 1)
ball.set_x(self.x + self.width)
is_destroyed = self.dec_health(ball.get_type())
else:
# top wall collision
ball.change_velocity(1, -1)
ball.set_y(self.y + self.y_fall - 1)
is_destroyed = self.dec_health(ball.get_type())
if b_vx > 0 and b_vy > 0:
# bottom right movement
if old_x < self.x:
# left wall collission
ball.change_velocity(-1, 1)
ball.set_x(self.x - 1)
is_destroyed = self.dec_health(ball.get_type())
else:
# top wall collision
ball.change_velocity(1, -1)
ball.set_y(self.y + self.y_fall - 1)
is_destroyed = self.dec_health(ball.get_type())
if not is_destroyed or (not ball.check_fire()) or self.i == -1:
return is_destroyed
Is = [-1, 1, 0, 0]
Js = [0, 0, -1, 1]
for idx in range(4):
ni = self.i + Is[idx]
nj = self.j + Js[idx]
if bricks[ni][nj] != 'X':
bricks[ni][nj].destroy()
return is_destroyed
