def main():
try:
_client = carla.Client('localhost', 2000)
_client.set_timeout(CARLA_CONNECT_TIMEOUT)
for m in MAPS:
_world = _client.load_world(m)
_map = _world.get_map()
time.sleep(1)
spawn_points = list(
map(attrgetter('location'), _map.get_spawn_points()))
point_nw = min(spawn_points, key=lambda p: abs(p.x + p.y))
point_se = max(spawn_points, key=lambda p: abs(p.x + p.y))
coord_nw = _map.transform_to_geolocation(point_nw)
coord_se = _map.transform_to_geolocation(point_se)
qk_nw = quadkey.from_geo((coord_nw.latitude, coord_nw.longitude),
OCCUPANCY_TILE_LEVEL)
qk_se = quadkey.from_geo((coord_se.latitude, coord_se.longitude),
OCCUPANCY_TILE_LEVEL)
parent = quadkey.from_str(commonprefix([qk_nw.key, qk_se.key]))
logging.info(f'{m}\n------')
logging.info(f'NW @ {OCCUPANCY_TILE_LEVEL}: {qk_nw.key}')
logging.info(f'SE @ {OCCUPANCY_TILE_LEVEL}: {qk_se.key}')
logging.info(f'Containing Tile @ {parent.level}: {parent.key}\n')
except Exception as e:
logging.error(e)
def run(self):
self.init_egos()
self.init_others()
self.init_quad_keys()
self.init_msgs()
self.tss.update_position(
quadkey.from_geo(self.start_location[:2], self.grid_tile_level))
self.rate_count_thread.start()
time.sleep(1)
self.start_time = time.time()
while True:
msg = random.choice(self.gen_msgs)
self.tss.publish_graph(msg)
with self.lock:
self.msg_count += 1
self.bytes_count += len(msg)
if self.last_tick >= 0 and time.monotonic(
) - self.last_tick < 1 / self.rate:
time.sleep(
max(
0.0, 1 / (self.rate * self.n_sample_egos) -
(time.monotonic() - self.last_tick)))
self.last_tick = time.monotonic()
def tick(self, ego, clock):
if not self._show_info:
return
t = ego.vehicle.get_transform()
v = ego.vehicle.get_velocity()
c = ego.vehicle.get_control()
heading = 'N' if abs(t.rotation.yaw) < 89.5 else ''
heading += 'S' if abs(t.rotation.yaw) > 90.5 else ''
heading += 'E' if 179.5 > t.rotation.yaw > 0.5 else ''
heading += 'W' if -0.5 > t.rotation.yaw > -179.5 else ''
vehicles = ego.world.get_actors().filter('vehicle.*')
qk = quadkey.from_geo(
(ego.sensors['gnss'].lat, ego.sensors['gnss'].lon),
OCCUPANCY_TILE_LEVEL)
self._info_text = [
'Server: % 16.0f FPS' % self.server_fps,
'Client: % 16.0f FPS' % clock.get_fps(), '',
'Vehicle: % 20s' %
get_actor_display_name(ego.vehicle, truncate=20),
'Map: % 20s' % ego.map.name,
'Simulation time: % 12s' %
datetime.timedelta(seconds=int(self.simulation_time)), '',
'Speed: % 15.0f km/h' %
(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)),
'Heading: % 16.0f\N{DEGREE SIGN} % 2s' %
(t.rotation.yaw, heading),
'Location: % 20s' % ('(% 5.1f, % 5.1f)' %
(t.location.x, t.location.y)),
'Tile: % 24s' % qk.key,
'GNSS: % 20s' %
('(%2.6f, % 3.6f)' %
(ego.sensors['gnss'].lat, ego.sensors['gnss'].lon)),
'Height: % 18.0f m' % t.location.z, ''
]
if isinstance(c, carla.VehicleControl):
self._info_text += [('Throttle:', c.throttle, 0.0, 1.0),
('Steer:', c.steer, -1.0, 1.0),
('Brake:', c.brake, 0.0, 1.0),
('Reverse:', c.reverse),
('Hand brake:', c.hand_brake),
('Manual:', c.manual_gear_shift),
'Gear: %s' % {
-1: 'R',
0: 'N'
}.get(c.gear, c.gear)]
elif isinstance(c, carla.WalkerControl):
self._info_text += [('Speed:', c.speed, 0.0, 5.556),
('Jump:', c.jump)]
if len(vehicles) > 1:
self._info_text += ['Nearby vehicles:']
distance = lambda l: math.sqrt((l.x - t.location.x)**2 +
(l.y - t.location.y)**2 +
(l.z - t.location.z)**2)
vehicles = [(distance(x.get_location()), x) for x in vehicles
if x.id != ego.vehicle.id]
for d, vehicle in sorted(vehicles):
if d > 200.0:
break
vehicle_type = get_actor_display_name(vehicle, truncate=22)
self._info_text.append('% 4dm %s' % (d, vehicle_type))
def init_quad_keys(self):
self.gen_quads_pool = []
for ego in self.gen_ego_pool:
center: QuadKey = quadkey.from_geo(
ego.position.object.as_tuple()[:2], self.grid_tile_level)
self.gen_quads_pool.append(
[QuadKey(k) for k in center.nearby(self.grid_radius)])
def create_and_spawn(self):
self.init()
# Create egos
for i in range(common.constants.SCENE2_N_EGOS):
logging.info(f'Creating ego {i + 1} of {common.constants.SCENE2_N_EGOS}.')
ego: Ego = Ego(self._sim,
strategy=RandomPathEgoStrategy(i, self._waypoint_provider),
name=f'{SCENE2_EGO_PREFIX}_{i}',
render=i == 0,
debug=False,
record=False)
self._egos.append(ego)
# Print info
for a in self.egos:
strat: RandomPathEgoStrategy = cast(RandomPathEgoStrategy, a.strategy)
geo_start: carla.GeoLocation = self._map.transform_to_geolocation(strat.point_start.location)
geo_end: carla.GeoLocation = self._map.transform_to_geolocation(strat.point_end.location)
qk_start: QuadKey = quadkey.from_geo((geo_start.latitude, geo_start.longitude), REMOTE_GRID_TILE_LEVEL)
qk_end: QuadKey = quadkey.from_geo((geo_end.latitude, geo_end.longitude), REMOTE_GRID_TILE_LEVEL)
logging.info(f'{a.name} will be driving from {strat.point_start.location} ({qk_start.key}) to {strat.point_end.location} ({qk_end.key}).')
super().create_and_spawn()
def get_tile_list(ved_db):
"""
Retrieves the tile list corresponding to the unique
locations in the VED database
:param ved_db: The VED database
:return: List of tiles encoded as quadkeys
"""
tiles = dict()
with get_unique_locations(ved_db) as locations:
for p in tqdm(locations):
# qk is a Quadkey object
qk = quadkey.from_geo((p[0], p[1]), level=26)
tile = qk.key[:14]
if tile in tiles:
tiles[tile].append(qk)
else:
tiles[tile] = [qk]
tile_list = [(k, v) for k, v in tiles.items()]
return tile_list
def get_occupied_cells(for_actor: DynamicActor) -> FrozenSet[QuadKey]:
qks: List[QuadKey] = list(
map(lambda c: quadkey.from_geo(c.components(), OCCUPANCY_TILE_LEVEL),
for_actor.props.bbox.value))
return frozenset(quadkey.QuadKey.bbox(qks))
def to_quadkey(self, level=20) -> quadkey.QuadKey:
return quadkey.from_geo(self.value[:2], level)