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
