def _get_oncoming_vehicles(
self, frame: Dict[int, ip.AgentState],
scenario_map: ip.Map) -> List[Tuple[ip.AgentState, float]]:
oncoming_vehicles = []
ego_junction_lane = scenario_map.get_lane(self.config.junction_road_id,
self.config.junction_lane_id)
lanes_to_cross = self._get_lanes_to_cross(scenario_map)
agent_lanes = [(i,
scenario_map.best_lane_at(s.position, s.heading, True))
for i, s in frame.items()]
for lane_to_cross in lanes_to_cross:
lane_sequence = self._get_predecessor_lane_sequence(lane_to_cross)
midline = self.get_lane_path_midline(lane_sequence)
crossing_point = lane_to_cross.boundary.intersection(
ego_junction_lane.boundary).centroid
crossing_lon = midline.project(crossing_point)
# find agents in lane to cross
for agent_id, agent_lane in agent_lanes:
agent_state = frame[agent_id]
if agent_id != self.agent_id and agent_lane in lane_sequence:
agent_lon = midline.project(Point(agent_state.position))
dist = crossing_lon - agent_lon
if 0 < dist < self.MAX_ONCOMING_VEHICLE_DIST:
oncoming_vehicles.append((agent_state, dist))
return oncoming_vehicles
def applicable(state: ip.AgentState, scenario_map: ip.Map) -> bool:
""" True if in non-outer lane of roundabout and not in junction """
current_lane = scenario_map.best_lane_at(state.position, state.heading)
all_lane_ids = [lane.id for lane in current_lane.lane_section.all_lanes if lane != current_lane]
return (scenario_map.in_roundabout(state.position, state.heading) and
current_lane.parent_road.junction is None and
not all([np.abs(current_lane.id) > np.abs(lid) for lid in all_lane_ids]) and # Not in outer lane
ip.FollowLane.applicable(state, scenario_map))
def get_applicable_actions(agent_state: ip.AgentState, scenario_map: ip.Map, goal: ip.Goal = None) \
-> List[Type['MacroAction']]:
""" Return all applicable macro actions.
Args:
agent_state: Current state of the examined agent
scenario_map: The road layout of the scenario
goal: If given and ahead within current lane boundary, then will always return at least a Continue
Returns:
A list of applicable macro action types
"""
actions = []
current_lane = scenario_map.best_lane_at(agent_state.position, agent_state.heading)
if goal is not None:
goal_point = goal.point_on_lane(current_lane)
if current_lane.boundary.contains(goal_point) and \
current_lane.distance_at(agent_state.position) < current_lane.distance_at(goal_point):
actions = [Continue]
for macro_action in ip.util.all_subclasses(MacroAction):
try:
if macro_action not in actions and macro_action.applicable(agent_state, scenario_map):
actions.append(macro_action)
except NotImplementedError:
continue
return actions
def _get_lane_sequence(self, state: ip.AgentState,
scenario_map: ip.Map) -> List[ip.Lane]:
lanes = []
for position, heading in zip(self._trajectory.path,
self._trajectory.heading):
possible_lanes = scenario_map.lanes_at(position, heading)
if not possible_lanes:
continue
if len(possible_lanes) > 1:
lane, min_dist = None, np.inf
for ll in possible_lanes:
final_point = np.array(ll.midline.coords[-1])
dist = np.min(
np.linalg.norm(self._trajectory.path - final_point,
axis=1))
if dist < min_dist:
lane, min_dist = ll, dist
else:
lane = possible_lanes[0]
if not lanes:
lanes.append(lane)
elif lane != lanes[-1]:
lanes.append(lane)
return lanes
def get_possible_args(state: ip.AgentState, scenario_map: ip.Map, goal: ip.Goal = None) -> List[Dict]:
""" Return empty dictionary if no goal point is provided, otherwise check if goal point in lane and
return center of goal point as termination point. """
if goal is not None:
current_lane = scenario_map.best_lane_at(state.position, state.heading)
gp = goal.point_on_lane(current_lane)
if current_lane.boundary.contains(gp):
return [{"termination_point": np.array(gp.coords)[0]}]
return [{}]
def applicable(state: ip.AgentState, scenario_map: ip.Map) -> bool:
""" Checks whether the turn maneuver is applicable for an agent.
Turn is applicable if the agents current lane or next lane is in a junction.
Args:
state: Current state of the agent
scenario_map: local road map
Returns:
Boolean indicating whether the maneuver is applicable
"""
currently_in_junction = scenario_map.junction_at(
state.position) is not None
current_lane = scenario_map.best_lane_at(state.position, state.heading)
next_lanes = current_lane.link.successor
next_lane_is_junction = (next_lanes is not None and any(
[ll.parent_road.junction is not None for ll in next_lanes]))
return currently_in_junction or next_lane_is_junction
def applicable(state: ip.AgentState, scenario_map: ip.Map) -> bool:
""" True if either Turn (in junction) or GiveWay is applicable (ahead of junction) """
in_junction = scenario_map.junction_at(state.position) is not None
# Uncomment the following to disallow turns from inner lanes of a roundabout
# if ContinueNextExit.applicable(state, scenario_map):
# return False
if in_junction:
return ip.Turn.applicable(state, scenario_map)
else:
return ip.GiveWay.applicable(state, scenario_map)
def applicable(state: ip.AgentState, scenario_map: ip.Map) -> bool:
""" Checks whether the follow lane maneuver is applicable for an agent.
Follow lane is applicable if the agent is in a drivable lane.
Args:
state: Current state of the agent
scenario_map: local road map
Returns:
Boolean indicating whether the maneuver is applicable
"""
return len(scenario_map.lanes_at(state.position,
drivable_only=True)) > 0
def _get_target_lane(self, final_point, state: ip.AgentState,
current_lane: ip.Lane,
scenario_map: ip.Map) -> ip.Lane:
target_lanes = scenario_map.lanes_at(final_point, drivable_only=True)
if len(target_lanes) == 1:
return target_lanes[0]
distance = -current_lane.distance_at(state.position)
while distance <= self.TARGET_SWITCH_LENGTH:
distance += current_lane.length
for lane in current_lane.lane_section.all_lanes:
if abs(current_lane.id - lane.id) == 1:
if lane in target_lanes:
return lane
successors = current_lane.link.successor
if successors is None:
current_lane = None
elif len(successors) == 1:
current_lane = current_lane.link.successor[0]
elif len(successors) > 1:
next_lanes = [
s for s in successors
if len(scenario_map.get_adjacent_lanes(s)) > 0
]
if len(next_lanes) == 0:
current_lane = None
elif len(next_lanes) == 1:
current_lane = next_lanes[0]
elif len(next_lanes) > 1 and scenario_map.road_in_roundabout(
current_lane.parent_road):
for lane in next_lanes:
if scenario_map.road_in_roundabout(lane.parent_road):
current_lane = lane
break
else:
current_lane = None
raise RuntimeError(f"Target lane not found at {final_point}!")
def get_possible_args(state: ip.AgentState, scenario_map: ip.Map, goal: ip.Goal = None) -> List[Dict]:
""" Return turn endpoint if approaching junction; if in junction
return all possible turns within angle threshold"""
targets = []
in_junction = scenario_map.junction_at(state.position) is not None
if in_junction:
# lanes = scenario_map.lanes_within_angle(state.position, state.heading, Exit.LANE_ANGLE_THRESHOLD,
# max_distance=0.5)
# if not lanes:
lane = scenario_map.best_lane_at(state.position, state.heading, goal=goal)
targets.append(np.array(lane.midline.coords[-1]))
# for lane in lanes:
# target = np.array(lane.midline.coords[-1])
# if not any([np.allclose(p, target, atol=0.25) for p in targets]):
# targets.append(target)
else:
current_lane = scenario_map.best_lane_at(state.position, state.heading)
for connecting_lane in current_lane.link.successor:
if not scenario_map.road_in_roundabout(connecting_lane.parent_road):
targets.append(np.array(connecting_lane.midline.coords[-1]))
return [{"turn_target": t} for t in targets]
def play_forward_macro_action(agent_id: int, scenario_map: ip.Map,
frame: Dict[int, ip.AgentState], macro_action: "MacroAction"):
""" Play forward current frame with the given macro action for the current agent.
Assumes constant velocity lane follow behaviour for other agents.
Args:
agent_id: ID of the ego agent
scenario_map: The road layout of the current scenario
frame: The current frame of the environment
macro_action: The macro action to play forward
Returns:
A new frame describing the future state of the environment
"""
def _lane_at_distance(lane: ip.Lane, ds: float) -> Tuple[Optional[ip.Lane], float]:
current_lane = lane
total_length = 0.0
while True:
if total_length <= ds < total_length + current_lane.length:
return current_lane, ds - total_length
total_length += current_lane.length
successor = current_lane.link.successor
if successor is None:
break
current_lane = successor[0]
logger.debug(f"No Lane found at distance {ds} for Agent ID{aid}!")
return None, current_lane.length
if not macro_action:
return frame
trajectory = macro_action.get_trajectory()
new_frame = {agent_id: trajectory.final_agent_state}
duration = trajectory.duration
for aid, agent in frame.items():
if aid != agent_id:
state = copy(agent)
agent_lane = scenario_map.best_lane_at(agent.position, agent.heading)
if agent_lane is None:
continue
agent_distance = agent_lane.distance_at(agent.position) + duration * agent.speed
final_lane, distance_in_lane = _lane_at_distance(agent_lane, agent_distance)
if final_lane is None:
continue
state.position = final_lane.point_at(distance_in_lane)
state.heading = final_lane.get_heading_at(distance_in_lane)
new_frame[aid] = state
return new_frame
def check_change_validity(state: ip.AgentState, scenario_map: ip.Map) -> bool:
""" True if current lane not in junction, or at appropriate distance from a junction """
in_junction = scenario_map.junction_at(state.position) is not None
if in_junction:
return False
# Disallow lane changes if close to junction as could enter junction boundary by the end of the lane change,
# unless currently in a non-junction roundabout lane where we can pass directly straight through the junction
current_lane = scenario_map.best_lane_at(state.position, state.heading)
successor = current_lane.link.successor
if successor is not None:
successor_distances = [(s.boundary.distance(Point(state.position)), s) for s in successor]
distance_to_successor, nearest_successor = min(successor_distances, key=lambda x: x[0])
# All connecting roads are single laned (in total these have 2 lanes including the zero-width center lane)
# then check if at least minimum change length available
if all([len(lane.lane_section.all_lanes) <= 2 for lane in successor]) and \
nearest_successor.parent_road.junction is None:
return distance_to_successor > ip.SwitchLane.MIN_SWITCH_LENGTH
elif nearest_successor.parent_road.junction is not None:
return distance_to_successor > ip.SwitchLane.TARGET_SWITCH_LENGTH + state.metadata.length or \
scenario_map.road_in_roundabout(current_lane.parent_road)
return False
return current_lane.length - current_lane.distance_at(state.position) > ip.SwitchLane.MIN_SWITCH_LENGTH
def _get_lanes_to_cross(self, scenario_map: ip.Map) -> List[ip.Lane]:
ego_road = scenario_map.roads.get(self.config.junction_road_id)
ego_lane = scenario_map.get_lane(self.config.junction_road_id,
self.config.junction_lane_id)
ego_incoming_lane = ego_lane.link.predecessor[0]
lanes = []
for connection in ego_road.junction.connections:
for lane_link in connection.lane_links:
lane = lane_link.to_lane
same_predecessor = (ego_incoming_lane.id == lane_link.from_id
and ego_incoming_lane.parent_road.id
== connection.incoming_road.id)
if not (same_predecessor
or self._has_priority(ego_road, lane.parent_road)):
if ego_lane.midline.intersects(lane.boundary):
lanes.append(lane)
return lanes
def play_forward_maneuver(
agent_id: int,
scenario_map: ip.Map,
frame: Dict[int, ip.AgentState],
maneuver: "Maneuver",
offset: float = None) -> Dict[int, ip.AgentState]:
""" Play forward current frame with the given maneuver for the current agent.
Assumes constant velocity lane follow behaviour for other agents.
Args:
agent_id: ID of the ego agent
scenario_map: The road layout of the current scenario
frame: The current frame of the environment
maneuver: The maneuver to play forward
offset: If not None, then add an extra point at the end of the maneuver's trajectory with distance given
by this parameter
Returns:
A new frame describing the future state of the environment
"""
if not maneuver:
return frame
trajectory = maneuver.trajectory
if offset is not None:
trajectory = ip.util.add_offset_point(trajectory, offset)
new_frame = {agent_id: trajectory.final_agent_state}
duration = maneuver.trajectory.duration
for aid, agent in frame.items():
if aid != agent_id:
state = copy(agent)
agent_lane = scenario_map.best_lane_at(agent.position,
agent.heading)
if agent_lane is None:
continue
agent_distance = agent_lane.distance_at(
agent.position) + duration * agent.speed
state.position = agent_lane.point_at(agent_distance)
state.heading = agent_lane.get_heading_at(agent_distance)
new_frame[aid] = state
return new_frame
def applicable(state: ip.AgentState, scenario_map: ip.Map) -> bool:
""" Checks whether the switch lane left maneuver is applicable for an agent.
Switch lane left is applicable if it is legal to switch to a lane left of the current lane.
Args:
state: Current state of the agent
scenario_map: local road map
Returns:
Boolean indicating whether the maneuver is applicable
"""
# TODO: Add check for lane marker
current_lane = scenario_map.best_lane_at(state.position, state.heading)
left_lane_id = current_lane.id + (-1 if np.sign(current_lane.id) > 0
else 1) # Assumes right hand driving
left_lane = current_lane.lane_section.get_lane(left_lane_id)
return (left_lane is not None and left_lane_id != 0
and left_lane.type == ip.LaneTypes.DRIVING
and (current_lane.id < 0) == (left_lane_id < 0))
def _get_lane_sequence(self, state: ip.AgentState,
scenario_map: ip.Map) -> List[ip.Lane]:
junction_lane = scenario_map.get_lane(self.config.junction_road_id,
self.config.junction_lane_id)
return [junction_lane]
def _get_lane_sequence(self, state: ip.AgentState,
scenario_map: ip.Map) -> List[ip.Lane]:
current_lane = scenario_map.best_lane_at(state.position, state.heading)
target_lane = self._get_target_lane(self.config.termination_point,
state, current_lane, scenario_map)
return [target_lane]
def applicable(state: ip.AgentState, scenario_map: ip.Map) -> bool:
""" True if vehicle on a lane, and not approaching junction or not in junction"""
in_junction = scenario_map.best_lane_at(state.position, state.heading).parent_road.junction is not None
return (ip.FollowLane.applicable(state, scenario_map) and not in_junction and
not Exit.applicable(state, scenario_map))
def _get_lane_sequence(self, state: ip.AgentState,
scenario_map: ip.Map) -> List[ip.Lane]:
current_lane = scenario_map.best_lane_at(state.position, state.heading)
lane_seq = [current_lane]
return lane_seq
