def _is_light_red_us_style(self, lights_list, debug=False):
"""
Private function specialized to check US 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)
if ego_vehicle_waypoint.is_intersection:
# It is too late. Do not block the intersection! Keep going!
return False, None
if self._local_planner.target_waypoint is not None:
if self._local_planner.target_waypoint.is_intersection:
min_angle = 180.0
sel_magnitude = 0.0
sel_traffic_light = None
for traffic_light in lights_list:
loc = traffic_light.get_location()
magnitude, angle = compute_magnitude_angle(
loc, ego_vehicle_location,
self._vehicle.get_transform().rotation.yaw)
if magnitude < 60.0 and angle < min(25.0, min_angle):
sel_magnitude = magnitude
sel_traffic_light = traffic_light
min_angle = angle
if sel_traffic_light is not None:
if debug:
print(
'=== Magnitude = {} | Angle = {} | ID = {}'.format(
sel_magnitude, min_angle,
sel_traffic_light.id))
if self._last_traffic_light is None:
self._last_traffic_light = sel_traffic_light
if self._last_traffic_light.state == carla.TrafficLightState.Red:
return True, self._last_traffic_light
else:
self._last_traffic_light = None
return False, None
def _is_light_red_us_style(self, lights_list): # pylint: disable=too-many-branches
"""
This method is specialized to check US 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)
if ego_vehicle_waypoint.is_junction:
# It is too late. Do not block the intersection! Keep going!
return (False, None)
if self._local_planner.target_waypoint is not None:
if self._local_planner.target_waypoint.is_junction:
min_angle = 180.0
sel_magnitude = 0.0 # pylint: disable=unused-variable
sel_traffic_light = None
for traffic_light in lights_list:
loc = traffic_light[1]
magnitude, angle = compute_magnitude_angle(
loc.pose.position, ego_vehicle_location,
math.degrees(self._vehicle_yaw))
if magnitude < 60.0 and angle < min(25.0, min_angle):
sel_magnitude = magnitude
sel_traffic_light = traffic_light[0]
min_angle = angle
if sel_traffic_light is not None:
# print('=== Magnitude = {} | Angle = {} | ID = {}'.format(
# sel_magnitude, min_angle, sel_traffic_light))
if self._last_traffic_light is None:
self._last_traffic_light = sel_traffic_light
state = None
for status in self._traffic_lights:
if status.id == sel_traffic_light:
state = status.state
break
if state is None:
rospy.logwarn(
"Couldn't get state of traffic light {}".format(
sel_traffic_light))
return (False, None)
if state == CarlaTrafficLightStatus.RED:
return (True, self._last_traffic_light)
else:
self._last_traffic_light = None
return (False, None)