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 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 __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 __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 __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 __init__(self, carla_world): """ Constructor """ self._world = carla_world self._map = carla_world.get_map() self._ego_vehicle = None self._ego_vehicle_location = None # self.current_route_list = RouteList() self._on_tick = None self._goal = None self._role_name = rospy.get_param("~role_name", 'ego_vehicle') self._dao = GlobalRoutePlannerDAO( self._map, sampling_resolution=WAYPOINT_DISTANCE) self._random_route_planner = RandomRoutePlanner(self._dao) self._random_route_planner.setup() self._global_planner = GlobalRoutePlanner(self._dao) self._global_planner.setup() self._getWaypointService = rospy.Service( '/carla_waypoint_publisher/{}/get_waypoint'.format( self._role_name), GetWaypoint, self._get_waypoint) self._getActorWaypointService = rospy.Service( '/carla_waypoint_publisher/{}/get_actor_waypoint'.format( self._role_name), GetActorWaypoint, self._get_actor_waypoint) self._getRouteService = rospy.Service( '/carla_client_interface/{}/get_route'.format(self._role_name), RoutePlanService, self._get_route) # self._getAgentPotentialRoutesService = rospy.Service( # '/carla_client_interface/{}/agent_potential_routes'.format(self._role_name), # RoutePlanService, self._get_agent_potential_route_service) self._update_lock = threading.Lock() rospy.loginfo("Waiting for ego vehicle .....")
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 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 __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 __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 __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 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
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 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
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 __init__(self, reference_actor, actor, distance, comparison_operator=operator.lt, distance_type="cartesianDistance", freespace=False, name="TriggerDistanceToVehicle"): """ Setup trigger distance """ super(InTriggerDistanceToVehicle, self).__init__(name) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) self._reference_actor = reference_actor self._actor = actor self._distance = distance self._distance_type = distance_type self._freespace = freespace self._comparison_operator = comparison_operator if distance_type == "longitudinal": self._global_rp = GlobalRoutePlanner( CarlaDataProvider.get_world().get_map(), 1.0) else: self._global_rp = None
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
def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=80): """ Setup all relevant parameters and create scenario """ self._world = world self._map = CarlaDataProvider.get_map() self._source_dist = 40 self._sink_dist = 10 self._source_dist_interval = [25, 50] self._opposite_speed = 35 / 3.6 self._green_light_delay = 5 # Wait before the ego's lane traffic light turns green self._direction = 'left' self._route_planner = GlobalRoutePlanner(self._map, 2.0) self.timeout = timeout super(SignalizedJunctionRightTurn, self).__init__("SignalizedJunctionRightTurn", ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable)
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 __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()
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 __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 _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