(self)
| 24 | self._parent_actor = parent_actor |
| 25 | |
| 26 | def get_observation(self): |
| 27 | ev_transform = self._parent_actor.vehicle.get_transform() |
| 28 | route_plan = self._parent_actor.route_plan |
| 29 | |
| 30 | # lateral_dist |
| 31 | waypoint, road_option = route_plan[0] |
| 32 | wp_transform = waypoint.transform |
| 33 | |
| 34 | d_vec = ev_transform.location - wp_transform.location |
| 35 | np_d_vec = np.array([d_vec.x, d_vec.y], dtype=np.float32) |
| 36 | wp_unit_forward = wp_transform.rotation.get_forward_vector() |
| 37 | np_wp_unit_right = np.array([-wp_unit_forward.y, wp_unit_forward.x], dtype=np.float32) |
| 38 | |
| 39 | lateral_dist = np.abs(np.dot(np_wp_unit_right, np_d_vec)) |
| 40 | lateral_dist = np.clip(lateral_dist, 0, 2) |
| 41 | |
| 42 | # angle_diff |
| 43 | angle_diff = np.deg2rad(np.abs(trans_utils.cast_angle(ev_transform.rotation.yaw - wp_transform.rotation.yaw))) |
| 44 | angle_diff = np.clip(angle_diff, -2, 2) |
| 45 | |
| 46 | # route_locs |
| 47 | location_list = [] |
| 48 | route_length = len(route_plan) |
| 49 | for i in range(self._route_steps): |
| 50 | if i < route_length: |
| 51 | waypoint, road_option = route_plan[i] |
| 52 | else: |
| 53 | waypoint, road_option = route_plan[-1] |
| 54 | |
| 55 | wp_location_world_coord = waypoint.transform.location |
| 56 | wp_location_actor_coord = trans_utils.loc_global_to_ref(wp_location_world_coord, ev_transform) |
| 57 | location_list += [wp_location_actor_coord.x, wp_location_actor_coord.y] |
| 58 | |
| 59 | # dist_remaining_in_km |
| 60 | dist_remaining_in_km = (self._parent_actor.route_length - self._parent_actor.route_completed) / 1000.0 |
| 61 | |
| 62 | obs = { |
| 63 | 'lateral_dist': np.array([lateral_dist], dtype=np.float32), |
| 64 | 'angle_diff': np.array([angle_diff], dtype=np.float32), |
| 65 | 'route_locs': np.array(location_list, dtype=np.float32), |
| 66 | 'dist_remaining': np.array([dist_remaining_in_km], dtype=np.float32) |
| 67 | } |
| 68 | return obs |
| 69 | |
| 70 | def clean(self): |
| 71 | self._parent_actor = None |
no test coverage detected