def _is_light_red_us_style(self, lights_list, debug=False):
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
traffic_light_color = "NONE" # default, if no traffic lights are seen
if ego_vehicle_waypoint.is_junction:
# It is too late. Do not block the intersection! Keep going!
return "JUNCTION"
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
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 "RED"
elif (self._last_traffic_light.state ==
carla.TrafficLightState.Yellow):
traffic_light_color = "YELLOW"
elif (self._last_traffic_light.state ==
carla.TrafficLightState.Green):
if traffic_light_color is not "YELLOW": # (more severe)
traffic_light_color = "GREEN"
else:
import pdb
pdb.set_trace()
# investigate https://carla.readthedocs.io/en/latest/python_api/#carlatrafficlightstate
else:
self._last_traffic_light = None
return traffic_light_color
def _is_light_red_us_style(self, lights_list, debug=False):
"""
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.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:
potential_lights = []
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 < 80.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.libcarla.TrafficLightState.Red:
return (True, self._last_traffic_light)
print(traffic_light.state)
else:
self._last_traffic_light = None
return (False, None)
def _is_light_red_us_style(self, lights_list, debug=False):
"""This method is specialized to check US style traffic lights."""
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
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
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:
logging.debug(
'=== 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: # pylint: disable=no-member
return (True, self._last_traffic_light)
else:
self._last_traffic_light = None
return (False, None)
def run_step(self, debug=True):
# not enough waypoints in the horizon? => add more!
if len(self._waypoints_queue) < int(
self._waypoints_queue.maxlen * 0.5):
if not self._global_plan:
self._compute_next_waypoints(k=100)
if len(self._waypoints_queue) == 0:
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 0.0
control.hand_brake = False
control.manual_gear_shift = False
return control
veh_location = self._vehicle.get_location() # type: carla.Location
veh_waypoint = self._map.get_waypoint(
veh_location) # type: carla.Waypoint
veh_yaw = self._vehicle.get_transform(
).rotation.yaw # TODO type: float, range TODO
local_plan = self.get_filled_waypoint_buffer(
) # type: List[carla.Waypoint]
# Calculate best waypoint to follow considering current yaw
L = 2.9
fx = veh_location.x + L * np.cos(veh_yaw)
fy = veh_location.y + L * np.sin(veh_yaw)
distances = []
for waypoint in local_plan:
wp = waypoint.transform.location
dx = fx - wp.x
dy = fy - wp.y
distance = np.sqrt(dx**2 + dy**2)
distances.append(distance)
target_idx = np.argmin(distances)
closest_error = distances[target_idx]
self._target_waypoint = local_plan[target_idx]
# Calculate path curvature
waypoints_to_look_ahead = 6
reference_waypoint = local_plan[target_idx + waypoints_to_look_ahead]
ref_location = reference_waypoint.transform.location
# delta_x = ref_location.x - veh_location.x
# delta_y = ref_location.y - veh_location.y
# theta_radians = math.atan2(delta_y, delta_x)
# FIXME Sometimes yaw oscilates from 179 to -179 which leads to temporarily bad calculations
distance, relative_angle = compute_magnitude_angle(
ref_location, veh_location,
veh_yaw) #np.rad2deg(theta_radians) - veh_yaw
# plt.cla()
# plt.plot([self._vehicle.get_transform().location.x, lookahead_waypoint.transform.location.x], [self._vehicle.get_transform().location.y, lookahead_waypoint.transform.location.y], "-r", label="debug")
# # plt.plot(, , ".b", label="lookahead")
# plt.axis("equal")
# plt.legend()
# plt.grid(True)
# plt.title("Rel angle: {}, yaw {}".format(str(angle), yaw))
# plt.pause(0.0001)
if abs(relative_angle) < 15:
target_speed = 50
elif abs(relative_angle) < 20:
target_speed = 40
elif abs(relative_angle) < 30:
target_speed = 30
else:
target_speed = 20
print(
'Relative angle to reference waypoint: {:3d} | Vehicle yaw angle: {:3d} | Target speed {} km/h'
.format(int(relative_angle), int(veh_yaw), target_speed))
control = self._vehicle_controller.run_step(target_speed,
self._target_waypoint)
# purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
# i, (waypoint, _)
for i, waypoint in enumerate(self._waypoint_buffer):
if distance_vehicle(waypoint,
vehicle_transform) < self._min_distance:
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
self._waypoint_buffer.popleft()
if debug:
#draw_waypoints(self._vehicle.get_world(), [self._target_waypoint], self._vehicle.get_location().z + 1.0, color=carla.Color(0, 255, 0))
draw_waypoints(self._vehicle.get_world(), [reference_waypoint],
veh_location.z + 1.0)
return control
