def are_on_same_lane(self, location1, location2):
""" Returns True if the two locations are on the same lane.
Args:
location1: Location in world coordinates.
location1: Location in world coordinates.
"""
loc1 = to_carla_location(location1)
waypoint1 = self._map.get_waypoint(loc1,
project_to_road=False,
lane_type=carla.LaneType.Driving)
if not waypoint1:
# First location is not on a drivable lane.
return False
loc2 = to_carla_location(location2)
waypoint2 = self._map.get_waypoint(loc2,
project_to_road=False,
lane_type=carla.LaneType.Driving)
if not waypoint2:
# Second location is not on a drivable lane.
return False
if waypoint1.road_id == waypoint2.road_id:
return waypoint1.lane_id == waypoint2.lane_id
else:
# Return False if we're in intersection and the other
# obstacle isn't.
if waypoint1.is_intersection and not waypoint2.is_intersection:
return False
if waypoint2.lane_type == carla.LaneType.Driving:
# This may return True when the lane is different, but in
# with a different road_id.
# TODO(ionel): Figure out how lane id map across road id.
return True
return False
def on_global_trajectory(self, msg):
self._logger.info('Global trajectory contains {} waypoints'.format(
len(msg.data)))
if len(msg.data) > 0:
# The last waypoint is the goal location.
self._goal_location = to_carla_location(msg.data[-1][0].location)
else:
# Trajectory does not contain any waypoints. We assume we have
# arrived at destionation.
self._goal_location = to_carla_location(
self._vehicle_transform.location)
assert self._goal_location, 'Planner does not have a goal'
self._waypoints = deque()
for waypoint_option in msg.data:
self._waypoints.append(waypoint_option[0])
def must_obbey_traffic_light(self, ego_location, tl_location):
""" Returns True if the ego vehicle must obbey the traffic light.
Args:
ego_location: Location of the ego vehicle in world coordinates.
tl_location: Location of the traffic light in world coordinates.
"""
loc = to_carla_location(ego_location)
waypoint = self._map.get_waypoint(loc,
project_to_road=False,
lane_type=carla.LaneType.Any)
if waypoint and waypoint.is_intersection:
# Do not obbey traffic light if ego is already in the intersection.
return False
# TODO(ionel): Implement.
# return (math.fabs(
# old_carla_map.get_lane_orientation_degrees(
# [vehicle_transform.location.x,
# vehicle_transform.location.y,
# 38]) -
# old_carla_map.get_lane_orientation_degrees(
# [closest_lane_point[0], closest_lane_point[1], 38])) < 1)
return True
def get_freenet_coordinates(self, location):
""" Returns s, d for a given Cartesian world location. """
# TODO(ionel): This method assumes that the location has the
# same orientation as the lanes (i.e., it will always return a
# positive d).
loc = to_carla_location(location)
waypoint = self._map.get_waypoint(loc,
project_to_road=False,
lane_type=carla.LaneType.Any)
current_lane_w = waypoint
d0_location = None
while True:
# Keep on moving left until we're outside the road or on the
# oposite lane.
left_lane_w = current_lane_w.get_left_lane()
if (left_lane_w.lane_type != carla.LaneType.Driving
or (current_lane_w.transform.rotation.yaw -
left_lane_w.transform.rotation.yaw) > 170):
# If the left lane is drivable then we've reached the left hand
# side of a one way road. Alternatively, if the lane is rotated
# then the lane is on the other side of the road.
d0_location = current_lane_w
half_lane = carla.Location(0, -current_lane_w.lane_width / 2.0,
0)
d0_location = current_lane_w.transform.transform(half_lane)
break
current_lane_w = left_lane_w
# TODO(ionel): Handle the case when the road id changes -> s resets.
# TODO(ionel): Handle case when the center lane is bidirectional.
return waypoint.s, self.__get_distance(location, d0_location)
def log_stop_signs(world):
world_map = world.get_map()
(_, traffic_lights, traffic_stops, speed_signs) = get_actors(world)
transforms_of_interest = []
# Add transforms that are close to stop signs.
for stop_sign in traffic_stops:
for offset in range(10, 25, 5):
offset_loc = pylot.simulation.utils.Location(x=-offset, y=0, z=0)
offset_rot = pylot.simulation.utils.Rotation(
pitch=0, yaw=0, roll=0)
offset_trans = pylot.simulation.utils.Transform(
offset_loc, offset_rot)
transform = stop_sign.transform * offset_trans
location = to_carla_location(transform.location)
w = world_map.get_waypoint(
location,
project_to_road=True,
lane_type=carla.LaneType.Driving)
camera_transform = to_pylot_transform(w.transform)
camera_transform.location.z += 2.0
transform = to_carla_transform(camera_transform)
transforms_of_interest.append(transform)
# Ensure all traffic lights are red.
change_traffic_light_colors(world, carla.TrafficLightState.Red)
world.tick()
time.sleep(3)
(_, traffic_lights, traffic_stops, speed_signs) = get_actors(world)
log_obstacles(world,
transforms_of_interest,
traffic_lights,
carla.TrafficLightState.Red,
speed_signs,
traffic_stops)
def log_speed_limits(world):
world_map = world.get_map()
(_, traffic_lights, traffic_stops, speed_signs) = get_actors(world)
transforms_of_interest = []
# Add transforms that are close to speed limit signs.
for speed_sign in speed_signs:
for offset in range(10, 25, 5):
# Speed signs have different coordinate systems, hence
# we need to offset y, instead of x.
offset_loc = pylot.simulation.utils.Location(x=0, y=offset, z=0)
offset_rot = pylot.simulation.utils.Rotation(pitch=0,
yaw=0,
roll=0)
offset_trans = pylot.simulation.utils.Transform(
offset_loc, offset_rot)
transform = speed_sign.transform * offset_trans
location = to_carla_location(transform.location)
w = world_map.get_waypoint(location,
project_to_road=True,
lane_type=carla.LaneType.Driving)
camera_transform = to_pylot_transform(w.transform)
camera_transform.location.z += 2.0
transform = to_carla_transform(camera_transform)
transforms_of_interest.append(transform)
# Ensure all traffic lights are red.
change_traffic_light_colors(world, carla.TrafficLightState.Red)
world.tick()
time.sleep(1)
(_, traffic_lights, traffic_stops, speed_signs) = get_actors(world)
for weather in find_weather_presets():
change_weather(world, weather)
log_obstacles(world, transforms_of_interest, traffic_lights,
carla.TrafficLightState.Red, speed_signs, traffic_stops,
weather, world_map.name)
def _must_obbey_american_traffic_light(self, ego_transform, tl_locations):
ego_loc = to_carla_location(ego_transform.location)
ego_waypoint = self._map.get_waypoint(ego_loc,
project_to_road=False,
lane_type=carla.LaneType.Any)
# We're not on a road, or we're already in the intersection. Carry on.
if ego_waypoint is None or ego_waypoint.is_intersection:
return (False, None)
min_angle = 25.0
selected_tl_loc = None
for tl_loc in tl_locations:
if is_within_distance_ahead(
ego_loc,
tl_loc,
ego_transform.rotation.yaw,
self._flags.traffic_light_max_dist_thres):
magnitude, angle = compute_magnitude_angle(
tl_loc,
ego_transform.location,
ego_transform.rotation.yaw)
if magnitude < 60.0 and angle < min(25.0, min_angle):
min_angle = angle
selected_tl_loc = tl_loc
if selected_tl_loc is not None:
return (True, selected_tl_loc)
else:
return (False, None)
def is_on_bidirectional_lane(self, location):
loc = to_carla_location(location)
waypoint = self._map.get_waypoint(
loc,
project_to_road=False,
lane_type=carla.LaneType.Bidirectional)
return not waypoint
def __update_waypoints(self):
""" Updates the waypoints.
Depending on setup, the method either recomputes the waypoints
between the ego vehicle and the goal location, or just garbage collects
waypoints that have already been achieved.
Returns:
(wp_steer, wp_speed): The waypoints to be used to compute steer and
speed angles.
"""
if self._recompute_waypoints:
ego_location = to_carla_location(self._vehicle_transform.location)
self._waypoints = self._map.compute_waypoints(
ego_location, self._goal_location)
self.__remove_completed_waypoints()
if not self._waypoints or len(self._waypoints) == 0:
# If waypoints are empty (e.g., reached destination), set waypoint
# to current vehicle location.
self._waypoints = deque([self._vehicle_transform])
return (self._waypoints[min(
len(self._waypoints) - 1,
self._wp_num_steer)], self._waypoints[min(
len(self._waypoints) - 1, self._wp_num_speed)])
def _must_obbey_european_traffic_light(self, ego_transform, tl_locations):
ego_loc = to_carla_location(ego_transform.location)
ego_waypoint = self._map.get_waypoint(ego_loc,
project_to_road=False,
lane_type=carla.LaneType.Any)
# We're not on a road, or we're already in the intersection. Carry on.
if ego_waypoint is None or ego_waypoint.is_intersection:
return (False, None)
# Iterate through traffic lights.
for tl_loc in tl_locations:
tl_waypoint = self._map.get_waypoint(to_carla_location(tl_loc))
if (tl_waypoint.road_id != ego_waypoint.road_id
or tl_waypoint.lane_id != ego_waypoint.lane_id):
continue
if is_within_distance_ahead(
ego_loc, tl_loc, ego_transform.rotation.yaw,
self._flags.traffic_light_max_dist_thres):
return (True, tl_loc)
return (False, None)
def is_on_lane(self, location):
loc = to_carla_location(location)
waypoint = self._map.get_waypoint(loc,
project_to_road=False,
lane_type=carla.LaneType.Driving)
if not waypoint:
# The map didn't return a waypoint because the location not within
# mapped location.
return False
else:
return True
def is_at_stop(self, location):
""" Returns True if the location is at a stop sign.
Args:
location: Location in world coordinates.
"""
# TODO(ionel): This method doesn't work yet because the opendrive do
# not contained waypoints annotated as stops.
loc = to_carla_location(location)
waypoint = self._map.get_waypoint(loc,
project_to_road=False,
lane_type=carla.LaneType.Stop)
return not waypoint
def get_closest_lane_waypoint(self, location):
""" Returns the road closest waypoint to location.
Args:
location: Location in world coordinates.
Returns:
A waypoint or None if no waypoint is found.
"""
loc = to_carla_location(location)
waypoint = self._map.get_waypoint(loc,
project_to_road=True,
lane_type=carla.LaneType.Any)
return waypoint
def is_on_opposite_lane(self, transform):
loc = to_carla_location(transform.location)
waypoint = self._map.get_waypoint(loc,
project_to_road=False,
lane_type=carla.LaneType.Driving)
if not waypoint:
return True
if waypoint.is_intersection:
return False
# XXX(ionel): Check logic.
if (abs(waypoint.transform.rotation.yaw - transform.rotation.yaw) >
140):
return True
else:
return False
def must_obbey_traffic_light(self, ego_location, tl_location):
""" Returns True if the ego vehicle must obbey the traffic light.
Args:
ego_location: Location of the ego vehicle in world coordinates.
tl_location: Location of the traffic light in world coordinates.
"""
loc = to_carla_location(ego_location)
waypoint = self._map.get_waypoint(loc,
project_to_road=False,
lane_type=carla.LaneType.Any)
if waypoint and waypoint.is_intersection:
# Do not obbey traffic light if ego is already in the intersection.
return False
# TODO(ionel): Implement.
return True
def is_intersection(self, location):
""" Returns True if the location is in an intersection.
Args:
location: Location in world coordinates.
"""
loc = to_carla_location(location)
waypoint = self._map.get_waypoint(loc,
project_to_road=False,
lane_type=carla.LaneType.Any)
if not waypoint:
# The map didn't return a waypoint because the location not within
# mapped location.
return False
else:
# XXX(ionel): is_intersection will be deprecated in the future
# Carla releases.
return waypoint.is_intersection
def distance_to_intersection(self, location, max_distance_to_check=30):
loc = to_carla_location(location)
waypoint = self._map.get_waypoint(loc,
project_to_road=False,
lane_type=carla.LaneType.Any)
if not waypoint:
return None
# We're already in an intersection.
if waypoint.is_intersection:
return 0
for i in range(1, max_distance_to_check + 1):
waypoints = waypoint.next(1)
if not waypoints or len(waypoints) == 0:
return None
for w in waypoints:
if w.is_intersection:
return i
waypoint = waypoints[0]
return None
def on_position_update(self, can_bus_msg):
""" Callback to be invoked on the receipt of a new update to the
position of the vehicle.
Uses the position of the vehicle to get future waypoints and draw
lane markings using those waypoints.
Args:
can_bus_msg: Contains the current location of the ego vehicle.
"""
vehicle_location = can_bus_msg.data.transform.location
lane_waypoints = []
next_wp = [
self._world_map.get_waypoint(to_carla_location(vehicle_location))
]
while len(next_wp) == 1:
lane_waypoints.append(next_wp[0])
next_wp = next_wp[0].next(self._waypoint_precision)
# Get the left and right markings of the lane and send it as a message.
left_markings = [
to_pylot_location(
self.lateral_shift(w.transform, -w.lane_width * 0.5))
for w in lane_waypoints
]
right_markings = [
to_pylot_location(
self.lateral_shift(w.transform, w.lane_width * 0.5))
for w in lane_waypoints
]
# Construct the DetectedLaneMessage.
detected_lanes = [
DetectedLane(left, right)
for left, right in zip(left_markings, right_markings)
]
output_msg = DetectedLaneMessage(detected_lanes, can_bus_msg.timestamp)
self.get_output_stream(self._output_stream_name).send(output_msg)
def log_traffic_lights(world):
world_map = world.get_map()
(traffic_lights, _, traffic_stops, speed_signs) = get_actors(world)
tl_colors = [
carla.TrafficLightState.Yellow, carla.TrafficLightState.Green,
carla.TrafficLightState.Red
]
for light in traffic_lights:
print("Working for traffic light {}".format(light.id))
# For every traffic light, get the neighbouring lights except the one
# directly opposite.
group_lights = []
for n_light in light.get_group_traffic_lights():
if not check_lights_opposite(light, n_light):
group_lights.append(convert_to_pylot_traffic_light(n_light))
transforms_of_interest = []
for offset in range(10, 40, 5):
# Traffic lights have different coordinate systems, hence
# we need to offset y, instead of x and add that to the trigger
# volume location.
offset_loc = pylot.simulation.utils.Location(
x=light.trigger_volume.location.x,
y=light.trigger_volume.location.y + offset,
z=light.trigger_volume.location.z)
offset_rot = pylot.simulation.utils.Rotation(pitch=0,
yaw=0,
roll=0)
offset_trans = pylot.simulation.utils.Transform(
offset_loc, offset_rot)
# Transform the offset relative to the traffic light.
transform = to_pylot_transform(
light.get_transform()) * offset_trans
location = to_carla_location(transform.location)
# Get the waypoint nearest to the transform.
w = world_map.get_waypoint(location,
project_to_road=True,
lane_type=carla.LaneType.Driving)
w_rotation = w.transform.rotation
camera_transform = to_pylot_transform(w.transform)
camera_transform.location.z += 2.0
transform = to_carla_transform(camera_transform)
transforms_of_interest.append(transform)
# Get the right lanes.
wp_right = w.get_right_lane()
while wp_right and wp_right.lane_type == carla.LaneType.Driving \
and w_rotation == wp_right.transform.rotation:
camera_transform = to_pylot_transform(wp_right.transform)
camera_transform.location.z += 2.0
transform = to_carla_transform(camera_transform)
transforms_of_interest.append(transform)
wp_right = wp_right.get_right_lane()
# Get the left lanes.
wp_left = w.get_left_lane()
while wp_left and wp_left.lane_type == carla.LaneType.Driving and \
w_rotation == wp_left.transform.rotation:
camera_transform = to_pylot_transform(wp_left.transform)
camera_transform.location.z += 2.0
transform = to_carla_transform(camera_transform)
transforms_of_interest.append(transform)
wp_left = wp_left.get_left_lane()
print("The total number of transforms were: {}".format(
len(transforms_of_interest)))
for tl_color in tl_colors:
change_traffic_light_colors(world, tl_color)
world.tick()
time.sleep(3)
log_obstacles(world, transforms_of_interest, group_lights,
tl_color, speed_signs, traffic_stops)
