def interpolate_trajectory(world_map,
waypoints_trajectory,
hop_resolution=1.0):
"""
Given some raw keypoints interpolate a full dense trajectory to be used by the user.
Args:
world: an reference to the CARLA world so we can use the planner
waypoints_trajectory: the current coarse trajectory
hop_resolution: is the resolution, how dense is the provided trajectory going to be made
Return:
route: full interpolated route both in GPS coordinates and also in its original form.
"""
dao = GlobalRoutePlannerDAO(world_map, hop_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
# Obtain route plan
route = []
for i in range(len(waypoints_trajectory) -
1): # Goes until the one before the last.
waypoint = waypoints_trajectory[i]
waypoint_next = waypoints_trajectory[i + 1]
interpolated_trace = grp.trace_route(waypoint, waypoint_next)
for wp_tuple in interpolated_trace:
route.append((wp_tuple[0].transform, wp_tuple[1]))
return route
def _set_route(self, hop_resolution=1.0):
world = CarlaDataProvider.get_world()
dao = GlobalRoutePlannerDAO(world.get_map(), hop_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
spawn_points = CarlaDataProvider._spawn_points
# start, target = random.choice(spawn_points, size=2)
start_idx, target_idx = random.choice(len(spawn_points), size=2)
# DEBUG
# start_idx, target_idx = 57, 87
# start_idx, target_idx = 2, 18
# print (start_idx, target_idx)
start = spawn_points[start_idx]
target = spawn_points[target_idx]
route = grp.trace_route(start.location, target.location)
self.route = [(w.transform, c) for w, c in route]
CarlaDataProvider.set_ego_vehicle_route([(w.transform.location, c)
for w, c in route])
gps_route = location_route_to_gps(self.route, *_get_latlon_ref(world))
self.config.agent.set_global_plan(gps_route, self.route)
self.timeout = self._estimate_route_timeout()
def __init__(self,
actor,
osc_position,
distance,
along_route=False,
comparison_operator=operator.lt,
name="InTriggerDistanceToOSCPosition"):
"""
Setup parameters
"""
super(InTriggerDistanceToOSCPosition, self).__init__(name)
self._actor = actor
self._osc_position = osc_position
self._distance = distance
self._along_route = along_route
self._comparison_operator = comparison_operator
self._map = CarlaDataProvider.get_map()
if self._along_route:
# Get the global route planner, used to calculate the route
dao = GlobalRoutePlannerDAO(self._map, 0.5)
grp = GlobalRoutePlanner(dao)
grp.setup()
self._grp = grp
else:
self._grp = None
def __init__(self,
actor,
other_actor,
time,
condition_freespace=False,
along_route=False,
comparison_operator=operator.lt,
name="TimeToArrival"):
"""
Setup parameters
"""
super(InTimeToArrivalToVehicle, self).__init__(name)
self.logger.debug("%s.__init__()" % (self.__class__.__name__))
self._map = CarlaDataProvider.get_map()
self._actor = actor
self._other_actor = other_actor
self._time = time
self._condition_freespace = condition_freespace
self._along_route = along_route
self._comparison_operator = comparison_operator
if self._along_route:
# Get the global route planner, used to calculate the route
dao = GlobalRoutePlannerDAO(self._map, 0.5)
grp = GlobalRoutePlanner(dao)
grp.setup()
self._grp = grp
else:
self._grp = None
def interpolate_trajectory(world, waypoints_trajectory, hop_resolution=1.0):
"""
This method is fixed based on original interpolate_trajectory
Given some raw keypoints interpolate a full dense trajectory to be used by the user.
:param world: an reference to the CARLA world so we can use the planner
:param waypoints_trajectory: the current coarse trajectory
:param hop_resolution: is the resolution, how dense is the provided trajectory going to be made
:return: the full interpolated route both in waypoints.
"""
dao = GlobalRoutePlannerDAO(world.get_map(), hop_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
# Obtain route plan
route = [] # waypoints
for i in range(len(waypoints_trajectory) -
1): # Goes until the one before the last.
waypoint = waypoints_trajectory[i]
waypoint_next = waypoints_trajectory[i + 1]
interpolated_trace = grp.trace_route(waypoint, waypoint_next)
for wp_tuple in interpolated_trace:
route.append((wp_tuple[0], wp_tuple[1]))
return route
def interpolate_trajectory(world, waypoints_trajectory, hop_resolution=1.0):
"""
Given some raw keypoints interpolate a full dense trajectory to be used by the user.
:param world: an reference to the CARLA world so we can use the planner
:param waypoints_trajectory: the current coarse trajectory
:param hop_resolution: is the resolution, how dense is the provided trajectory going to be made
:return: the full interpolated route both in GPS coordinates and also in its original form.
"""
dao = GlobalRoutePlannerDAO(world.get_map(), hop_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
# Obtain route plan
route = []
for i in range(len(waypoints_trajectory) - 1): # Goes until the one before the last.
waypoint = waypoints_trajectory[i]
waypoint_next = waypoints_trajectory[i + 1]
interpolated_trace = grp.trace_route(waypoint, waypoint_next)
for wp_tuple in interpolated_trace:
route.append((wp_tuple[0].transform, wp_tuple[1]))
# Increase the route position to avoid fails
lat_ref, lon_ref = _get_latlon_ref(world)
return location_route_to_gps(route, lat_ref, lon_ref), route
def main():
client = carla.Client('localhost', 2000)
client.set_timeout(10)
world = client.get_world()
map = world.get_map()
vehicle = world.get_actor(87)
location1 = map.get_waypoint(vehicle.get_location())
print(location1)
#waypoint1 = vehicle.get_location()
#custom_controller = VehiclePIDController(vehicle, args_lateral = {'K_P': 1, 'K_D': 0.0, 'K_I': 0}, args_longitudinal = {'K_P': 1, 'K_D': 0.0, 'K_I': 0.0})
vehicle2 = world.get_actor(86)
vehicle2.set_simulate_physics(True)
location2 = map.get_waypoint(vehicle2.get_location())
print(location2)
#waypoint2 = vehicle.get_location()
#vehicle2.set_transform(waypoint.transform)
#control_signal = custom_controller.run_step(1, waypoint)
#vehicle.apply_control(control_signal)
#Routeplanner
amap = world.get_map()
sampling_resolution = 2
dao = GlobalRoutePlannerDAO(amap, sampling_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
spawn_points = world.get_map().get_spawn_points()
spawn_point = spawn_points[50]
vehicle2.set_transform(spawn_point)
spawn_point2 = spawn_points[100]
vehicle.set_transform(spawn_point2)
a = carla.Location(spawn_points[50].location)
b = carla.Location(spawn_points[100].location)
w1 = grp.trace_route(a, b)
i = 0
for w in w1:
if i % 10 == 0:
world.debug.draw_string(w[0].transform.location,
'O',
draw_shadow=False,
color=carla.Color(r=255, g=0, b=0),
life_time=120.0,
persistent_lines=True)
else:
world.debug.draw_string(w[0].transform.location,
'O',
draw_shadow=False,
color=carla.Color(r=0, g=0, b=255),
life_time=1000.0,
persistent_lines=True)
i += 1
def get_planner_command(self, current_point, end_point):
current_location = current_point.location
end_location = end_point.location
global_Dao = GlobalRoutePlannerDAO(self.map)
global_planner = GlobalRoutePlanner(global_Dao)
global_planner.setup()
commands = global_planner.abstract_route_plan(current_location,
end_location)
direction = commands[0].value
return direction
def get_global_planner(
world: carla.libcarla.World, planner_resolution: float
) -> agents.navigation.global_route_planner.GlobalRoutePlanner:
"""
Get a global route planner object. The planner object can be used to retrieve point to point routes and route topologies.
"""
world_map = world.get_map()
dao = GlobalRoutePlannerDAO(world_map, planner_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
return grp
class Planner:
def __init__(self, world_map, sampling_resolution,
waypoint_error_threshold):
self.global_route_planner_dao = GlobalRoutePlannerDAO(world_map, 1)
self.global_route_planner = GlobalRoutePlanner(
self.global_route_planner_dao)
self.global_route_planner.setup()
self.endpoint = None
self.waypoints = None
self.target_waypoint_i = 0
self.waypoint_error_threshold = waypoint_error_threshold
def set_endpoint(self, vehicle, endpoint):
self.endpoint = endpoint
vehicle_location = vehicle.get_transform().location
endpoint_location = endpoint.transform.location
self.waypoints = self.global_route_planner.trace_route(
vehicle_location, endpoint_location)
self.waypoints = [waypoint[0] for waypoint in self.waypoints]
self.target_waypoint_i = 0
print("Set new endpoint")
print("Path length: ", len(self.waypoints))
# print("Path: ")
# for w in self.waypoints:
# print(w.transform.location)
print("First waypoint: ", self.waypoints[0].transform.location)
def get_target_waypoint(self, vehicle):
vehicle_location = vehicle.get_transform().location
target_waypoint = self.waypoints[self.target_waypoint_i]
if target_waypoint.transform.location.distance(
vehicle_location) < self.waypoint_error_threshold:
if self.target_waypoint_i + 1 >= len(self.waypoints):
print('No more waypoints')
return None
self.target_waypoint_i += 1
target_waypoint = self.waypoints[self.target_waypoint_i]
print('New waypoint (', self.target_waypoint_i, '/',
len(self.waypoints), '): ',
target_waypoint.transform.location)
return target_waypoint
def reached_endpoint(self, vehicle):
vehicle_location = vehicle.get_transform().location
end_waypoint = self.waypoints[-1]
if end_waypoint.transform.location.distance(
vehicle_location) < self.waypoint_error_threshold:
print('Reached endpoint')
return True
return False
def calculate_route(self, goal):
"""
Calculate a route from the current location to 'goal'
"""
rospy.loginfo("Calculating route to x={}, y={}, z={}".format(
goal.location.x, goal.location.y, goal.location.z))
dao = GlobalRoutePlannerDAO(self.world.get_map())
grp = GlobalRoutePlanner(dao)
grp.setup()
route = grp.trace_route(
self.ego_vehicle.get_location(),
carla.Location(goal.location.x, goal.location.y, goal.location.z))
return route
def main():
#pygame.init()
client = carla.Client('localhost', 2000)
client.set_timeout(10)
world = client.get_world()
vehicle2 = world.get_actor(86)
frame = None
#custom_controller = VehiclePIDController(vehicle2, args_lateral = {'K_P': 1, 'K_D': 0.0, 'K_I': 0}, args_longitudinal = {'K_P': 1, 'K_D': 0.0, 'K_I': 0.0})
#print('Enabling synchronous mode')
#settings = world.get_settings()
#settings.synchronous_mode = True
#world.apply_settings(settings)
vehicle2.set_simulate_physics(True)
amap = world.get_map()
sampling_resolution = 2
dao = GlobalRoutePlannerDAO(amap, sampling_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
spawn_points = world.get_map().get_spawn_points()
driving_car = BasicAgent(vehicle2, target_speed=15)
destiny = spawn_points[100].location
driving_car.set_destination((destiny.x, destiny.y, destiny.z))
vehicle2.set_autopilot(True)
#clock = pygame.time.Clock()
while True:
#clock.tick()
world.tick()
ts = world.wait_for_tick()
# Get control commands
control_hero = driving_car.run_step()
vehicle2.apply_control(control_hero)
if frame is not None:
if ts.frame_count != frame + 1:
logging.warning('frame skip!')
print("frame skip!")
frame = ts.frame_count
def _trace_route(self, start_waypoint, end_waypoint):
"""
This method sets up a global router and returns the optimal route
from start_waypoint to end_waypoint
"""
# Setting up global router
if self._grp is None:
dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(), self._hop_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
self._grp = grp
# Obtain route plan
route = self._grp.trace_route(start_waypoint.transform.location, end_waypoint.transform.location)
return route
class Planner(object):
def __init__(self, vehicle = None):
self._vehicle = None
self.global_planner = None
self.local_planner = None
self.resolution = 20.0
self.route_trace = None
if vehicle is not None:
self.initialize(vehicle)
def initialize(self, vehicle):
self._vehicle = vehicle
dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(), self.resolution)
self.global_planner = GlobalRoutePlanner(dao)
self.global_planner.setup()
self.local_planner = LocalPlanner(self._vehicle)
def set_destination(self, location):
start_waypoint = self._vehicle.get_world().get_map().get_waypoint(self._vehicle.get_location())
end_waypoint = self._vehicle.get_world().get_map().get_waypoint(carla.Location(location[0], location[1], location[2]))
self.route_trace = self.global_planner.trace_route(start_waypoint.transform.location, end_waypoint.transform.location)
#self.route_trace.pop(0)
#assert self.route_trace
self.local_planner.set_global_plan(self.route_trace)
def run_step(self):
return self.local_planner.run_step(False)
def view_plan(self):
for w in self.route_trace:
self._vehicle.get_world().debug.draw_string(w[0].transform.location, 'o', draw_shadow=False,
color=carla.Color(r=255, g=0, b=0), life_time=120.0,
persistent_lines=True)
def done(self):
return self.local_planner.done()
def global_plan(
world: carla.World, # pylint: disable=no-member
origin: carla.Location, # pylint: disable=no-member
destination: carla.Location, # pylint: disable=no-member
) -> Tuple[Sequence[carla.Waypoint], Sequence[Any], float]: # pylint: disable=no-member
"""Generates the optimal plan between two location, respecting the topology.
Args:
world: The `CARLA` world.
origin: The starting location.
destination: The final destination.
Returns:
waypoints: A sequence of waypoints.
roadoptions: A sequence of commands to navigate at each waypoint.
distances: The distance per pair of waypoints of the plan.
"""
try:
from agents.navigation.global_route_planner import GlobalRoutePlanner # pylint: disable=import-error
from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO # pylint: disable=import-error
except ImportError:
raise ImportError(
"Missing CARLA installation, "
"make sure the environment variable CARLA_ROOT is provided "
"and that the PythonAPI is `easy_install`ed")
# Setup global planner.
grp_dao = GlobalRoutePlannerDAO(wmap=world.get_map(),
sampling_resolution=1)
grp = GlobalRoutePlanner(grp_dao)
grp.setup()
# Generate plan.
waypoints, roadoptions = zip(*grp.trace_route(origin, destination))
# Accummulate pairwise distance.
distances = [0.0]
for i in range(1, len(waypoints)):
loc_tm1 = waypoints[i - 1].transform.location
loc_tm1 = np.asarray([loc_tm1.x, loc_tm1.y, loc_tm1.z])
loc_t = waypoints[i].transform.location
loc_t = np.asarray([loc_t.x, loc_t.y, loc_t.z])
distances.append(np.linalg.norm(loc_tm1 - loc_t))
return waypoints, roadoptions, distances
def main():
global actor_list
client = carla.Client(host, port)
client.set_timeout(10.0)
try:
world = client.get_world()
except:
print("ERROR: Cannot get world !")
import sys
sys.exit()
#set_weather(world)
try:
blueprint_library = world.get_blueprint_library()
# add vehicle
vehicle = add_vehicle_component(world, blueprint_library)
# put the vehicle to drive around.
vehicle.set_autopilot(True)
map = world.get_map()
dao = GlobalRoutePlannerDAO(map)
planner = GlobalRoutePlanner(dao)
planner.setup()
G = planner._graph
plt.subplot(111)
nx.draw_networkx_edges(G, pos=nx.spring_layout(G))
plt.savefig("path.pdf")
#time.sleep(1.0)
finally:
print('destroying actors')
for actor in actor_list:
actor.destroy()
actor_list = []
print('done.')
def _set_route(self, hop_resolution=1.0):
world = CarlaDataProvider.get_world()
dao = GlobalRoutePlannerDAO(world.get_map(), hop_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
spawn_points = CarlaDataProvider._spawn_points
start = spawn_points[self.start_idx]
target = spawn_points[self.target_idx]
route = grp.trace_route(start.location, target.location)
self.route = [(w.transform, c) for w, c in route]
CarlaDataProvider.set_ego_vehicle_route([(w.transform.location, c)
for w, c in route])
gps_route = location_route_to_gps(self.route, *_get_latlon_ref(world))
self.agent.set_global_plan(gps_route, self.route)
self.timeout = self._estimate_route_timeout()
def _trace_route(self, start_waypoint, end_waypoint):
"""
This method sets up a global router and returns the
optimal route from start_waypoint to end_waypoint.
:param start_waypoint: initial position
:param end_waypoint: final position
"""
# Setting up global router
if self._grp is None:
wld = self.vehicle.get_world()
dao = GlobalRoutePlannerDAO(
wld.get_map(), sampling_resolution=self._sampling_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
self._grp = grp
# Obtain route plan
route = self._grp.trace_route(start_waypoint.transform.location,
end_waypoint.transform.location)
return route
def main():
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
world = client.get_world()
_map = world.get_map()
dao = GlobalRoutePlannerDAO(_map)
grp = GlobalRoutePlanner(dao)
grp.setup()
blueprint_library = world.get_blueprint_library()
# vehicle
blueprint = random.choice(
world.get_blueprint_library().filter('vehicle.lin*'))
spawn_points = world.get_map().get_spawn_points()
spawn_point = random.choice(
spawn_points) if spawn_points else carla.Transform()
_vehicle = world.try_spawn_actor(blueprint, spawn_point)
print(_vehicle)
start_waypoint = _map.get_waypoint(_vehicle.get_location())
end_waypoint = random.choice(
spawn_points) if spawn_points else carla.Transform()
# Obtain route plan
x1 = start_waypoint.transform.location.x
y1 = start_waypoint.transform.location.y
x2 = end_waypoint.location.x
y2 = end_waypoint.location.y
print(x1, x2, y1, y2)
_graph, _id_map = grp.build_graph()
print(_graph)
map_name = route.get('map')
world = client.get_world()
world_map = world.get_map()
print("Dealing with map : ", map_name)
if map_name != world_map.name:
print("Loading map : ", map_name)
world = client.load_world(map_name)
world.wait_for_tick()
world_map = world.get_map()
dao = GlobalRoutePlannerDAO(world_map)
grp = GlobalRoutePlanner(dao)
grp.setup()
corrected_route = ET.SubElement(corrected_xml_data, 'route')
corrected_route.set('id', route.get('id'))
corrected_route.set('map', route.get('map'))
print("Dealing with route : ", route.get('id'))
for point in route.iter('waypoint'):
location = carla.Location(float(point.attrib['x']),
float(point.attrib['y']),
float(point.attrib['z']))
edge_nodes = grp._localize(location)
if edge_nodes is not None and True:
n1, n2 = edge_nodes
if n1 > -1 and n2 > -1:
edge = grp._graph.edges[n1, n2]
if edge['intersection']:
class HDMap(object):
def __init__(self, carla_map, log_file_name=None):
self._map = carla_map
# Setup global planner.
self._grp = GlobalRoutePlanner(
GlobalRoutePlannerDAO(
self._map,
1.0 # Distance between waypoints
))
self._grp.setup()
self._logger = setup_logging('hd_map', log_file_name)
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_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 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 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 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 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 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 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 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 _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 _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 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 get_left_lane(self, location):
# TODO(ionel): Implement!
pass
def get_right_lane(self, location):
# TODO(ionel): Implement!
pass
def __get_distance(self, location1, location2):
return math.sqrt((location1.x - location2.x)**2 +
(location1.y - location2.y)**2)
def compute_waypoints(self, source_loc, destination_loc):
""" Computes waypoints between two locations.
Assumes that the ego vehicle has the same orientation as
the lane on whch it is on.
Args:
source_loc: Source world location.
destination_loc: Destination world location.
"""
start_waypoint = self._map.get_waypoint(
source_loc, project_to_road=True, lane_type=carla.LaneType.Driving)
end_waypoint = self._map.get_waypoint(destination_loc,
project_to_road=True,
lane_type=carla.LaneType.Driving)
assert start_waypoint and end_waypoint, 'Map could not find waypoints'
route = self._grp.trace_route(start_waypoint.transform.location,
end_waypoint.transform.location)
# TODO(ionel): The planner returns several options in intersections.
# We always take the first one, but this is not correct.
return deque(
[to_pylot_transform(waypoint[0].transform) for waypoint in route])
class World(object):
"""Keeping data for the world."""
# only consider a car for running red light if it is within such distance
RED_LIGHT_ENFORCE_DISTANCE = 15 # m
def __init__(self, world: carla.World, route_resolution=1.0):
"""
Args:
world (carla.World): the carla world instance
route_resolution (float): the resolution in meters for planned route
"""
self._world = world
self._map = world.get_map()
self._waypoints = self._map.generate_waypoints(2.0)
self._actors = [] # including vehicles and walkers
self._actor_dict = {}
self._actor_locations = {}
self._route_resolution = route_resolution
dao = GlobalRoutePlannerDAO(world.get_map(),
sampling_resolution=route_resolution)
self._global_route_planner = GlobalRoutePlanner(dao)
self._global_route_planner.setup()
self._prepare_traffic_light_data()
@property
def route_resolution(self):
"""The sampling resolution of route."""
return self._route_resolution
def trace_route(self, origin, destination):
"""Find the route from ``origin`` to ``destination``.
Args:
origin (carla.Location):
destination (carla.Location):
Returns:
list[tuple(carla.Waypoint, RoadOption)]
"""
return self._global_route_planner.trace_route(origin, destination)
def add_actor(self, actor: carla.Actor):
self._actors.append(actor)
self._actor_dict[actor.id] = actor
def get_actors(self):
return self._actors
def update_actor_location(self, aid, loc):
"""Update the next location of the actor.
Args:
aid (int): actor id
loc (carla.Location): location of the actor
"""
self._actor_locations[aid] = loc
def get_actor_location(self, aid):
"""Get the latest location of the actor.
The reason of using this instead of calling ``carla.Actor.get_location()``
directly is that the location of actors may not have been updated before
world.tick().
Args:
aid (int): actor id
Returns:
carla.Location:
"""
loc = self._actor_locations.get(aid, None)
if loc is None:
loc = self._actor_dict[aid].get_location()
return loc
def get_waypoints(self):
"""Get the coordinates of way points
Returns:
list[carla.Waypoint]:
"""
return self._waypoints
def transform_to_geolocation(self, location: carla.Location):
"""Transform a map coordiate to geo coordinate.
Returns:
np.ndarray: ``[latitude, longitude, altidude]``
"""
loc = self._map.transform_to_geolocation(location)
return np.array([loc.latitude, loc.longitude, loc.altitude])
def _prepare_traffic_light_data(self):
# Adapted from RunningRedLightTest.__init__() in
# https://github.com/carla-simulator/scenario_runner/blob/master/srunner/scenariomanager/scenarioatomics/atomic_criteria.py
actors = self._world.get_actors()
self._traffic_light_actors = []
traffic_light_centers = []
# traffic_light_waypoints are the waypoints which enter into the intersection
# covered by the traffic light
traffic_light_waypoints = []
max_num_traffic_light_waypoints = 0
for actor in actors:
if 'traffic_light' in actor.type_id:
center, waypoints = self._get_traffic_light_waypoints(actor)
self._traffic_light_actors.append(actor)
traffic_light_centers.append(_to_numpy_loc(center))
waypoints = [_to_numpy_waypoint(wp) for wp in waypoints]
traffic_light_waypoints.append(waypoints)
if len(waypoints) > max_num_traffic_light_waypoints:
max_num_traffic_light_waypoints = len(waypoints)
logging.info("Found %d traffic lights" %
len(self._traffic_light_actors))
self._traffic_light_centers = np.array(traffic_light_centers,
np.float32)
np_traffic_light_waypoints = []
for waypoints in traffic_light_waypoints:
pad = max_num_traffic_light_waypoints - len(waypoints)
if pad > 0:
waypoints.extend([dummy_waypoint] * pad)
np_traffic_light_waypoints.append(
alf.nest.map_structure(lambda *x: np.stack(x), *waypoints))
self._traffic_light_waypoints = alf.nest.map_structure(
lambda *x: np.stack(x), *np_traffic_light_waypoints)
def on_tick(self):
"""Should be called after every world tick() to update data."""
self._traffic_light_states = np.array(
[a.state for a in self._traffic_light_actors], dtype=np.int)
def _get_traffic_light_waypoints(self, traffic_light):
# Copied from RunningRedLightTest.get_traffic_light_waypoints() in
# https://github.com/carla-simulator/scenario_runner/blob/master/srunner/scenariomanager/scenarioatomics/atomic_criteria.py
base_transform = traffic_light.get_transform()
base_rot = base_transform.rotation.yaw
area_loc = base_transform.transform(
traffic_light.trigger_volume.location)
# Discretize the trigger box into points
area_ext = traffic_light.trigger_volume.extent
x_values = np.arange(-0.9 * area_ext.x, 0.9 * area_ext.x,
1.0) # 0.9 to avoid crossing to adjacent lanes
area = []
for x in x_values:
point = _rotate_point(carla.Vector3D(x, 0, area_ext.z), base_rot)
point_location = area_loc + carla.Location(x=point.x, y=point.y)
area.append(point_location)
# Get the waypoints of these points, removing duplicates
ini_wps = []
for pt in area:
wpx = self._map.get_waypoint(pt)
# As x_values are arranged in order, only the last one has to be checked
if not ini_wps or ini_wps[-1].road_id != wpx.road_id or ini_wps[
-1].lane_id != wpx.lane_id:
ini_wps.append(wpx)
# Advance them until the intersection
wps = []
for wpx in ini_wps:
while not wpx.is_intersection:
next_wp = wpx.next(0.5)[0]
if next_wp and not next_wp.is_intersection:
wpx = next_wp
else:
break
if wpx.transform.location.distance(
area_loc) >= self.RED_LIGHT_ENFORCE_DISTANCE - 2:
# if area_loc is too far from wpx, when the vehicle is going over
# wpx, it is already too far from area_loc. And red light will not
# be detected.
logging.fatal(
"traffic light center is too far from traffic light "
"waypoint: %s. Need to increase RED_LIGHT_ENFORCE_DISTANCE"
% wpx.transform.location.distance(area_loc))
wps.append(wpx)
# self._draw_waypoints(wps, vertical_shift=1.0, persistency=50000.0)
return area_loc, wps
def is_running_red_light(self, actor):
"""Whether actor is running red light.
Adapted from RunningRedLightTest.update() in https://github.com/carla-simulator/scenario_runner/blob/master/srunner/scenariomanager/scenarioatomics/atomic_criteria.py
Args:
actor (carla.Actor): the vehicle actor
Returns:
red light id if running red light, None otherwise
"""
veh_transform = actor.get_transform()
veh_location = veh_transform.location
veh_extent = actor.bounding_box.extent.x
tail_close_pt = _rotate_point(
carla.Vector3D(-0.8 * veh_extent, 0.0, veh_location.z),
veh_transform.rotation.yaw)
tail_close_pt = veh_location + carla.Location(tail_close_pt)
tail_far_pt = _rotate_point(
carla.Vector3D(-veh_extent - 1, 0.0, veh_location.z),
veh_transform.rotation.yaw)
tail_far_pt = veh_location + carla.Location(tail_far_pt)
tail_far_wp = self._map.get_waypoint(tail_far_pt)
veh_seg = (np.expand_dims(_to_numpy_loc(tail_close_pt), axis=0),
np.expand_dims(_to_numpy_loc(tail_far_pt), axis=0))
is_red = self._traffic_light_states == carla.TrafficLightState.Red
dist = self._traffic_light_centers - _to_numpy_loc(veh_location)
dist = np.linalg.norm(dist, axis=-1)
candidate_light_index = np.nonzero(
is_red & (dist <= self.RED_LIGHT_ENFORCE_DISTANCE))[0]
ve_dir = _to_numpy_loc(veh_transform.get_forward_vector())
waypoints = self._traffic_light_waypoints
for index in candidate_light_index:
wp_dir = _get_forward_vector(waypoints.rotation[index])
dot_ve_wp = (ve_dir * wp_dir).sum(axis=-1)
same_lane = ((tail_far_wp.road_id == waypoints.road_id[index])
& (tail_far_wp.lane_id == waypoints.lane_id[index])
& (dot_ve_wp > 0))
yaw_wp = waypoints.rotation[index][:, 1]
lane_width = waypoints.lane_width[index]
location_wp = waypoints.location[index]
d = np.stack([
0.4 * lane_width,
np.zeros_like(lane_width), location_wp[:, 2]
],
axis=-1)
left_lane_wp = _rotate_np_point(d, yaw_wp + 0.5 * math.pi)
left_lane_wp = location_wp + left_lane_wp
right_lane_wp = _rotate_np_point(d, yaw_wp - 0.5 * math.pi)
right_lane_wp = location_wp + right_lane_wp
if np.any(same_lane
& _is_segments_intersecting(veh_seg, (left_lane_wp,
right_lane_wp))):
# If veh_seg intersects with (left_lane_wp, right_lane_wp), that
# means the vehicle is crossing the line dividing intersection
# and the outside area.
return self._traffic_light_actors[index].id
return None
def _draw_waypoints(self, waypoints, vertical_shift, persistency=-1):
"""Draw a list of waypoints at a certain height given in vertical_shift."""
for wp in waypoints:
loc = wp.transform.location + carla.Location(z=vertical_shift)
size = 0.2
color = carla.Color(255, 0, 0)
self._world.debug.draw_point(loc,
size=size,
color=color,
life_time=persistency)
def set_destination(self, location):
start_waypoint = self._map.get_waypoint(self._vehicle.get_location())
end_waypoint = self._map.get_waypoint(
carla.Location(location[0], location[1], location[2]))
solution = []
# Setting up global router
dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map())
grp = GlobalRoutePlanner(dao)
grp.setup()
# Obtain route plan
x1 = start_waypoint.transform.location.x
y1 = start_waypoint.transform.location.y
x2 = end_waypoint.transform.location.x
y2 = end_waypoint.transform.location.y
route = grp.plan_route((x1, y1), (x2, y2))
current_waypoint = start_waypoint
route.append(RoadOption.VOID)
for action in route:
# Generate waypoints to next junction
wp_choice = current_waypoint.next(self._hop_resolution)
while len(wp_choice) == 1:
current_waypoint = wp_choice[0]
solution.append((current_waypoint, RoadOption.LANEFOLLOW))
wp_choice = current_waypoint.next(self._hop_resolution)
# Stop at destination
if current_waypoint.transform.location.distance(
end_waypoint.transform.location
) < self._hop_resolution:
break
if action == RoadOption.VOID: break
# Select appropriate path at the junction
if len(wp_choice) > 1:
# Current heading vector
current_transform = current_waypoint.transform
current_location = current_transform.location
projected_location = current_location + \
carla.Location(
x=math.cos(math.radians(current_transform.rotation.yaw)),
y=math.sin(math.radians(current_transform.rotation.yaw)))
v_current = vector(current_location, projected_location)
direction = 0
if action == RoadOption.LEFT:
direction = 1
elif action == RoadOption.RIGHT:
direction = -1
elif action == RoadOption.STRAIGHT:
direction = 0
select_criteria = float('inf')
# Choose correct path
for wp_select in wp_choice:
v_select = vector(current_location,
wp_select.transform.location)
cross = float('inf')
if direction == 0:
cross = abs(np.cross(v_current, v_select)[-1])
else:
cross = direction * np.cross(v_current, v_select)[-1]
if cross < select_criteria:
select_criteria = cross
current_waypoint = wp_select
# Generate all waypoints within the junction
# along selected path
solution.append((current_waypoint, action))
current_waypoint = current_waypoint.next(
self._hop_resolution)[0]
while current_waypoint.is_intersection:
solution.append((current_waypoint, action))
current_waypoint = current_waypoint.next(
self._hop_resolution)[0]
assert solution
self._current_plan = solution
self._local_planner.set_global_plan(self._current_plan)
def main():
global world, actor_list, flag, semantic_flag, frame
client = carla.Client(host, port)
client.set_timeout(TIME_OUT)
try:
world = client.get_world()
except:
print("ERROR: Cannot get world !")
import sys
sys.exit()
set_weather()
try:
blueprint_library = world.get_blueprint_library()
# add vehicle
vehicle = add_vehicle(blueprint_library)
# put the vehicle to drive around.
#vehicle.set_autopilot(True)
map = world.get_map()
spawn_points = map.get_spawn_points()
destination = spawn_points[random.randint(0,len(spawn_points)-1)].location
#agent = BasicAgent(vehicle, target_speed=20)
#agent.set_destination((destination.x,
# destination.y,
# destination.z))
dao = GlobalRoutePlannerDAO(map)
planner = GlobalRoutePlanner(dao)
planner.setup()
vehicle.set_simulate_physics(False)
my_location = vehicle.get_location()
trace_list = planner.trace_route(my_location, destination)
waypoints = []
for (waypoint, road_option) in trace_list:
waypoints.append(waypoint)
next_point = waypoints[0].transform
camera = add_camera(blueprint_library, vehicle)
semantic_camera = add_semantic_camera(blueprint_library, vehicle)
while True:
vehicle.set_transform(next_point)
my_location = next_point.location
#my_location = vehicle.get_location()
me2destination = my_location.distance(destination)
# too close to the destination, choose another one
if me2destination < 50 :
destination = spawn_points[random.randint(0,len(spawn_points)-1)].location
#agent.set_destination((destination.x,destination.y,destination.z))
print("destination change !!!")
# get planed path
trace_list = planner.trace_route(my_location, destination)
waypoints = []
for (waypoint, road_option) in trace_list:
waypoints.append(waypoint)
# get raw image
camera.listen(lambda image: get_raw_img(image))
while not flag:
sleep(0.001)
camera.stop()
flag = False
# get semantic imgae
semantic_camera.listen(lambda image: get_semantic_img(image))
while not semantic_flag:
sleep(0.001)
semantic_camera.stop()
semantic_flag = False
get_instruction(waypoints)
draw_lane(waypoints)
# get ground truth
camera.listen(lambda image: get_nav_img(image))
while not flag:
sleep(0.001)
camera.stop()
flag = False
deal_semantic_img()
# wait debug infomation disappear
sleep(1.0)
# choose a new point for vehicle
next_point = waypoints[random.randint(0,min(len(waypoints)-1, 50))].transform
finally:
print('destroying actors')
for actor in actor_list:
actor.destroy()
actor_list = []
print('done.')
def _initialize_global_route_planner(self):
dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(), self._hop_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
return grp
def main():
global actor_list, flag, frame
client = carla.Client(host, port)
client.set_timeout(5.0)
try:
world = client.get_world()
except:
print("ERROR: Cannot get world !")
import sys
sys.exit()
set_weather(world)
#carla.TrafficLight.set_green_time(float(999))
#carla.TrafficLight.set_red_time(0)
try:
blueprint_library = world.get_blueprint_library()
# add vehicle
vehicle = add_vehicle_component(world, blueprint_library)
# put the vehicle to drive around.
#vehicle.set_autopilot(True)
map = world.get_map()
spawn_points = map.get_spawn_points()
destination = spawn_points[random.randint(0,
len(spawn_points) -
1)].location
agent = BasicAgent(vehicle, target_speed=20)
agent.set_destination((destination.x, destination.y, destination.z))
dao = GlobalRoutePlannerDAO(map)
planner = GlobalRoutePlanner(dao)
planner.setup()
vehicle.set_simulate_physics(False)
my_location = vehicle.get_location()
trace_list = planner.trace_route(my_location, destination)
waypoints = []
for (waypoint, road_option) in trace_list:
waypoints.append(waypoint)
next_point = waypoints[0].transform
camera = add_camera_component(world, blueprint_library, vehicle)
camera.stop()
while True:
vehicle.set_transform(next_point)
my_location = next_point.location
#my_location = vehicle.get_location()
me2destination = my_location.distance(destination)
if me2destination < 50:
destination = spawn_points[random.randint(
0,
len(spawn_points) - 1)].location
#agent.set_destination((destination.x,destination.y,destination.z))
print("destination change !!!")
trace_list = planner.trace_route(my_location, destination)
waypoints = []
for (waypoint, road_option) in trace_list:
waypoints.append(waypoint)
print(len(waypoints))
if len(waypoints) < 5:
print('my_location:', my_location.x, my_location.y,
my_location.z)
print('destination:', destination.x, destination.y,
destination.z)
print('me2destination:', me2destination)
next_point = waypoints[random.randint(0,
min(len(waypoints) - 1,
50))].transform
camera.listen(lambda image: deal_image(image))
while not flag:
sleep(0.01)
camera.stop()
flag = False
get_instruct(waypoints)
for waypoint in waypoints[0:min(len(waypoints) - 1, 30)]:
box_point = carla.Location(waypoint.transform.location.x,
waypoint.transform.location.y,
waypoint.transform.location.z - 0.4)
box = carla.BoundingBox(box_point,
carla.Vector3D(x=2, y=0.1, z=0.5))
rotation = carla.Rotation(
pitch=waypoint.transform.rotation.pitch,
yaw=waypoint.transform.rotation.yaw,
roll=waypoint.transform.rotation.roll)
world.debug.draw_box(box=box,
rotation=rotation,
thickness=1.2,
life_time=0)
sleep(0.3)
camera.listen(lambda image: deal_image(image))
while not flag:
sleep(0.01)
camera.stop()
flag = False
sleep(1.0)
finally:
print('destroying actors')
for actor in actor_list:
actor.destroy()
actor_list = []
print('done.')
class LocalPlannerNew(object):
def __init__(self, vehicle, resolution=15, threshold_before=2.5, threshold_after=5.0):
from agents.navigation.global_route_planner import GlobalRoutePlanner
from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO
# Max skip avoids misplanning when route includes both lanes.
self._max_skip = 20
self._threshold_before = threshold_before
self._threshold_after = threshold_after
self._vehicle = vehicle
self._map = vehicle.get_world().get_map()
self._grp = GlobalRoutePlanner(GlobalRoutePlannerDAO(self._map, resolution))
self._grp.setup()
self._route = None
self._waypoints_queue = deque(maxlen=20000)
self.target = (None, None)
self.checkpoint = (None, None)
self.distance_to_goal = float('inf')
self.distances = deque(maxlen=20000)
def set_route(self, start, target):
self._waypoints_queue.clear()
self._route = self._grp.trace_route(start, target)
self.distance_to_goal = 0.0
prev = None
for node in self._route:
self._waypoints_queue.append(node)
cur = node[0].transform.location
if prev is not None:
delta = np.sqrt((cur.x - prev.x) ** 2 + (cur.y - prev.y) ** 2)
self.distance_to_goal += delta
self.distances.append(delta)
prev = cur
self.target = self._waypoints_queue[0]
self.checkpoint = (
self._map.get_waypoint(self._vehicle.get_location()),
RoadOption.LANEFOLLOW)
def run_step(self):
assert self._route is not None
u = self._vehicle.get_transform().location
max_index = -1
for i, (node, command) in enumerate(self._waypoints_queue):
if i > self._max_skip:
break
v = node.transform.location
distance = np.sqrt((u.x - v.x) ** 2 + (u.y - v.y) ** 2)
if int(self.checkpoint[1]) == 4 and int(command) != 4:
threshold = self._threshold_before
else:
threshold = self._threshold_after
if distance < threshold:
self.checkpoint = (node, command)
max_index = i
for i in range(max_index + 1):
if self.distances:
self.distance_to_goal -= self.distances[0]
self.distances.popleft()
self._waypoints_queue.popleft()
if len(self._waypoints_queue) > 0:
self.target = self._waypoints_queue[0]
def calculate_timeout(self, fps=10):
_numpy = lambda p: np.array([p.transform.location.x, p.transform.location.y])
distance = 0
node_prev = None
for node_cur, _ in self._route:
if node_prev is None:
node_prev = node_cur
distance += np.linalg.norm(_numpy(node_cur) - _numpy(node_prev))
node_prev = node_cur
timeout_in_seconds = ((distance / 1000.0) / 5.0) * 3600.0 + 20.0
timeout_in_frames = timeout_in_seconds * fps
return timeout_in_frames
class LocalPlannerOld(object):
def __init__(self, vehicle, resolution=1.5):
from agents.navigation.global_route_planner import GlobalRoutePlanner
from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO
self._dt = 1.0 / 10.0
self._target_speed = 20.0 # Km/h
self._sampling_radius = self._target_speed * 1 / 3.6 # 1 seconds horizon
self._min_distance = self._sampling_radius * 0.9
self._vehicle = vehicle
self._map = vehicle.get_world().get_map()
self._grp = GlobalRoutePlanner(GlobalRoutePlannerDAO(self._map, resolution))
self._grp.setup()
self._route = None
self._waypoints_queue = deque(maxlen=20000)
self.target = (None, None)
self.checkpoint = (None, None)
self.distance_to_goal = float('inf')
self.distances = deque(maxlen=20000)
def set_route(self, start, target):
self._waypoints_queue.clear()
self._route = self._grp.trace_route(start, target)
self.distance_to_goal = 0.0
prev = None
for node in self._route:
self._waypoints_queue.append(node)
cur = node[0].transform.location
if prev is not None:
delta = np.sqrt((cur.x - prev.x) ** 2 + (cur.y - prev.y) ** 2)
self.distance_to_goal += delta
self.distances.append(delta)
prev = cur
self.target = self._waypoints_queue[0]
self.checkpoint = (
self._map.get_waypoint(self._vehicle.get_location()),
RoadOption.LANEFOLLOW)
def run_step(self):
assert self._route is not None
vehicle_transform = self._vehicle.get_transform()
max_index = -1
for i, (waypoint, _) in enumerate(self._waypoints_queue):
if distance_vehicle(waypoint, vehicle_transform) < self._min_distance:
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
if self.distances:
self.distance_to_goal -= self.distances[0]
self.distances.popleft()
self._waypoints_queue.popleft()
if len(self._waypoints_queue) > 0:
self.target = self._waypoints_queue[0]
def calculate_timeout(self, fps=10):
_numpy = lambda p: np.array([p.transform.location.x, p.transform.location.y])
distance = 0
node_prev = None
for node_cur, _ in self._route:
if node_prev is None:
node_prev = node_cur
distance += np.linalg.norm(_numpy(node_cur) - _numpy(node_prev))
node_prev = node_cur
timeout_in_seconds = ((distance / 1000.0) / 5.0) * 3600.0 + 20.0
timeout_in_frames = timeout_in_seconds * fps
return timeout_in_frames
class HDMap(object):
"""Wrapper class around the CARLA map.
All Pylot methods should strive to use this class instead of directly
accessing a CARLA map. This will make it easier to extend the probject
with support for other types of HD maps in the future.
Args:
simulator_map (carla.Map): An instance of a CARLA map.
Attributes:
_map (carla.Map): An instance of a CARLA map.
_grp: An instance of a CARLA global route planner (uses A*).
"""
def __init__(self, simulator_map, log_file=None):
self._logger = erdos.utils.setup_logging('hd_map', log_file)
self._map = simulator_map
# Setup global planner.
self._grp = GlobalRoutePlanner(
GlobalRoutePlannerDAO(
self._map,
1.0 # Distance between waypoints
))
self._grp.setup()
def get_closest_lane_waypoint(self, location: Location):
"""Returns the road closest waypoint to location.
Args:
location (:py:class:`~pylot.utils.Location`): Location in world
coordinates.
Returns:
:py:class:`~pylot.utils.Transform`: Transform or None if no
waypoint is found.
"""
waypoint = self._get_waypoint(location,
project_to_road=True,
lane_type=carla.LaneType.Any)
if waypoint:
return Transform.from_simulator_transform(waypoint.transform)
else:
return None
def is_intersection(self, location: Location):
"""Checks if a location is in an intersection.
Args:
location (:py:class:`~pylot.utils.Location`): Location in world
coordinates.
Returns:
bool: True if the location is in an intersection.
"""
waypoint = self._get_waypoint(location,
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:
return self.__is_intersection(waypoint)
def __is_intersection(self, waypoint):
if waypoint.is_junction:
return True
if hasattr(waypoint, 'is_intersection'):
return waypoint.is_intersection
return False
def is_on_lane(self, location: Location):
"""Checks if a location is on a lane.
Args:
location (:py:class:`~pylot.utils.Location`): Location in world
coordinates.
Returns:
bool: True if the location is on a lane.
"""
waypoint = self._get_waypoint(location,
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 are_on_same_lane(self, location1: Location, location2: Location):
"""Checks if two locations are on the same lane.
Args:
location1 (:py:class:`~pylot.utils.Location`): Location in world
coordinates.
location2 (:py:class:`~pylot.utils.Location`): Location in world
coordinates.
Returns:
bool: True if the two locations are on the same lane.
"""
waypoint1 = self._get_waypoint(location1,
project_to_road=False,
lane_type=carla.LaneType.Driving)
if not waypoint1:
# First location is not on a drivable lane.
return False
waypoint2 = self._get_waypoint(location2,
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 self.__is_intersection(
waypoint1) and not self.__is_intersection(waypoint2):
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 is_on_opposite_lane(self, transform: Transform):
"""Checks if a transform is on an opposite lane.
Args:
transform (:py:class:`~pylot.utils.Transform`): Transform in world
coordinates.
Returns:
bool: True if the transform is on the opposite lane.
"""
waypoint = self._get_waypoint(transform.location,
project_to_road=False,
lane_type=carla.LaneType.Driving)
if not waypoint:
return True
if self.__is_intersection(waypoint):
return False
# XXX(ionel): Check logic.
if (abs(waypoint.transform.rotation.yaw - transform.rotation.yaw) >
140):
return True
else:
return False
def is_at_stop(self, location: Location):
"""Checks if a location is close to a stop sign.
Args:
location (:py:class:`~pylot.utils.Location`): Location in world
coordinates.
Returns:
bool: True if the location is at a stop sign.
"""
# TODO(ionel): This method doesn't work yet because the opendrive do
# not contained waypoints annotated as stops.
# waypoint = self._get_waypoint(location,
# project_to_road=False,
# lane_type=carla.LaneType.Stop)
raise NotImplementedError
def distance_to_intersection(self,
location: Location,
max_distance_to_check: float = 30):
"""Computes the distance (in meters) from location to an intersection.
The method starts from location, moves forward until it reaches an
intersection or exceeds max_distance_to_check.
Args:
location (:py:class:`~pylot.utils.Location`): The starting location
in world coordinates.
max_distance_to_check (:obj:`int`): Max distance to move forward
(in meters).
Returns:
:obj:`int`: The distance in meters, or None if there is no
intersection within max_distance_to_check.
"""
waypoint = self._get_waypoint(location,
project_to_road=False,
lane_type=carla.LaneType.Any)
if not waypoint:
return None
# We're already in an intersection.
if self.__is_intersection(waypoint):
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 self.__is_intersection(w):
return i
waypoint = waypoints[0]
return None
def is_on_bidirectional_lane(self, location: Location):
"""Checks if a location is a bidirectional lane.
Args:
location (:py:class:`~pylot.utils.Location`): Location in world
coordinates.
Returns:
bool: True if the location is on a bidirectional lane.
"""
waypoint = self._get_waypoint(location,
project_to_road=False,
lane_type=carla.LaneType.Bidirectional)
return not waypoint
def must_obey_traffic_light(self, ego_location: Location,
tl_location: Location):
"""Checks if an ego vehicle must obey a traffic light.
Args:
ego_location (:py:class:`~pylot.utils.Location`): Location of the
ego vehicle in world coordinates.
tl_location (:py:class:`~pylot.utils.Location`): Location of the
traffic light in world coordinates.
Returns:
bool: True if the ego vehicle must obey the traffic light.
"""
waypoint = self._get_waypoint(ego_location,
project_to_road=False,
lane_type=carla.LaneType.Any)
if waypoint and self.__is_intersection(waypoint):
# Do not obey traffic light if ego is already in the intersection.
return False
# TODO(ionel): Implement.
return True
def _must_obey_european_traffic_light(self, ego_transform: Transform,
tl_locations,
tl_max_dist_thresh: float):
ego_waypoint = self._get_waypoint(ego_transform.location,
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 self.__is_intersection(ego_waypoint):
return (False, None)
# Iterate through traffic lights.
for tl_loc in tl_locations:
tl_waypoint = self._get_waypoint(tl_loc)
if (tl_waypoint.road_id != ego_waypoint.road_id
or tl_waypoint.lane_id != ego_waypoint.lane_id):
continue
if ego_transform.is_within_distance_ahead(tl_loc,
tl_max_dist_thresh):
return (True, tl_loc)
return (False, None)
def _must_obey_american_traffic_light(self, ego_transform: Transform,
tl_locations,
tl_max_dist_thresh: float):
ego_waypoint = self._get_waypoint(ego_transform.location,
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 self.__is_intersection(ego_waypoint):
return (False, None)
min_angle = 25.0
selected_tl_loc = None
for tl_loc in tl_locations:
if ego_transform.is_within_distance_ahead(tl_loc,
tl_max_dist_thresh):
angle, distance = ego_transform.get_angle_and_magnitude(tl_loc)
if distance < 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 get_lane(self,
location: Location,
waypoint_precision: float = 0.05,
lane_id: int = 0):
lane_waypoints = []
# Consider waypoints in opposite direction of camera so we can get
# lane data for adjacent lanes in opposing directions.
previous_wp = [
self._get_waypoint(location,
project_to_road=False,
lane_type=carla.LaneType.Any)
]
while len(previous_wp) == 1:
lane_waypoints.append(previous_wp[0])
previous_wp = previous_wp[0].previous(waypoint_precision)
next_wp = [
self._get_waypoint(location,
project_to_road=False,
lane_type=carla.LaneType.Any)
]
while len(next_wp) == 1:
lane_waypoints.append(next_wp[0])
next_wp = next_wp[0].next(waypoint_precision)
# Get the left and right markings of the lane and send it as a message.
left_markings = [
self._lateral_shift(w.transform, -w.lane_width * 0.5)
for w in lane_waypoints
]
right_markings = [
self._lateral_shift(w.transform, w.lane_width * 0.5)
for w in lane_waypoints
]
return Lane(lane_id, left_markings, right_markings)
def get_left_lane(self, location: Location):
waypoint = self._get_waypoint(location,
project_to_road=False,
lane_type=carla.LaneType.Any)
if waypoint:
left_lane_waypoint = waypoint.get_left_lane()
if left_lane_waypoint:
return Transform.from_simulator_transform(
left_lane_waypoint.transform)
return None
def get_right_lane(self, location: Location):
waypoint = self._get_waypoint(location,
project_to_road=False,
lane_type=carla.LaneType.Any)
if waypoint:
right_lane_waypoint = waypoint.get_right_lane()
if right_lane_waypoint:
return Transform.from_simulator_transform(
right_lane_waypoint.transform)
return None
def get_all_lanes(self, location: Location):
lanes = [self.get_lane(location)]
waypoint = self._get_waypoint(location,
project_to_road=False,
lane_type=carla.LaneType.Any)
if waypoint:
wp_left = waypoint.get_left_lane()
w_rotation = waypoint.transform.rotation
while wp_left and wp_left.lane_type == carla.LaneType.Driving:
left_location = Location.from_simulator_location(
wp_left.transform.location)
lanes.append(
self.get_lane(left_location, lane_id=wp_left.lane_id))
# If left lane is facing the opposite direction, its left
# lane would point back to the current lane, so we select
# its right lane to get the left lane relative to current.
if w_rotation == wp_left.transform.rotation:
wp_left = wp_left.get_left_lane()
else:
wp_left = wp_left.get_right_lane()
wp_right = waypoint.get_right_lane()
while wp_right and wp_right.lane_type == carla.LaneType.Driving:
right_location = Location.from_simulator_location(
wp_right.transform.location)
lanes.append(
self.get_lane(right_location, lane_id=wp_right.lane_id))
# Same logic as above. If right lane of current is in
# opposite direction, move rightwards by selecting it's
# left lane.
if w_rotation == wp_right.transform.rotation:
wp_right = wp_right.get_right_lane()
else:
wp_right = wp_left.get_left_lane()
return lanes
def compute_waypoints(self, source_loc: Location,
destination_loc: Location):
"""Computes waypoints between two locations.
Assumes that the ego vehicle has the same orientation as the lane on
whch it is on.
Args:
source_loc (:py:class:`~pylot.utils.Location`): Source location in
world coordinates.
destination_loc (:py:class:`~pylot.utils.Location`): Destination
location in world coordinates.
Returns:
list(:py:class:`~pylot.utils.Transform`): List of waypoint
transforms.
"""
start_waypoint = self._get_waypoint(source_loc,
project_to_road=True,
lane_type=carla.LaneType.Driving)
end_waypoint = self._get_waypoint(destination_loc,
project_to_road=True,
lane_type=carla.LaneType.Driving)
assert start_waypoint and end_waypoint, 'Map could not find waypoints'
route = self._grp.trace_route(start_waypoint.transform.location,
end_waypoint.transform.location)
# TODO(ionel): The planner returns several options in intersections.
# We always take the first one, but this is not correct.
return deque([
Transform.from_simulator_transform(waypoint[0].transform)
for waypoint in route
])
def _lateral_shift(self, transform, shift):
transform.rotation.yaw += 90
shifted = transform.location + shift * transform.get_forward_vector()
return Location.from_simulator_location(shifted)
def _get_waypoint(self, location: Location, project_to_road: bool,
lane_type):
try:
waypoint = self._map.get_waypoint(location.as_simulator_location(),
project_to_road=project_to_road,
lane_type=lane_type)
except RuntimeError as err:
self._logger.error('get_waypoint call failed: {}'.format(err))
waypoint = None
return waypoint
class World(object):
"""Keeping data for the world."""
def __init__(self, world: carla.World, route_resolution=1.0):
"""
Args:
world (carla.World): the carla world instance
route_resolution (float): the resolution in meters for planned route
"""
self._world = world
self._map = world.get_map()
self._waypoints = self._map.generate_waypoints(2.0)
self._actors = [] # including vehicles and walkers
self._actor_dict = {}
self._actor_locations = {}
self._route_resolution = route_resolution
dao = GlobalRoutePlannerDAO(world.get_map(),
sampling_resolution=route_resolution)
self._global_route_planner = GlobalRoutePlanner(dao)
self._global_route_planner.setup()
@property
def route_resolution(self):
"""The sampling resolution of route."""
return self._route_resolution
def trace_route(self, origin, destination):
"""Find the route from ``origin`` to ``destination``.
Args:
origin (carla.Location):
destination (carla.Location):
Returns:
list[tuple(carla.Waypoint, RoadOption)]
"""
return self._global_route_planner.trace_route(origin, destination)
def add_actor(self, actor: carla.Actor):
self._actors.append(actor)
self._actor_dict[actor.id] = actor
def get_actors(self):
return self._actors
def update_actor_location(self, aid, loc):
"""Update the next location of the actor.
Args:
aid (int): actor id
loc (carla.Location): location of the actor
"""
self._actor_locations[aid] = loc
def get_actor_location(self, aid):
"""Get the latest location of the actor.
The reason of using this instead of calling ``carla.Actor.get_location()``
directly is that the location of actors may not have been updated before
world.tick().
Args:
aid (int): actor id
Returns:
carla.Location:
"""
loc = self._actor_locations.get(aid, None)
if loc is None:
loc = self._actor_dict[aid].get_location()
return loc
def get_waypoints(self):
"""Get the coordinates of way points
Returns:
list[carla.Waypoint]:
"""
return self._waypoints
def transform_to_geolocation(self, location: carla.Location):
"""Transform a map coordiate to geo coordinate.
Returns:
np.ndarray: ``[latitude, longitude, altidude]``
"""
loc = self._map.transform_to_geolocation(location)
return np.array([loc.latitude, loc.longitude, loc.altitude])
