def __init__(self, pelican_crossing, ego, **kwargs): super().__init__(**kwargs) coordinates = pelican_crossing.bounding_box() self.area = rendering.make_polygon(list(coordinates)) self.area.set_color(*RGB.WHITE.value) self.markings = rendering.Compound([ rendering.make_polyline([ tuple(pelican_crossing.outbound_intersection_bounding_box. rear_left), tuple(pelican_crossing.outbound_intersection_bounding_box. rear_right) ]), rendering.make_polyline([ tuple(pelican_crossing.inbound_intersection_bounding_box. front_left), tuple(pelican_crossing.inbound_intersection_bounding_box. front_right) ]), rendering.make_polyline([ tuple(pelican_crossing.static_bounding_box.rear_left), tuple(pelican_crossing.static_bounding_box.front_left) ]), rendering.make_polyline([ tuple(pelican_crossing.static_bounding_box.rear_right), tuple(pelican_crossing.static_bounding_box.front_right) ]) ]) offset_rear_right = Point( coordinates.rear_right.x + (pelican_crossing.constants.width * 0.15), coordinates.rear_right.y) offset_rear_left = Point( coordinates.rear_left.x + (pelican_crossing.constants.width * 0.15), coordinates.rear_left.y) offset_front_right = Point( coordinates.front_right.x - (pelican_crossing.constants.width * 0.15), coordinates.front_right.y) offset_front_left = Point( coordinates.front_left.x - (pelican_crossing.constants.width * 0.15), coordinates.front_left.y) self.inner = rendering.Compound([ rendering.make_polyline( [tuple(offset_rear_right), tuple(offset_rear_left)]), rendering.make_polyline( [tuple(offset_front_right), tuple(offset_front_left)]) ]) self.inner.add_attr(mods.FactoredLineStyle(0x0F0F, 1)) self.outbound_traffic_light_view = TrafficLightView( pelican_crossing.outbound_traffic_light, ego) self.inbound_traffic_light_view = TrafficLightView( pelican_crossing.inbound_traffic_light, ego)
def __init__(self, init_state, constants): super().__init__(init_state, constants) position = Point(self.constants.x_position, 0.0).transform( self.constants.road.constants.orientation, self.constants.road.constants.position) self.static_bounding_box = geometry.make_rectangle( self.constants.width, self.constants.road.width).transform( self.constants.road.constants.orientation, position) self.outbound_intersection_bounding_box = geometry.make_rectangle( self.constants.width, self.constants.road.outbound.width).transform( self.constants.road.constants.orientation, Point(0, (self.constants.road.inbound.width - position.y) * 0.5).translate(position)) self.inbound_intersection_bounding_box = geometry.make_rectangle( self.constants.width, self.constants.road.inbound.width).transform( self.constants.road.constants.orientation, Point(0, -(self.constants.road.outbound.width - position.y) * 0.5).translate(position)) outbound_traffic_light_position = geometry.Point( self.static_bounding_box.rear_left.x, self.static_bounding_box.rear_left.y + 20.0) inbound_traffic_light_position = geometry.Point( self.static_bounding_box.front_right.x, self.static_bounding_box.front_right.y - 20.0) outbound_traffic_light_constants = TrafficLightConstants( width=10, height=20, position=outbound_traffic_light_position, orientation=self.constants.road.constants.orientation) inbound_traffic_light_constants = TrafficLightConstants( width=10, height=20, position=inbound_traffic_light_position, orientation=self.constants.road.constants.orientation) self.outbound_traffic_light = TrafficLight( init_state, outbound_traffic_light_constants) self.inbound_traffic_light = TrafficLight( init_state, inbound_traffic_light_constants) self.outbound_spawn = Point( self.constants.x_position + (self.constants.width * 0.15), (self.constants.road.width / 2.0) + (self.constants.road.constants.lane_width / 2.0)).transform( self.constants.road.constants.orientation, self.constants.road.constants.position) self.inbound_spawn = Point( self.constants.x_position - (self.constants.width * 0.15), -(self.constants.road.width / 2.0) - (self.constants.road.constants.lane_width / 2.0)).transform( self.constants.road.constants.orientation, self.constants.road.constants.position)
def chase_target(self): target = copy.deepcopy(self._field.actors['pacman'].position).move( *map( lambda n: 2 * n, self._field.actors['pacman'].direction.get_offset() ) ) assistant = copy.deepcopy(self._field.actors['red_ghost'].position) assistant.move(-target.x, -target.y) assistant = Point(-assistant.x, -assistant.y) assistant.move(target.x, target.y) return assistant
def make_frenet_point(point, spline_points): spline_distances = [ calc_distance(point, reference) for reference in spline_points ] closest_mean_distance = math.inf closest_pair = None closest_index = None for i, element in enumerate( zip(spline_points, spline_distances, spline_points[1:], spline_distances[1:])): spline_point_first, distance_first, spline_point_second, distance_second = element mean_distance = (distance_first + distance_second) / 2 if mean_distance < closest_mean_distance: closest_mean_distance = mean_distance closest_pair = spline_point_first, spline_point_second closest_index = i preceding_spline_points = spline_points[: closest_index] # select spline points up to but excluding closest_pair (question: should spline_point_first be included?) spline_point_first, spline_point_second = closest_pair tangent_x = spline_point_second.x - spline_point_first.x tangent_y = spline_point_second.y - spline_point_first.y vector_x = point.x - spline_point_first.x vector_y = point.y - spline_point_first.y tangent_length = math.sqrt(tangent_x**2 + tangent_y**2) projected_vector_norm = (vector_x * tangent_x + vector_y * tangent_y) / tangent_length projected_vector_x = projected_vector_norm * tangent_x / tangent_length projected_vector_y = projected_vector_norm * tangent_y / tangent_length frenet_d = calc_distance(Point(x=vector_x, y=vector_y), Point(x=projected_vector_x, y=projected_vector_y)) d = (point.x - spline_point_first.x) * ( spline_point_second.y - spline_point_first.y) - ( point.y - spline_point_first.y) * (spline_point_second.x - spline_point_first.x) frenet_d = math.copysign( frenet_d, -d ) if frenet_d != 0.0 else frenet_d # sign of frenet_d should be opposite of d frenet_s = projected_vector_norm for preceding_spline_point, next_preceding_spline_point in zip( preceding_spline_points, preceding_spline_points[1:]): frenet_s += calc_distance(preceding_spline_point, next_preceding_spline_point) return FrenetPoint(s=frenet_s, d=frenet_d)
def __init__(self, services, display): self._services = services self._display = display self._actor_drawers = {} self._block_drawers = {} self._text_drawer = TextDrawer(self._services, self._display) self._actor_drawers['pacman'] = PacmanDrawer(self._services, self._display) self._actor_drawers['red_ghost'] = EnemyDrawer(self._services, self._display) self._actor_drawers['pink_ghost'] = EnemyDrawer( self._services, self._display) self._actor_drawers['blue_ghost'] = EnemyDrawer( self._services, self._display) self._actor_drawers['orange_ghost'] = EnemyDrawer( self._services, self._display) for x in range(Grid.size.width): for y in range(Grid.size.height): self._block_drawers[Point(x, y)] = BlockDrawer( self._services, self._display) self._level_drawers = TextDrawer(services, display) self._scores_drawers = TextDrawer(services, display) self._lives_drawers = TextDrawer(services, display) self._background_drawer = BackgroundDrawer(services, display, Size(16, Grid.size.height))
def draw(self, model): if not model: return for block in model.field.grid: self._block_drawers[block.cell].draw(block, model.field) for actor in model.field.actors.values(): self._actor_drawers[actor.name].draw(actor) self._level_drawers.draw(Point(Grid.size.width + 1, 1), 'level: {}'.format(model.level), TextDrawer.Align.LEFT) self._scores_drawers.draw(Point(Grid.size.width + 1, 3), 'scores: {}'.format(model.scores), TextDrawer.Align.LEFT) self._lives_drawers.draw(Point(Grid.size.width + 1, 5), 'lives: {}'.format(model.lives), TextDrawer.Align.LEFT)
class TextDrawer: class Align: LEFT = 0 CENTER = 1 RIGHT = 2 _offset = Point(-8, -8) def __init__(self, services, display): self._services = services self._display = display self._drawers = [] def draw(self, position, text, align=Align.CENTER): if align == self.Align.RIGHT: position = position.shift(-len(text), 0) elif align == self.Align.CENTER: position = position.shift(-len(text) / 2, 0) self.clear() for i, char in enumerate(text): if char == ' ': continue drawer = AnimationDrawer(self._services, self._display) self._drawers.append(drawer) drawer.draw(position.shift(i, 0), self._offset, 'char', char) def clear(self): while self._drawers: drawer = self._drawers.pop() drawer.clear()
def make_body_state(env_state, index): body_state = env_state[index] assert len(body_state) == 4 return DynamicBodyState(position=Point(x=float(body_state[0]), y=float(body_state[1])), velocity=float(body_state[2]), orientation=float(body_state[3]))
class BlueGhost(Enemy): def __init__(self, services, position, direction, mode, field): super().__init__( services, 'blue_ghost', position, direction, mode, field ) self.state_driver_3.add_transition( EventId.BLUE_GHOST_OUT, (self.Mode.HOME,), self.Mode.EXIT ) scatter_target = Point(26, 30) @property def chase_target(self): target = copy.deepcopy(self._field.actors['pacman'].position).move( *map( lambda n: 2 * n, self._field.actors['pacman'].direction.get_offset() ) ) assistant = copy.deepcopy(self._field.actors['red_ghost'].position) assistant.move(-target.x, -target.y) assistant = Point(-assistant.x, -assistant.y) assistant.move(target.x, target.y) return assistant
def occlusion_zone(self, observation_point): points = [Point(x, y) for x, y in self.bounding_box()] max_angle = 0 max_triangle = None for pair in itertools.combinations(points, 2): p1, p2 = pair triangle = geometry.Triangle(rear=observation_point, front_left=p1, front_right=p2) angle = triangle.angle() if abs(angle) > abs(max_angle): max_angle = angle max_triangle = triangle anchor_triangle = max_triangle.normalise() enlarged = geometry.Triangle( rear=anchor_triangle.rear, front_left=anchor_triangle.front_left.enlarge( anchor_triangle.rear), front_right=anchor_triangle.front_right.enlarge( anchor_triangle.rear)) return geometry.ConvexQuadrilateral( rear_left=anchor_triangle.front_left, front_left=enlarged.front_left, front_right=enlarged.front_right, rear_right=anchor_triangle.front_right)
def make_cartesian_point(frenet_point, spline2d): x, y = spline2d.calc_position(frenet_point.s) if x is None or y is None: return None yaw = spline2d.calc_yaw(frenet_point.s) fx = x + frenet_point.d * math.cos(yaw + math.pi / 2.0) fy = y + frenet_point.d * math.sin(yaw + math.pi / 2.0) return Point(x=fx, y=fy)
def predict_step(body_state): # assume noop action distance_velocity = body_state.velocity * self.frenet_planner.constants.dt return DynamicBodyState(position=Point( x=body_state.position.x + distance_velocity * math.cos(body_state.orientation), y=body_state.position.y + distance_velocity * math.sin(body_state.orientation)), velocity=body_state.velocity, orientation=body_state.orientation)
def get_target(self): if self.mode[0] == self.Mode.NONE: if self.mode[1] == self.Mode.SCATTER: return self.scatter_target elif self.mode[1] == self.Mode.CHASE: return self.chase_target else: return Point( random.randint(0, Grid.size.width), random.randint(0, Grid.size.height) ) elif self.mode[0] == self.Mode.DEAD: return self.dead_target else: return Point( random.randint(0, Grid.size.width), random.randint(0, Grid.size.height) )
def __init__(self, constants): self.constants = constants self.static_bounding_box = geometry.make_rectangle( self.constants.length, self.constants.road_direction.lane_width * 0.75, left_offset=0).translate( Point(self.constants.x_position, 0).translate(self.constants.road_direction. static_bounding_box.rear_left))
def _draw_content(self, content): while self._content_drawers: drawer = self._content_drawers.pop() drawer.clear() for i, line in enumerate(content): x = Interface.size.width / 2 y = (Interface.size.height - len(content)) / 2 + i drawer = TextDrawer(self._services, self._display) self._content_drawers.append(drawer) drawer.draw(Point(x, y), line)
def make_cartesian_path(spline2d, frenet_states): points = [] for frenet_state in frenet_states: x, y = spline2d.calc_position(frenet_state.s) if x is None: break yaw = spline2d.calc_yaw(frenet_state.s) fx = x + frenet_state.d * math.cos(yaw + math.pi / 2.0) fy = y + frenet_state.d * math.sin(yaw + math.pi / 2.0) points.append(Point(x=fx, y=fy)) return CartesianPath(points)
class RedGhost(Enemy): def __init__(self, services, position, direction, mode, field): super().__init__( services, 'red_ghost', position, direction, mode, field ) scatter_target = Point(26, 0) @property def chase_target(self): return self._field.actors['pacman'].position
def draw(self, model): if not model: return x = Interface.size.width / 2 y = Interface.size.height / 4 self._title_drawer.draw(Point(x, y), model.menu.current_page.title) if type(model.menu.current_page) == RatingsItem: self._draw_ratings(model.menu, []) elif type(model.menu.current_page) == RecordItem: self._draw_record(model.menu, []) else: self._draw_page(model.menu, [])
def make_target_spline(waypoints, samples=775): spline2d = Spline2D([waypoint.x for waypoint in waypoints], [waypoint.y for waypoint in waypoints]) spline_points = [] for s in np.linspace(start=0, stop=spline2d.s[-1], num=samples, endpoint=False): # exclude end point (spline2d.s[-1]) x, y = spline2d.calc_position(s) spline_points.append(Point(x=x, y=y)) return spline_points, spline2d
class ActorDrawer: _offset = Point(-16, -16) def __init__(self, services, display): self._drawer = AnimationDrawer(services, display) def draw(self, actor, animation_name): self._drawer.draw(actor.position, self._offset, actor.name, animation_name) def clear(self): self._drawer.clear()
def one_step_lookahead( body_state, throttle): # one-step lookahead with no steering distance_velocity = body_state.velocity * self.time_resolution return DynamicBodyState( position=Point( x=body_state.position.x + distance_velocity * math.cos(body_state.orientation), y=body_state.position.y + distance_velocity * math.sin(body_state.orientation)), velocity=max( self.body.constants.min_velocity, min( self.body.constants.max_velocity, body_state.velocity + (throttle * self.time_resolution))), orientation=body_state.orientation)
def update(self): if self.mode[2] == self.Mode.HOME: current_node = self.current_node if current_node in (0, 3, 6) and current_node != self.last_node: self.direction = Direction.SOUTH self.last_node = current_node elif current_node in (2, 5, 8) and current_node != self.last_node: self.direction = Direction.NORTH self.last_node = current_node elif self.mode[2] == self.Mode.EXIT: current_node = self.current_node if current_node is not None and current_node != self.last_node: self.direction = self._field.enemy_graph.exit[current_node] new_position = self._field.enemy_graph.nodes[current_node] self.position = Point(new_position.x, new_position.y) self.last_node = current_node super().update()
def get_grid(self, grid_name): data = self._grids[grid_name] grid = Grid({}) for i, line in enumerate(data): for j, char in enumerate(line): block_info = self._block_types[char] connections = {} for direction in Direction: o_x, o_y = direction.get_offset() i_o = (i + o_y) % Grid.size.height j_o = (j + o_x) % Grid.size.width connection = self._block_types[data[i_o][j_o]][2] connections[direction] = connection cell = Point(j, i) grid[cell] = Block(cell, block_info[0], connections) if block_info[1]: grid.anchors[block_info[1]] = cell return grid
def choose_crossing_action(self, state, condition): body_state = make_body_state(state, self.index) if self.waypoint is None and self.target_orientation is None and condition: closest_point = self.road_centre.closest_point_from(body_state.position) relative_angle = math.atan2(closest_point.y - body_state.position.y, closest_point.x - body_state.position.x) if self.initial_distance is None: self.initial_distance = body_state.position.distance(closest_point) self.waypoint = Point( x=closest_point.x + self.initial_distance * math.cos(relative_angle), y=closest_point.y + self.initial_distance * math.sin(relative_angle), ) self.target_orientation = math.atan2(self.waypoint.y - body_state.position.y, self.waypoint.x - body_state.position.x) self.prior_orientation = body_state.orientation crossing_initiated = True else: crossing_initiated = False steering_action = make_steering_action(body_state, self.body.constants, self.time_resolution, self.target_orientation, self.noop_action) return [self.noop_action[0], steering_action], crossing_initiated
class OrangeGhost(Enemy): def __init__(self, services, position, direction, mode, field): super().__init__( services, 'orange_ghost', position, direction, mode, field ) self.state_driver_3.add_transition( EventId.ORANGE_GHOST_OUT, (self.Mode.HOME,), self.Mode.EXIT ) scatter_target = Point(1, 30) @property def chase_target(self): if self._field.actors['pacman'].position.distance(self.position) > 8: return self._field.actors['pacman'].position else: return self.scatter_target
def stopping_zones(self): braking_distance = (self.state.velocity** 2) / (2 * -self.constants.min_throttle) reaction_distance = self.state.velocity * REACTION_TIME total_distance = braking_distance + reaction_distance if total_distance == 0: return None, None if self.steering_angle == 0: zone = geometry.make_rectangle( total_distance, self.constants.width, rear_offset=0).transform( self.state.orientation, Point(self.constants.length * 0.5, 0).transform(self.state.orientation, self.state.position)) braking_zone, reaction_zone = zone.split_longitudinally( braking_distance / total_distance) return braking_zone, reaction_zone return None, None
class PinkGhost(Enemy): def __init__(self, services, position, direction, mode, field): super().__init__( services, 'pink_ghost', position, direction, mode, field ) self.state_driver_3.add_transition( EventId.PINK_GHOST_OUT, (self.Mode.HOME,), self.Mode.EXIT ) scatter_target = Point(1, 0) @property def chase_target(self): return copy.deepcopy(self._field.actors['pacman'].position).move( *map( lambda n: 4 * n, self._field.actors['pacman'].direction.get_offset() ) )
class BackgroundDrawer: _offset = Point(-8, -8) def __init__(self, services, display, size): self._services = services self._display = display self._size = size self._drawers = {} for i in range(Interface.size.width): for j in range(Interface.size.height): self._drawers[i, j] = AnimationDrawer(services, display) def draw(self, position): for j in range(self._size.height): for i in range(self._size.width): args = (position.shift(i, j), self._offset, 'block') if j == 0: if i == 0: self._drawers[i, j].draw(*args, 'wall_10') elif i == self._size.width - 1: self._drawers[i, j].draw(*args, 'wall_11') else: self._drawers[i, j].draw(*args, 'wall_8') elif j == self._size.height - 1: if i == 0: self._drawers[i, j].draw(*args, 'wall_12') elif i == self._size.width - 1: self._drawers[i, j].draw(*args, 'wall_13') else: self._drawers[i, j].draw(*args, 'wall_2') else: if i == 0: self._drawers[i, j].draw(*args, 'wall_6') elif i == self._size.width - 1: self._drawers[i, j].draw(*args, 'wall_4') else: self._drawers[i, j].draw(*args, 'empty')
def main(): test_data = read_test_data("frenet.csv") print("Start...") waypoints = [ Point(x=0.0, y=0.0), Point(x=10.0, y=-6.0), Point(x=20.5, y=5.0), Point(x=35.0, y=6.5), Point(x=70.5, y=0.0) ] obstacles = [ Point(x=20.0, y=10.0), Point(x=30.0, y=6.0), Point(x=30.0, y=8.0), Point(x=35.0, y=8.0), Point(x=50.0, y=3.0) ] frenet_planner = FrenetPlanner(constants=FrenetPlannerConstants( lateral_range_left=7.0, lateral_range_right=7.0, lateral_range_samples=15, timesteps_min=20, # min prediction ticks timesteps_max=25, # max prediction ticks dt=0.2, # time tick [s] velocity_target=30.0 / 3.6, # target speed [m/s] velocity_target_offset=5.0 / 3.6, velocity_range_samples=3, weight_jerk=0.1, # cost weight weight_time=0.1, # cost weight weight_velocity=1.0, # cost weight weight_lateral=1.0, # cost weight weight_longitudinal=1.0, # cost weight robot_radius=2.0, # robot radius [m] max_velocity=50.0 / 3.6, # maximum speed [m/s] max_thottle=2.0, # maximum acceleration [m/ss] max_curvature=1.0 # maximum curvature [1/m] )) target_spline_points, target_spline2d = make_target_spline(waypoints) # initial state frenet_state = FrenetState( s=0.0, # initial longitudinal position s_d=10.0 / 3.6, # initial longitudinal speed [m/s] s_dd=0.0, s_ddd=0.0, d=2.0, # initial lateral position [m] d_d=0.0, # initial lateral speed [m/s] d_dd=0.0, # initial lateral acceleration [m/s] d_ddd=0.0) for i in range(TIMESTEPS): path = frenet_planner.frenet_optimal_planning(target_spline2d, frenet_state, obstacles) frenet_state = path.frenet_states[1] translated_path_points = [ make_cartesian_point( make_frenet_point(point, target_spline_points), target_spline2d) for point in path.cartesian_path.points ] translated_path_points = [ point for point in translated_path_points if point is not None ] translated_obstacles = [ make_cartesian_point( make_frenet_point(obstacle, target_spline_points), target_spline2d) for obstacle in obstacles ] translated_obstacles = [ point for point in translated_obstacles if point is not None ] assert frenet_state == test_data[ i], f"{frenet_state} == {test_data[i]}" if math.sqrt( (path.cartesian_path.points[1].x - target_spline_points[-1].x)**2 + (path.cartesian_path.points[1].y - target_spline_points[-1].y)**2 ) <= 1.0: # if distance from current position to last target position is less than error threshold print("Reached goal") assert i == len(test_data) - 1 break if SHOW_ANIMATION: # pragma: no cover pyplot.cla() pyplot.gca().set_aspect("equal", adjustable="box") # for stopping simulation with the esc key. pyplot.gcf().canvas.mpl_connect( "key_release_event", lambda event: [exit(0) if event.key == "escape" else None]) pyplot.plot([point.x for point in target_spline_points], [point.y for point in target_spline_points], color="blue", label="target spline") pyplot.plot([point.x for point in path.cartesian_path.points[1:]], [point.y for point in path.cartesian_path.points[1:]], marker="x", color="green", label="plan") pyplot.plot([point.x for point in translated_path_points[1:]], [point.y for point in translated_path_points[1:]], marker="x", color="orange", label="plan (translated)") pyplot.scatter(path.cartesian_path.points[1].x, path.cartesian_path.points[1].y, color="red", label="ego") pyplot.scatter([obstacle.x for obstacle in obstacles], [obstacle.y for obstacle in obstacles], marker="x", color="purple", label="obstacles") pyplot.scatter([obstacle.x for obstacle in translated_obstacles], [obstacle.y for obstacle in translated_obstacles], marker="x", color="orange", label="obstacles (translated)") pyplot.gca().add_artist( pyplot.Circle(tuple(path.cartesian_path.points[1]), radius=frenet_planner.constants.robot_radius, color="red")) pyplot.xlim( path.cartesian_path.points[1].x - ANIMATION_AREA * 0.25, path.cartesian_path.points[1].x + ANIMATION_AREA * 1.75) pyplot.ylim(path.cartesian_path.points[1].y - ANIMATION_AREA, path.cartesian_path.points[1].y + ANIMATION_AREA) pyplot.title(f"t={i}, v[km/h]={frenet_state.s_d * 3.6}") pyplot.legend(loc="lower left") pyplot.grid(True) pyplot.tight_layout() pyplot.pause(0.0001) print("Finish") if SHOW_ANIMATION: # pragma: no cover pyplot.grid(True) pyplot.pause(0.0001) pyplot.show()
def cell(self): return Point( math.floor(self.position.x), math.floor(self.position.y) )