def generate_scene(cls, quads: List[List[QuadKey]],
others: List[PEMDynamicActor],
egos: List[PEMDynamicActor],
with_occupant: bool) -> PEMTrafficScene:
grid: PEMOccupancyGrid = PEMOccupancyGrid(cells=[])
idx: int = random.randint(0, len(egos) - 1)
ego: PEMDynamicActor = egos[idx]
quadkeys: List[QuadKey] = quads[idx]
for qk in quadkeys:
state: GridCellState = random.choice(STATES)
occupant: Union[PEMDynamicActor, None] = random.choice(
others
) if with_occupant and state == GridCellState.occupied() else None
grid.cells.append(
PEMGridCell(hash=qk.to_quadint(),
state=PEMRelation(cls.rand_prob(), state),
occupant=PEMRelation(cls.rand_prob(), occupant)))
scene: PEMTrafficScene = PEMTrafficScene(timestamp=time.time(),
measured_by=ego,
occupancy_grid=grid)
return scene
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())))
def _on_remote_graph(self, msg: bytes):
in_time: float = time.time()
try:
scene: PEMTrafficScene = PEMTrafficScene.from_bytes(msg)
obs: PEMTrafficSceneObservation = PEMTrafficSceneObservation(time.time(), scene, meta={'sender': int(self.ego_id)})
if self.alive and self.recording and self.remote_grid_sink:
if OBS_GRAPH_REMOTE not in self.remote_grid_sink.accumulator:
self.remote_grid_sink.push(OBS_GRAPH_REMOTE, [obs])
else:
self.remote_grid_sink.accumulator[OBS_GRAPH_REMOTE].append(obs)
self.timings.start('d6', custom_time=in_time)
self.timings.stop('d6')
except KeyError:
return
self.timings.start('d5', custom_time=scene.last_timestamp)
self.timings.stop('d5', custom_time=in_time)
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(),
state=PEMRelation(confidence=.5,
object=GridCellState.occupied())),
PEMGridCell(hash=REF_TILE_1_2.to_quadint(),
state=PEMRelation(confidence=.5,
object=GridCellState.free()))
])),
meta={
'sender': 1,
'parent': REF_PARENT_1
}),
PEMTrafficSceneObservation(timestamp=REF_TIME_1 + 2.0,
scene=PEMTrafficScene(),
meta={
'sender': 1,
'parent': REF_PARENT_1
})
def on_graph(message: bytes):
try:
graph: PEMTrafficScene = PEMTrafficScene.from_bytes(message)
graph_queue.append(graph2json(graph))
except:
print('Failed to parse graph.')
def _on_grid(self, obs: OccupancyGridObservation):
_ts = time.monotonic()
ts1, grid = obs.timestamp, obs.value
actors_ego_obs = self.om.latest(OBS_ACTOR_EGO)
actors_others_obs = self.om.latest(OBS_ACTORS_RAW)
if not grid or not actors_ego_obs or not actors_others_obs or not actors_ego_obs.value or len(actors_ego_obs.value) < 1:
return
ts: float = min([ts1, actors_ego_obs.timestamp, actors_others_obs.timestamp])
self.timings.start('d0', custom_time=ts)
self.timings.stop('d0')
ts2: float = time.time()
ego_actor: DynamicActor = actors_ego_obs.value[0]
# Turned off for evaluation, because occupants are not fused anyway
# visible_actors: Dict[str, DynamicActor] = get_occupied_cells_multi_map(actors_others_obs.value + [ego_actor])
visible_actors: Dict[str, DynamicActor] = {}
# Generate PEM complex object attributes
pem_ego = map_pem_actor(ego_actor)
pem_grid = PEMOccupancyGrid(cells=[])
for cell in grid.cells:
group_key = f'cell_occupant_{cell.quad_key.key}'
state_relation: PEMRelation[GridCellState] = PEMRelation(cell.state.confidence, GridCellState(cell.state.value))
occupant_relation: PEMRelation[Optional[PEMDynamicActor]] = PEMRelation(0, None)
if cell.quad_key.key in visible_actors:
actor = visible_actors[cell.quad_key.key]
self.tracker.track(group_key, str(actor.id))
occupant_relation = PEMRelation(self.tracker.get(group_key, str(actor.id)), map_pem_actor(actor))
else:
occupant_relation = PEMRelation(cell.state.confidence, None)
self.tracker.cycle_group(group_key)
# Consistency between state and occupant presence
if occupant_relation.object and state_relation.object != GridCellState.occupied():
if occupant_relation.confidence > state_relation.confidence:
state_relation = PEMRelation(occupant_relation.confidence, GridCellState.occupied())
else:
occupant_relation = PEMRelation(state_relation.confidence, None)
pem_grid.cells.append(PEMGridCell(
hash=self._get_quadint(cell.quad_key),
state=state_relation,
occupant=occupant_relation
))
now = time.time()
# Generate PEM graph
graph = PEMTrafficScene(timestamp=ts,
last_timestamp=now,
measured_by=pem_ego,
occupancy_grid=pem_grid)
encoded_msg = graph.to_bytes()
self.timings.start('d1', custom_time=ts2)
self.timings.stop('d1')
obs: PEMTrafficSceneObservation = PEMTrafficSceneObservation(now, graph, meta={'sender': int(self.ego_id)})
self.inbound.publish(OBS_GRAPH_LOCAL, obs)
if self.alive and self.recording and self.local_grid_sink:
if OBS_GRAPH_LOCAL not in self.local_grid_sink.accumulator:
self.local_grid_sink.push(OBS_GRAPH_LOCAL, [obs])
else:
self.local_grid_sink.accumulator[OBS_GRAPH_LOCAL].append(obs)
if self.tss.active:
self.tss.publish_graph(encoded_msg)
# Debug logging
self.tsdiffhistory3.append(time.monotonic() - self.last_publish)
self.last_publish = time.monotonic()
# logging.debug(f'PUBLISH: {np.mean(self.tsdiffhistory3)}')
self.tsdiffhistory2.append(time.monotonic() - _ts)
def get_empty_observation(with_ts: float) -> PEMTrafficSceneObservation:
return PEMTrafficSceneObservation(
timestamp=with_ts,
scene=PEMTrafficScene(occupancy_grid=PEMOccupancyGrid(cells=[])),
meta={'empty': True})
def read_data(
self,
downsample_ground_truth: float = 1.,
downsample_observations: float = 1.
) -> Tuple[List[PEMTrafficSceneObservation],
List[PEMTrafficSceneObservation], List[Ogtc]]:
assert downsample_ground_truth <= 1 and downsample_observations <= 1
logging.debug(
f'downsample_ground_truth={downsample_ground_truth}, downsample_observations={downsample_observations}'
)
logging.debug('Reading directory info.')
files_actual: List[str] = list(
filter(lambda s: s.startswith(self.file_prefix),
os.listdir(self.data_dir_actual)))
files_observed: List[str] = list(
filter(lambda s: s.startswith(self.file_prefix),
os.listdir(self.data_dir_observed)))
occupancy_ground_truth: List[Ogtc] = []
occupancy_observations_local: List[PEMTrafficSceneObservation] = []
occupancy_observations_remote: List[PEMTrafficSceneObservation] = []
logging.debug('Reading ground truth.')
for file_name in files_actual:
with open(os.path.join(self.data_dir_actual, file_name),
'rb') as f:
try:
occupancy_ground_truth += pickle.load(f)
except EOFError:
logging.warning(f'File {file_name} corrupt.')
logging.debug(
f'Got {len(occupancy_ground_truth)} ground truth data points.')
logging.debug(f'Reading and decoding observations.')
for file_name in files_observed:
with open(os.path.join(self.data_dir_observed, file_name),
'rb') as f:
try:
if 'remote' not in file_name:
occupancy_observations_local.extend(pickle.load(f))
else:
data = pickle.load(f)
assert isinstance(data, list)
if len(data) < 1:
continue
if isinstance(data[0], RawBytesObservation):
for obs in data:
try:
occupancy_observations_remote.append(
PEMTrafficSceneObservation(
timestamp=obs.timestamp,
scene=PEMTrafficScene.from_bytes(
obs.value),
meta=obs.meta))
except KeyError:
continue
elif isinstance(data[0], PEMTrafficSceneObservation):
occupancy_observations_remote.extend(data)
except EOFError:
logging.warning(f'File {file_name} corrupt.')
logging.debug(
f'Got {len(occupancy_observations_local)} local and {len(occupancy_observations_remote)} remote observations.'
)
occupancy_ground_truth = sorted(random.sample(
occupancy_ground_truth,
k=math.floor(
len(occupancy_ground_truth) * downsample_ground_truth)),
key=attrgetter('ts'))
occupancy_observations_local = sorted(random.sample(
occupancy_observations_local,
k=math.floor(
len(occupancy_observations_local) * downsample_observations)),
key=attrgetter('timestamp'))
occupancy_observations_remote = sorted(random.sample(
occupancy_observations_remote,
k=math.floor(
len(occupancy_observations_remote) * downsample_observations)),
key=attrgetter('timestamp'))
return occupancy_observations_local, occupancy_observations_remote, occupancy_ground_truth
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)
PEMRelation[GridCellState](confidence=0.65,
object=GridCellState.occupied()),
'occupant':
PEMRelation[PEMDynamicActor](confidence=0.91, object=ego2)
}),
PEMGridCell(
**{
'hash':
2222,
'state':
PEMRelation[GridCellState](confidence=0.88,
object=GridCellState.free()),
'occupant':
None
})
]
grid1.cells = cells
scene1: PEMTrafficScene = PEMTrafficScene()
scene1.occupancy_grid = grid1
scene1.measured_by = ego1
encoded_msg: bytes = scene1.to_bytes()
scene1: PEMTrafficScene = PEMTrafficScene.from_bytes(encoded_msg)
print(scene1.measured_by.id)
print(scene1.measured_by.position.object.x)
print(scene1.occupancy_grid.cells[0].state.confidence)
print(scene1.occupancy_grid.cells[0].state.object)
print(scene1.occupancy_grid.cells[0].occupant.object.id)