def _ttc_by_path(env_observation):
ego = env_observation.ego_vehicle_state
waypoint_paths = env_observation.waypoint_paths
neighborhood_vehicle_states = env_observation.neighborhood_vehicle_states
# first sum up the distance between waypoints along a path
# ie. [(wp1, path1, 0),
# (wp2, path1, 0 + dist(wp1, wp2)),
# (wp3, path1, 0 + dist(wp1, wp2) + dist(wp2, wp3))]
wps_with_lane_dist = []
for path_idx, path in enumerate(waypoint_paths):
lane_dist = 0.0
for w1, w2 in zip(path, path[1:]):
wps_with_lane_dist.append((w1, path_idx, lane_dist))
lane_dist += np.linalg.norm(w2.pos - w1.pos)
wps_with_lane_dist.append((path[-1], path_idx, lane_dist))
# next we compute the TTC along each of the paths
ttc_by_path_index = [1000] * len(waypoint_paths)
lane_dist_by_path_index = [1] * len(waypoint_paths)
if neighborhood_vehicle_states is not None:
for v in neighborhood_vehicle_states:
# find all waypoints that are on the same lane as this vehicle
wps_on_lane = [(wp, path_idx, dist)
for wp, path_idx, dist in wps_with_lane_dist
if wp.lane_id == v.lane_id]
if not wps_on_lane:
# this vehicle is not on a nearby lane
continue
# find the closest waypoint on this lane to this vehicle
nearest_wp, path_idx, lane_dist = min(
wps_on_lane,
key=lambda tup: np.linalg.norm(tup[0].pos - vec_2d(v.position)
))
if np.linalg.norm(nearest_wp.pos - vec_2d(v.position)) > 2:
# this vehicle is not close enough to the path, this can happen
# if the vehicle is behind the ego, or ahead past the end of
# the waypoints
continue
relative_speed_m_per_s = (ego.speed - v.speed) * 1000 / 3600
if abs(relative_speed_m_per_s) < 1e-5:
relative_speed_m_per_s = 1e-5
ttc = lane_dist / relative_speed_m_per_s
ttc /= 10
if ttc <= 0:
# discard collisions that would have happened in the past
continue
lane_dist /= 100
lane_dist_by_path_index[path_idx] = min(
lane_dist_by_path_index[path_idx], lane_dist)
ttc_by_path_index[path_idx] = min(ttc_by_path_index[path_idx], ttc)
return ttc_by_path_index, lane_dist_by_path_index
def _closest_linked_wp_in_kd_tree_batched(self, points, linked_wps, tree, k=1):
p2ds = np.array([vec_2d(p) for p in points])
closest_indices = tree.query(
p2ds, k=min(k, len(linked_wps)), return_distance=False, sort_results=True
)
return [[linked_wps[idx] for idx in idxs] for idxs in closest_indices]
def ttc_by_path(ego, waypoint_paths, neighborhood_vehicle_states, ego_closest_wp):
# TODO: Phase this out; used for penalizing close proximity to other cars
will_crash = False
wps_with_lane_dist = []
for path_idx, path in enumerate(waypoint_paths):
lane_dist = 0.0
for w1, w2 in zip(path, path[1:]):
wps_with_lane_dist.append((w1, path_idx, lane_dist))
lane_dist += np.linalg.norm(w2.pos - w1.pos)
wps_with_lane_dist.append((path[-1], path_idx, lane_dist))
# next we compute the TTC along each of the paths
ttc_by_path_index = [1] * len(waypoint_paths)
lane_dist_by_path_index = [1] * len(waypoint_paths)
headings_of_cars = [0] * len(waypoint_paths)
speed_of_closest = 1
wps = [path[0] for path in waypoint_paths]
ego_closest_wp = min(wps, key=lambda wp: wp.dist_to(ego.position))
neighborhood_vehicle_states = neighborhood_vehicle_states or []
for v in neighborhood_vehicle_states:
# find all waypoints that are on the same lane as this vehicle
wps_on_lane = [
(wp, path_idx, dist)
for wp, path_idx, dist in wps_with_lane_dist
if wp.lane_id == v.lane_id
]
if not wps_on_lane:
# this vehicle is not on a nearby lane
continue
# find the closest waypoint on this lane to this vehicle
nearest_wp, path_idx, lane_dist = min(
wps_on_lane,
key=lambda tup: np.linalg.norm(tup[0].pos - vec_2d(v.position)),
)
if np.linalg.norm(nearest_wp.pos - vec_2d(v.position)) > 2:
# this vehicle is not close enough to the path, this can happen
# if the vehicle is behind the ego, or ahead past the end of
# the waypoints
continue
if ego_closest_wp.lane_index == nearest_wp.lane_index:
if np.linalg.norm(vec_2d(ego.position) - vec_2d(v.position)) < 6:
will_crash = True
relative_speed_m_per_s = (ego.speed - v.speed) * 1000 / 3600
if abs(relative_speed_m_per_s) < 1e-5:
relative_speed_m_per_s = 1e-5
dist_wp_vehicle_vector = vec_2d(v.position) - vec_2d(nearest_wp.pos)
# take into account the position of the car instead of its nearest waypoint
direction_vector = np.array(
[math.cos(nearest_wp.heading), math.sin(nearest_wp.heading),]
).dot(dist_wp_vehicle_vector)
dist_to_vehicle = lane_dist + sign(direction_vector) * (
np.linalg.norm(vec_2d(nearest_wp.pos) - vec_2d(v.position))
)
ttc = dist_to_vehicle / relative_speed_m_per_s
ttc = ttc / 10
lane_dist = dist_to_vehicle / 100
if lane_dist_by_path_index[path_idx] > lane_dist:
if nearest_wp.lane_index == v.lane_index:
headings_of_cars[path_idx] = math.sin(
nearest_wp.relative_heading(nv_heading_to_ego_heading(v.heading))
)
# speed
if ego_closest_wp.lane_index == v.lane_index:
speed_of_closest = (v.speed - ego.speed) / 120
lane_dist_by_path_index[path_idx] = min(
lane_dist_by_path_index[path_idx], lane_dist
)
if ttc <= 0:
# discard collisions that would have happened in the past
continue
ttc_by_path_index[path_idx] = min(ttc_by_path_index[path_idx], ttc)
return (
ttc_by_path_index,
lane_dist_by_path_index,
headings_of_cars,
speed_of_closest,
will_crash,
)
def dist_to(self, p) -> float:
"""Calculates straight line distance to the given 2D point"""
return np.linalg.norm(self.pos - vec_2d(p))
def cal_ego_lane_dist_and_speed(env_obs: Observation, observe_lane_num):
"""Calculate the distance from ego vehicle to its front vehicles (if have) at observed lanes,
also the relative speed of the front vehicle which positioned at the same lane.
"""
ego = env_obs.ego_vehicle_state
waypoint_paths = env_obs.waypoint_paths
wps = [path[0] for path in waypoint_paths]
closest_wp = min(wps, key=lambda wp: wp.dist_to(ego.position))
wps_with_lane_dist = []
for path_idx, path in enumerate(waypoint_paths):
lane_dist = 0.0
for w1, w2 in zip(path, path[1:]):
wps_with_lane_dist.append((w1, path_idx, lane_dist))
lane_dist += np.linalg.norm(w2.pos - w1.pos)
wps_with_lane_dist.append((path[-1], path_idx, lane_dist))
# TTC calculation along each path
ego_closest_wp = min(wps, key=lambda wp: wp.dist_to(ego.position))
wps_on_lane = [(wp, path_idx, dist)
for wp, path_idx, dist in wps_with_lane_dist
# if wp.lane_id == v.lane_id
]
ego_lane_index = closest_wp.lane_index
lane_dist_by_path = [1] * len(waypoint_paths)
ego_lane_dist = [0] * observe_lane_num
speed_of_closest = 0.0
for v in env_obs.neighborhood_vehicle_states:
nearest_wp, path_idx, lane_dist = min(
wps_on_lane,
key=lambda tup: np.linalg.norm(tup[0].pos - vec_2d(v.position)
),
)
if np.linalg.norm(nearest_wp.pos - vec_2d(v.position)) > 2:
# this vehicle is not close enough to the path, this can happen
# if the vehicle is behind the ego, or ahead past the end of
# the waypoints
continue
# relative_speed_m_per_s = (ego.speed - v.speed) * 1000 / 3600
# relative_speed_m_per_s = max(abs(relative_speed_m_per_s), 1e-5)
dist_wp_vehicle_vector = vec_2d(v.position) - vec_2d(
nearest_wp.pos)
direction_vector = np.array([
math.cos(math.radians(nearest_wp.heading)),
math.sin(math.radians(nearest_wp.heading)),
]).dot(dist_wp_vehicle_vector)
dist_to_vehicle = lane_dist + np.sign(direction_vector) * (
np.linalg.norm(vec_2d(nearest_wp.pos) - vec_2d(v.position)))
lane_dist = dist_to_vehicle / 100.0
if lane_dist_by_path[path_idx] > lane_dist:
if ego_closest_wp.lane_index == v.lane_index:
speed_of_closest = (v.speed - ego.speed) / 120.0
lane_dist_by_path[path_idx] = min(lane_dist_by_path[path_idx],
lane_dist)
# current lane is centre
flag = observe_lane_num // 2
ego_lane_dist[flag] = lane_dist_by_path[ego_lane_index]
max_lane_index = len(lane_dist_by_path) - 1
if max_lane_index == 0:
right_sign, left_sign = 0, 0
else:
right_sign = -1 if ego_lane_index + 1 > max_lane_index else 1
left_sign = -1 if ego_lane_index - 1 >= 0 else 1
ego_lane_dist[flag + right_sign] = lane_dist_by_path[ego_lane_index +
right_sign]
ego_lane_dist[flag + left_sign] = lane_dist_by_path[ego_lane_index +
left_sign]
res = np.asarray(ego_lane_dist + [speed_of_closest])
return res
