def _no_close_spawn(vehicles): vehicles = list(vehicles.values()) for c1, v1 in enumerate(vehicles): for c2 in range(c1 + 1, len(vehicles)): v2 = vehicles[c2] dis = norm(v1.position[0] - v2.position[0], v1.position[1] - v2.position[1]) assert distance_greater(v1.position, v2.position, length=2.2)
def observe(self, vehicle): num_others = self.config["lidar"]["num_others"] state = self.state_observe(vehicle) other_v_info = [] if vehicle.lidar.available: cloud_points, detected_objects = vehicle.lidar.perceive(vehicle) if self.config["lidar"]["num_others"] > 0: surrounding_vehicles = list( vehicle.lidar.get_surrounding_vehicles(detected_objects)) surrounding_vehicles.sort( key=lambda v: norm(vehicle.position[0] - v.position[0], vehicle.position[1] - v.position[1])) surrounding_vehicles += [None] * num_others for tmp_v in surrounding_vehicles[:num_others]: if tmp_v is not None: tmp_v = tmp_v other_v_info += self.state_observe(tmp_v).tolist() else: other_v_info += [0] * self.state_length other_v_info += self._add_noise_to_cloud_points( cloud_points, gaussian_noise=self.config["lidar"]["gaussian_noise"], dropout_prob=self.config["lidar"]["dropout_prob"]) self.cloud_points = cloud_points self.detected_objects = detected_objects return np.concatenate((state, np.asarray(other_v_info)))
def neighbour_vehicles(self, vehicle, lane_index: Tuple = None) -> Tuple: """ Find the preceding and following vehicles of a given vehicle. :param vehicle: the vehicle whose neighbours must be found :param lane_index: the lane on which to look for preceding and following vehicles. It doesn't have to be the current vehicle lane but can also be another lane, in which case the vehicle is projected on it considering its local coordinates in the lane. :return: its preceding vehicle, its following vehicle """ lane_index = lane_index or vehicle.lane_index if not lane_index: return None, None lane = self.map.road_network.get_lane(lane_index) s = self.map.road_network.get_lane(lane_index).local_coordinates(vehicle.position)[0] s_front = s_rear = None v_front = v_rear = None for v in self.vehicles: if norm(v.position[0] - vehicle.position[0], v.position[1] - vehicle.position[1]) > 100: # coarse filter continue if v is not vehicle: s_v, lat_v = lane.local_coordinates(v.position) if not lane.on_lane(v.position, s_v, lat_v, margin=1): continue if s <= s_v and (s_front is None or s_v <= s_front): s_front = s_v v_front = v if s_v < s and (s_rear is None or s_v > s_rear): s_rear = s_v v_rear = v return v_front, v_rear
def test_random_traffic(): env = PGDriveEnv({ "random_traffic": True, "traffic_mode": "respawn", "traffic_density": 0.3, "start_seed": 5, # "fast": True, "use_render": True }) has_traffic = False try: last_pos = None for i in range(20): obs = env.reset(force_seed=5) assert env.engine.traffic_manager.random_traffic new_pos = [ v.position for v in env.engine.traffic_manager.traffic_vehicles ] if len(new_pos) > 0: has_traffic = True if last_pos is not None and len(new_pos) == len(last_pos): assert sum([ norm(lastp[0] - newp[0], lastp[1] - newp[1]) >= 0.5 for lastp, newp in zip(last_pos, new_pos) ]), [(lastp, newp) for lastp, newp in zip(last_pos, new_pos)] last_pos = new_pos assert has_traffic finally: env.close()
def close_vehicles_to(self, vehicle, distance: float, count: int = None, see_behind: bool = True) -> object: """ Find the closest vehicles for IDM vehicles :param vehicle: IDM vehicle :param distance: How much distance :param count: Num of vehicles to return :param see_behind: Whether find vehicles behind this IDM vehicle or not :return: """ raise DeprecationWarning("This func is Deprecated") vehicles = [ v for v in self.vehicles if norm((v.position - vehicle.position)[0], (v.position - vehicle.position)[1]) < distance and v is not vehicle and (see_behind or -2 * vehicle.LENGTH < vehicle.lane_distance_to(v)) ] vehicles = sorted(vehicles, key=lambda v: abs(vehicle.lane_distance_to(v))) if count: vehicles = vehicles[:count] return vehicles
def _get_info_for_checkpoint(self, lanes_id, lanes, ego_vehicle, print_info=False): ref_lane = lanes[0] later_middle = (float(self.get_current_lane_num()) / 2 - 0.5) * self.get_current_lane_width() check_point = ref_lane.position(ref_lane.length, later_middle) if lanes_id == 0: # calculate ego v lane heading lanes_heading = ref_lane.heading_at( ref_lane.local_coordinates(ego_vehicle.position)[0]) else: lanes_heading = ref_lane.heading_at( min(self.PRE_NOTIFY_DIST, ref_lane.length)) dir_vec = check_point - ego_vehicle.position dir_norm = norm(dir_vec[0], dir_vec[1]) if dir_norm > self.NAVI_POINT_DIST: dir_vec = dir_vec / dir_norm * self.NAVI_POINT_DIST proj_heading, proj_side = ego_vehicle.projection(dir_vec) bendradius = 0.0 dir = 0.0 angle = 0.0 if isinstance(ref_lane, CircularLane): bendradius = ref_lane.radius / ( BlockParameterSpace.CURVE[Parameter.radius].max + self.get_current_lane_num() * self.get_current_lane_width()) dir = ref_lane.direction if dir == 1: angle = ref_lane.end_phase - ref_lane.start_phase elif dir == -1: angle = ref_lane.start_phase - ref_lane.end_phase ret = [] ret.append( clip((proj_heading / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0)) ret.append(clip((proj_side / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0)) ret.append(clip(bendradius, 0.0, 1.0)) ret.append(clip((dir + 1) / 2, 0.0, 1.0)) ret.append( clip((np.rad2deg(angle) / BlockParameterSpace.CURVE[Parameter.angle].max + 1) / 2, 0.0, 1.0)) # if print_info: # print("[2ND] Distance to this navigation: {:.3f}, Angle: {:.3f}. Return value: {}".format( # norm(proj_heading, proj_side), # np.rad2deg(np.arctan2(proj_side, proj_heading)), # ret # )) # else: # print("[1ND] Distance to this navigation: {:.3f}, Angle: {:.3f}. Return value: {}".format( # norm(proj_heading, proj_side), # np.rad2deg(np.arctan2(proj_side, proj_heading)), # ret # )) return ret, lanes_heading, check_point
def check_pos(vehicles): while vehicles: v_1 = vehicles[0] for v_2 in vehicles[1:]: v_1_pos = v_1.position v_2_pos = v_2.position assert norm( v_1_pos[0] - v_2_pos[0], v_1_pos[1] - v_2_pos[1] ) > v_1.WIDTH / 2 + v_2.WIDTH / 2, "Vehicles overlap after reset()" assert not v_1.crash_vehicle, "Vehicles overlap after reset()" vehicles.remove(v_1)
def check_pos(vehicles): while vehicles: v_1 = vehicles[0] for v_2 in vehicles[1:]: v_1_pos = v_1.position v_2_pos = v_2.position assert norm( v_1_pos[0] - v_2_pos[0], v_1_pos[1] - v_2_pos[1] ) > v_1.WIDTH / 2 + v_2.WIDTH / 2, "Vehicles overlap after reset()" if v_1.crash_vehicle: x = 1 raise ValueError("Vehicles overlap after reset()") vehicles.remove(v_1)
def test_fixed_traffic(): env = PGDriveEnv({ "random_traffic": False, "traffic_mode": "respawn", # "fast": True, "use_render": True }) try: last_pos = None for i in range(20): obs = env.reset() assert env.engine.traffic_manager.random_seed == env.current_seed new_pos = [v.position for v in env.engine.traffic_manager.vehicles] if last_pos is not None and len(new_pos) == len(last_pos): assert sum([ norm(lastp[0] - newp[0], lastp[1] - newp[1]) <= 1e-3 for lastp, newp in zip(last_pos, new_pos) ]), [(lastp, newp) for lastp, newp in zip(last_pos, new_pos)] last_pos = new_pos finally: env.close()
def _get_info_for_checkpoint(self, lanes_id, lanes, ego_vehicle): ref_lane = lanes[0] later_middle = (float(self.get_current_lane_num()) / 2 - 0.5) * self.get_current_lane_width() check_point = ref_lane.position(ref_lane.length, later_middle) if lanes_id == 0: # calculate ego v lane heading lanes_heading = ref_lane.heading_at( ref_lane.local_coordinates(ego_vehicle.position)[0]) else: lanes_heading = ref_lane.heading_at( min(self.PRE_NOTIFY_DIST, ref_lane.length)) dir_vec = check_point - ego_vehicle.position dir_norm = norm(dir_vec[0], dir_vec[1]) if dir_norm > self.NAVI_POINT_DIST: dir_vec = dir_vec / dir_norm * self.NAVI_POINT_DIST proj_heading, proj_side = ego_vehicle.projection(dir_vec) bendradius = 0.0 dir = 0.0 angle = 0.0 if isinstance(ref_lane, CircularLane): bendradius = ref_lane.radius / ( BlockParameterSpace.CURVE[Parameter.radius].max + self.get_current_lane_num() * self.get_current_lane_width()) dir = ref_lane.direction if dir == 1: angle = ref_lane.end_phase - ref_lane.start_phase elif dir == -1: angle = ref_lane.start_phase - ref_lane.end_phase return (clip((proj_heading / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0), clip((proj_side / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0), clip(bendradius, 0.0, 1.0), clip((dir + 1) / 2, 0.0, 1.0), clip((np.rad2deg(angle) / BlockParameterSpace.CURVE[Parameter.angle].max + 1) / 2, 0.0, 1.0)), lanes_heading, check_point
def vehicle_state(self, vehicle): """ Wrap vehicle states to list """ # update out of road info = [] if hasattr(vehicle, "side_detector") and vehicle.side_detector is not None: info += self._add_noise_to_cloud_points( vehicle.side_detector.get_cloud_points(), gaussian_noise=self.config["side_detector"]["gaussian_noise"], dropout_prob=self.config["side_detector"]["dropout_prob"]) else: pass # raise ValueError() # print("Current side detector min: {}, max: {}, mean: {}".format(min(info), max(info), np.mean(info))) # current_reference_lane = vehicle.routing_localization.current_ref_lanes[-1] if self.obs_mode in ["w_ego", "w_both"]: lateral_to_left, lateral_to_right, = vehicle.dist_to_left_side, vehicle.dist_to_right_side total_width = float( (vehicle.routing_localization.get_current_lane_num() + 1) * vehicle.routing_localization.get_current_lane_width()) lateral_to_left /= total_width lateral_to_right /= total_width info += [ clip(lateral_to_left, 0.0, 1.0), clip(lateral_to_right, 0.0, 1.0) ] current_reference_lane = vehicle.routing_localization.current_ref_lanes[ -1] info.append(vehicle.heading_diff(current_reference_lane)) _, lateral = vehicle.lane.local_coordinates(vehicle.position) info.append( clip((lateral * 2 / vehicle.routing_localization.get_current_lane_width() + 1.0) / 2.0, 0.0, 1.0)) info += [ # vehicle.heading_diff(current_reference_lane), # Note: speed can be negative denoting free fall. This happen when emergency brake. clip((vehicle.speed + 1) / (vehicle.max_speed + 1), 0.0, 1.0), clip((vehicle.throttle_brake + 1) / 2, 0.0, 1.0), clip((vehicle.steering / vehicle.max_steering + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][0] + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][1] + 1) / 2, 0.0, 1.0) ] heading_dir_last = vehicle.last_heading_dir heading_dir_now = vehicle.heading cos_beta = heading_dir_now.dot(heading_dir_last) / ( norm(*heading_dir_now) * norm(*heading_dir_last)) beta_diff = np.arccos(clip(cos_beta, 0.0, 1.0)) # print(beta) yaw_rate = beta_diff / 0.1 # print(yaw_rate) info.append(clip(yaw_rate, 0.0, 1.0)) if vehicle.lane_line_detector is not None: info += self._add_noise_to_cloud_points( vehicle.lane_line_detector.get_cloud_points(), gaussian_noise=self.config["lane_line_detector"] ["gaussian_noise"], dropout_prob=self.config["lane_line_detector"]["dropout_prob"]) return info
def points_lateral_direction(start_p, end_p): direction = (end_p - start_p) / norm((end_p - start_p)[0], (end_p - start_p)[1]) return np.array([-direction[1], direction[0]])
def points_direction(start_p, end_p): return (end_p - start_p) / norm((end_p - start_p)[0], (end_p - start_p)[1])
def points_distance(start_p, end_p): return norm((end_p - start_p)[0], (end_p - start_p)[1])