def extract_junction_points(self, tlight_find_radius=25.):
carla_topology = self.carla_map.get_topology()
junctions = carlautil.get_junctions_from_topology_graph(carla_topology)
tlights = util.filter_to_list(lambda a: 'traffic_light' in a.type_id,
self.carla_world.get_actors())
tlight_distances = np.zeros((
len(tlights),
len(junctions),
))
f = lambda j: carlautil.location_to_ndarray(j.bounding_box.location)
junction_locations = util.map_to_ndarray(f, junctions)
g = lambda tl: carlautil.transform_to_location_ndarray(tl.
get_transform())
tlight_locations = util.map_to_ndarray(g, tlights)
for idx, junction in enumerate(junctions):
tlight_distances[:, idx] = np.linalg.norm(tlight_locations -
junction_locations[idx],
axis=1)
is_controlled_junction = (tlight_distances < tlight_find_radius).any(
axis=0)
is_uncontrolled_junction = np.logical_not(is_controlled_junction)
controlled_junction_locations \
= junction_locations[is_controlled_junction]
uncontrolled_junction_locations \
= junction_locations[is_uncontrolled_junction]
return util.AttrDict(controlled=controlled_junction_locations,
uncontrolled=uncontrolled_junction_locations)
def __init__(self, carla_world, carla_map, debug=False):
"""
Parameters
----------
carla_world : carla.World
carla_map : carla.Map
debug : bool
"""
super().__init__(carla_world, carla_map, debug=debug)
"""Generate slope topology of the map"""
# get waypoints
wps = self.carla_map.generate_waypoints(4.0)
def h(wp):
l = wp.transform.location
pitch = wp.transform.rotation.pitch
return np.array([l.x, l.y, l.z, pitch])
loc_and_pitch_of_wps = util.map_to_ndarray(h, wps)
self.wp_locations = loc_and_pitch_of_wps[:, :-1]
self.wp_pitches = loc_and_pitch_of_wps[:, -1]
self.wp_pitches = self.wp_pitches % 360.
self.wp_is_sloped = np.logical_and(
self.SLOPE_DEGREES < self.wp_pitches,
self.wp_pitches < self.SLOPE_UDEGREES)
"""Generate intersection topology of the map"""
# get nodes in graph
topology = self.carla_map.get_topology()
G = nx.Graph()
G.add_edges_from(topology)
tlights = util.filter_to_list(lambda a: 'traffic_light' in a.type_id,
self.carla_world.get_actors())
junctions = carlautil.get_junctions_from_topology_graph(G)
tlight_distances = np.zeros((
len(tlights),
len(junctions),
))
f = lambda j: carlautil.location_to_ndarray(j.bounding_box.location)
junction_locations = util.map_to_ndarray(f, junctions)
g = lambda tl: carlautil.transform_to_location_ndarray(tl.
get_transform())
tlight_locations = util.map_to_ndarray(g, tlights)
for idx, junction in enumerate(junctions):
tlight_distances[:, idx] = np.linalg.norm(tlight_locations -
junction_locations[idx],
axis=1)
is_controlled_junction = (tlight_distances <
self.TLIGHT_FIND_RADIUS).any(axis=0)
is_uncontrolled_junction = np.logical_not(is_controlled_junction)
self.controlled_junction_locations \
= junction_locations[is_controlled_junction]
self.uncontrolled_junction_locations \
= junction_locations[is_uncontrolled_junction]
def get_normalized_sensor_data(lidar_measurement):
"""Obtain LIDAR point cloud with rotation oriented to
world frame of reference, and centered at (0, 0, 0).
First convert to world orientation which keeping the origin
using lidar_measurement.transform, mentioned in:
https://github.com/carla-simulator/carla/issues/2817
Rotate points 90 degrees CCW around origin (why?).
Adjust points 2.5 meters in the z direction to reflect
how the sensor is placed with resp. to the ego vehicle.
Lastly reflect the y-axis, mentioned in:
https://github.com/carla-simulator/carla/issues/2699
Parameters
----------
lidar_measurement : carla.LidarMeasurement or carla.SemanticLidarMeasurement
Returns
-------
np.array
Has shape (number of points, 3)
corresponding to (x, y, z) coordinates.
np.array
Labels of points. Has shape (number of points,)
TODO: still don't really understand what's going on with
the transformation matrix produced by transformation.
Check out open3d_lidar.py for fix
"""
raw_data = lidar_measurement.raw_data
if isinstance(lidar_measurement, carla.LidarMeasurement):
"""Array of 32-bits floats (XYZI of each point).
"""
points = np.frombuffer(raw_data, dtype=np.dtype('f4'))
points = np.reshape(points, (int(points.shape[0] / 4), 4))
labels = np.zeros(points.shape[0], dtype=np.uint32)
elif isinstance(lidar_measurement, carla.SemanticLidarMeasurement):
"""Array containing the point cloud with instance and semantic information.
For each point, four 32-bits floats are stored.
XYZ coordinates.
cosine of the incident angle.
Unsigned int containing the index of the object hit.
Unsigned int containing the semantic tag of the object it.
"""
data = np.frombuffer(raw_data,
dtype=np.dtype([('x', np.float32),
('y', np.float32),
('z', np.float32),
('CosAngle', np.float32),
('ObjIdx', np.uint32),
('ObjTag', np.uint32)]))
points = np.array([data['x'], data['y'], data['z'],
data['CosAngle']]).T
labels = data['ObjTag']
else:
raise ValueError("Unknown lidar measurement.")
transform_to_world = carla.Transform(carla.Location(),
lidar_measurement.transform.rotation)
transform_ccw = carla.Transform(carla.Location(), carla.Rotation(yaw=-90.))
matrix = np.array(transform_to_world.get_matrix())
points = np.dot(matrix, points.T).T
matrix = np.array(transform_ccw.get_matrix())
points = np.dot(matrix, points.T).T
points = points[:, :3] \
+ carlautil.location_to_ndarray(carla.Location(z=2.5))
points[:, 1] *= -1.0
return points, labels
def run_scene(self, scene_idx, spawn_point):
"""Sample frames from a scene where the car is placed on some spawn point.
"""
vehicle = None
pedestrian = None
camera = None
viewing_center = None
spectator = self.world.get_spectator()
# set vehicle blue prints
veh_blueprints = self.get_vehicle_blueprints()
blueprint = random.choice(veh_blueprints)
if blueprint.has_attribute("color"):
color = random.choice(
blueprint.get_attribute("color").recommended_values)
blueprint.set_attribute("color", color)
try:
# create a vehicle
has_vehicle = False
spawn_point = self.__get_random_spawn_point(scene_idx)
for _ in range(20):
_spawn_point = carlautil.carlacopy(spawn_point)
_spawn_point = self.__perturb_spawn_point(_spawn_point)
vehicle = self.world.try_spawn_actor(blueprint, _spawn_point)
if vehicle:
has_vehicle = True
spawn_point = _spawn_point
break
if not has_vehicle:
logging.warning("Failed to spawn vehicle with perturb at "
f"{carlautil.transform_to_str(spawn_point)}; "
"spawning vehicle without perturb.")
vehicle = self.world.spawn_actor(blueprint, spawn_point)
if self.show_demos:
self.world.wait_for_tick()
else:
self.world.tick()
vehicle.apply_control(carla.VehicleControl(hand_brake=True))
if self.add_pedestrian:
# reset the center of the scene
off_x = random.uniform(-self.max_shift, self.max_shift)
off_y = random.uniform(-self.max_shift, self.max_shift)
viewing_center = carla.Transform(
location=spawn_point.location +
carla.Vector3D(x=off_x, y=off_y),
rotation=spawn_point.rotation)
if self.debug:
self.__set_random_frame(spectator, viewing_center)
ped_blueprints = self.get_pedestrian_blueprints()
extent = vehicle.bounding_box.extent
r = vehicle.get_transform().rotation
f = carlautil.location_to_ndarray(
r.get_forward_vector()) * extent.x
r = carlautil.location_to_ndarray(
r.get_right_vector()) * extent.y
l = carlautil.location_to_ndarray(vehicle.get_location())
vertices = np.array(
(l + f + r, l - f + r, l - f - r, l + f - r))
vertices = util.npu.order_polytope_vertices(vertices[:, :2])
forbidden = shapely.geometry.Polygon(vertices)
has_pedestrian = False
spl = np.array(
[viewing_center.location.x, viewing_center.location.y])
box = np.array([[-1, -1], [1, -1], [1, 1], [-1, 1]
]) * self.ped_halfwidth
_box = None
for _ in range(20):
for _ in range(50):
off_x = random.uniform(-self.max_shift, self.max_shift)
off_y = random.uniform(-self.max_shift, self.max_shift)
_spl = spl + np.array([off_x, off_y])
_box = box + _spl
_poly = shapely.geometry.Polygon(_box)
intersection = forbidden.intersection(_poly)
if intersection.is_empty:
z = viewing_center.location.z + 0.8
yaw = math.degrees(random.uniform(0, 2 * math.pi))
_spawn_point = carla.Transform(
location=carla.Location(x=_spl[0],
y=_spl[1],
z=z),
rotation=carla.Rotation(yaw=yaw))
blueprint = random.choice(ped_blueprints)
if self.debug:
carlautil.debug_polytope_on_xy_plane(
self.world, vertices, z, life_time=5.)
carlautil.debug_polytope_on_xy_plane(
self.world, _box, z, life_time=5.)
pedestrian = self.world.try_spawn_actor(
blueprint, _spawn_point)
break
if pedestrian:
has_pedestrian = True
break
if not has_pedestrian:
logging.warning(
"Failed to spawn pedestrian at "
f"{carlautil.transform_to_str(viewing_center)}; "
"skipping pedestrian placement.")
else:
viewing_center = spawn_point
if self.show_demos:
self.world.wait_for_tick()
self.demo_azimuthal_rotation(self.world, spectator, vehicle)
self.demo_polar_rotation(spectator, vehicle)
self.demo_radius(spectator, vehicle)
self.demo_shift(spectator, vehicle)
self.demo_night_lights(spectator, vehicle)
self.demo_all_lights(spectator, vehicle)
else:
self.world.tick()
self.__set_random_weather()
# wait for car jiggle to stop
for _ in range(20):
self.world.tick()
self.__set_random_frame(
spectator,
viewing_center,
xy_shift_camera=not self.add_pedestrian)
self.world.tick()
camera, save_snapshot, scene_id = self.attach_camera_to_spectator(
scene_idx)
camera.listen(save_snapshot)
self.world.tick()
for jdx in range(self.n_frames):
self.__set_random_frame(
spectator,
viewing_center,
xy_shift_camera=not self.add_pedestrian)
self.world.tick()
camera.stop()
self.world.tick()
finally:
if vehicle:
vehicle.destroy()
if pedestrian:
pedestrian.destroy()
if camera:
camera.destroy()
if self.show_demos:
self.world.set_weather(self.original_weather)
self.world.wait_for_tick()
else:
self.world.tick()
time.sleep(2) # wait to avoid crashing