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