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)