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]
