| 36 | |
| 37 | |
| 38 | class ObsManager(): |
| 39 | def __init__(self, obs_configs, criteria_stop=None): |
| 40 | self._width = int(obs_configs['width_in_pixels']) |
| 41 | self._pixels_ev_to_bottom = obs_configs['pixels_ev_to_bottom'] |
| 42 | self._pixels_per_meter = obs_configs['pixels_per_meter'] |
| 43 | self._history_idx = obs_configs['history_idx'] |
| 44 | self._scale_bbox = obs_configs.get('scale_bbox', True) |
| 45 | self._scale_mask_col = obs_configs.get('scale_mask_col', 1.1) |
| 46 | |
| 47 | self._history_queue = deque(maxlen=20) |
| 48 | |
| 49 | self._image_channels = 3 |
| 50 | self._masks_channels = 3 + 3*len(self._history_idx) |
| 51 | self._parent_actor = None |
| 52 | self._world = None |
| 53 | |
| 54 | self._map_dir = Path(__file__).resolve().parent / 'maps' |
| 55 | |
| 56 | self._criteria_stop =criteria_stop |
| 57 | |
| 58 | super(ObsManager, self).__init__() |
| 59 | |
| 60 | def attach_ego_vehicle(self, ego_vehicle): |
| 61 | self._parent_actor = ego_vehicle |
| 62 | self._world = self._parent_actor.get_world() |
| 63 | |
| 64 | maps_h5_path = self._map_dir / (self._world.get_map().name + '.h5') |
| 65 | with h5py.File(maps_h5_path, 'r', libver='latest', swmr=True) as hf: |
| 66 | self._road = np.array(hf['road'], dtype=np.uint8) |
| 67 | self._lane_marking_all = np.array(hf['lane_marking_all'], dtype=np.uint8) |
| 68 | self._lane_marking_white_broken = np.array(hf['lane_marking_white_broken'], dtype=np.uint8) |
| 69 | |
| 70 | self._world_offset = np.array(hf.attrs['world_offset_in_meters'], dtype=np.float32) |
| 71 | assert np.isclose(self._pixels_per_meter, float(hf.attrs['pixels_per_meter'])) |
| 72 | |
| 73 | self._distance_threshold = np.ceil(self._width / self._pixels_per_meter) |
| 74 | |
| 75 | @staticmethod |
| 76 | def _get_stops(criteria_stop): |
| 77 | stop_sign = criteria_stop._target_stop_sign |
| 78 | stops = [] |
| 79 | if (stop_sign is not None) and (not criteria_stop._stop_completed): |
| 80 | bb_loc = carla.Location(stop_sign.trigger_volume.location) |
| 81 | bb_ext = carla.Vector3D(stop_sign.trigger_volume.extent) |
| 82 | bb_ext.x = max(bb_ext.x, bb_ext.y) |
| 83 | bb_ext.y = max(bb_ext.x, bb_ext.y) |
| 84 | trans = stop_sign.get_transform() |
| 85 | stops = [(carla.Transform(trans.location, trans.rotation), bb_loc, bb_ext)] |
| 86 | return stops |
| 87 | |
| 88 | def get_observation(self, route_plan): |
| 89 | ev_transform = self._parent_actor.get_transform() |
| 90 | ev_loc = ev_transform.location |
| 91 | ev_rot = ev_transform.rotation |
| 92 | ev_bbox = self._parent_actor.bounding_box |
| 93 | |
| 94 | def is_within_distance(w): |
| 95 | c_distance = abs(ev_loc.x - w.location.x) < self._distance_threshold \ |