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 __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 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 __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 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 __init__(self, carla_map):
self._map = carla_map
# Setup global planner.
self._grp = GlobalRoutePlanner(
GlobalRoutePlannerDAO(
self._map,
1.0 # Distance between waypoints
))
self._grp.setup()
def get_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 __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 __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_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 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
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()
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 __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, 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)
def __init__(self, args):
self.render_display = False
self.record_display = False
self.record_vision = True
self.record_dir = None #'/nfs/kun1/users/aviralkumar/carla_data/'
self.vision_size = args.vision_size
self.vision_fov = args.vision_fov
self.changing_weather_speed = float(args.weather)
self.frame_skip = args.frame_skip
self.max_episode_steps = args.steps
self.multiagent = args.multiagent
self.start_lane = args.lane
self.follow_traffic_lights = args.lights
if self.record_display:
assert self.render_display
self.actor_list = []
if self.render_display:
pygame.init()
self.render_display = pygame.display.set_mode(
(800, 600), pygame.HWSURFACE | pygame.DOUBLEBUF)
self.font = get_font()
self.clock = pygame.time.Clock()
self.client = carla.Client('localhost', 2000)
self.client.set_timeout(2.0)
self.world = self.client.get_world()
self.map = self.world.get_map()
## Define the route planner
self.route_planner_dao = GlobalRoutePlannerDAO(self.map,
sampling_resolution=0.1)
self.route_planner = CustomGlobalRoutePlanner(self.route_planner_dao)
# tests specific to map 4:
if self.start_lane and self.map.name != "Town04":
raise NotImplementedError
# remove old vehicles and sensors (in case they survived)
self.world.tick()
actor_list = self.world.get_actors()
for vehicle in actor_list.filter("*vehicle*"):
print("Warning: removing old vehicle")
vehicle.destroy()
for sensor in actor_list.filter("*sensor*"):
print("Warning: removing old sensor")
sensor.destroy()
self.vehicle = None
self.vehicles_list = [] # their ids
self.reset_vehicle() # creates self.vehicle
self.actor_list.append(self.vehicle)
blueprint_library = self.world.get_blueprint_library()
if self.render_display:
self.camera_display = self.world.spawn_actor(
blueprint_library.find('sensor.camera.rgb'),
carla.Transform(carla.Location(x=-5.5, z=2.8),
carla.Rotation(pitch=-15)),
attach_to=self.vehicle)
self.actor_list.append(self.camera_display)
bp = blueprint_library.find('sensor.camera.rgb')
bp.set_attribute('image_size_x', str(self.vision_size))
bp.set_attribute('image_size_y', str(self.vision_size))
bp.set_attribute('fov', str(self.vision_fov))
location = carla.Location(x=1.6, z=1.7)
self.camera_vision = self.world.spawn_actor(
bp,
carla.Transform(location, carla.Rotation(yaw=0.0)),
attach_to=self.vehicle)
self.actor_list.append(self.camera_vision)
if self.record_display or self.record_vision:
if self.record_dir is None:
self.record_dir = "carla-{}-{}x{}-fov{}".format(
self.map.name.lower(), self.vision_size, self.vision_size,
self.vision_fov)
if self.frame_skip > 1:
self.record_dir += '-{}'.format(self.frame_skip)
if self.changing_weather_speed > 0.0:
self.record_dir += '-weather'
if self.multiagent:
self.record_dir += '-mutiagent'
if self.follow_traffic_lights:
self.record_dir += '-lights'
self.record_dir += '-{}k'.format(self.max_episode_steps //
1000)
now = datetime.datetime.now()
self.record_dir += now.strftime("-%Y-%m-%d-%H-%M-%S")
if not os.path.exists(self.record_dir):
os.mkdir(self.record_dir)
if self.render_display:
self.sync_mode = CarlaSyncMode(self.world,
self.camera_display,
self.camera_vision,
fps=20)
else:
self.sync_mode = CarlaSyncMode(self.world,
self.camera_vision,
fps=20)
# weather
self.weather = Weather(self.world, self.changing_weather_speed)
# dummy variables, to match deep mind control's APIs
low = -1.0
high = 1.0
self.action_space = DotMap()
self.action_space.low.min = lambda: low
self.action_space.high.max = lambda: high
self.action_space.shape = [2]
self.observation_space = DotMap()
self.observation_space.shape = (3, self.vision_size, self.vision_size)
self.observation_space.dtype = np.dtype(np.uint8)
self.reward_range = None
self.metadata = None
self.action_space.sample = lambda: np.random.uniform(
low=low, high=high, size=self.action_space.shape[0]).astype(
np.float32)
# roaming carla agent
self.agent = None
self.world.tick()
self.reset_init() # creates self.agent
## Initialize the route planner
self.route_planner.setup()
## Collision detection
self._proximity_threshold = 10.0
self._traffic_light_threshold = 5.0
self.actor_list = self.world.get_actors()
for idx in range(len(self.actor_list)):
print(idx, self.actor_list[idx])
# import ipdb; ipdb.set_trace()
self.vehicle_list = self.actor_list.filter("*vehicle*")
self.lights_list = self.actor_list.filter("*traffic_light*")
self.object_list = self.actor_list.filter("*traffic.*")
## Initialize the route planner
self.route_planner.setup()
## The map is deterministic so for reward relabelling, we can
## instantiate the environment object and then query the distance function
## in the env, which directly uses this map_graph, and we need not save it.
self._map_graph = self.route_planner._graph
## This is a dummy for the target location, we can make this an input
## to the env in RL code.
self.target_location = carla.Location(x=-13.473097,
y=134.311234,
z=-0.010433)
## Now reset the env once
self.reset()
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.')
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 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)
