def _is_behind_ego_or_inside_birdview(c, ego_vehicle_location):
env_vehicle_direction = Vector3.from_carla_location(c.forward_vector).as_numpy()
relative_vector = (Vector3.from_carla_location(c.location) -
Vector3.from_carla_location(ego_vehicle_location)).as_numpy()
deviation = np.arccos(np.clip(np.dot(env_vehicle_direction, relative_vector) /
(np.linalg.norm(env_vehicle_direction) * np.linalg.norm(relative_vector)), -1.0, 1.0))
is_inside = True
is_behind_ego = np.abs(deviation) > np.pi / 1.5
if not is_behind_ego:
x, y = ego_vehicle_location.x, ego_vehicle_location.y
half_range = BIRD_VIEW_HEIGHT_M / 2 * 1.4
xmin, xmax = x - half_range, x + half_range
ymin, ymax = y - half_range, y + half_range
is_inside = xmin <= c.location.x <= xmax and ymin <= c.location.y <= ymax
return is_inside
def __init__(self, *, client: carla.Client, cmd_for_changing_lane=ChauffeurCommand.CHANGE_LANE_LEFT,
speed_range_token: str, no_columns=True, reward_type: RewardType = RewardType.DENSE):
super().__init__(client=client)
start_point = Transform(Vector3(-144.4, -22.41, 0), Vector2(-1.0, 0.0))
self._find_lane_waypoints(cmd_for_changing_lane, start_point.position.as_carla_location())
self._cmd_for_changing_lane = cmd_for_changing_lane
self._reward_type = reward_type
self._progress_monitor: Optional[LaneChangeProgressMonitor] = None
self._lane_alignment_monitor = LaneAlignmentMonitor(lane_alignment_frames=TARGET_LANE_ALIGNMENT_FRAMES,
cross_track_error_tolerance=CROSSTRACK_ERROR_TOLERANCE,
yaw_deg_error_tolerance=YAW_DEG_ERRORS_TOLERANCE)
self._early_stop_monitor: Optional[EarlyStopMonitor] = None
# vehicles shall fill bird view + first vehicle shall reach end of route forward part
# till the last one reaches bottom of the bird view; assume just VEHICLE_SLOT_LENGTH_M spacing
max_env_vehicles_number = int(BIRD_VIEW_HEIGHT_M * 1.2 // VEHICLE_SLOT_LENGTH_M)
env_vehicles = [] if no_columns else self._spawn_env_vehicles(max_env_vehicles_number)
self._controllers = self._wrap_with_controllers(env_vehicles)
env_vehicles_speed_range_mps = SPEED_RANGE_NAMES[speed_range_token]
self._speed_range_mps = env_vehicles_speed_range_mps
self._env_vehicle_column_ahead_range_m = (30, 50)
route_length_m = max(MAX_MANEUVER_LENGTH_M + BIRD_VIEW_HEIGHT_M,
max_env_vehicles_number * (MAX_VEHICLE_RANDOM_SPACE_M + VEHICLE_SLOT_LENGTH_M)) * 3
self._topology = Topology(self._world_map)
self._routes: List[List[carla.Transform]] = self._obtain_routes(self._target_lane_waypoint, route_length_m)
def __init__(self,
*,
client: carla.Client,
cmd_for_changing_lane=ChauffeurCommand.CHANGE_LANE_LEFT,
speed_range_token: str,
no_columns=True):
super().__init__(client=client)
start_point = Transform(Vector3(-144.4, -22.41, 0), Vector2(-1.0, 0.0))
self._find_lane_waypoints(cmd_for_changing_lane,
start_point.position.as_carla_location())
self._cmd_for_changing_lane = cmd_for_changing_lane
self._done_counter: int = TARGET_LANE_ALIGNMENT_FRAMES
# vehicles shall fill bird view + first vehicle shall reach end of route forward part
# till the last one reaches bottom of the bird view; assume just VEHICLE_SLOT_LENGTH_M spacing
max_env_vehicles_number = int(BIRD_VIEW_HEIGHT_M * 1.2 //
VEHICLE_SLOT_LENGTH_M)
env_vehicles = [] if no_columns else self._spawn_env_vehicles(
max_env_vehicles_number)
self._controllers = self._wrap_with_controllers(env_vehicles)
env_vehicles_speed_range_mps = SPEED_RANGE_NAMES[speed_range_token]
self._speed_range_mps = env_vehicles_speed_range_mps
self._env_vehicle_column_ahead_range_m = (5, 30)
route_length_m = max(
MAX_MANEUVER_LENGTH_M + BIRD_VIEW_HEIGHT_M,
max_env_vehicles_number *
(MAX_VEHICLE_RANDOM_SPACE_M + VEHICLE_SLOT_LENGTH_M)) * 3
self._topology = Topology(self._world_map)
self._routes: List[List[carla.Transform]] = self._obtain_routes(
self._target_lane_waypoint, route_length_m)
def step(self, vehicles):
commands = []
real_vehicle: RealTrafficVehicle
for real_vehicle in vehicles:
if real_vehicle.id in self._ignored_real_traffic_vehicle_ids:
continue
target_transform = real_vehicle.transform # transform in carla coordinates
if real_vehicle.id in self._vehicle_by_vehicle_id:
carla_vehicle = self._vehicle_by_vehicle_id[real_vehicle.id]
target_transform = Transform(
target_transform.position.with_z(
carla_vehicle.get_transform().location.z),
target_transform.orientation)
commands.append(
carla.command.ApplyTransform(
carla_vehicle, target_transform.as_carla_transform()))
else:
spawn_transform = Transform(
target_transform.position.with_z(500),
target_transform.orientation)
vehicle_blueprint = self._get_vehicle_blueprint(
real_vehicle.type_id)
vehicle_blueprint.set_attribute('role_name',
'real_traffic_replay')
carla_vehicle = self._world.try_spawn_actor(
vehicle_blueprint, spawn_transform.as_carla_transform())
if carla_vehicle is None:
LOGGER.info(
f"Error spawning vehicle with id {real_vehicle.id}. "
f"Ignoring it now in the future. Model: {real_vehicle.type_id}."
)
# Without ignoring such vehicles till the end of episode a vehicle might suddenly appears mid-road
# in future frames
self._ignored_real_traffic_vehicle_ids.add(real_vehicle.id)
continue
commands.append(
carla.command.ApplyTransform(
carla_vehicle, target_transform.as_carla_transform()))
self._vehicle_by_vehicle_id[real_vehicle.id] = carla_vehicle
if real_vehicle.debug:
self._world.debug.draw_string(
(target_transform.position +
Vector3(2, 0, 4)).as_carla_location(),
str(real_vehicle.debug))
now_vehicle_ids = {v.id for v in vehicles}
previous_vehicles_ids = set(self._vehicle_by_vehicle_id.keys())
for to_remove_id in previous_vehicles_ids - now_vehicle_ids:
actor = self._vehicle_by_vehicle_id[to_remove_id]
commands.append(carla.command.DestroyActor(actor.id))
del self._vehicle_by_vehicle_id[to_remove_id]
# TODO batch spawn and batch destroy
self._client.apply_batch_sync(commands, False)
class VehicleModel(NamedTuple):
type_id: str
wheelbase_m: float
bounding_box: BoundingBox # See tools/extract_vehicle_bounding_boxes.main.py script
z_offset: float # car z-coordinate when standing on z=0 plane. See tools/extract_vehicle_z_offsets.main.py
front_axle_offset: float # extract_vehicle_axle_positions.main.py
@property
def rear_axle_offset(self):
return self.front_axle_offset - self.wheelbase_m
VEHICLES = [
VehicleModel('vehicle.audi.a2', 2.5063290535550626,
BoundingBox(Vector3(x=0.000000, y=0.000000, z=0.780000),
Vector3(x=1.859311, y=0.896224, z=0.767747)), -0.010922241024672985,
1.249703369140633),
VehicleModel('vehicle.audi.tt', 2.6529807899879163,
BoundingBox(Vector3(x=0.000000, y=0.000000, z=0.700000),
Vector3(x=2.110247, y=1.004570, z=0.695304)), -0.005904807709157467,
1.2471203613281148),
VehicleModel('vehicle.carlamotors.carlacola', 2.6650390625,
BoundingBox(Vector3(x=0.000000, y=0.000000, z=1.230000),
Vector3(x=2.599615, y=1.307510, z=1.238425)),
-0.00285758962854743,
1.4586614990234352),
VehicleModel('vehicle.citroen.c3', 2.6835025857246966,
BoundingBox(Vector3(x=0.000000, y=0.000000, z=0.770000),
Vector3(x=1.987674, y=0.931037, z=0.812886)),
0.03734729811549187,
def extract_utm_trajectory_from_df(df) -> List[Transform]:
trajectory = df[['UTM_X', 'UTM_Y', 'UTM_ANGLE']].values
return [
Transform(Vector3(x, y, 0), Vector2(np.cos(angle), np.sin(angle)))
for x, y, angle in trajectory
]
def _build_graph(self, topology):
"""
slimmed version of carla agents.navigation.GlobalRoutePlanner._build_graph method
This function builds a networkx graph representation of topology.
The topology is read from self._topology.
graph node properties:
vertex - (x,y,z) position in world map
graph edge properties:
entry_vector - unit vector along tangent at entry point
exit_vector - unit vector along tangent at exit point
net_vector - unit vector of the chord from entry to exit
intersection - boolean indicating if the edge belongs to an
intersection
return : graph -> networkx graph representing the world map,
id_map-> mapping from (x,y,z) to node id
road_id_to_edge-> map from road id to edge in the graph
"""
graph = nx.DiGraph()
id_map = dict() # Map with structure {(x,y,z): id, ... }
road_id_to_edge = dict() # Map with structure {road_id: {lane_id: edge, ... }, ... }
for segment in topology:
path = segment['path']
entry_wp, exit_wp = segment['entry'], segment['exit']
intersection = entry_wp.is_junction
road_id, section_id, lane_id = entry_wp.road_id, entry_wp.section_id, entry_wp.lane_id
entry_xyz, exit_xyz = segment['entryxyz'], segment['exitxyz']
for vertex in entry_xyz, exit_xyz:
# Adding unique nodes and populating id_map
if vertex not in id_map:
new_id = len(id_map)
id_map[vertex] = new_id
graph.add_node(new_id, vertex=vertex)
n1 = id_map[entry_xyz]
n2 = id_map[exit_xyz]
if road_id not in road_id_to_edge:
road_id_to_edge[road_id] = dict()
if section_id not in road_id_to_edge[road_id]:
road_id_to_edge[road_id][section_id] = dict()
road_id_to_edge[road_id][section_id][lane_id] = (n1, n2)
entry_carla_vector = entry_wp.transform.rotation.get_forward_vector()
exit_carla_vector = exit_wp.transform.rotation.get_forward_vector()
net_vector = Vector3.from_carla_location(exit_wp.transform.location) - \
Vector3.from_carla_location(entry_wp.transform.location)
net_vector /= np.linalg.norm(net_vector.as_numpy()) + np.finfo(float).eps
segment = [entry_wp] + path
length = self._calc_segment_length(segment)
# Adding edge with attributes
graph.add_edge(
n1, n2,
length=length, path=path,
entry_waypoint=entry_wp, exit_waypoint=exit_wp,
entry_vector=np.array([entry_carla_vector.x, entry_carla_vector.y, entry_carla_vector.z]),
exit_vector=np.array([exit_carla_vector.x, exit_carla_vector.y, exit_carla_vector.z]),
net_vector=net_vector, intersection=intersection)
return graph, id_map, road_id_to_edge