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