def get_vehicle_velocity_vector(vehicle: carla.Vehicle, map_vehicle: carla.Map, velocity): """ Function to return a velocity vector which points to the direction of the next waypoint. :param velocity: Desired vehicle velocity :param map_vehicle: carla.Map :param vehicle: carla.Vehicle object :return: carla.Vector3D """ # Getting current waypoint and next from vehicle current_wp = map_vehicle.get_waypoint(vehicle.get_location()) next_wp = current_wp.next(1)[0] # Getting localization from the waypoints current_loc = get_localization_from_waypoint(current_wp) next_loc = get_localization_from_waypoint(next_wp) velocity_x = abs(next_loc.x - current_loc.x) velocity_y = abs(next_loc.y - current_loc.y) vector_vel0 = np.array([velocity_x, velocity_y, 0]) vector_vel = (velocity / np.linalg.norm(vector_vel0)) * vector_vel0 return carla.Vector3D(round(vector_vel[0], 3), round(vector_vel[1], 3), 0)
def get_nearest_traffic_light(vehicle: carla.Vehicle) -> Tuple[ carla.TrafficLight, float]: """ This method is specialized to check European style traffic lights. :param lights_list: list containing TrafficLight objects :return: a tuple given by (bool_flag, traffic_light), where - bool_flag is True if there is a traffic light in RED affecting us and False otherwise - traffic_light is the object itself or None if there is no red traffic light affecting us """ world = vehicle.get_world() # type: World lights_list = world.get_actors().filter("*traffic_light*") # type: List[carla.TrafficLight] ego_vehicle_location = vehicle.get_location() """ map = world.get_map() ego_vehicle_waypoint = map.get_waypoint(ego_vehicle_location) closest_traffic_light = None # type: Optional[carla.TrafficLight] closest_traffic_light_distance = math.inf for traffic_light in lights_list: object_waypoint = map.get_waypoint(traffic_light.get_location()) if object_waypoint.road_id != ego_vehicle_waypoint.road_id or \ object_waypoint.lane_id != ego_vehicle_waypoint.lane_id: continue distance_to_light = distance_transforms(traffic_light.get_transform(), vehicle.get_transform()) if distance_to_light < closest_traffic_light_distance: closest_traffic_light = traffic_light closest_traffic_light_distance = distance_to_light return closest_traffic_light, closest_traffic_light_distance """ min_angle = 180.0 closest_traffic_light = None # type: Optional[carla.TrafficLight] closest_traffic_light_distance = math.inf min_rotation_diff = 0 for traffic_light in lights_list: loc = traffic_light.get_location() distance_to_light, angle = compute_magnitude_angle(loc, ego_vehicle_location, vehicle.get_transform().rotation.yaw) rotation_diff = math.fabs( vehicle.get_transform().rotation.yaw - (traffic_light.get_transform().rotation.yaw - 90)) if distance_to_light < closest_traffic_light_distance and angle < 90 and (rotation_diff < 30 or math.fabs(360 - rotation_diff) < 30): closest_traffic_light_distance = distance_to_light closest_traffic_light = traffic_light min_angle = angle min_rotation_diff = rotation_diff # if closest_traffic_light is not None: # print("Ego rot: ", vehicle.get_transform().rotation.yaw, "TL rotation: ", closest_traffic_light.get_transform().rotation.yaw, ", diff: ", min_rotation_diff, ", dist: ", closest_traffic_light_distance) return closest_traffic_light, closest_traffic_light_distance
def closest_checkpoint(actor: carla.Vehicle, checkpoints: np.array): actor_location = location_to_numpy(actor.get_location()) distances = np.linalg.norm(checkpoints - actor_location, axis=1) azimuths = [ abs(calc_azimuth(actor_location[:2], point[:2])) for point in checkpoints ] cut = np.argmin(distances) if np.argmin(azimuths) >= cut + 1: return checkpoints[cut + 1] else: return checkpoints[cut]