Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
draw_arrow(map_surface, sp, color=COLOR_CHOCOLATE_0)
if self.show_connections:
dist = 1.5
to_pixel = lambda wp: world_to_pixel(wp.transform.location)
for wp in carla_map.generate_waypoints(dist):
col = (0, 255, 255) if wp.is_intersection else (0, 255, 0)
for nxt in wp.next(dist):
pygame.draw.line(map_surface, col, to_pixel(wp), to_pixel(nxt), 2)
if wp.lane_change & carla.LaneChange.Right:
r = wp.get_right_lane()
if r and r.lane_type == carla.LaneType.Driving:
pygame.draw.line(map_surface, col, to_pixel(wp), to_pixel(r), 2)
if wp.lane_change & carla.LaneChange.Left:
l = wp.get_left_lane()
if l and l.lane_type == carla.LaneType.Driving:
pygame.draw.line(map_surface, col, to_pixel(wp), to_pixel(l), 2)
actors = carla_world.get_actors()
# Draw Traffic Signs
font_size = world_to_pixel_width(1)
font = pygame.font.SysFont('Arial', font_size, True)
stops = [actor for actor in actors if 'stop' in actor.type_id]
yields = [actor for actor in actors if 'yield' in actor.type_id]
stop_font_surface = font.render("STOP", False, COLOR_ALUMINIUM_2)
stop_font_surface = pygame.transform.scale(
stop_font_surface, (stop_font_surface.get_width(), stop_font_surface.get_height() * 2))
yield_font_surface = font.render("YIELD", False, COLOR_ALUMINIUM_2)
if len(polygon) > 2:
pygame.draw.polygon(map_surface, COLOR_ALUMINIUM_5, polygon, 5)
pygame.draw.polygon(map_surface, COLOR_ALUMINIUM_5, polygon)
# Draw Shoulders and Parkings
PARKING_COLOR = COLOR_ALUMINIUM_4_5
SHOULDER_COLOR = COLOR_ALUMINIUM_5
final_color = SHOULDER_COLOR
# Draw Right
shoulder = []
for w in waypoints:
r = w.get_right_lane()
if r is not None and (
r.lane_type == carla.LaneType.Shoulder or r.lane_type == carla.LaneType.Parking):
if r.lane_type == carla.LaneType.Parking:
final_color = PARKING_COLOR
shoulder.append(r)
shoulder_left_side = [lateral_shift(w.transform, -w.lane_width * 0.5) for w in shoulder]
shoulder_right_side = [lateral_shift(w.transform, w.lane_width * 0.5) for w in shoulder]
polygon = shoulder_left_side + [x for x in reversed(shoulder_right_side)]
polygon = [world_to_pixel(x) for x in polygon]
if len(polygon) > 2:
pygame.draw.polygon(map_surface, final_color, polygon, 5)
pygame.draw.polygon(map_surface, final_color, polygon)
draw_lane_marking(
map_surface,
else:
break
set_waypoints.append(waypoints)
# Draw Shoulders, Parkings and Sidewalks
PARKING_COLOR = COLOR_ALUMINIUM_4_5
SHOULDER_COLOR = COLOR_ALUMINIUM_5
SIDEWALK_COLOR = COLOR_ALUMINIUM_3
shoulder = [[], []]
parking = [[], []]
sidewalk = [[], []]
for w in waypoints:
l = w.get_left_lane()
while l and l.lane_type != carla.LaneType.Driving:
if l.lane_type == carla.LaneType.Shoulder:
shoulder[0].append(l)
if l.lane_type == carla.LaneType.Parking:
parking[0].append(l)
if l.lane_type == carla.LaneType.Sidewalk:
sidewalk[0].append(l)
l = l.get_left_lane()
r = w.get_right_lane()
while r and r.lane_type != carla.LaneType.Driving:
if r.lane_type == carla.LaneType.Shoulder:
"""
This method places zero cost links in the topology graph
representing availability of lane changes.
"""
for segment in self._topology:
left_found, right_found = False, False
for waypoint in segment['path']:
if not segment['entry'].is_junction:
next_waypoint, next_road_option, next_segment = None, None, None
if bool(waypoint.lane_change & carla.LaneChange.Right) and not right_found:
next_waypoint = waypoint.get_right_lane()
if next_waypoint is not None and \
next_waypoint.lane_type == carla.LaneType.Driving and \
waypoint.road_id == next_waypoint.road_id:
next_road_option = RoadOption.CHANGELANERIGHT
next_segment = self._localize(next_waypoint.transform.location)
if next_segment is not None:
self._graph.add_edge(
self._id_map[segment['entryxyz']], next_segment[0], entry_waypoint=segment['entry'],
exit_waypoint=self._graph.edges[next_segment[0], next_segment[1]]['entry_waypoint'],
path=[], length=0, type=next_road_option, change_waypoint = waypoint)
right_found = True
if bool(waypoint.lane_change & carla.LaneChange.Left) and not left_found:
next_waypoint = waypoint.get_left_lane()
if next_waypoint is not None and next_waypoint.lane_type == carla.LaneType.Driving and \
waypoint.road_id == next_waypoint.road_id:
next_road_option = RoadOption.CHANGELANELEFT
next_segment = self._localize(next_waypoint.transform.location)
self._actor_dict[actor] = self._plan
else:
self._actor_dict[actor] = [element[0].transform.location for element in self._plan]
else:
local_planner = LocalPlanner( # pylint: disable=undefined-variable
actor, opt_dict={
'target_speed': self._target_speed * 3.6,
'lateral_control_dict': self._args_lateral_dict})
if self._plan is not None:
if isinstance(self._plan[0], carla.Location):
plan = []
for location in self._plan:
waypoint = CarlaDataProvider.get_map().get_waypoint(location,
project_to_road=True,
lane_type=carla.LaneType.Any)
plan.append((waypoint, RoadOption.LANEFOLLOW))
local_planner.set_global_plan(plan)
else:
local_planner.set_global_plan(self._plan)
self._local_planner_dict[actor] = local_planner
self._actor_dict[actor] = self._plan
y_boundary_vector = perpendicular_vec * extent.y
bbox = [
current_loc + carla.Location(x_boundary_vector - y_boundary_vector),
current_loc + carla.Location(x_boundary_vector + y_boundary_vector),
current_loc + carla.Location(-1 * x_boundary_vector - y_boundary_vector),
current_loc + carla.Location(-1 * x_boundary_vector + y_boundary_vector)]
bbox_wp = [
self._map.get_waypoint(bbox[0], lane_type=carla.LaneType.Any),
self._map.get_waypoint(bbox[1], lane_type=carla.LaneType.Any),
self._map.get_waypoint(bbox[2], lane_type=carla.LaneType.Any),
self._map.get_waypoint(bbox[3], lane_type=carla.LaneType.Any)]
# Case 2.1) Not quite outside yet
if bbox_wp[0].lane_type == (carla.LaneType.Driving or carla.LaneType.Parking) \
or bbox_wp[1].lane_type == (carla.LaneType.Driving or carla.LaneType.Parking) \
or bbox_wp[2].lane_type == (carla.LaneType.Driving or carla.LaneType.Parking) \
or bbox_wp[3].lane_type == (carla.LaneType.Driving or carla.LaneType.Parking):
self._onsidewalk_active = False
self._outside_lane_active = False
# Case 2.2) At the mini Shoulders between Driving and Sidewalk
elif bbox_wp[0].lane_type == carla.LaneType.Sidewalk \
or bbox_wp[1].lane_type == carla.LaneType.Sidewalk \
or bbox_wp[2].lane_type == carla.LaneType.Sidewalk \
or bbox_wp[3].lane_type == carla.LaneType.Sidewalk:
if not self._onsidewalk_active:
self._onsidewalk_active = True
self._sidewalk_start_location = current_loc
if l.lane_type == carla.LaneType.Shoulder:
shoulder[0].append(l)
if l.lane_type == carla.LaneType.Parking:
parking[0].append(l)
if l.lane_type == carla.LaneType.Sidewalk:
sidewalk[0].append(l)
l = l.get_left_lane()
r = w.get_right_lane()
while r and r.lane_type != carla.LaneType.Driving:
if r.lane_type == carla.LaneType.Shoulder:
shoulder[1].append(r)
if r.lane_type == carla.LaneType.Parking:
parking[1].append(r)
if r.lane_type == carla.LaneType.Sidewalk:
sidewalk[1].append(r)
r = r.get_right_lane()
draw_lane(map_surface, shoulder, SHOULDER_COLOR)
draw_lane(map_surface, parking, PARKING_COLOR)
draw_lane(map_surface, sidewalk, SIDEWALK_COLOR)
# draw_lane_marking(map_surface, shoulder)
# draw_lane_marking(map_surface, parking)
PARKING_COLOR = COLOR_ALUMINIUM_4_5
SHOULDER_COLOR = COLOR_ALUMINIUM_5
SIDEWALK_COLOR = COLOR_ALUMINIUM_3
shoulder = [[], []]
parking = [[], []]
sidewalk = [[], []]
for w in waypoints:
l = w.get_left_lane()
while l and l.lane_type != carla.LaneType.Driving:
if l.lane_type == carla.LaneType.Shoulder:
shoulder[0].append(l)
if l.lane_type == carla.LaneType.Parking:
parking[0].append(l)
if l.lane_type == carla.LaneType.Sidewalk:
sidewalk[0].append(l)
l = l.get_left_lane()
r = w.get_right_lane()
while r and r.lane_type != carla.LaneType.Driving:
if r.lane_type == carla.LaneType.Shoulder:
shoulder[1].append(r)
if r.lane_type == carla.LaneType.Parking:
parking[1].append(r)
def get_right_driving_lane(waypoint):
"""
Gets the driving / parking lane that is most to the right of the waypoint
as well as the number of lane changes done
"""
lane_changes = 0
while True:
wp_next = waypoint.get_right_lane()
lane_changes += 1
if wp_next is None or wp_next.lane_type == carla.LaneType.Sidewalk:
break
elif wp_next.lane_type == carla.LaneType.Shoulder:
# Filter Parkings considered as Shoulders
if is_lane_a_parking(wp_next):
lane_changes += 1
waypoint = wp_next
break
else:
waypoint = wp_next
return waypoint, lane_changes
if len(polygon) > 2:
pygame.draw.polygon(map_surface, final_color, polygon, 5)
pygame.draw.polygon(map_surface, final_color, polygon)
draw_lane_marking(
map_surface,
shoulder,
False)
# Draw Left
shoulder = []
for w in waypoints:
r = w.get_left_lane()
if r is not None and (
r.lane_type == carla.LaneType.Shoulder or r.lane_type == carla.LaneType.Parking):
if r.lane_type == carla.LaneType.Parking:
final_color = PARKING_COLOR
shoulder.append(r)
shoulder_left_side = [lateral_shift(w.transform, -w.lane_width * 0.5) for w in shoulder]
shoulder_right_side = [lateral_shift(w.transform, w.lane_width * 0.5) for w in shoulder]
polygon = shoulder_left_side + [x for x in reversed(shoulder_right_side)]
polygon = [world_to_pixel(x) for x in polygon]
if len(polygon) > 2:
pygame.draw.polygon(map_surface, final_color, polygon, 5)
pygame.draw.polygon(map_surface, final_color, polygon)
draw_lane_marking(
map_surface,