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