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 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
