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])
