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