def useless_left_right_distance_printing():
# env = PGDriveEnvV2(dict(vehicle_config=dict(side_detector=dict(num_lasers=8))))
for steering in [-0.01, 0.01, -1, 1]:
# for distance in [10, 50, 100]:
env = PGDriveEnvV2(
dict(map="SSSSSSSSSSS",
vehicle_config=dict(
side_detector=dict(num_lasers=0, distance=50)),
use_render=False,
fast=True))
try:
for _ in range(100000000):
o, r, d, i = env.step([steering, 1])
vehicle = env.vehicle
l, r = 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())
print("Left {}, Right {}, Total {}. Clip Total {}".format(
l / total_width, r / total_width, (l + r) / total_width,
clip(l / total_width, 0, 1) + clip(r / total_width, 0, 1)))
if d:
break
finally:
env.close()
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 reward(self, action):
# Reward for moving forward in current lane
current_lane = self.vehicle.lane
long_last, _ = current_lane.local_coordinates(self.vehicle.last_position)
long_now, lateral_now = current_lane.local_coordinates(self.vehicle.position)
reward = 0.0
lateral_factor = clip(1 - 2 * abs(lateral_now) / self.current_map.lane_width, 0.0, 1.0)
reward += self.config["driving_reward"] * (long_now - long_last) * lateral_factor
# Penalty for frequent steering
steering_change = abs(self.vehicle.last_current_action[0][0] - self.vehicle.last_current_action[1][0])
steering_penalty = self.config["steering_penalty"] * steering_change * self.vehicle.speed / 20
reward -= steering_penalty
# Penalty for frequent acceleration / brake
acceleration_penalty = self.config["acceleration_penalty"] * ((action[1])**2)
reward -= acceleration_penalty
# Penalty for waiting
low_speed_penalty = 0
if self.vehicle.speed < 1:
low_speed_penalty = self.config["low_speed_penalty"] # encourage car
reward -= low_speed_penalty
reward -= self.config["general_penalty"]
reward += self.config["speed_reward"] * (self.vehicle.speed / self.vehicle.max_speed)
# if reward > 3.0:
# print("Stop here.")
return reward
def reward_function(self, vehicle_id):
"""
Override this func to get a new reward function
:param vehicle_id: id of BaseVehicle
:return: reward
"""
vehicle = self.vehicles[vehicle_id]
step_info = dict()
action = vehicle.last_current_action[1]
# Reward for moving forward in current lane
current_lane = vehicle.lane if vehicle.lane in vehicle.routing_localization.current_ref_lanes else \
vehicle.routing_localization.current_ref_lanes[0]
long_last, _ = current_lane.local_coordinates(vehicle.last_position)
long_now, lateral_now = current_lane.local_coordinates(
vehicle.position)
# reward for lane keeping, without it vehicle can learn to overtake but fail to keep in lane
reward = 0.0
lateral_factor = clip(
1 - 2 * abs(lateral_now) /
vehicle.routing_localization.get_current_lane_width(), 0.0, 1.0)
reward += self.config["driving_reward"] * (long_now -
long_last) * lateral_factor
# Penalty for frequent steering
steering_change = abs(vehicle.last_current_action[0][0] -
vehicle.last_current_action[1][0])
steering_penalty = self.config[
"steering_penalty"] * steering_change * vehicle.speed / 20
reward -= steering_penalty
# Penalty for frequent acceleration / brake
acceleration_penalty = self.config["acceleration_penalty"] * (
(action[1])**2)
reward -= acceleration_penalty
# Penalty for waiting
low_speed_penalty = 0
if vehicle.speed < 1:
low_speed_penalty = self.config[
"low_speed_penalty"] # encourage car
reward -= low_speed_penalty
reward -= self.config["general_penalty"]
reward += self.config["speed_reward"] * (vehicle.speed /
vehicle.max_speed)
step_info["step_reward"] = reward
# for done
if vehicle.arrive_destination:
reward += self.config["success_reward"]
elif vehicle.out_of_route:
reward -= self.config["out_of_road_penalty"]
elif vehicle.crash_vehicle:
reward -= self.config["crash_vehicle_penalty"]
elif vehicle.crash_object:
reward -= self.config["crash_object_penalty"]
return reward, step_info
def reward_function(self, vehicle_id: str):
"""
Override this func to get a new reward function
:param vehicle_id: id of BaseVehicle
:return: reward
"""
vehicle = self.vehicles[vehicle_id]
step_info = dict()
# Reward for moving forward in current lane
if vehicle.lane in vehicle.navigation.current_ref_lanes:
current_lane = vehicle.lane
else:
current_lane = vehicle.navigation.current_ref_lanes[0]
current_road = vehicle.current_road
long_last, _ = current_lane.local_coordinates(vehicle.last_position)
long_now, lateral_now = current_lane.local_coordinates(
vehicle.position)
# reward for lane keeping, without it vehicle can learn to overtake but fail to keep in lane
if self.config["use_lateral"]:
lateral_factor = clip(
1 - 2 * abs(lateral_now) /
vehicle.navigation.get_current_lane_width(), 0.0, 1.0)
else:
lateral_factor = 1.0
reward = 0.0
reward += self.config["driving_reward"] * (long_now -
long_last) * lateral_factor
if vehicle.current_road.block_ID() == TollGate.ID:
if vehicle.overspeed: # Too fast!
reward = -self.config[
"overspeed_penalty"] * vehicle.speed / vehicle.max_speed
else:
# Good! At very low speed
pass
else:
reward += self.config["speed_reward"] * (vehicle.speed /
vehicle.max_speed)
step_info["step_reward"] = reward
if vehicle.arrive_destination:
reward = +self.config["success_reward"]
elif self._is_out_of_road(vehicle):
reward = -self.config["out_of_road_penalty"]
elif vehicle.crash_vehicle:
reward = -self.config["crash_vehicle_penalty"]
elif vehicle.crash_object:
reward = -self.config["crash_object_penalty"]
return reward, step_info
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 draw_scene(self):
# Set the active area that can be modify to accelerate
assert len(
self.engine.agents
) == 1, "Don't support multi-agent top-down observation yet!"
vehicle = self.engine.agents[DEFAULT_AGENT]
pos = self.canvas_runtime.pos2pix(*vehicle.position)
clip_size = (int(self.obs_window.get_size()[0] * 1.1),
int(self.obs_window.get_size()[0] * 1.1))
# self._refresh(self.canvas_ego, pos, clip_size)
self._refresh(self.canvas_runtime, pos, clip_size)
self.canvas_past_pos.fill(COLOR_BLACK)
# self._draw_ego_vehicle()
# Draw vehicles
# TODO PZH: I hate computing these in pygame-related code!!!
ego_heading = vehicle.heading_theta
ego_heading = ego_heading if abs(ego_heading) > 2 * np.pi / 180 else 0
for v in self.engine.traffic_manager.vehicles:
if v is vehicle:
continue
h = v.heading_theta
h = h if abs(h) > 2 * np.pi / 180 else 0
VehicleGraphics.display(vehicle=v,
surface=self.canvas_runtime,
heading=h,
color=VehicleGraphics.BLUE)
raw_pos = vehicle.position
self.stack_past_pos.append(raw_pos)
for p_index in self._get_stack_indices(len(self.stack_past_pos)):
p_old = self.stack_past_pos[p_index]
diff = p_old - raw_pos
diff = (diff[0] * self.scaling, diff[1] * self.scaling)
# p = (p_old[0] - pos[0], p_old[1] - pos[1])
diff = (diff[1], diff[0])
p = pygame.math.Vector2(tuple(diff))
# p = pygame.math.Vector2(p)
p = p.rotate(np.rad2deg(ego_heading) + 90)
p = (p[1], p[0])
p = (clip(p[0] + self.resolution[0] / 2, -self.resolution[0],
self.resolution[0]),
clip(p[1] + self.resolution[1] / 2, -self.resolution[1],
self.resolution[1]))
# p = self.canvas_background.pos2pix(p[0], p[1])
self.canvas_past_pos.fill((255, 255, 255), (p, (1, 1)))
# pygame.draw.circle(self.canvas_past_pos, (255, 255, 255), p, radius=1)
ret = self.obs_window.render(
canvas_dict=dict(
road_network=self.canvas_road_network,
traffic_flow=self.canvas_runtime,
target_vehicle=self.canvas_ego,
# navigation=self.canvas_navigation,
),
position=pos,
heading=vehicle.heading_theta)
ret["past_pos"] = self.canvas_past_pos
return ret
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
