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