def act(self, action: Union[dict, str] = None):
"""
Execute an action.
For now, no action is supported because the vehicle takes all decisions
of acceleration and lane changes on its own, based on the IDM and MOBIL models.
:param action: the action
"""
if self.crashed:
return
action = {}
front_vehicle, rear_vehicle = self.traffic_mgr.neighbour_vehicles(self)
# Lateral: MOBIL
self.follow_road()
if self.enable_lane_change:
self.change_lane_policy()
action['steering'] = self.steering_control(self.target_lane_index)
action['steering'] = clip(action['steering'], -self.MAX_STEERING_ANGLE,
self.MAX_STEERING_ANGLE)
# Longitudinal: IDM
action['acceleration'] = self.acceleration(ego_vehicle=self,
front_vehicle=front_vehicle,
rear_vehicle=rear_vehicle)
# action['acceleration'] = self.recover_from_stop(action['acceleration'])
action['acceleration'] = clip(action['acceleration'], -self.ACC_MAX,
self.ACC_MAX)
Vehicle.act(
self, action
) # Skip ControlledVehicle.act(), or the command will be override.
def vehicle_state(self, vehicle):
"""
Wrap vehicle states to list
"""
# update out of road
info = []
if hasattr(vehicle,
"side_detector") and vehicle.side_detector.available:
info += vehicle.side_detector.perceive(
vehicle,
vehicle.engine.physics_world.static_world).cloud_points
else:
lateral_to_left, lateral_to_right, = vehicle.dist_to_left_side, vehicle.dist_to_right_side
total_width = float((vehicle.navigation.map.MAX_LANE_NUM + 1) *
vehicle.navigation.map.MAX_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)
]
# print("Heading Diff: ", [
# vehicle.heading_diff(current_reference_lane)
# for current_reference_lane in vehicle.navigation.current_ref_lanes
# ])
current_reference_lane = vehicle.navigation.current_ref_lanes[-1]
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.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.available:
info += vehicle.lane_line_detector.perceive(
vehicle,
vehicle.engine.physics_world.static_world).cloud_points
# else:
# _, lateral = vehicle.lane.local_coordinates(vehicle.position)
# info.append(clip((lateral * 2 / vehicle.navigation.map.MAX_LANE_WIDTH + 1.0) / 2.0, 0.0, 1.0))
# add vehicle length/width
if self.config["random_agent_model"]:
info.append(clip(vehicle.LENGTH / vehicle.MAX_LENGTH, 0.0, 1.0))
info.append(clip(vehicle.WIDTH / vehicle.MAX_WIDTH, 0.0, 1.0))
return info
def vehicle_state(vehicle):
"""
Wrap vehicle states to list
"""
# update out of road
info = []
if hasattr(vehicle,
"side_detector") and vehicle.side_detector is not None:
info += vehicle.side_detector.get_cloud_points()
else:
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)
]
# print("Heading Diff: ", [
# vehicle.heading_diff(current_reference_lane)
# for current_reference_lane in vehicle.routing_localization.current_ref_lanes
# ])
current_reference_lane = vehicle.routing_localization.current_ref_lanes[
-1]
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.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 += vehicle.lane_line_detector.get_cloud_points()
else:
_, 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))
return info
def heading_diff(self, target_lane):
lateral = None
if isinstance(target_lane, StraightLane):
lateral = np.asarray(
get_vertical_vector(target_lane.end - target_lane.start)[1])
elif isinstance(target_lane, CircularLane):
if target_lane.direction == -1:
lateral = self.position - target_lane.center
else:
lateral = target_lane.center - self.position
elif isinstance(target_lane, WayPointLane):
lane_segment = target_lane.segment(
target_lane.local_coordinates(self.position)[0])
lateral = lane_segment["lateral_direction"]
lateral_norm = norm(lateral[0], lateral[1])
forward_direction = self.heading
# print(f"Old forward direction: {self.forward_direction}, new heading {self.heading}")
forward_direction_norm = norm(forward_direction[0],
forward_direction[1])
if not lateral_norm * forward_direction_norm:
return 0
cos = ((forward_direction[0] * lateral[0] +
forward_direction[1] * lateral[1]) /
(lateral_norm * forward_direction_norm))
# return cos
# Normalize to 0, 1
return clip(cos, -1.0, 1.0) / 2 + 0.5
def speed(self):
"""
km/h
"""
velocity = self.chassis_np.node().get_linear_velocity()
speed = norm(velocity[0], velocity[1]) * 3.6
return clip(speed, 0.0, 100000.0)
def speed(self):
"""
km/h
"""
velocity = self.body.get_linear_velocity()
speed = norm(velocity[0], velocity[1]) * 3.6
return clip(speed, 0.0, 100000.0)
def heading_diff(self, target_lane):
lateral = None
if isinstance(target_lane, StraightLane):
lateral = np.asarray(
get_vertical_vector(target_lane.end - target_lane.start)[1])
elif isinstance(target_lane, CircularLane):
if target_lane.direction == -1:
lateral = self.position - target_lane.center
else:
lateral = target_lane.center - self.position
else:
raise ValueError("Unknown target lane type: {}".format(
type(target_lane)))
lateral_norm = norm(lateral[0], lateral[1])
forward_direction = self.heading
# print(f"Old forward direction: {self.forward_direction}, new heading {self.heading}")
forward_direction_norm = norm(forward_direction[0],
forward_direction[1])
if not lateral_norm * forward_direction_norm:
return 0
cos = ((forward_direction[0] * lateral[0] +
forward_direction[1] * lateral[1]) /
(lateral_norm * forward_direction_norm))
# return cos
# Normalize to 0, 1
return clip(cos, -1.0, 1.0) / 2 + 0.5
def _set_incremental_action(self, action: np.ndarray):
self.throttle_brake = action[1]
self.steering += action[0] * self.STEERING_INCREMENT
self.steering = clip(self.steering, -1, 1)
steering = self.steering * self.max_steering
self.system.setSteeringValue(steering, 0)
self.system.setSteeringValue(steering, 1)
self._apply_throttle_brake(action[1])
def act(self, agent_id):
# clip to -1, 1
action = [
clip(self.engine.external_actions[agent_id][i], -1.0, 1.0)
for i in range(len(self.engine.external_actions[agent_id]))
]
if not self.discrete_action:
return action
else:
return self.convert_to_continuous_action(action)
def act(self, action: Union[dict, str] = None) -> None:
"""
Perform a high-level action to change the desired lane or speed.
- If a high-level action is provided, update the target speed and lane;
- then, perform longitudinal and lateral control.
:param action: a high-level action
"""
self.follow_road()
if action == "FASTER":
self.target_speed += self.DELTA_SPEED
elif action == "SLOWER":
self.target_speed -= self.DELTA_SPEED
elif action == "LANE_RIGHT":
_from, _to, _id = self.target_lane_index
target_lane_index = _from, _to, clip(
_id + 1, 0,
len(self.traffic_mgr.map.road_network.graph[_from][_to]) - 1)
if self.traffic_mgr.map.road_network.get_lane(
target_lane_index).is_reachable_from(self.position):
self.target_lane_index = target_lane_index
elif action == "LANE_LEFT":
_from, _to, _id = self.target_lane_index
target_lane_index = _from, _to, clip(
_id - 1, 0,
len(self.traffic_mgr.map.road_network.graph[_from][_to]) - 1)
if self.traffic_mgr.map.road_network.get_lane(
target_lane_index).is_reachable_from(self.position):
self.target_lane_index = target_lane_index
action = {
"steering": self.steering_control(self.target_lane_index),
"acceleration": self.speed_control(self.target_speed)
}
action['steering'] = clip(action['steering'], -self.MAX_STEERING_ANGLE,
self.MAX_STEERING_ANGLE)
super().act(action)
def get_surrounding_vehicles_info(self, ego_vehicle, num_others: int = 4):
from pgdrive.utils.math_utils import norm, clip
surrounding_vehicles = list(self.get_surrounding_vehicles())
surrounding_vehicles.sort(
key=lambda v: norm(ego_vehicle.position[0] - v.position[0], ego_vehicle.position[1] - v.position[1])
)
surrounding_vehicles += [None] * num_others
res = []
for vehicle in surrounding_vehicles[:num_others]:
if vehicle is not None:
# assert isinstance(vehicle, IDMVehicle or Base), "Now PGDrive Doesn't support other vehicle type"
relative_position = ego_vehicle.projection(vehicle.position - ego_vehicle.position)
# It is possible that the centroid of other vehicle is too far away from ego but lidar shed on it.
# So the distance may greater than perceive distance.
res.append(clip((relative_position[0] / self.perceive_distance + 1) / 2, 0.0, 1.0))
res.append(clip((relative_position[1] / self.perceive_distance + 1) / 2, 0.0, 1.0))
relative_velocity = ego_vehicle.projection(vehicle.velocity - ego_vehicle.velocity)
res.append(clip((relative_velocity[0] / ego_vehicle.max_speed + 1) / 2, 0.0, 1.0))
res.append(clip((relative_velocity[1] / ego_vehicle.max_speed + 1) / 2, 0.0, 1.0))
else:
res += [0.0] * 4
return res
def vehicle_state(vehicle):
"""
Wrap vehicle states to list
"""
# update out of road
current_reference_lane = vehicle.routing_localization.current_ref_lanes[
-1]
lateral_to_left, lateral_to_right = ObservationType.vehicle_to_left_right(
vehicle)
if lateral_to_left < 0 or lateral_to_right < 0:
vehicle.out_of_road = True
total_width = float((vehicle.routing_localization.map.lane_num + 1) *
vehicle.routing_localization.map.lane_width)
info = [
clip(lateral_to_left / total_width, 0.0,
1.0), # lateral_to_left = distance to yellow line
clip(lateral_to_right / total_width, 0.0,
1.0), # lateral_to_right = distance to pavement
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.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) / (
np.linalg.norm(heading_dir_now) * np.linalg.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))
_, lateral = vehicle.lane.local_coordinates(vehicle.position)
info.append(
clip((lateral * 2 / vehicle.routing_localization.map.lane_width +
1.0) / 2.0, 0.0, 1.0))
# breakpoint()
return info
def steering_control(self, target_lane_index: LaneIndex) -> float:
"""
Steer the vehicle to follow the center of an given lane.
1. Lateral position is controlled by a proportional controller yielding a lateral speed command
2. Lateral speed command is converted to a heading reference
3. Heading is controlled by a proportional controller yielding a heading rate command
4. Heading rate command is converted to a steering angle
:param target_lane_index: index of the lane to follow
:return: a steering wheel angle command [rad]
"""
target_lane = self.traffic_mgr.map.road_network.get_lane(
target_lane_index)
lane_coords = target_lane.local_coordinates(self.position)
lane_next_coords = lane_coords[0] + self.speed * self.PURSUIT_TAU
lane_future_heading = target_lane.heading_at(lane_next_coords)
# Lateral position control
lateral_speed_command = -self.KP_LATERAL * lane_coords[1]
# Lateral speed to heading
heading_command = math.asin(
clip(lateral_speed_command / utils.not_zero(self.speed), -1, 1))
heading_ref = lane_future_heading + clip(heading_command, -np.pi / 4,
np.pi / 4)
# Heading control
heading_rate_command = self.KP_HEADING * utils.wrap_to_pi(heading_ref -
self.heading)
# Heading rate to steering angle
steering_angle = math.asin(
clip(
self.LENGTH / 2 / utils.not_zero(self.speed) *
heading_rate_command, -1, 1))
steering_angle = clip(steering_angle, -self.MAX_STEERING_ANGLE,
self.MAX_STEERING_ANGLE)
return float(steering_angle)
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
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
if self.config["use_lateral"]:
lateral_factor = clip(
1 - 2 * abs(lateral_now) / vehicle.routing_localization.get_current_lane_width(), 0.0, 1.0
)
else:
lateral_factor = 1.0
reward += self.config["driving_reward"] * (long_now - long_last) * lateral_factor
# 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.crash_vehicle:
reward = self.config["crash_vehicle_penalty"]
elif vehicle.crash_object:
reward = self.config["crash_object_penalty"]
elif vehicle.out_of_route:
reward = self.config["out_of_road_penalty"]
elif vehicle.crash_sidewalk:
reward = self.config["out_of_road_penalty"]
elif vehicle.arrive_destination:
reward += self.config["success_reward"]
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
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 speed(self):
"""
km/h
"""
speed = self.chassis_np.node().get_linear_velocity().length() * 3.6
return clip(speed, 0.0, 100000.0)
def vehicle_state(self, vehicle):
# update out of road
info = []
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 += [
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.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
# Add more information about the road
if hasattr(current_reference_lane, "heading"):
info.append(clip(current_reference_lane.heading, 0.0, 1.0))
else:
info.append(0.0)
info.append(clip(current_reference_lane.length / DISTANCE, 0.0, 1.0))
if hasattr(current_reference_lane, "direction"):
dir = current_reference_lane.direction
if isinstance(dir, int):
info.append(self._to_zero_and_one(dir))
info.append(0.0)
elif len(dir) == 2:
info.append(self._to_zero_and_one(dir[0]))
info.append(self._to_zero_and_one(dir[1]))
else:
info.extend([0.0, 0.0])
else:
info.extend([0.0, 0.0])
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))
yaw_rate = beta_diff / 0.1
info.append(clip(yaw_rate, 0.0, 1.0))
long, 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.append(clip(long / DISTANCE, 0.0, 1.0))
return info
def _to_zero_and_one(v):
return (clip(v, -1, +1) + 1) / 2
def overwritten_get_surrounding_vehicles_info(self,
lidar,
ego_vehicle,
num_others: int = 4):
# surrounding_vehicles = list(lidar.get_surrounding_vehicles())
surrounding_vehicles = list(
self._env.scene_manager.traffic_manager.vehicles)[1:]
surrounding_vehicles.sort(
key=lambda v: norm(ego_vehicle.position[0] - v.position[0],
ego_vehicle.position[1] - v.position[1]))
# dis = [(str(v), norm(ego_vehicle.position[0] - v.position[0], ego_vehicle.position[1] - v.position[1]))
# for v in surrounding_vehicles]
surrounding_vehicles += [None] * num_others
res = []
for vehicle in surrounding_vehicles[:num_others]:
if vehicle is not None:
relative_position = ego_vehicle.projection(
vehicle.position - ego_vehicle.position)
dist = norm(relative_position[0], relative_position[1])
if dist > DISTANCE:
if self.config["use_extra_state"]:
res += [0.0] * self._traffic_vehicle_state_dim
else:
res += [0.0] * self._traffic_vehicle_state_dim_wo_extra
continue
relative_velocity = ego_vehicle.projection(
vehicle.velocity - ego_vehicle.velocity)
res.append(clip(dist / DISTANCE, 0.0, 1.0))
res.append(
clip(
norm(relative_velocity[0], relative_velocity[1]) /
ego_vehicle.max_speed, 0.0, 1.0))
res.append(
clip((relative_position[0] / DISTANCE + 1) / 2, 0.0, 1.0))
res.append(
clip((relative_position[1] / DISTANCE + 1) / 2, 0.0, 1.0))
res.append(
clip(
(relative_velocity[0] / ego_vehicle.max_speed + 1) / 2,
0.0, 1.0))
res.append(
clip(
(relative_velocity[1] / ego_vehicle.max_speed + 1) / 2,
0.0, 1.0))
if self.config["use_extra_state"]:
res.extend(self.traffic_vehicle_state(vehicle))
else:
if self.config["use_extra_state"]:
res += [0.0] * self._traffic_vehicle_state_dim
else:
res += [0.0] * self._traffic_vehicle_state_dim_wo_extra
# p1 = []
# p2 = []
# for vehicle in surrounding_vehicles[:num_others]:
# if vehicle is not None:
# relative_position = ego_vehicle.projection(vehicle.position - ego_vehicle.position)
# relative_velocity = ego_vehicle.projection(vehicle.velocity - ego_vehicle.velocity)
# p1.append(relative_position)
# p2.append(relative_velocity)
# print("Detected Others Position: {}, Velocity: {}".format(p1, p2))
return res
def update_navigation_localization(self, ego_vehicle):
position = ego_vehicle.position
lane, lane_index = ray_localization(position, ego_vehicle.pg_world)
if lane is None:
lane, lane_index = ego_vehicle.lane, ego_vehicle.lane_index
if self.FORCE_CALCULATE:
lane_index, _ = self.map.road_network.get_closest_lane_index(
position)
lane = self.map.road_network.get_lane(lane_index)
self._update_target_checkpoints(lane_index)
target_road_1_start = self.checkpoints[
self.target_checkpoints_index[0]]
target_road_1_end = self.checkpoints[self.target_checkpoints_index[0] +
1]
target_road_2_start = self.checkpoints[
self.target_checkpoints_index[1]]
target_road_2_end = self.checkpoints[self.target_checkpoints_index[1] +
1]
target_lanes_1 = self.map.road_network.graph[target_road_1_start][
target_road_1_end]
target_lanes_2 = self.map.road_network.graph[target_road_2_start][
target_road_2_end]
res = []
self.current_ref_lanes = target_lanes_1
ckpts = []
lanes_heading = []
for lanes_id, lanes in enumerate([target_lanes_1, target_lanes_2]):
ref_lane = lanes[0]
later_middle = (float(self.map.lane_num) / 2 -
0.5) * self.map.lane_width
if isinstance(ref_lane, CircularLane) and ref_lane.direction == 1:
ref_lane = lanes[-1]
later_middle *= -1
check_point = ref_lane.position(ref_lane.length, later_middle)
if lanes_id == 0:
# calculate ego v lane heading
lanes_heading.append(
ref_lane.heading_at(
ref_lane.local_coordinates(ego_vehicle.position)[0]))
else:
lanes_heading.append(
ref_lane.heading_at(
min(self.PRE_NOTIFY_DIST, ref_lane.length)))
ckpts.append(check_point)
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.map.lane_num * self.map.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
res += [
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)
]
if self.show_navi_point:
pos_of_goal = ckpts[0]
self.goal_node_path.setPos(pos_of_goal[0], -pos_of_goal[1], 1.8)
self.goal_node_path.setH(self.goal_node_path.getH() + 3)
self.update_navi_arrow(lanes_heading)
self.navi_info = res
return lane, lane_index
