def _closest_linked_lp_in_kd_tree_with_pose_batched(
poses,
lanepoints,
tree,
within_radius: float,
k: int = 10,
keep_all_k: bool = False,
):
linked_lanepoints = LanePoints._closest_linked_lp_in_kd_tree_batched(
[pose.position[:2] for pose in poses], lanepoints, tree, k=k)
radius_sq = within_radius * within_radius
linked_lanepoints = [
sorted(
l_lps,
key=lambda _llp: squared_dist(poses[idx].position[:2], _llp.lp.
pos),
) for idx, l_lps in enumerate(linked_lanepoints)
]
# exclude those outside radius except closest
if not keep_all_k:
linked_lanepoints = [[
_llp for i, _llp in enumerate(_llps)
if squared_dist(poses[idx].position[:2], _llp.lp.pos) <=
radius_sq or i == 0
] for idx, _llps in enumerate(linked_lanepoints)]
# Get the nearest point for the points where the radius check failed
unfound_lanepoints = [(i, poses[i])
for i, group in enumerate(linked_lanepoints)
if len(group) == 0]
if len(unfound_lanepoints) > 0:
remaining_linked_lps = LanePoints._closest_linked_lp_in_kd_tree_batched(
[pose.position[:2] for _, pose in unfound_lanepoints],
lanepoints,
tree=tree,
k=k,
)
# Replace the empty lanepoint locations
for (i, _), lps in [
g for g in zip(unfound_lanepoints, remaining_linked_lps)
]:
linked_lanepoints[i] = [lps]
return [
sorted(
l_lps,
key=lambda _llp: squared_dist(poses[
idx].position[:2], _llp.lp.pos) + abs(poses[
idx].heading.relative_to(_llp.lp.heading)),
) for idx, l_lps in enumerate(linked_lanepoints)
]
def scan_for_vehicles(
target_prefix,
angle_a,
angle_b,
activation_dist_squared,
self_vehicle_state,
other_vehicle_states,
short_circuit: bool = False,
):
if target_prefix:
other_vehicle_states = filter(
lambda v: v.id.startswith(target_prefix), other_vehicle_states
)
min_angle, max_angle = min(angle_a, angle_b), max(angle_a, angle_b)
vehicles = []
for vehicle_state in other_vehicle_states:
sqd = squared_dist(self_vehicle_state.position, vehicle_state.position)
# check for activation distance
if sqd < activation_dist_squared:
direction = np.sum(
[vehicle_state.position, -self_vehicle_state.position], axis=0
)
angle = Heading(vec_to_radians(direction[:2]))
rel_angle = angle.relative_to(self_vehicle_state.heading)
if min_angle <= rel_angle <= max_angle:
vehicles.append(vehicle_state)
if short_circuit:
break
return vehicles
def __call__(self):
near_points: List[ViaPoint] = list()
hit_points: List[ViaPoint] = list()
vehicle_position = self._vehicle.position[:2]
@lru_cache()
def closest_point_on_lane(position, lane_id):
lane = self._plan.road_map.lane_by_id(lane_id)
return lane.center_at_point(position)
for via in self._vias:
closest_position_on_lane = closest_point_on_lane(
tuple(vehicle_position), via.lane_id
)
closest_position_on_lane = closest_position_on_lane[:2]
dist_from_lane_sq = squared_dist(vehicle_position, closest_position_on_lane)
if dist_from_lane_sq > self._acquisition_range ** 2:
continue
point = ViaPoint(
tuple(via.position),
lane_index=via.lane_index,
road_id=via.road_id,
required_speed=via.required_speed,
)
near_points.append(point)
dist_from_point_sq = squared_dist(vehicle_position, via.position)
if (
dist_from_point_sq <= via.hit_distance ** 2
and via not in self._consumed_via_points
and np.isclose(
self._vehicle.speed, via.required_speed, atol=self._speed_accuracy
)
):
self._consumed_via_points.add(via)
hit_points.append(point)
return (
sorted(
near_points,
key=lambda point: squared_dist(point.position, vehicle_position),
),
hit_points,
)
def scan_for_vehicles(
target_prefix,
angle_a,
angle_b,
activation_dist_squared,
self_vehicle_state,
other_vehicle_states,
short_circuit: bool = False,
):
"""Sense test for vehicles within a semi-circle radius of a vehicle.
Args:
target_prefix:
The whitelist of vehicles with vehicle_ids starting with this prefix for quick elimination.
angle_a:
The minimum sweep angle between -pi and pi.
angle_b:
The maximum sweep angle between -pi and pi.
activation_dist_squared:
The distance to check for the target.
self_vehicle_state:
The vehicle state of the vehicle that is scanning.
other_vehicle_states:
The set of vehicles to test for.
Returns:
If the tested for vehicle is within the semi-circle range of the base vehicle.
"""
if target_prefix:
other_vehicle_states = filter(lambda v: v.id.startswith(target_prefix),
other_vehicle_states)
min_angle, max_angle = min(angle_a, angle_b), max(angle_a, angle_b)
vehicles = []
for vehicle_state in other_vehicle_states:
sqd = squared_dist(self_vehicle_state.position, vehicle_state.position)
# check for activation distance
if sqd < activation_dist_squared:
direction = np.sum(
[vehicle_state.position, -self_vehicle_state.position], axis=0)
angle = Heading(vec_to_radians(direction[:2]))
rel_angle = angle.relative_to(self_vehicle_state.heading)
if min_angle <= rel_angle <= max_angle:
vehicles.append(vehicle_state)
if short_circuit:
break
return vehicles
def scan_for_vehicle(
target_prefix: str,
angle_a: float,
angle_b: float,
activation_dist_squared: float,
self_vehicle_state,
other_vehicle_state,
):
"""Sense test for another vehicle within a semi-circle range of a vehicle.
Args:
target_prefix:
The whitelist of vehicles with vehicle_ids starting with this prefix for quick elimination.
angle_a:
The minimum sweep angle between -pi and pi.
angle_b:
The maximum sweep angle between -pi and pi.
activation_dist_squared:
The distance to check for the target.
self_vehicle_state:
The vehicle state of the vehicle that is scanning.
other_vehicle_state:
The vehicle to test for.
Returns:
If the tested for vehicle is within the semi-circle range of the base vehicle.
"""
if target_prefix and not other_vehicle_state.id.startswith(target_prefix):
return False
min_angle, max_angle = min(angle_a, angle_b), max(angle_a, angle_b)
sqd = squared_dist(self_vehicle_state.position,
other_vehicle_state.position)
# check for activation distance
if sqd < activation_dist_squared:
direction = np.sum(
[other_vehicle_state.position, -self_vehicle_state.position],
axis=0)
angle = Heading(vec_to_radians(direction[:2]))
rel_angle = angle.relative_to(self_vehicle_state.heading)
return min_angle <= rel_angle <= max_angle
return False
def scan_for_vehicle(
target_prefix,
angle_a,
angle_b,
activation_dist_squared,
self_vehicle_state,
other_vehicle_state,
):
if target_prefix and not other_vehicle_state.id.startswith(target_prefix):
return False
min_angle, max_angle = min(angle_a, angle_b), max(angle_a, angle_b)
sqd = squared_dist(self_vehicle_state.position, other_vehicle_state.position)
# check for activation distance
if sqd < activation_dist_squared:
direction = np.sum(
[other_vehicle_state.position, -self_vehicle_state.position], axis=0
)
angle = Heading(vec_to_radians(direction[:2]))
rel_angle = angle.relative_to(self_vehicle_state.heading)
return min_angle <= rel_angle <= max_angle
return False
def step(
self,
sim,
):
captures_by_agent_id = defaultdict(list)
# Do an optimization to only check if there are pending agents.
if not sim.agent_manager.pending_agent_ids:
return
social_vehicle_ids = sim.vehicle_index.social_vehicle_ids
vehicles = {
v_id: sim.vehicle_index.vehicle_by_id(v_id)
for v_id in social_vehicle_ids
}
existing_agent_vehicles = (
sim.vehicle_index.vehicle_by_id(v_id)
for v_id in sim.vehicle_index.agent_vehicle_ids)
def largest_vehicle_plane_dimension(vehicle):
return max(*vehicle.chassis.dimensions.as_lwh[:2])
agent_vehicle_comp = [(v.position[:2],
largest_vehicle_plane_dimension(v), v)
for v in existing_agent_vehicles]
for agent_id in sim.agent_manager.pending_agent_ids:
trap = self._traps[agent_id]
if trap is None:
continue
trap.step_trigger(sim.timestep_sec)
if not trap.ready:
continue
# Order vehicle ids by distance.
sorted_vehicle_ids = sorted(
list(social_vehicle_ids),
key=lambda v: squared_dist(vehicles[v].position[:2], trap.
mission.start.position),
)
for v_id in sorted_vehicle_ids:
vehicle = vehicles[v_id]
point = Point(vehicle.position)
if any(
v.startswith(prefix)
for prefix in trap.exclusion_prefixes):
continue
if not point.within(trap.geometry):
continue
captures_by_agent_id[agent_id].append((
v_id,
trap,
replace(
trap.mission,
start=Start(vehicle.position[:2],
vehicle.pose.heading),
),
))
# TODO: Resolve overlap using a tree instead of just removing.
social_vehicle_ids.remove(v_id)
break
# Use fed in trapped vehicles.
agents_given_vehicle = set()
used_traps = []
for agent_id in sim._agent_manager.pending_agent_ids:
if agent_id not in self._traps:
continue
trap = self._traps[agent_id]
captures = captures_by_agent_id[agent_id]
if not trap.ready:
continue
vehicle = None
if len(captures) > 0:
vehicle_id, trap, mission = rand.choice(captures)
vehicle = TrapManager._hijack_vehicle(sim, vehicle_id,
agent_id, mission)
elif trap.patience_expired:
if len(agent_vehicle_comp) > 0:
agent_vehicle_comp.sort(key=lambda v: squared_dist(
v[0], trap.mission.start.position))
# Make sure there is not an agent vehicle in the same location
pos, largest_dimension, _ = agent_vehicle_comp[0]
if (squared_dist(pos, trap.mission.start.position) <
largest_dimension):
continue
vehicle = TrapManager._make_vehicle(sim, agent_id,
trap.mission)
else:
continue
if vehicle == None:
continue
agents_given_vehicle.add(agent_id)
used_traps.append((agent_id, trap))
for provider in sim.providers:
if (sim.agent_manager.agent_interface_for_agent_id(
agent_id).action_space in provider.action_spaces):
provider.create_vehicle(
VehicleState(
vehicle_id=vehicle.id,
vehicle_type="passenger",
pose=vehicle.pose,
dimensions=vehicle.chassis.dimensions,
speed=vehicle.speed,
source="EGO-HIJACK",
))
if len(agents_given_vehicle) > 0:
self.reset_traps(used_traps)
sim.agent_manager.remove_pending_agent_ids(agents_given_vehicle)
def step(self, sim):
"""Run hijacking and update agent and actor states."""
captures_by_agent_id = defaultdict(list)
# Do an optimization to only check if there are pending agents.
if not sim.agent_manager.pending_agent_ids:
return
social_vehicle_ids = [
v_id for v_id in sim.vehicle_index.social_vehicle_ids()
if not sim.vehicle_index.vehicle_is_shadowed(v_id)
]
vehicles = {
v_id: sim.vehicle_index.vehicle_by_id(v_id)
for v_id in social_vehicle_ids
}
def largest_vehicle_plane_dimension(vehicle):
return max(*vehicle.chassis.dimensions.as_lwh[:2])
vehicle_comp = [(v.position[:2], largest_vehicle_plane_dimension(v), v)
for v in vehicles.values()]
for agent_id in sim.agent_manager.pending_agent_ids:
trap = self._traps[agent_id]
if trap is None:
continue
trap.step_trigger(sim.last_dt)
if not trap.ready:
continue
# Order vehicle ids by distance.
sorted_vehicle_ids = sorted(
list(social_vehicle_ids),
key=lambda v: squared_dist(vehicles[v].position[:2], trap.
mission.start.position[:2]),
)
for v_id in sorted_vehicle_ids:
# Skip the capturing process if history traffic is used
if sim.scenario.traffic_history is not None:
break
if not trap.includes(v_id):
continue
vehicle = vehicles[v_id]
point = vehicle.pose.point.as_shapely
if not point.within(trap.geometry):
continue
captures_by_agent_id[agent_id].append((
v_id,
trap,
replace(
trap.mission,
start=Start(vehicle.position[:2],
vehicle.pose.heading),
),
))
# TODO: Resolve overlap using a tree instead of just removing.
social_vehicle_ids.remove(v_id)
break
# Use fed in trapped vehicles.
agents_given_vehicle = set()
used_traps = []
for agent_id in sim._agent_manager.pending_agent_ids:
if agent_id not in self._traps:
continue
trap = self._traps[agent_id]
captures = captures_by_agent_id[agent_id]
if not trap.ready:
continue
vehicle = None
if len(captures) > 0:
vehicle_id, trap, mission = rand.choice(captures)
vehicle = sim.switch_control_to_agent(vehicle_id,
agent_id,
mission,
recreate=True,
is_hijacked=False)
elif trap.patience_expired:
# Make sure there is not a vehicle in the same location
mission = trap.mission
nv_dims = Vehicle.agent_vehicle_dims(mission)
new_veh_maxd = max(nv_dims.as_lwh[:2])
overlapping = False
for pos, largest_dimension, _ in vehicle_comp:
if (squared_dist(pos, mission.start.position[:2]) <=
(0.5 * (largest_dimension + new_veh_maxd))**2):
overlapping = True
break
if overlapping:
continue
vehicle = TrapManager._make_vehicle(sim, agent_id, mission,
trap.default_entry_speed)
else:
continue
if vehicle == None:
continue
sim.create_vehicle_in_providers(vehicle, agent_id)
agents_given_vehicle.add(agent_id)
used_traps.append((agent_id, trap))
if len(agents_given_vehicle) > 0:
self.remove_traps(used_traps)
sim.agent_manager.remove_pending_agent_ids(agents_given_vehicle)
