def _is_light_red_europe_style(self, lights_list):
"""
Private function 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
"""
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
for traffic_light in lights_list:
object_waypoint = self._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
loc = traffic_light.get_location()
if is_within_distance_ahead(
loc, ego_vehicle_location,
self._vehicle.get_transform().rotation.yaw,
self._proximity_threshold):
if traffic_light.state == carla.TrafficLightState.Red:
return True, traffic_light
return False, None
def _is_light_red_europe_style(self, lights_list):
"""
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
"""
ego_vehicle_location = self._vehicle_location
ego_vehicle_waypoint = self.get_waypoint(ego_vehicle_location)
if not ego_vehicle_waypoint:
if not rospy.is_shutdown():
rospy.logwarn("Could not get waypoint for ego vehicle.")
return (False, None)
for traffic_light in lights_list:
object_waypoint = traffic_light[1]
if object_waypoint.road_id != ego_vehicle_waypoint.road_id or \
object_waypoint.lane_id != ego_vehicle_waypoint.lane_id:
continue
if is_within_distance_ahead(object_waypoint.pose.position, ego_vehicle_location,
math.degrees(self._vehicle_yaw),
self._proximity_threshold):
traffic_light_state = CarlaTrafficLightStatus.RED
for status in self._traffic_lights:
if status.id == traffic_light[0]:
traffic_light_state = status.state
break
if traffic_light_state == CarlaTrafficLightStatus.RED:
return (True, traffic_light[0])
return (False, None)
def _is_vehicle_hazard(self, vehicle_list):
"""
Private function used to 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.
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.
:param vehicle_list: list of potential obstacle to check
:return: a tuple given by (bool_flag, vehicle), where
- bool_flag is True if there is a vehicle ahead blocking us
and False otherwise
- vehicle is the blocker object itself
"""
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
for target_vehicle in vehicle_list:
# do not account for the ego vehicle
if target_vehicle.id == self._vehicle.id:
continue
# if the object is not in our lane it's not an obstacle
target_vehicle_waypoint = self._map.get_waypoint(
target_vehicle.get_location())
if target_vehicle_waypoint.road_id != ego_vehicle_waypoint.road_id or \
target_vehicle_waypoint.lane_id != ego_vehicle_waypoint.lane_id:
continue
loc = target_vehicle.get_location()
if is_within_distance_ahead(
loc, ego_vehicle_location,
self._vehicle.get_transform().rotation.yaw,
self._proximity_threshold):
return True, target_vehicle
return False, None
def _is_vehicle_hazard(self, vehicle_list, objects):
"""
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.
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.
:param vehicle_list: list of potential obstacle to check
:return: a tuple given by (bool_flag, vehicle), where
- bool_flag is True if there is a vehicle ahead blocking us
and False otherwise
- vehicle is the blocker object itself
"""
ego_vehicle_location = self._vehicle_location
ego_vehicle_waypoint = self.get_waypoint(ego_vehicle_location)
if not ego_vehicle_waypoint:
if not rospy.is_shutdown():
rospy.logwarn("Could not get waypoint for ego vehicle.")
return (False, None)
for target_vehicle_id in vehicle_list:
# do not account for the ego vehicle
if target_vehicle_id == self._vehicle_id:
continue
target_vehicle_location = None
for elem in objects:
if elem.id == target_vehicle_id:
target_vehicle_location = elem.pose
break
if not target_vehicle_location:
rospy.logwarn("Location of vehicle {} not found".format(target_vehicle_id))
continue
# if the object is not in our lane it's not an obstacle
target_vehicle_waypoint = self.get_waypoint(target_vehicle_location.position)
if not target_vehicle_waypoint:
if not rospy.is_shutdown():
rospy.logwarn("Could not get waypoint for target vehicle.")
return (False, None)
if target_vehicle_waypoint.road_id != ego_vehicle_waypoint.road_id or \
target_vehicle_waypoint.lane_id != ego_vehicle_waypoint.lane_id:
#rospy.loginfo("Vehicle {} is not in our lane".format(target_vehicle_id))
continue
if is_within_distance_ahead(target_vehicle_location.position, self._vehicle_location,
math.degrees(self._vehicle_yaw),
self._proximity_threshold):
return (True, target_vehicle_id)
return (False, None)
