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 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.
"""
grp = GlobalRoutePlanner(world.get_map(), hop_resolution)
# 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 calculate_route(self, goal):
"""
Calculate a route from the current location to 'goal'
"""
self.loginfo("Calculating route to x={}, y={}, z={}".format(
goal.location.x, goal.location.y, goal.location.z))
grp = GlobalRoutePlanner(self.world.get_map(), sampling_resolution=1)
route = grp.trace_route(
self.ego_vehicle.get_location(),
carla.Location(goal.location.x, goal.location.y, goal.location.z))
return route
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
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 _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 interpolate_trajectory(waypoints_trajectory, hop_resolution=1.0):
"""
Given some raw keypoints interpolate a full dense trajectory to be used by the user.
returns the full interpolated route both in GPS coordinates and also in its original form.
Args:
- waypoints_trajectory: the current coarse trajectory
- hop_resolution: distance between the trajectory's waypoints
"""
grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), hop_resolution)
# Obtain route plan
route = []
for i in range(len(waypoints_trajectory) - 1):
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]))
lat_ref, lon_ref = _get_latlon_ref(CarlaDataProvider.get_world())
return location_route_to_gps(route, lat_ref, lon_ref), route
def main(cr_method = "TEP", ctrlPolicy = "TEPControl", PriorityPolicy = "FCFS",totalVehicle = 1, scenario = 6, spwnInterval = 0, randomSeed = 960489, preGenRoute = 1, logging = 1, errMargin = 0.5):
###############################################
# Config
###############################################
syncmode = 1 # Whether ticks are synced
freqSimulation = 40 # [HZ] The frequency at which the simulation is ran
freqOnBoard = 20 # [HZ] The frequency at which vehicle on board controller is simulated
random.seed(randomSeed) # Random seed
maxVehicle = 24 # Max simultaneous vehicle
# preGenRoute
# logging = 1 # Whether to log vehicle spawns and dest
# totalVehicle = 64 # Total vehicles for entire simulation
# scenario = 6 # 0 is random 1/tick, 1 is 4/tick all roads (Ensure totalVehicle is a multiple of 4 if scenario is 1)
# spwnInterval = 4 # Time between each spawn cycle
# cr_method = "DCR" # Which conflict resolution method is used
# ctrlPolicy = "DCRControl" # Which control policy is used
# PriorityPolicy = "PriorityScore" # Which priorityPolicy is used
# cr_method = "MPIP" # Which conflict resolution method is used
# ctrlPolicy = "MPIPControl" # Which control policy is used
# PriorityPolicy = "FCFS" # Which priorityPolicy is used
###############################################
# Plotting Config
###############################################
plot = 0 # Whether to plot figures afterwards or not
plotVel = 0 # Whether to plot velocity or not
plotTheta = 0 # Whether to plot theta or not
plotLoc = 0 # Whether to plot location or not
###############################################
# Other variables
###############################################
weather = carla.WeatherParameters(
cloudiness=0.0,
precipitation=0.0,
precipitation_deposits=0.0,
wind_intensity=0.0,
sun_azimuth_angle=70.0,
sun_altitude_angle=70.0) # Doesn't affect simulation, but does affect visuals
multiProcessTest = 0
try:
# Init carla at port 2000
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
# Retrieve world
world = client.get_world()
if world.get_map().name != 'Town07':
client.load_world('Town07')
else:
client.reload_world()
world = client.get_world()
client.set_timeout(2.0)
world.set_weather(weather)
if syncmode == 1:
settings = world.get_settings()
if settings.synchronous_mode == False:
settings.fixed_delta_seconds = 1/freqSimulation
settings.synchronous_mode = True
settings.no_rendering_mode = True
world.apply_settings(settings)
world.tick()
tick0 = world.get_snapshot()
else:
tick0 = world.wait_for_tick()
#TODO are these used?
ts0 = tick0.timestamp
ts0s = tick0.timestamp.elapsed_seconds
lastTick_s = ts0s
# ! Clear previous actors if present
for actor in world.get_actors().filter("vehicle.*"):
actor.destroy()
# get map and establish grp
carlaMap = world.get_map()
hop_resolution = 0.1
dao = GlobalRoutePlannerDAO(carlaMap, hop_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
# Retrive blueprints
blueprint_library = world.get_blueprint_library()
# Vehicle type definition
if False:
bp = random.choice(blueprint_library.filter('vehicle'))
else:
bp = blueprint_library.find('vehicle.audi.a2')
# # A blueprint contains the list of attributes that define a vehicle's
# # instance, we can read them and modify some of them. For instance,
# # let's randomize its color.
# if bp.has_attribute('color'):
# color = random.choice(bp.get_attribute('color').recommended_values)
# bp.set_attribute('color', color)
#* Generate spawn
i = 0
# spwnRand = random.randint(1,4)
# destRand = random.randint(1,4)
# Change laneList if not 4 way cross road
# Method does not allow same lane for entry and exit
laneList= np.array([1,2,3,4])
if scenario == 0:
kmax = 1
spwnRand = np.array([random.choice(laneList) for iter in range(totalVehicle)])
destRand = np.array([random.choice(np.delete(laneList,spwnRand[iter]-1)) for iter in range(totalVehicle)])
velRand = np.array([8+random.uniform(-1,1) for iter in range(totalVehicle)])
elif scenario == 1:
kmax = 4
spwnRand = np.array([[1,2,3,4] for iter in range(int(totalVehicle/4))]).flatten()
destRand = np.array([random.choice(np.delete(laneList,spwnRand[iter]-1)) for iter in range(totalVehicle)])
velRand = np.array([8+random.uniform(-1,1) for iter in range(totalVehicle)])
elif scenario == 2: # Artificial AMPIP example
totalVehicle = 2
spwnInterval = 4.5
kmax = 1
spwnRand = [4,2]
destRand = [1,1]
velRand = [6,9]
elif scenario == 3:
# Artificial Deadlock for AMPIP, proof of not deadlock free in real environments
totalVehicle = 4
spwnInterval = 2.8
kmax = 2
spwnRand = [4,2,3,1]
destRand = [2,4,1,3]
velRand = [6.9,6.9,9.2,9.2]
elif scenario == 4:
# fast spawning to test spawn delay
kmax = 1
totalVehicle = 8
spwnInterval = 1.5
# spwnRand = [1,2,3,4,1,2,3,4]
# destRand = [4,1,2,3,4,1,2,3]
spwnRand = np.ones(totalVehicle, dtype = int)*2
destRand = np.ones(totalVehicle, dtype = int)*4
velRand = np.array([8+random.uniform(-1,1) for iter in range(totalVehicle)])
elif scenario == 5:
# testing for DCR
kmax = 4
totalVehicle = 4
spwnInterval = 0
spwnRand = [1,2,3,4]
destRand = [3,4,1,2]
velRand = [8,8,8,8]
# velRand = np.array([8+random.uniform(-1,1) for iter in range(totalVehicle)])
elif scenario == 6:
# testing for theta PID
kmax = 1
totalVehicle = 1
spwnInterval = 0
spwnRand = [3]
destRand = [1]
velRand = [7]
# idRand is only used for tie breaking, used to avoid odd behavior
idRand = np.array([random.randint(100000,999999) for iter in range(totalVehicle)])
spwnTime = [0]
destTime = [0]
# Integrate into map object?
# Map Locations, spwnLoc contains loc, 0= intersec, 1 = N, 2 = E, 3 = S, 4 = W.
intersection = carla.Transform(carla.Location(x=-150.0, y=-35.0, z=0.3), carla.Rotation(yaw=180))
northSpawn = carla.Transform(carla.Location(x=-151.9, y=-70.0, z=0.272), carla.Rotation(yaw=90))
eastSpawn = carla.Transform(carla.Location(x=-115.0, y=-36.6, z=0.265), carla.Rotation(yaw=-180))
southSpawn = carla.Transform(carla.Location(x=-148.49, y=0.0, z=0.31), carla.Rotation(yaw=-90))
westSpawn = carla.Transform(carla.Location(x=-185.0, y=-33.3, z=0.01), carla.Rotation(yaw=0))
spwnLoc = [intersection,northSpawn,eastSpawn,southSpawn,westSpawn]
northExit = carla.Transform(carla.Location(x=-148.2, y=-70.0, z=0.3), carla.Rotation(yaw=-90))
eastExit = carla.Transform(carla.Location(x=-115.0, y=-33.3, z=0.3), carla.Rotation(yaw=0))
southExit = carla.Transform(carla.Location(x=-152, y=0.0, z=0.3), carla.Rotation(yaw=90))
westExit = carla.Transform(carla.Location(x=-185.0, y=-37.0, z=0.3), carla.Rotation(yaw=-180))
exitLoc = [intersection,northExit,eastExit,southExit,westExit]
# Pre generate the routes for all spawn and exit combinations
if preGenRoute == 1:
routeDictionary = {}
for _spwnLoc in enumerate(spwnLoc):
for _exitLoc in enumerate(exitLoc):
if _spwnLoc[0] > 0 and _exitLoc[0] > 0 and _spwnLoc[0] != _exitLoc[0]:
routeDictionary[(_spwnLoc[0],_exitLoc[0])] = grp.trace_route(_spwnLoc[1].location,_exitLoc[1].location)
# Create Objects to use in loop
#<<
worldX_obj = ah.worldX(world,intersection.location,8,tick0,hop_resolution)
actorDict_obj = ah.actorDict()
# ctrl_obj = ac.actorControl(ctrlPolicy)
#>>
# Random variables
# TODO Clean up
lastCycle = 1
if plot == 1:
velDict = {}
locDict = {}
thetaDict = {}
aclDict = {}
ctrDict = {}
print("Initialized with Method:",cr_method,", Control Policy: ", ctrlPolicy, ", Total Vehicles: ", totalVehicle, "Scenario: ",scenario, "Spawn Interval: ",spwnInterval, "Random Seed: ",randomSeed)
#* << Main Loop >>
notComplete = 1
while notComplete:
if syncmode == 1:
world.tick()
tick = world.get_snapshot()
else:
tick = world.wait_for_tick()
ts = tick.timestamp
dt = ts.delta_seconds
worldX_obj.tock(tick)
# TODO Spawn Vehicles code ( separate class or function)
if ts.elapsed_seconds - ts0s - spwnTime[-1] > spwnInterval and i < totalVehicle and len(actorDict_obj.actor_list) <= maxVehicle:
if False:
bp = random.choice(blueprint_library.filter('vehicle'))
else:
bp = blueprint_library.find('vehicle.audi.a2')
for _k in range(kmax):
if i < totalVehicle:
spwn = world.try_spawn_actor(bp, spwnLoc[spwnRand[i]])
if spwn is not None:
# Add new spwn to actor_list
actorDict_obj.actor_list.append(spwn)
#// spwn.set_autopilot()
# Create inbox for new vehicle
worldX_obj.msg.inbox[spwn.id] = []
# Create actorX object for new vehicle
spwnX = ah.actorX(spwn,0,dt,ts.elapsed_seconds-ts0s,spwnLoc[spwnRand[i]],spwnRand[i],exitLoc[destRand[i]],destRand[i],velRand[i],idRand[i],i)
# Trace route using A* and set to spwnX.route
if preGenRoute == 1:
spwnX.route = routeDictionary.get((spwnRand[i],destRand[i]))
else:
spwnX.route = grp.trace_route(spwnLoc[spwnRand[i]].location,spwnX.dest.location)
# Create conflict resolution object and save it
spwnX.cr = cr.conflictResolution(cr_method,[errMargin,PriorityPolicy]).obj
# Setup conflict resolution using egoX and worldX
spwnX.cr.setup(spwnX,worldX_obj)
# Create control object and save it
spwnX.co = ac.actorControl(ctrlPolicy)
# Add new objects to dictionary
actorDict_obj.addKey(spwn.id,spwnX)
# Add spawn time to list for analysis
spwnTime.append(ts.elapsed_seconds-ts0s)
# TODO see if setting velocity reduces start delay
# Set vehicle velocity to its reference
vel3D = proj3D(velRand[i],np.radians(spwnLoc[spwnRand[i]].rotation.yaw))
spwn.set_velocity(vel3D)
# Set gear to 1 to avoid spawn delay bug
spwn.apply_control(carla.VehicleControl(manual_gear_shift=True,gear=1))
# Set gear back to automatic
# spwn.apply_control(carla.VehicleControl(manual_gear_shift=False))
# Print out to console
if logging == 1:
print('[%d,%d] created %s at %d with dest %d, elapsed time: %d s' % (i,spwn.id,spwn.type_id,spwnRand[i],destRand[i],spwnTime[i+1]))
i += 1
if plot == 1:
if plotVel == 1:
#* To Graph out velocities over time
velDict[spwnX.id] = []
ctrDict[spwnX.id] = []
aclDict[spwnX.id] = []
if plotLoc == 1:
#* To Graph out location over time
locDict[spwnX.id] = []
if plotTheta == 1:
thetaDict[spwnX.id] = []
#* Destroy Vehicles code
# TODO separate class or function
for actor in actorDict_obj.actor_list:
if actor.get_location().distance(carla.Location(x=-150, y=-35, z=0.3)) > 38 and actor.get_location().distance(carla.Location(x=0, y=0, z=0)) > 5:
if logging == 1:
print(actor.id,' left the area at ', round(actor.get_location().x,2),round(actor.get_location().y,2),round(actor.get_location().z,2), ', elapsed time: ', round(ts.elapsed_seconds-ts0s), "s")
destTime.append(ts.elapsed_seconds-ts0s)
actorDict_obj.actor_list.remove(actor)
del actorDict_obj.dict[actor.id]
worldX_obj.msg.clear(actor.id)
actor.destroy()
#* Temporary code to detect time it takes to reach ref vel
if plot == 1:
if plotLoc == 1:
for actorX in actorDict_obj.dict.values():
locDict.get(actorX.id).append([ts.elapsed_seconds-ts0s,actorX.location.x,actorX.location.y])
if plotVel == 1:
for actorX in actorDict_obj.dict.values():
velDict.get(actorX.id).append([ts.elapsed_seconds-ts0s,actorX.velNorm])
aclDict.get(actorX.id).append([ts.elapsed_seconds-ts0s,actorX.ego.get_acceleration().x,actorX.ego.get_acceleration().y])
ctrDict.get(actorX.id).append([ts.elapsed_seconds-ts0s,actorX.ego.get_control().throttle,actorX.ego.get_control().steer,actorX.ego.get_control().brake,actorX.ego.get_control().gear,actorX.ego.get_control().manual_gear_shift])
if plotTheta == 1:
for actorX in actorDict_obj.dict.values():
thetaDict.get(actorX.id).append([ts.elapsed_seconds-ts0s,actorX.rotation.yaw,actorX.ego.get_control().steer])
#* Code to enforce a different freq for on board calculations and simulation
tickRatio = freqSimulation/freqOnBoard
if lastCycle < tickRatio:
lastCycle += 1
continue
else:
lastCycle = 1
currTick_s = ts.elapsed_seconds
dt_ob = currTick_s - lastTick_s
lastTick_s = currTick_s
# Feed world info to classes and functions streamlined
worldX_obj.update(actorDict_obj)
#* Loop to communicate information
# TODO separate class or function)
# <<
for actorX in actorDict_obj.dict.values():
worldX_obj.msg.receive(actorX)
actorX.updateParameters(dt_ob)
worldX_obj.msg.clearCloud()
#* Loop to resolve conflicts
# <<
for actorX in actorDict_obj.dict.values():
actorX.cr.resolve(actorX,worldX_obj)
msg = actorX.cr.outbox(actorX)
worldX_obj.msg.broadcast(actorX.id,actorX.location,msg,250)
# >>
#* Loop to apply vehicle control (TODO separate class or function)
# <<
for actorX in actorDict_obj.dict.values():
actorX.co.control(actorX,worldX_obj)
# >>
#* End the loop
# TODO Integrate in while loop if useless
if i >= totalVehicle and len(actorDict_obj.actor_list) == 0:
notComplete = 0
if logging == 1:
worldX_obj0 = copy.copy(worldX_obj)
finally:
print("Ended with Method:",cr_method,", Control Policy: ", ctrlPolicy, ", Total Vehicles: ", totalVehicle, "Scenario: ",scenario, "Spawn Interval: ",spwnInterval, "Random Seed: ",randomSeed)
# Save lists as csv
data = []
spwnTime.remove(0)
destTime.remove(0)
data = zip(spwnRand,destRand,spwnTime,destTime)
filename = str(cr_method) + "_" + str(ctrlPolicy) + "_" + str(totalVehicle).zfill(3) + "_" + str(scenario) + "_" + str(spwnInterval)+ "_" + str(randomSeed).zfill(6) + "_" + f'{errMargin:.1f}'.zfill(2) + "_" + datetime.datetime.now().strftime('%Y-%m-%d-%H-%M.csv')
dirname = './data/'
with open(dirname+filename, 'w') as log:
wr = csv.writer(log, quoting=csv.QUOTE_ALL)
for row in data:
wr.writerow(row)
print("Log saved in: ",dirname+filename,sep="")
timespent = []
for i_s in enumerate(spwnTime):
timespent.append(destTime[i_s[0]]-i_s[1])
averageTime = np.average(timespent)
print("Average time per vehicle: ", averageTime, "Total time:", destTime[-1])
# Clean up actors
print('destroying actors')
for actor in actorDict_obj.actor_list:
actor.destroy()
print('done.')
world.tick()
# print("setting synchronous_mode to False")
# settings.synchronous_mode = False
# world.apply_settings(settings)
# tick = world.wait_for_tick()
if plot == 1:
import matplotlib.pyplot as plt
if plotTheta == 1:
for value in thetaDict.values():
t = [value[k][0] for k in range(len(value))]
theta = [value[k][1] for k in range(len(value))]
u = [value[k][2] for k in range(len(value))]
fig, (ax1, ax2) = plt.subplots(2,1,num=10)
fig.suptitle('Theta and steer input over time')
ax1.plot(t,theta)
ax2.plot(t,u)
plt.show()
if plotVel == 1:
plt.figure()
i = 0
for value in velDict.values():
t = [value[k][0] for k in range(len(value))]
v = [value[k][1] for k in range(len(value))]
plt.plot(t,v)
plt.axhline(velRand[i],0,1)
i += 1
for value in ctrDict.values():
t = [value[k][0] for k in range(len(value))]
throttle = [value[k][1] for k in range(len(value))]
steer = [value[k][2] for k in range(len(value))]
brake = [value[k][3] for k in range(len(value))]
gear = [value[k][4] for k in range(len(value))]
manual = [value[k][5] for k in range(len(value))]
figCtr, axCtr = plt.subplots(4,1)
axCtr[0].plot(t,throttle)
axCtr[0].set_ylim([0,1.1])
axCtr[1].plot(t,steer)
axCtr[1].set_ylim([-1,1])
axCtr[2].plot(t,brake)
axCtr[2].set_ylim([0,1.1])
axCtr[3].plot(t,gear)
axCtr[3].set_ylim([0,6])
axCtr[3].plot(t,manual)
axCtr[0].set_title('Control Inputs')
for value in aclDict.values():
plt.figure()
t = [value[k][0] for k in range(len(value))]
a = [np.linalg.norm([value[k][1],value[k][2]]) for k in range(len(value))]
plt.title('Acceleration / time (m/s^2)')
p_a = plt.plot(t,a)
p_v = plt.plot(t,v)
p_th = plt.plot(t,throttle)
p_br = plt.plot(t,brake)
# ax_br.set_ylim([0,15])
ax = plt.gca()
ax.set_ylim([0,16])
plt.legend(["Acceleration", "Velocity","Throttle","Brake"])
if plotLoc == 1:
for value in locDict.values():
t = [value[k][0] for k in range(len(value))]
x = [value[k][1] for k in range(len(value))]
y = [value[k][2] for k in range(len(value))]
fig, (ax1, ax2) = plt.subplots(2)
fig.suptitle('X and Y over time (m)')
ax1.plot(t,x)
ax2.plot(t,y)
plt.show()
pass
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)
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 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.road_options = None
self.target_waypoint_i = 0
self.waypoint_error_threshold = waypoint_error_threshold
def _remove_extra(route):
def road_option(point):
return point[1]
# never remove last or first waypoint
for i in range(len(route) - 2, 0, -1):
if road_option(route[i]) == RoadOption.RIGHT or road_option(
route[i]) == RoadOption.LEFT:
continue
if road_option(route[i]) == road_option(
route[i - 1]) and road_option(route[i]) == road_option(
route[i + 1]):
del route[i]
def set_endpoint(self, vehicle, endpoint):
self.endpoint = endpoint
vehicle_location = vehicle.get_transform().location
endpoint_location = endpoint.transform.location
route = self.global_route_planner.trace_route(vehicle_location,
endpoint_location)
Planner._remove_extra(route)
self.waypoints = [point[0] for point in route]
self.road_options = [point[1] for point in route]
self.target_waypoint_i = 0
print("Set new endpoint")
print("Path length: ", len(self.waypoints))
print("Path: ")
for (w, r) in zip(self.waypoints, self.road_options):
print(w.transform.location, r)
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, ' ',
self.road_options[self.target_waypoint_i])
return target_waypoint
def get_target_road_option(self, vehicle):
return self.road_options[self.target_waypoint_i]
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
class BasicAgent(object):
"""
BasicAgent implements an agent that navigates the scene.
This agent respects traffic lights and other vehicles, but ignores stop signs.
It has several functions available to specify the route that the agent must follow,
as well as to change its parameters in case a different driving mode is desired.
"""
def __init__(self, vehicle, target_speed=20, opt_dict={}):
"""
Initialization the agent paramters, the local and the global planner.
:param vehicle: actor to apply to agent logic onto
:param target_speed: speed (in Km/h) at which the vehicle will move
:param opt_dict: dictionary in case some of its parameters want to be changed.
This also applies to parameters related to the LocalPlanner.
"""
self._vehicle = vehicle
self._world = self._vehicle.get_world()
self._map = self._world.get_map()
self._last_traffic_light = None
# Base parameters
self._ignore_traffic_lights = False
self._ignore_stop_signs = False
self._ignore_vehicles = False
self._target_speed = target_speed
self._sampling_resolution = 2.0
self._base_tlight_threshold = 5.0 # meters
self._base_vehicle_threshold = 5.0 # meters
self._max_brake = 0.5
# Change parameters according to the dictionary
opt_dict['target_speed'] = target_speed
if 'ignore_traffic_lights' in opt_dict:
self._ignore_traffic_lights = opt_dict['ignore_traffic_lights']
if 'ignore_stop_signs' in opt_dict:
self._ignore_stop_signs = opt_dict['ignore_stop_signs']
if 'ignore_vehicles' in opt_dict:
self._ignore_vehicles = opt_dict['ignore_vehicles']
if 'sampling_resolution' in opt_dict:
self._sampling_resolution = opt_dict['sampling_resolution']
if 'base_tlight_threshold' in opt_dict:
self._base_tlight_threshold = opt_dict['base_tlight_threshold']
if 'base_vehicle_threshold' in opt_dict:
self._base_vehicle_threshold = opt_dict['base_vehicle_threshold']
if 'max_brake' in opt_dict:
self._max_steering = opt_dict['max_brake']
# Initialize the planners
self._local_planner = LocalPlanner(self._vehicle, opt_dict=opt_dict)
self._global_planner = GlobalRoutePlanner(self._map,
self._sampling_resolution)
def add_emergency_stop(self, control):
"""
Overwrites the throttle a brake values of a control to perform an emergency stop.
The steering is kept the same to avoid going out of the lane when stopping during turns
:param speed (carl.VehicleControl): control to be modified
"""
control.throttle = 0.0
control.brake = self._max_brake
control.hand_brake = False
return control
def set_target_speed(self, speed):
"""
Changes the target speed of the agent
:param speed (float): target speed in Km/h
"""
self._local_planner.set_speed(speed)
def follow_speed_limits(self, value=True):
"""
If active, the agent will dynamically change the target speed according to the speed limits
:param value (bool): whether or not to activate this behavior
"""
self._local_planner.follow_speed_limits(value)
def get_local_planner(self):
"""Get method for protected member local planner"""
return self._local_planner
def get_global_planner(self):
"""Get method for protected member local planner"""
return self._global_planner
def set_destination(self, end_location, start_location=None):
"""
This method creates a list of waypoints between a starting and ending location,
based on the route returned by the global router, and adds it to the local planner.
If no starting location is passed, the vehicle local planner's target location is chosen,
which corresponds (by default), to a location about 5 meters in front of the vehicle.
:param end_location (carla.Location): final location of the route
:param start_location (carla.Location): starting location of the route
"""
if not start_location:
start_location = self._local_planner.target_waypoint.transform.location
clean_queue = True
else:
start_location = self._vehicle.get_location()
clean_queue = False
start_waypoint = self._map.get_waypoint(start_location)
end_waypoint = self._map.get_waypoint(end_location)
route_trace = self.trace_route(start_waypoint, end_waypoint)
self._local_planner.set_global_plan(route_trace,
clean_queue=clean_queue)
def set_global_plan(self,
plan,
stop_waypoint_creation=True,
clean_queue=True):
"""
Adds a specific plan to the agent.
:param plan: list of [carla.Waypoint, RoadOption] representing the route to be followed
:param stop_waypoint_creation: stops the automatic random creation of waypoints
:param clean_queue: resets the current agent's plan
"""
self._local_planner.set_global_plan(
plan,
stop_waypoint_creation=stop_waypoint_creation,
clean_queue=clean_queue)
def trace_route(self, start_waypoint, end_waypoint):
"""
Calculates the shortest route between a starting and ending waypoint.
:param start_waypoint (carla.Waypoint): initial waypoint
:param end_waypoint (carla.Waypoint): final waypoint
"""
start_location = start_waypoint.transform.location
end_location = end_waypoint.transform.location
return self._global_planner.trace_route(start_location, end_location)
def run_step(self):
"""Execute one step of navigation."""
hazard_detected = False
# Retrieve all relevant actors
actor_list = self._world.get_actors()
vehicle_list = actor_list.filter("*vehicle*")
lights_list = actor_list.filter("*traffic_light*")
vehicle_speed = get_speed(self._vehicle) / 3.6
# Check for possible vehicle obstacles
max_vehicle_distance = self._base_vehicle_threshold + vehicle_speed
affected_by_vehicle, _, _ = self._vehicle_obstacle_detected(
vehicle_list, max_vehicle_distance)
if affected_by_vehicle:
hazard_detected = True
# Check if the vehicle is affected by a red traffic light
max_tlight_distance = self._base_tlight_threshold + vehicle_speed
affected_by_tlight, _ = self._affected_by_traffic_light(
lights_list, max_tlight_distance)
if affected_by_tlight:
hazard_detected = True
control = self._local_planner.run_step()
if hazard_detected:
control = self.add_emergency_stop(control)
return control
def done(self):
"""Check whether the agent has reached its destination."""
return self._local_planner.done()
def ignore_traffic_lights(self, active=True):
"""(De)activates the checks for traffic lights"""
self._ignore_traffic_lights = active
def ignore_stop_signs(self, active=True):
"""(De)activates the checks for stop signs"""
self._ignore_stop_signs = active
def ignore_vehicles(self, active=True):
"""(De)activates the checks for stop signs"""
self._ignore_vehicles = active
def _affected_by_traffic_light(self, lights_list=None, max_distance=None):
"""
Method to check if there is a red light affecting the vehicle.
:param lights_list (list of carla.TrafficLight): list containing TrafficLight objects.
If None, all traffic lights in the scene are used
:param max_distance (float): max distance for traffic lights to be considered relevant.
If None, the base threshold value is used
"""
if self._ignore_traffic_lights:
return (False, None)
if not lights_list:
lights_list = self._world.get_actors().filter("*traffic_light*")
if not max_distance:
max_distance = self._base_tlight_threshold
if self._last_traffic_light:
if self._last_traffic_light.state != carla.TrafficLightState.Red:
self._last_traffic_light = None
else:
return (True, self._last_traffic_light)
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
for traffic_light in lights_list:
object_location = get_trafficlight_trigger_location(traffic_light)
object_waypoint = self._map.get_waypoint(object_location)
if object_waypoint.road_id != ego_vehicle_waypoint.road_id:
continue
ve_dir = ego_vehicle_waypoint.transform.get_forward_vector()
wp_dir = object_waypoint.transform.get_forward_vector()
dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z
if dot_ve_wp < 0:
continue
if traffic_light.state != carla.TrafficLightState.Red:
continue
if is_within_distance(object_waypoint.transform,
self._vehicle.get_transform(), max_distance,
[0, 90]):
self._last_traffic_light = traffic_light
return (True, traffic_light)
return (False, None)
def _vehicle_obstacle_detected(self,
vehicle_list=None,
max_distance=None,
up_angle_th=90,
low_angle_th=0,
lane_offset=0):
"""
Method to check if there is a vehicle in front of the agent blocking its path.
:param vehicle_list (list of carla.Vehicle): list contatining vehicle objects.
If None, all vehicle in the scene are used
:param max_distance: max freespace to check for obstacles.
If None, the base threshold value is used
"""
if self._ignore_vehicles:
return (False, None, -1)
if not vehicle_list:
vehicle_list = self._world.get_actors().filter("*vehicle*")
if not max_distance:
max_distance = self._base_vehicle_threshold
ego_transform = self._vehicle.get_transform()
ego_wpt = self._map.get_waypoint(self._vehicle.get_location())
# Get the right offset
if ego_wpt.lane_id < 0 and lane_offset != 0:
lane_offset *= -1
# Get the transform of the front of the ego
ego_forward_vector = ego_transform.get_forward_vector()
ego_extent = self._vehicle.bounding_box.extent.x
ego_front_transform = ego_transform
ego_front_transform.location += carla.Location(
x=ego_extent * ego_forward_vector.x,
y=ego_extent * ego_forward_vector.y,
)
for target_vehicle in vehicle_list:
target_transform = target_vehicle.get_transform()
target_wpt = self._map.get_waypoint(target_transform.location,
lane_type=carla.LaneType.Any)
# Simplified version for outside junctions
if not ego_wpt.is_junction or not target_wpt.is_junction:
if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id + lane_offset:
next_wpt = self._local_planner.get_incoming_waypoint_and_direction(
steps=3)[0]
if not next_wpt:
continue
if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id + lane_offset:
continue
target_forward_vector = target_transform.get_forward_vector()
target_extent = target_vehicle.bounding_box.extent.x
target_rear_transform = target_transform
target_rear_transform.location -= carla.Location(
x=target_extent * target_forward_vector.x,
y=target_extent * target_forward_vector.y,
)
if is_within_distance(target_rear_transform,
ego_front_transform, max_distance,
[low_angle_th, up_angle_th]):
return (True, target_vehicle,
compute_distance(target_transform.location,
ego_transform.location))
# Waypoints aren't reliable, check the proximity of the vehicle to the route
else:
route_bb = []
ego_location = ego_transform.location
extent_y = self._vehicle.bounding_box.extent.y
r_vec = ego_transform.get_right_vector()
p1 = ego_location + carla.Location(extent_y * r_vec.x,
extent_y * r_vec.y)
p2 = ego_location + carla.Location(-extent_y * r_vec.x,
-extent_y * r_vec.y)
route_bb.append([p1.x, p1.y, p1.z])
route_bb.append([p2.x, p2.y, p2.z])
for wp, _ in self._local_planner.get_plan():
if ego_location.distance(
wp.transform.location) > max_distance:
break
r_vec = wp.transform.get_right_vector()
p1 = wp.transform.location + carla.Location(
extent_y * r_vec.x, extent_y * r_vec.y)
p2 = wp.transform.location + carla.Location(
-extent_y * r_vec.x, -extent_y * r_vec.y)
route_bb.append([p1.x, p1.y, p1.z])
route_bb.append([p2.x, p2.y, p2.z])
if len(route_bb) < 3:
# 2 points don't create a polygon, nothing to check
return (False, None, -1)
ego_polygon = Polygon(route_bb)
# Compare the two polygons
for target_vehicle in vehicle_list:
target_extent = target_vehicle.bounding_box.extent.x
if target_vehicle.id == self._vehicle.id:
continue
if ego_location.distance(
target_vehicle.get_location()) > max_distance:
continue
target_bb = target_vehicle.bounding_box
target_vertices = target_bb.get_world_vertices(
target_vehicle.get_transform())
target_list = [[v.x, v.y, v.z] for v in target_vertices]
target_polygon = Polygon(target_list)
if ego_polygon.intersects(target_polygon):
return (True, target_vehicle,
compute_distance(target_vehicle.get_location(),
ego_location))
return (False, None, -1)
return (False, None, -1)
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 Agent():
def __init__(self, vehicle, ignore_traffic_light=False):
self.vehicle = vehicle
self.ignore_traffic_light = ignore_traffic_light
self.world = vehicle.get_world()
self.map = self.world.get_map()
self.control_vehicle = pid.VehiclePIDController(vehicle,
max_throttle=1,
args_lateral={
'K_P': 0.58,
'K_D': 0.4,
'K_I': 0.5
},
args_longitudinal={
'K_P': 0.15,
'K_D': 0.05,
'K_I': 0.07
})
# Relacionados à rota
self.spawn_location = None
self.destination_location = None
self.dao = GlobalRoutePlannerDAO(self.map, 2.0)
self.grp = GlobalRoutePlanner(self.dao)
self.grp.setup()
self.route = []
# Relacionados aos obstáculos e sensores
self.obstacle_info = {'distance': None, 'actor': None}
self.emergency_brake_distance = 3
self.dynamic_brake_distance = 0
self.previous_obstacle = None
self.status = ""
# Sensores
# Sensor de colisão sendo usado para definir a posição da câmera do simulador
# Não é usado de fato como sensor e seus dados não são tratados
self.camera_bp = self.world.get_blueprint_library().find(
'sensor.other.collision')
self.camera_transform = carla.Transform(carla.Location(x=-5.5, z=3.5),
carla.Rotation(pitch=345))
self._camera = self.world.spawn_actor(self.camera_bp,
self.camera_transform,
attach_to=vehicle)
# Sensor de obstáculos
# É uma implementação de um sensor de radar que já identifica o obstáculo
# e a distância até ele
self.obstacle_sensor_bp = self.world.get_blueprint_library().find(
'sensor.other.obstacle')
self.obstacle_sensor_bp.set_attribute('distance', '30.0')
self.obstacle_sensor_bp.set_attribute('only_dynamics', 'True')
self.obstacle_sensor_bp.set_attribute('sensor_tick', '0.1')
self.obstacle_sensor_bp.set_attribute('hit_radius', '1.0')
# Posicionado na frente do radiador do veículo
self.obstacle_sensor_transform = carla.Transform(
carla.Location(x=1.5, z=0.5), carla.Rotation())
self.obstacle_sensor = self.world.spawn_actor(
self.obstacle_sensor_bp,
self.obstacle_sensor_transform,
attach_to=vehicle)
self.obstacle_sensor.listen(lambda data: self.obstacle_detection(data))
def get_speed(self, vehicle):
velocity = vehicle.get_velocity()
return 3.6 * math.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2)
def obstacle_detection(self, data):
self.obstacle_info = {
'distance': data.distance,
'actor': data.other_actor
}
def set_route(self, spawn_location, destination_location):
self.spawn_location = spawn_location
self.destination_location = destination_location
self.route = [
wp[0] for wp in self.grp.trace_route(self.spawn_location,
self.destination_location)
]
def emergency_stop(self):
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
return control
def show_path(self, distance=15):
route_lenght = len(self.route)
for i in range(distance):
if i < route_lenght:
self.world.debug.draw_string(self.route[i].transform.location,
'o',
draw_shadow=False,
color=carla.Color(r=0, g=255,
b=0),
life_time=1,
persistent_lines=True)
def traffic_light_manager(self, waypoint):
"""
"""
actual_landmarks = waypoint.get_landmarks_of_type(
self.dynamic_brake_distance, "1000001", False)
if actual_landmarks:
next_traffic_light = self.world.get_traffic_light(
actual_landmarks[0])
if (str(next_traffic_light.get_state()) == "Red"
or str(next_traffic_light.get_state()) == "Yellow" and
actual_landmarks[0].distance < self.dynamic_brake_distance
and self.ignore_traffic_light == False):
return True
return False
def equal_location(self, vehicle, waypoint):
if ((abs(vehicle.get_location().x - waypoint.transform.location.x) <= 4
and abs(vehicle.get_location().y - waypoint.transform.location.y)
<= 4)):
return True
return False
def arrived(self):
return len(self.route) <= 1
def run_step(self, speed=30):
# Confere se o veículo está percorrendo a rota e atualiza o índice da rota
if self.equal_location(self.vehicle, self.route[0]):
self.route.pop(0)
ego_vehicle_wp = self.map.get_waypoint(self.vehicle.get_location())
current_speed = self.get_speed(self.vehicle)
# Se o semáforo estiver vermelho ou amarelo, pare
if self.traffic_light_manager(ego_vehicle_wp):
if self.status != 'semáforo':
self.status = 'semáforo'
print(self.status)
return self.emergency_stop()
# Sem obstáculo ou obstáculo à uma distância segura
if self.obstacle_info['actor'] == None or self.obstacle_info[
'distance'] > self.dynamic_brake_distance:
if self.status != 'normal':
self.status = 'normal'
print(self.status)
self.dynamic_brake_distance = (current_speed / 7)**2 / 2
return self.control_vehicle.run_step(speed, self.route[0])
else:
# Obstáculo na iminência de colisão
if self.obstacle_info['distance'] <= self.emergency_brake_distance:
if self.status != 'freia':
self.status = 'freia'
print(self.status)
return self.emergency_stop()
# Obstáculo à frente em distância que o veículo consegue desviar
if self.obstacle_info[
'distance'] <= self.dynamic_brake_distance and self.obstacle_info[
'actor'].id != self.previous_obstacle:
if self.status != 'desvia':
self.previous_obstacle = self.obstacle_info['actor'].id
self.status = 'desvia'
print(self.status, end=' ')
vehicle_waypoint = self.map.get_waypoint(
self.vehicle.get_location())
# Confere se existe alguma faixa na rua em que se está para mudar na
# tentativa de não colidir com o osbtáculo. Se sim, gera uma rota até
# o destino a partir da outra faixa escolhida (se houver)
if vehicle_waypoint.get_left_lane() and (
int(vehicle_waypoint.lane_id) *
int(vehicle_waypoint.get_left_lane().lane_id) > 0):
print('pra esquerda')
self.set_route(
vehicle_waypoint.get_left_lane().transform.location,
self.destination_location)
elif vehicle_waypoint.get_right_lane() and (
int(vehicle_waypoint.lane_id) *
int(vehicle_waypoint.get_right_lane().lane_id) > 0):
print('pra direita')
self.set_route(
vehicle_waypoint.get_right_lane().transform.location,
self.destination_location)
return self.control_vehicle.run_step(speed, self.route[0])
else:
return self.control_vehicle.run_step(speed, self.route[0])
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
def game_loop(args):
pygame.init()
pygame.font.init()
world = None
try:
client = carla.Client(args.host, args.port)
client.set_timeout(4.0)
display = pygame.display.set_mode((args.width, args.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
hud = HUD(args.width, args.height)
world = World(client.get_world(), hud, args.filter)
controller = KeyboardControl(world, False)
agent = BasicAgent(world.player, 30)
spawn_points = world.map.get_spawn_points()
destination = spawn_points[random.randint(0,
len(spawn_points) -
1)].location
agent.set_destination((destination.x, destination.y, destination.z))
clock = pygame.time.Clock()
dao = GlobalRoutePlannerDAO(world.map)
planner = GlobalRoutePlanner(dao)
planner.setup()
while True:
if controller.parse_events(client, world, clock):
return
# as soon as the server is ready continue!
if not world.world.wait_for_tick(10.0):
continue
world.tick(clock)
world.render(display)
pygame.display.flip()
control = agent.run_step()
control.manual_gear_shift = False
world.player.apply_control(control)
my_location = world.player.get_location()
me2destination = my_location.distance(destination)
if me2destination < 10:
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)
for (waypoint, road_option) in trace_list[0:20]:
world.world.debug.draw_string(waypoint.transform.location,
'O',
draw_shadow=False,
color=carla.Color(r=255,
g=0,
b=0),
life_time=1.0,
persistent_lines=True)
#origin = world.player.get_location()
#turn_list = planner.abstract_route_plan(origin, spawn_point.location)
#print('next cmd:',turn_list[0])
finally:
if world is not None:
world.destroy()
pygame.quit()
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])
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
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 main():
global actor_list
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
#add_camera_component(world, 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)
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
x = []
y = []
#theta = waypoints[0].transform.rotation.roll
theta = math.atan2((waypoints[3].transform.location.y -
waypoints[0].transform.location.y),
(waypoints[3].transform.location.x -
waypoints[0].transform.location.x))
for i in range(min(len(waypoints) - 1, 50)):
_x = waypoints[i].transform.location.x - waypoints[
0].transform.location.x
_y = waypoints[i].transform.location.y - waypoints[
0].transform.location.y
new_theta = math.pi / 2 - theta
x_ = _x * math.cos(new_theta) - _y * math.sin(new_theta)
y_ = _y * math.cos(new_theta) + _x * math.sin(new_theta)
x.append(x_)
y.append(y_)
scale = 10
plt.figure(figsize=(8, 4))
plt.xlim(-scale, scale)
plt.ylim(0, scale)
plt.plot(x, y, "r-", linewidth=50)
plt.show()
sleep(0.3)
finally:
print('destroying actors')
for actor in actor_list:
actor.destroy()
actor_list = []
print('done.')
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
def main():
#pygame.init()
client = carla.Client('localhost', 2000)
client.set_timeout(10.0)
world = client.get_world()
frame = None
#Create WaypointMap
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 Cars on Waypoint
model1 = random.choice(
world.get_blueprint_library().filter('vehicle.bmw.*'))
model2 = random.choice(
world.get_blueprint_library().filter('vehicle.bmw.*'))
spawn_points = world.get_map().get_spawn_points()
spawn_point_numeric_value1 = randrange(300)
print("Spawpoint 1: ")
print(spawn_point_numeric_value1)
spawn_point1 = spawn_points[spawn_point_numeric_value1]
vehicle1 = world.try_spawn_actor(model1, spawn_point1)
time.sleep(5)
location1 = vehicle1.get_location()
print(location1)
print(vehicle1.id)
spawn_point_numeric_value2 = randrange(300)
print("Spawpoint 2: ")
print(spawn_point_numeric_value2)
spawn_point2 = spawn_points[spawn_point_numeric_value2]
vehicle2 = world.try_spawn_actor(model2, spawn_point2)
time.sleep(5)
location2 = vehicle1.get_location()
print(location2)
print(vehicle2.id)
#print path of vehicle 2
a = carla.Location(spawn_point1.location)
b = carla.Location(spawn_point2.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
#Start Car
vehicle1.set_simulate_physics(True)
driving_car = BasicAgent(vehicle1, target_speed=50)
destiny = spawn_point2.location
driving_car.set_destination((destiny.x, destiny.y, destiny.z))
#vehicle1.set_autopilot(True)
while True:
world.tick()
ts = world.wait_for_tick()
# Get control commands
control_hero = driving_car.run_step()
vehicle1.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
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:
carla_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, carla_map):
self._map = carla_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):
"""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._map.get_waypoint(location.as_carla_location(),
project_to_road=True,
lane_type=carla.LaneType.Any)
if waypoint:
return pylot.utils.Transform.from_carla_transform(
waypoint.transform)
else:
return None
def is_intersection(self, 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._map.get_waypoint(location.as_carla_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:
# XXX(ionel): is_intersection will be deprecated in the future
# Carla releases.
return waypoint.is_intersection
def is_on_lane(self, 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._map.get_waypoint(location.as_carla_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, location2):
"""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._map.get_waypoint(location1.as_carla_location(),
project_to_road=False,
lane_type=carla.LaneType.Driving)
if not waypoint1:
# First location is not on a drivable lane.
return False
waypoint2 = self._map.get_waypoint(location2.as_carla_location(),
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):
"""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._map.get_waypoint(
transform.location.as_carla_location(),
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):
"""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._map.get_waypoint(location.as_carla_location(),
project_to_road=False,
lane_type=carla.LaneType.Stop)
return not waypoint
def distance_to_intersection(self, location, max_distance_to_check=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._map.get_waypoint(location.as_carla_location(),
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):
"""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._map.get_waypoint(
location.as_carla_location(),
project_to_road=False,
lane_type=carla.LaneType.Bidirectional)
return not waypoint
def must_obey_traffic_light(self, ego_location, tl_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._map.get_waypoint(ego_location.as_carla_location(),
project_to_road=False,
lane_type=carla.LaneType.Any)
if waypoint and waypoint.is_intersection:
# Do not obey 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_obey_european_traffic_light(self, ego_transform, tl_locations,
tl_max_dist_thresh):
ego_loc = ego_transform.location.as_carla_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(tl_loc.as_carla_location())
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, tl_locations,
tl_max_dist_thresh):
ego_loc = ego_transform.location.as_carla_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 ego_transform.is_within_distance_ahead(tl_loc,
tl_max_dist_thresh):
_, magnitude, angle = ego_transform.get_vector_magnitude_angle(
tl_loc)
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):
"""Transforms a location into Freenet coordinates.
Args:
location (:py:class:`~pylot.utils.Location`): Location in world
coordinates.
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).
waypoint = self._map.get_waypoint(location.as_carla_location(),
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, location.distance(d0_location)
def get_left_lane(self, location):
raise NotImplementedError
def get_right_lane(self, location):
raise NotImplementedError
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 (: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._map.get_waypoint(
source_loc.as_carla_location(),
project_to_road=True,
lane_type=carla.LaneType.Driving)
end_waypoint = self._map.get_waypoint(
destination_loc.as_carla_location(),
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([
pylot.utils.Transform.from_carla_transform(waypoint[0].transform)
for waypoint in route
])
p1 = carla.Location(x=243, y=120, z=1) # Start point
p2 = carla.Location(x=248, y=-60, z=1) # End point
#Get pointer to a random Model3 Tesla
bp = rg.choice(bpl.filter('vehicle.tesla.model3'))
#Spawn a car and add it to the list of actors
actors = []
car = world.spawn_actor(bp, carla.Transform(p1))
actors.append(car)
# Plan a preliminary route (without regard to traffic
dao = GlobalRoutePlannerDAO(world.get_map(), sampling_resolution=1)
rp = GlobalRoutePlanner(dao)
rp.setup()
route = rp.trace_route(p1, p2)
# Spawn an obstacle car on the planned route
bp = rg.choice(bpl.filter('vehicle.audi.*'))
obs = world.spawn_actor(bp, route[40][0].transform)
#Make obstacle car drive slowly
obs.apply_control(carla.VehicleControl(throttle=0.3, steer=0))
actors.append(obs)
#Initialize car and spectator
t = route[0][0].transform
car.set_transform(t)
t.location.z += 25 # 15 meters above car
sp.set_transform(t)
car.apply_control(carla.VehicleControl(throttle=0, steer=0))
def compute_route_waypoints(world_map,
start_waypoint,
end_waypoint,
resolution=1.0,
plan=None):
"""
Returns a list of (waypoint, RoadOption)-tuples that describes a route
starting at start_waypoint, ending at end_waypoint.
start_waypoint (carla.Waypoint):
Starting waypoint of the route
end_waypoint (carla.Waypoint):
Destination waypoint of the route
resolution (float):
Resolution, or lenght, of the steps between waypoints
(in meters)
plan (list(RoadOption) or None):
If plan is not None, generate a route that takes every option as provided
in the list for every intersections, in the given order.
(E.g. set plan=[RoadOption.STRAIGHT, RoadOption.LEFT, RoadOption.RIGHT]
to make the route go straight, then left, then right.)
If plan is None, we use the GlobalRoutePlanner to find a path between
start_waypoint and end_waypoint.
"""
if plan is None:
# Setting up global router
dao = GlobalRoutePlannerDAO(world_map, resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
# Obtain route plan
route = grp.trace_route(start_waypoint.transform.location,
end_waypoint.transform.location)
else:
# Compute route waypoints
route = []
current_waypoint = start_waypoint
for i, action in enumerate(plan):
# Generate waypoints to next junction
wp_choice = [current_waypoint]
while len(wp_choice) == 1:
current_waypoint = wp_choice[0]
route.append((current_waypoint, RoadOption.LANEFOLLOW))
wp_choice = current_waypoint.next(resolution)
# Stop at destination
if i > 0 and current_waypoint.transform.location.distance(
end_waypoint.transform.location) < resolution:
break
if action == RoadOption.VOID:
break
# Make sure that next intersection waypoints are far enough
# from each other so we choose the correct path
step = resolution
while len(wp_choice) > 1:
wp_choice = current_waypoint.next(step)
wp0, wp1 = wp_choice[:2]
if wp0.transform.location.distance(
wp1.transform.location) < resolution:
step += resolution
else:
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=np.cos(np.radians(current_transform.rotation.yaw)),
y=np.sin(np.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
route.append((current_waypoint, action))
current_waypoint = current_waypoint.next(resolution)[0]
while current_waypoint.is_intersection:
route.append((current_waypoint, action))
current_waypoint = current_waypoint.next(resolution)[0]
assert route
# Change action 5 wp before intersection
num_wp_to_extend_actions_with = 5
action = route[0][1]
for i in range(1, len(route)):
next_action = route[i][1]
if next_action != action:
if next_action != RoadOption.LANEFOLLOW:
for j in range(num_wp_to_extend_actions_with):
route[i - j - 1] = (route[i - j - 1][0], route[i][1])
action = next_action
return route
