def _quadkey_to_box(qk: QuadKey) -> List[Point2D]:
return [
Point2D(*(qk.to_pixel(TileAnchor.ANCHOR_NW))),
Point2D(*(qk.to_pixel(TileAnchor.ANCHOR_NE))),
Point2D(*(qk.to_pixel(TileAnchor.ANCHOR_SW))),
Point2D(*(qk.to_pixel(TileAnchor.ANCHOR_SE)))
]
def run(args=sys.argv[1:]):
argparser = argparse.ArgumentParser(description='TalkyCars Grid Collector')
argparser.add_argument('--rate',
'-r',
default=10,
type=int,
help='Tick Rate')
argparser.add_argument('--host',
default='127.0.0.1',
help='IP of the host server (default: 127.0.0.1)')
argparser.add_argument('-p',
'--port',
default=2000,
type=int,
help='TCP port to listen to (default: 2000)')
argparser.add_argument('-o',
'--out_dir',
default=os.path.join(data_dir(), EVAL2_DATA_DIR),
type=str,
help='Directory to dump data to')
args, _ = argparser.parse_known_args(args)
# Initialize Carla client
client = carla.Client(args.host, args.port)
client.set_timeout(CARLA_CONNECT_TIMEOUT)
GridCollector(carla_client=client,
base_tile=QuadKey(EVAL2_BASE_KEY),
ego_prefix=SCENE2_EGO_PREFIX,
rate=args.rate,
data_dir=os.path.normpath(
os.path.join(os.path.dirname(__file__),
args.out_dir))).start()
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 push_ground_truth(self):
occupied_cells: Dict[str, Set[QuadKey]] = self.split_by_level(
self.fetch_occupied_cells(), level=REMOTE_GRID_TILE_LEVEL)
now: float = time.time()
for tile, cells in occupied_cells.items():
self.ground_truth_buffer.append(
OccupancyGroundTruthContainer(occupied_cells=frozenset(cells),
tile=QuadKey(tile),
ts=now))
def split_by_level(
cls, observations: List[PEMTrafficSceneObservation]
) -> List[PEMTrafficSceneObservation]:
scenes: Dict[str, List[PEMTrafficSceneObservation]] = {
} # parent tile to scene
for o in observations:
contained_parents: Set[str] = set()
for cell in o.value.occupancy_grid.cells:
parent_str: str = cls.cache_get(
cell.hash)[:REMOTE_GRID_TILE_LEVEL]
contained_parents.add(parent_str)
if parent_str not in scenes:
scenes[parent_str] = []
for parent in contained_parents:
scenes[parent].append(
PEMTrafficSceneObservation(
timestamp=o.timestamp,
scene=PEMTrafficScene(
**{
'timestamp':
o.value.timestamp,
'min_timestamp':
o.value.min_timestamp,
'max_timestamp':
o.value.max_timestamp,
'occupancy_grid':
PEMOccupancyGrid(
**{
'cells': [
c for c in
o.value.occupancy_grid.cells
if cls.cache_get(
c.hash).startswith(parent)
]
})
}),
meta={
'parent': QuadKey(parent),
**o.meta
}))
return list(itertools.chain(*list(scenes.values())))
from typing import List, Tuple, Set
from pyquadkey2.quadkey import QuadKey
from common.observation import PEMTrafficSceneObservation
from common.serialization.schema import GridCellState
from common.serialization.schema.base import PEMTrafficScene
from common.serialization.schema.occupancy import PEMOccupancyGrid, PEMGridCell
from common.serialization.schema.relation import PEMRelation
from evaluation.perception import OccupancyGroundTruthContainer as Ogtc
from evaluation.perception.grid_evaluator import GridEvaluator, State5Tuple
REF_TIME_1: float = 1573220495.8997931
REF_PARENT_1: QuadKey = QuadKey('120203233')
REF_TILE_1_1: QuadKey = QuadKey('1202032330')
REF_TILE_1_2: QuadKey = QuadKey('1202032331')
MOCK_ACTUAL_1: List[Ogtc] = [
Ogtc(tile=REF_PARENT_1,
occupied_cells=frozenset([REF_TILE_1_1]),
ts=REF_TIME_1)
]
MOCK_LOCAL_1: List[PEMTrafficSceneObservation] = [
PEMTrafficSceneObservation(
timestamp=REF_TIME_1 + 1.0,
scene=PEMTrafficScene(
timestamp=REF_TIME_1 + 0.15,
occupancy_grid=PEMOccupancyGrid(cells=[
PEMGridCell(
hash=REF_TILE_1_1.to_quadint(),
def _get_quadint(self, qk: QuadKey) -> int:
if qk.key not in self.quadint_cache:
self.quadint_cache[qk.key] = qk.to_quadint()
return self.quadint_cache[qk.key]
def cache_get_inverse(quadstr: str) -> int:
if quadstr not in quad_int_lookup:
quad_int_lookup[quadstr] = QuadKey(quadstr).to_quadint()
return quad_int_lookup[quadstr]
def fuse(cls, local_observation: PEMTrafficSceneObservation,
remote_observation: PEMTrafficSceneObservation,
ref_time: float) -> PEMTrafficSceneObservation:
assert local_observation.meta['parent'] == remote_observation.meta[
'parent']
cells: List[PEMGridCell] = []
ts1: float = local_observation.value.timestamp
ts2: float = remote_observation.value.min_timestamp + (
remote_observation.value.max_timestamp -
remote_observation.value.min_timestamp) / 2
weights: Tuple[float, float] = (cls._decay(ts1, ref_time),
cls._decay(ts2, ref_time))
states: List[GridCellState] = GridCellState.options()
local_cells: Dict[QuadKey, PEMGridCell] = {
QuadKey(cls.cache_get(c.hash)): c
for c in local_observation.value.occupancy_grid.cells
}
remote_cells: Dict[QuadKey, PEMGridCell] = {
QuadKey(cls.cache_get(c.hash)): c
for c in remote_observation.value.occupancy_grid.cells
}
keys: Set[QuadKey] = set().union(set(local_cells.keys()),
set(remote_cells.keys()))
for qk in keys:
if qk in local_cells and qk not in remote_cells:
cells.append(local_cells[qk])
elif qk not in local_cells and qk in remote_cells:
cells.append(remote_cells[qk])
else:
weight_sum: float = sum(weights)
new_cell: PEMGridCell = PEMGridCell(hash=local_cells[qk].hash)
state_vec: List[float] = [0] * GridCellState.N
for i in range(len(state_vec)):
v1: float = local_cells[qk].state.confidence * weights[
0] if local_cells[qk].state.object == states[i] else 0
v2: float = remote_cells[qk].state.confidence * weights[
1] if remote_cells[qk].state.object == states[i] else 0
# Override unknown
if i == Gss.UNKNOWN and sum(state_vec[:Gss.UNKNOWN]) > 0:
if v1 > 0:
v1 = 0
weight_sum -= weights[0]
elif v2 > 0:
v2 = 0
weight_sum -= weights[1]
state_vec[i] = v1 + v2
state_vec = [c / weight_sum for c in state_vec]
max_conf: float = max(state_vec)
max_state: int = state_vec.index(max_conf)
new_cell.state = PEMRelation(
object=GridCellState(value=Gss(max_state)),
confidence=max_conf)
cells.append(new_cell)
return PEMTrafficSceneObservation(
timestamp=ref_time, # shouldn't matter
scene=PEMTrafficScene(
**{'occupancy_grid': PEMOccupancyGrid(cells=cells)}),
meta=local_observation.meta)
def __init__(self,
client: carla.Client,
strategy: EgoStrategy = None,
name: str = 'hero',
render: bool = False,
debug: bool = False,
record: bool = False,
is_standalone: bool = False,
grid_radius: float = OCCUPANCY_RADIUS_DEFAULT,
lidar_angle: float = LIDAR_ANGLE_DEFAULT):
self.killer: GracefulKiller = GracefulKiller(
) if is_standalone else None
self.sim: carla.Client = client
self.client: TalkyClient = None
self.world: carla.World = client.get_world()
self.map: carla.Map = self.world.get_map()
self.alive: bool = True
self.name: str = name
self.vehicle: carla.Vehicle = None
self.player: carla.Vehicle = None # for compatibility
self.grid: Grid = None
self.hud: HUD = None
self.strategy: EgoStrategy = strategy
self.debug: bool = debug
self.record: bool = record
self.n_ticked: int = 0
self.display = None
self.sensor_tick_pool: ThreadPool = ThreadPool(processes=6)
self.sensors: Dict[str, carla.Sensor] = {
'gnss': None,
'lidar': None,
'camera_rgb': None,
'position': None,
}
# Initialize visual stuff
if render:
pygame.init()
pygame.display.set_caption(f'PyGame – {name}')
pygame.font.init()
self.display = pygame.display.set_mode(
(RES_X, RES_Y), pygame.HWSURFACE | pygame.DOUBLEBUF)
self.hud = HUD(RES_X, RES_X)
# Initialize strategy
if not self.strategy:
self.strategy = ManualEgoStrategy(
) if render else EmptyEgoStrategy()
self.strategy.init(self)
# Initialize callbacks
if self.hud:
self.world.on_tick(self.hud.on_world_tick)
# Initialize player vehicle
self.vehicle = self.strategy.player
self.player = self.vehicle
# Initialize Talky Client
self.client = TalkyClient(for_subject_id=self.vehicle.id,
dialect=ClientDialect.CARLA)
self.client.gm.radius = grid_radius
# Initialize sensors
grid_range = grid_radius * QuadKey('0' * OCCUPANCY_TILE_LEVEL).side()
lidar_min_range = (grid_range + .5) / math.cos(
math.radians(lidar_angle))
lidar_range = min(
LIDAR_MAX_RANGE,
max(lidar_min_range,
LIDAR_Z_OFFSET / math.sin(math.radians(lidar_angle))) * 2)
self.sensors['gnss'] = GnssSensor(self.vehicle,
self.client,
offset_z=GNSS_Z_OFFSET)
self.sensors['lidar'] = LidarSensor(self.vehicle,
self.client,
offset_z=LIDAR_Z_OFFSET,
range=lidar_range,
angle=lidar_angle)
self.sensors['actors'] = ActorsSensor(
self.vehicle, self.client,
with_noise=True) # Set to false for evaluation
self.sensors['position'] = PositionSensor(self.vehicle, self.client)
if render:
self.sensors['camera_rgb'] = CameraRGBSensor(
self.vehicle, self.hud)
# Initialize subscriptions
def on_grid(grid: OccupancyGridObservation):
self.grid = grid.value
self.client.outbound.subscribe(OBS_GRID_LOCAL, on_grid)
if is_standalone:
lock = Lock()
clock = pygame.time.Clock()
def on_tick(*args):
if lock.locked() or self.killer.kill_now:
return
lock.acquire()
clock.tick_busy_loop(60)
if self.tick(clock):
self.killer.kill_now = True
lock.release()
self.world.on_tick(on_tick)
while True:
if self.killer.kill_now:
try:
self.destroy()
return
finally:
return
try:
self.world.wait_for_tick()
except RuntimeError:
self.killer.kill_now = True
continue