def create_start_end_npc_planner(carla_map: carla.Map,
start_loc: carla.Location,
end_loc: carla.Location, speed: float,
vehicle_pid_controller: VehiclePIDController):
start_wp = carla_map.get_waypoint(start_loc)
end_wp = carla_map.get_waypoint(end_loc)
print(
f"start road_id:{start_wp.road_id} land_id:{start_wp.lane_id}, end road_id:{end_wp.road_id}, land_id:{end_wp.lane_id}"
)
pairs = carla_map.get_topology()
G = nx.Graph()
G.add_edges_from([(pair[0].road_id, pair[1].road_id) for pair in pairs])
path = nx.shortest_path(G, start_wp.road_id, end_wp.road_id)
print(f"path={path}")
INTERVAL = 1
waypoints = find_waypoins(start_wp, path, INTERVAL, end_loc)
route_line = LineString([(wp.transform.location.x, wp.transform.location.y)
for wp in waypoints])
def fun(curr_loc: carla.Location):
distance = route_line.project(Point(curr_loc.x, curr_loc.y))
DELTA = 2
next_point = route_line.interpolate(distance + DELTA)
next_waypoint = carla_map.get_waypoint(
carla.Location(next_point.x, next_point.y, 0))
return vehicle_pid_controller.run_step(speed, next_waypoint.transform)
return fun
def map_dynamic_actor(actor: carla.Actor,
world_map: carla.Map) -> DynamicActor:
location: carla.Location = actor.get_location()
gnss: carla.GeoLocation = world_map.transform_to_geolocation(location)
velocity: carla.Vector3D = actor.get_velocity()
acceleration: carla.Vector3D = actor.get_acceleration()
color: str = str(
actor.attributes['color']) if 'color' in actor.attributes else None
extent: carla.Vector3D = actor.bounding_box.extent
transform: carla.Transform = carla.Transform(
rotation=actor.get_transform().rotation)
transformed_extent: Tuple[carla.Location, carla.Location, carla.Location,
carla.Location] = (
transform.transform(
carla.Location(+extent.x, +extent.y, 0)),
transform.transform(
carla.Location(+extent.x, -extent.y, 0)),
transform.transform(
carla.Location(-extent.x, -extent.y, 0)),
transform.transform(
carla.Location(-extent.x, +extent.y, 0)),
)
bbox: Tuple[Point2D, Point2D, Point2D, Point2D] = cast(
Tuple[Point2D, Point2D, Point2D, Point2D],
tuple(
map(
lambda t: geoloc2point2d(
world_map.transform_to_geolocation(
carla.Vector3D(location.x + t.x, location.y + t.y,
location.z))), transformed_extent)))
return DynamicActor(
id=actor.id,
type=UncertainProperty(1., resolve_carla_type(actor.type_id)),
type_id=actor.type_id,
location=UncertainProperty(1.,
Point3D(location.x, location.y,
location.z)),
gnss=UncertainProperty(
1., Point3D(gnss.latitude, gnss.longitude, gnss.altitude)),
dynamics=ActorDynamics(velocity=UncertainProperty(
1., Point3D(velocity.x, velocity.y, velocity.z)),
acceleration=UncertainProperty(
1.,
Point3D(acceleration.x, acceleration.y,
acceleration.z))),
props=ActorProperties(color=UncertainProperty(1., color),
extent=UncertainProperty(
1., Point3D(extent.x, extent.y, extent.z)),
bbox=UncertainProperty(1., bbox)))
def map_from_opendrive(opendrive: str, log_file_name: str = None):
try:
from carla import Map
except ImportError:
raise Exception('Error importing CARLA.')
from pylot.map.hd_map import HDMap
return HDMap(Map('map', opendrive), log_file_name)
def get_vehicle_velocity_vector(vehicle: carla.Vehicle, map_vehicle: carla.Map,
velocity):
"""
Function to return a velocity vector which points to the direction of the next waypoint.
:param velocity: Desired vehicle velocity
:param map_vehicle: carla.Map
:param vehicle: carla.Vehicle object
:return: carla.Vector3D
"""
# Getting current waypoint and next from vehicle
current_wp = map_vehicle.get_waypoint(vehicle.get_location())
next_wp = current_wp.next(1)[0]
# Getting localization from the waypoints
current_loc = get_localization_from_waypoint(current_wp)
next_loc = get_localization_from_waypoint(next_wp)
velocity_x = abs(next_loc.x - current_loc.x)
velocity_y = abs(next_loc.y - current_loc.y)
vector_vel0 = np.array([velocity_x, velocity_y, 0])
vector_vel = (velocity / np.linalg.norm(vector_vel0)) * vector_vel0
return carla.Vector3D(round(vector_vel[0], 3), round(vector_vel[1], 3), 0)
def draw_topology(
carla_map: carla.Map
) -> None:
'''
Visualizes the waypoint topology of the provided Carla Map via matplotlib.
Parameters
----------
carla_map : carla.Map
The Carla Map in which to plot the connected waypoint topology.
'''
G = networkx.Graph()
topology = carla_map.get_topology()
G.add_edges_from(topology)
pos = {}
for node in topology:
if node[0] not in pos:
pos[node[0]] = (
node[0].transform.location.x,
node[0].transform.location.y
)
if node[1] not in pos:
pos[node[1]] = (
node[1].transform.location.x,
node[1].transform.location.y
)
M = G.number_of_edges()
edge_colors = range(2, M + 2)
nodes = networkx.draw_networkx_nodes(
G,
pos,
node_size=0.4,
node_color='blue'
)
edges = networkx.draw_networkx_edges(
G,
pos,
node_size=0.2,
arrowstyle='->',
arrowsize=10,
edge_color=edge_colors,
edge_cmap=plt.cm.Blues,
width=2
)
ax = plt.gca()
ax.set_axis_off()
plt.show()
def simple_loc_reward(self, map: carla.Map, location: carla.Location):
"""Calculates simple reward for given location."""
# calc closest drivable point distance
reward = 0.0
wp = map.get_waypoint(location, carla.LaneType.Driving)
wp_location = wp.transform.location
dist = self.__euclid_dist(wp_location, location)
if dist < 0.5 and dist > -0.5:
reward += 0.5
else:
reward -= np.exp(dist)
return reward
def random_spawn_point(
world_map: carla.Map,
different_from: carla.Location = None) -> carla.Transform:
"""Returns a random spawning location.
:param world_map: a carla.Map instance obtained by calling world.get_map()
:param different_from: ensures that the location of the random spawn point is different from the one specified here.
:return: a carla.Transform instance.
"""
available_spawn_points = world_map.get_spawn_points()
if different_from is not None:
while True:
spawn_point = random.choice(available_spawn_points)
if spawn_point.location != different_from:
return spawn_point
else:
return random.choice(available_spawn_points)
def draw_settings(
carla_map: carla.Map, # pylint: disable=no-member
pixels_per_meter: int = 5,
scale: float = 1.0,
margin: int = 150,
) -> Tuple[float, pygame.Surface]:
"""Calculates the `PyGame` surfaces settings.
Args:
carla_map: The `CARLA` town map.
pixels_per_meter: The number of pixels rendered per meter.
scale: The scaling factor of the rendered map.
margin: The number of pixels used for margin.
Returns:
offset: The world offset.
surface: The empty `PyGame` surface.
"""
# The graph representing the CARLA map.
waypoints = carla_map.generate_waypoints(1)
# Calculate the width of the surface.
max_x = max(
waypoints,
key=lambda x: x.transform.location.x,
).transform.location.x + margin
max_y = max(
waypoints,
key=lambda x: x.transform.location.y,
).transform.location.y + margin
min_x = min(
waypoints,
key=lambda x: x.transform.location.x,
).transform.location.x - margin
min_y = min(
waypoints,
key=lambda x: x.transform.location.y,
).transform.location.y - margin
world_offset = (min_x, min_y)
width = max(max_x - min_x, max_y - min_y)
width_in_pixels = int(pixels_per_meter * width)
return world_offset, pygame.Surface((width_in_pixels, width_in_pixels)) # pylint: disable=too-many-function-args
def generate_opendrive_content_hash(map: carla.Map) -> str:
opendrive_content = map.to_opendrive()
hash_func = hashlib.sha1()
hash_func.update(opendrive_content.encode("UTF-8"))
opendrive_hash = str(hash_func.hexdigest())
return opendrive_hash