def _bh_is_vehicle_hazard(self, ego_wpt, ego_loc, vehicle_list,
proximity_th, up_angle_th, low_angle_th=0, lane_offset=0):
"""
Check if a given vehicle is an obstacle in our way. To this end we take
into account the road and lane the target vehicle is on and run a
geometry test to check if the target vehicle is under a certain distance
in front of our ego vehicle. We also check the next waypoint, just to be
sure there's not a sudden road id change.
WARNING: This method is an approximation that could fail for very large
vehicles, which center is actually on a different lane but their
extension falls within the ego vehicle lane. Also, make sure to remove
the ego vehicle from the list. Lane offset is set to +1 for right lanes
and -1 for left lanes, but this has to be inverted if lane values are
negative.
:param ego_wpt: waypoint of ego-vehicle
:param ego_log: location of ego-vehicle
:param vehicle_list: list of potential obstacle to check
:param proximity_th: threshold for the agent to be alerted of
a possible collision
:param up_angle_th: upper threshold for angle
:param low_angle_th: lower threshold for angle
:param lane_offset: for right and left lane changes
:return: a tuple given by (bool_flag, vehicle, distance), where:
- bool_flag is True if there is a vehicle ahead blocking us
and False otherwise
- vehicle is the blocker object itself
- distance is the meters separating the two vehicles
"""
# Get the right offset
if ego_wpt.lane_id < 0 and lane_offset != 0:
lane_offset *= -1
for target_vehicle in vehicle_list:
target_vehicle_loc = target_vehicle.get_location()
# If the object is not in our next or current lane it's not an obstacle
target_wpt = self._map.get_waypoint(target_vehicle_loc)
if target_wpt.road_id != ego_wpt.road_id or \
target_wpt.lane_id != ego_wpt.lane_id + lane_offset:
next_wpt = self._local_planner.get_incoming_waypoint_and_direction(steps=5)[0]
if target_wpt.road_id != next_wpt.road_id or \
target_wpt.lane_id != next_wpt.lane_id + lane_offset:
continue
if is_within_distance(target_vehicle_loc, ego_loc,
self._vehicle.get_transform().rotation.yaw,
proximity_th, up_angle_th, low_angle_th):
return (True, target_vehicle, compute_distance(target_vehicle_loc, ego_loc))
return (False, None, -1)
def _vehicle_obstacle_detected(self, vehicle_list=None, max_distance=None):
"""
Method to check if there is a vehicle in front of the agent blocking its path.
:param vehicle_list (list of carla.Vehicle): list contatining vehicle objects.
If None, all vehicle in the scene are used
:param max_distance: max freespace to check for obstacles.
If None, the base threshold value is used
"""
if self._ignore_vehicles:
return (False, None)
if not vehicle_list:
vehicle_list = self._world.get_actors().filter("*vehicle*")
if not max_distance:
max_distance = self._base_vehicle_threshold
ego_transform = self._vehicle.get_transform()
ego_wpt = self._map.get_waypoint(self._vehicle.get_location())
# Get the transform of the front of the ego
ego_forward_vector = ego_transform.get_forward_vector()
ego_extent = self._vehicle.bounding_box.extent.x
ego_front_transform = ego_transform
ego_front_transform.location += carla.Location(
x=ego_extent * ego_forward_vector.x,
y=ego_extent * ego_forward_vector.y,
)
for target_vehicle in vehicle_list:
target_transform = target_vehicle.get_transform()
target_wpt = self._map.get_waypoint(target_transform.location)
if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id:
next_wpt = self._local_planner.get_incoming_waypoint_and_direction(
steps=3)[0]
if not next_wpt:
continue
if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id:
continue
target_forward_vector = target_transform.get_forward_vector()
target_extent = target_vehicle.bounding_box.extent.x
target_rear_transform = target_transform
target_rear_transform.location -= carla.Location(
x=target_extent * target_forward_vector.x,
y=target_extent * target_forward_vector.y,
)
if is_within_distance(target_rear_transform, ego_front_transform,
max_distance, [0, 90]):
return (True, target_vehicle)
return (False, None)
def _affected_by_traffic_light(self, lights_list=None, max_distance=None):
"""
Method to check if there is a red light affecting the vehicle.
:param lights_list (list of carla.TrafficLight): list containing TrafficLight objects.
If None, all traffic lights in the scene are used
:param max_distance (float): max distance for traffic lights to be considered relevant.
If None, the base threshold value is used
"""
if self._ignore_traffic_lights:
return (False, None)
if not lights_list:
lights_list = self._world.get_actors().filter("*traffic_light*")
if not max_distance:
max_distance = self._base_tlight_threshold
if self._last_traffic_light:
if self._last_traffic_light.state != carla.TrafficLightState.Red:
self._last_traffic_light = None
else:
return (True, self._last_traffic_light)
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
for traffic_light in lights_list:
object_location = get_trafficlight_trigger_location(traffic_light)
object_waypoint = self._map.get_waypoint(object_location)
if object_waypoint.road_id != ego_vehicle_waypoint.road_id:
continue
ve_dir = ego_vehicle_waypoint.transform.get_forward_vector()
wp_dir = object_waypoint.transform.get_forward_vector()
dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z
if dot_ve_wp < 0:
continue
if traffic_light.state != carla.TrafficLightState.Red:
continue
if is_within_distance(object_waypoint.transform,
self._vehicle.get_transform(), max_distance,
[0, 90]):
self._last_traffic_light = traffic_light
return (True, traffic_light)
return (False, None)
def _vehicle_obstacle_detected(self,
vehicle_list=None,
max_distance=None,
up_angle_th=90,
low_angle_th=0,
lane_offset=0):
"""
Method to check if there is a vehicle in front of the agent blocking its path.
:param vehicle_list (list of carla.Vehicle): list contatining vehicle objects.
If None, all vehicle in the scene are used
:param max_distance: max freespace to check for obstacles.
If None, the base threshold value is used
"""
if self._ignore_vehicles:
return (False, None, -1)
if not vehicle_list:
vehicle_list = self._world.get_actors().filter("*vehicle*")
if not max_distance:
max_distance = self._base_vehicle_threshold
ego_transform = self._vehicle.get_transform()
ego_wpt = self._map.get_waypoint(self._vehicle.get_location())
# Get the right offset
if ego_wpt.lane_id < 0 and lane_offset != 0:
lane_offset *= -1
# Get the transform of the front of the ego
ego_forward_vector = ego_transform.get_forward_vector()
ego_extent = self._vehicle.bounding_box.extent.x
ego_front_transform = ego_transform
ego_front_transform.location += carla.Location(
x=ego_extent * ego_forward_vector.x,
y=ego_extent * ego_forward_vector.y,
)
for target_vehicle in vehicle_list:
target_transform = target_vehicle.get_transform()
target_wpt = self._map.get_waypoint(target_transform.location,
lane_type=carla.LaneType.Any)
# Simplified version for outside junctions
if not ego_wpt.is_junction or not target_wpt.is_junction:
if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id + lane_offset:
next_wpt = self._local_planner.get_incoming_waypoint_and_direction(
steps=3)[0]
if not next_wpt:
continue
if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id + lane_offset:
continue
target_forward_vector = target_transform.get_forward_vector()
target_extent = target_vehicle.bounding_box.extent.x
target_rear_transform = target_transform
target_rear_transform.location -= carla.Location(
x=target_extent * target_forward_vector.x,
y=target_extent * target_forward_vector.y,
)
if is_within_distance(target_rear_transform,
ego_front_transform, max_distance,
[low_angle_th, up_angle_th]):
return (True, target_vehicle,
compute_distance(target_transform.location,
ego_transform.location))
# Waypoints aren't reliable, check the proximity of the vehicle to the route
else:
route_bb = []
ego_location = ego_transform.location
extent_y = self._vehicle.bounding_box.extent.y
r_vec = ego_transform.get_right_vector()
p1 = ego_location + carla.Location(extent_y * r_vec.x,
extent_y * r_vec.y)
p2 = ego_location + carla.Location(-extent_y * r_vec.x,
-extent_y * r_vec.y)
route_bb.append([p1.x, p1.y, p1.z])
route_bb.append([p2.x, p2.y, p2.z])
for wp, _ in self._local_planner.get_plan():
if ego_location.distance(
wp.transform.location) > max_distance:
break
r_vec = wp.transform.get_right_vector()
p1 = wp.transform.location + carla.Location(
extent_y * r_vec.x, extent_y * r_vec.y)
p2 = wp.transform.location + carla.Location(
-extent_y * r_vec.x, -extent_y * r_vec.y)
route_bb.append([p1.x, p1.y, p1.z])
route_bb.append([p2.x, p2.y, p2.z])
if len(route_bb) < 3:
# 2 points don't create a polygon, nothing to check
return (False, None, -1)
ego_polygon = Polygon(route_bb)
# Compare the two polygons
for target_vehicle in vehicle_list:
target_extent = target_vehicle.bounding_box.extent.x
if target_vehicle.id == self._vehicle.id:
continue
if ego_location.distance(
target_vehicle.get_location()) > max_distance:
continue
target_bb = target_vehicle.bounding_box
target_vertices = target_bb.get_world_vertices(
target_vehicle.get_transform())
target_list = [[v.x, v.y, v.z] for v in target_vertices]
target_polygon = Polygon(target_list)
if ego_polygon.intersects(target_polygon):
return (True, target_vehicle,
compute_distance(target_vehicle.get_location(),
ego_location))
return (False, None, -1)
return (False, None, -1)