def __init__(self, host="127.0.0.1", port=2000,
viewer_res=(1280, 720), obs_res=(1280, 720),
reward_fn=None, encode_state_fn=None,
synchronous=True, fps=30, action_smoothing=0.9,
start_carla=True):
"""
Initializes a gym-like environment that can be used to interact with CARLA.
Connects to a running CARLA enviromment (tested on version 0.9.5) and
spwans a lincoln mkz2017 passenger car with automatic transmission.
This vehicle can be controlled using the step() function,
taking an action that consists of [steering_angle, throttle].
host (string):
IP address of the CARLA host
port (short):
Port used to connect to CARLA
viewer_res (int, int):
Resolution of the spectator camera (placed behind the vehicle by default)
as a (width, height) tuple
obs_res (int, int):
Resolution of the observation camera (placed on the dashboard by default)
as a (width, height) tuple
reward_fn (function):
Custom reward function that is called every step.
If None, no reward function is used.
encode_state_fn (function):
Function that takes the image (of obs_res resolution) from the
observation camera and encodes it to some state vector to returned
by step(). If None, step() returns the full image.
action_smoothing:
Scalar used to smooth the incomming action signal.
1.0 = max smoothing, 0.0 = no smoothing
fps (int):
FPS of the client. If fps <= 0 then use unbounded FPS.
Note: Sensors will have a tick rate of fps when fps > 0,
otherwise they will tick as fast as possible.
synchronous (bool):
If True, run in synchronous mode (read the comment above for more info)
start_carla (bool):
Automatically start CALRA when True. Note that you need to
set the environment variable ${CARLA_ROOT} to point to
the CARLA root directory for this option to work.
"""
# Start CARLA from CARLA_ROOT
self.carla_process = None
if start_carla:
if "CARLA_ROOT" not in os.environ:
raise Exception("${CARLA_ROOT} has not been set!")
dist_dir = os.path.join(os.environ["CARLA_ROOT"], "Dist")
if not os.path.isdir(dist_dir):
raise Exception("Expected to find directory \"Dist\" under ${CARLA_ROOT}!")
sub_dirs = [os.path.join(dist_dir, sub_dir) for sub_dir in os.listdir(dist_dir) if os.path.isdir(os.path.join(dist_dir, sub_dir))]
if len(sub_dirs) == 0:
raise Exception("Could not find a packaged distribution of CALRA! " +
"(try building CARLA with the \"make package\" " +
"command in ${CARLA_ROOT})")
sub_dir = sub_dirs[0]
carla_path = os.path.join(sub_dir, "LinuxNoEditor", "CarlaUE4.sh")
launch_command = [carla_path]
launch_command += ["Town07"]
if synchronous: launch_command += ["-benchmark"]
launch_command += ["-fps=%i" % fps]
print("Running command:")
print(" ".join(launch_command))
self.carla_process = subprocess.Popen(launch_command, stdout=subprocess.PIPE, universal_newlines=True)
print("Waiting for CARLA to initialize")
for line in self.carla_process.stdout:
if "LogCarla: Number Of Vehicles" in line:
break
time.sleep(2)
# Initialize pygame for visualization
pygame.init()
pygame.font.init()
width, height = viewer_res
if obs_res is None:
out_width, out_height = width, height
else:
out_width, out_height = obs_res
self.display = pygame.display.set_mode((width, height), pygame.HWSURFACE | pygame.DOUBLEBUF)
self.clock = pygame.time.Clock()
self.synchronous = synchronous
# Setup gym environment
self.seed()
self.action_space = gym.spaces.Box(np.array([-1, 0]), np.array([1, 1]), dtype=np.float32) # steer, throttle
self.observation_space = gym.spaces.Box(low=0.0, high=1.0, shape=(*obs_res, 3), dtype=np.float32)
self.metadata["video.frames_per_second"] = self.fps = self.average_fps = fps
self.spawn_point = 1
self.action_smoothing = action_smoothing
self.encode_state_fn = (lambda x: x) if not callable(encode_state_fn) else encode_state_fn
self.reward_fn = (lambda x: 0) if not callable(reward_fn) else reward_fn
self.world = None
try:
# Connect to carla
self.client = carla.Client(host, port)
self.client.set_timeout(60.0)
# Create world wrapper
self.world = World(self.client)
if self.synchronous:
settings = self.world.get_settings()
settings.synchronous_mode = True
self.world.apply_settings(settings)
# Get spawn location
#lap_start_wp = self.world.map.get_waypoint(carla.Location(x=-180.0, y=110))
lap_start_wp = self.world.map.get_waypoint(self.world.map.get_spawn_points()[1].location)
spawn_transform = lap_start_wp.transform
spawn_transform.location += carla.Location(z=1.0)
# Create vehicle and attach camera to it
self.vehicle = Vehicle(self.world, spawn_transform,
on_collision_fn=lambda e: self._on_collision(e),
on_invasion_fn=lambda e: self._on_invasion(e))
# Create hud
self.hud = HUD(width, height)
self.hud.set_vehicle(self.vehicle)
self.world.on_tick(self.hud.on_world_tick)
# Create cameras
self.dashcam = Camera(self.world, out_width, out_height,
transform=camera_transforms["dashboard"],
attach_to=self.vehicle, on_recv_image=lambda e: self._set_observation_image(e),
sensor_tick=0.0 if self.synchronous else 1.0/self.fps)
self.camera = Camera(self.world, width, height,
transform=camera_transforms["spectator"],
attach_to=self.vehicle, on_recv_image=lambda e: self._set_viewer_image(e),
sensor_tick=0.0 if self.synchronous else 1.0/self.fps)
except Exception as e:
self.close()
raise e
# Generate waypoints along the lap
self.route_waypoints = compute_route_waypoints(self.world.map, lap_start_wp, lap_start_wp, resolution=1.0,
plan=[RoadOption.STRAIGHT] + [RoadOption.RIGHT] * 2 + [RoadOption.STRAIGHT] * 5)
self.current_waypoint_index = 0
self.checkpoint_waypoint_index = 0
# Reset env to set initial state
self.reset()
def run_carla_client(self):
client = carla.Client(self.host, self.port)
client.set_timeout(20.0)
self.available_maps = client.get_available_maps()
print("Available maps are: ", self.available_maps)
logging.info('listening to server %s:%s', self.host, self.port)
for episode in range(self.number_of_episodes):
print("Starting episode number %d" % episode)
uuid_run = str(uuid.uuid1())
episode_path = os.path.join(
self.base_path,
"episode_{}__{}".format(str(episode), uuid_run))
os.mkdir(episode_path)
np.save(os.path.join(episode_path, "intrinsics.npy"), self.K)
cur_map = random.choice(self.available_maps)
print("About to load the map %s" % cur_map)
self.world = client.load_world(cur_map)
self.world.tick()
# Initialize the actor lists
self.vehicles_list = []
self.sensors_list = []
self.vehicle_dir_paths = []
self.actors = []
self.position_list = []
self.rotation_list = []
self.camR_sensor_indices = []
self.vehicle_bbox_list = []
self.vehicle_extrinsics = []
self.vehicle_names = []
# Get all the blueprints
blueprints = self.world.get_blueprint_library().filter(
self.filterv)
for blueprint in blueprints:
print(blueprint.id)
for attr in blueprint:
print(' - {}'.format(attr))
# Get all the spawn points
spawn_points = self.world.get_map().get_spawn_points()
random.shuffle(spawn_points)
self.spawn_vehicles(blueprints, episode_path, spawn_points)
# self.spawn_points = spawn_points
print("CamR sensor indices are : ", self.camR_sensor_indices)
print("Done with actor creation")
print("Total number of sensors are: ", len(self.sensors_list))
self.world.tick()
last_saved_frame = 0
# Create a synchronous mode context.
with CarlaSyncMode(self.world, self.sensors_list,
fps=self.fps) as sync_mode:
cnt = 0
for v in self.vehicles_list:
# print("Bounding box for this vehicle is: ", v.bounding_box.location, v.bounding_box.extent)
bbox_loc, bbox_ext = v.bounding_box.location, v.bounding_box.extent
bbox = [
bbox_loc.x - bbox_ext.x, bbox_loc.y - bbox_ext.y,
bbox_loc.z - bbox_ext.z, bbox_loc.x + bbox_ext.x,
bbox_loc.y + bbox_ext.y, bbox_loc.z + bbox_ext.z
]
self.vehicle_bbox_list.append(bbox)
print("bbox coords are: ", bbox)
v.set_autopilot(False)
# print("All vehicles put to autopilot")
self.world.tick()
while True:
if cnt == self.num_datapoints_per_episode:
print("Done with episode %d." % episode)
time.sleep(1)
# self.destroy_actors()
break
# Randomly change weather
weather = carla.WeatherParameters(
cloudiness=np.random.randint(0, 70),
precipitation=np.random.randint(0, 75),
sun_altitude_angle=np.random.randint(30, 90))
self.world.set_weather(weather)
self.world.tick()
# print("Randomly set weather")
# Randomly teleport vehicle
# random.shuffle(spawn_points)
# self.vehicles_list[0].set_transform(spawn_points[0])
# self.world.tick()
# print("Randomly teleported vehicle")
# print("Getting the data")
# Advance the simulation and wait for the data.
data, frame = sync_mode.tick(timeout=12.0)
# print('Got the data :))')
data = data[1:] # Remove the world tick datapoint
# print("Location of car is: ", self.vehicles_list[-1].get_location())
if frame - last_saved_frame > self.min_frame_difference: # Don't want too similar frames
print("Data looks different. Will save")
cnt += 1
last_saved_frame = frame
self.save_data(data, frame)
if self.should_randomize_camR:
self.randomize_camR()
def go(q):
# setup CARLA
client = carla.Client("127.0.0.1", 2000)
client.set_timeout(10.0)
world = client.load_world(args.town)
blueprint_library = world.get_blueprint_library()
world_map = world.get_map()
vehicle_bp = blueprint_library.filter('vehicle.tesla.*')[0]
spawn_points = world_map.get_spawn_points()
assert len(spawn_points) > args.num_selected_spawn_point, \
f'''No spawn point {args.num_selected_spawn_point}, try a value between 0 and
{len(spawn_points)} for this town.'''
spawn_point = spawn_points[args.num_selected_spawn_point]
vehicle = world.spawn_actor(vehicle_bp, spawn_point)
max_steer_angle = vehicle.get_physics_control().wheels[0].max_steer_angle
# make tires less slippery
# wheel_control = carla.WheelPhysicsControl(tire_friction=5)
physics_control = vehicle.get_physics_control()
physics_control.mass = 2326
# physics_control.wheels = [wheel_control]*4
physics_control.torque_curve = [[20.0, 500.0], [5000.0, 500.0]]
physics_control.gear_switch_time = 0.0
vehicle.apply_physics_control(physics_control)
blueprint = blueprint_library.find('sensor.camera.rgb')
blueprint.set_attribute('image_size_x', str(W))
blueprint.set_attribute('image_size_y', str(H))
blueprint.set_attribute('fov', '70')
blueprint.set_attribute('sensor_tick', '0.05')
transform = carla.Transform(carla.Location(x=0.8, z=1.45))
camera = world.spawn_actor(blueprint, transform, attach_to=vehicle)
camera.listen(cam_callback)
world.set_weather(
carla.WeatherParameters(
cloudyness=args.cloudyness,
precipitation=args.precipitation,
precipitation_deposits=args.precipitation_deposits,
wind_intensity=args.wind_intensity,
sun_azimuth_angle=args.sun_azimuth_angle,
sun_altitude_angle=args.sun_altitude_angle))
# reenable IMU
imu_bp = blueprint_library.find('sensor.other.imu')
imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle)
imu.listen(imu_callback)
def destroy():
print("clean exit")
imu.destroy()
camera.destroy()
vehicle.destroy()
print("done")
atexit.register(destroy)
vehicle_state = VehicleState()
# launch fake car threads
threading.Thread(target=health_function).start()
threading.Thread(target=fake_driver_monitoring).start()
threading.Thread(target=fake_gps).start()
threading.Thread(target=can_function_runner,
args=(vehicle_state, )).start()
# can loop
rk = Ratekeeper(100, print_delay_threshold=0.05)
# init
throttle_ease_out_counter = REPEAT_COUNTER
brake_ease_out_counter = REPEAT_COUNTER
steer_ease_out_counter = REPEAT_COUNTER
vc = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False)
is_openpilot_engaged = False
throttle_out = steer_out = brake_out = 0
throttle_op = steer_op = brake_op = 0
throttle_manual = steer_manual = brake_manual = 0
old_steer = old_brake = old_throttle = 0
throttle_manual_multiplier = 0.7 #keyboard signal is always 1
brake_manual_multiplier = 0.7 #keyboard signal is always 1
steer_manual_multiplier = 45 * STEER_RATIO #keyboard signal is always 1
while 1:
# 1. Read the throttle, steer and brake from op or manual controls
# 2. Set instructions in Carla
# 3. Send current carstate to op via can
cruise_button = 0
throttle_out = steer_out = brake_out = 0
throttle_op = steer_op = brake_op = 0
throttle_manual = steer_manual = brake_manual = 0
# --------------Step 1-------------------------------
if not q.empty():
message = q.get()
m = message.split('_')
if m[0] == "steer":
steer_manual = float(m[1])
is_openpilot_engaged = False
if m[0] == "throttle":
throttle_manual = float(m[1])
is_openpilot_engaged = False
if m[0] == "brake":
brake_manual = float(m[1])
is_openpilot_engaged = False
if m[0] == "reverse":
#in_reverse = not in_reverse
cruise_button = CruiseButtons.CANCEL
is_openpilot_engaged = False
if m[0] == "cruise":
if m[1] == "down":
cruise_button = CruiseButtons.DECEL_SET
is_openpilot_engaged = True
if m[1] == "up":
cruise_button = CruiseButtons.RES_ACCEL
is_openpilot_engaged = True
if m[1] == "cancel":
cruise_button = CruiseButtons.CANCEL
is_openpilot_engaged = False
throttle_out = throttle_manual * throttle_manual_multiplier
steer_out = steer_manual * steer_manual_multiplier
brake_out = brake_manual * brake_manual_multiplier
#steer_out = steer_out
# steer_out = steer_rate_limit(old_steer, steer_out)
old_steer = steer_out
old_throttle = throttle_out
old_brake = brake_out
# print('message',old_throttle, old_steer, old_brake)
if is_openpilot_engaged:
sm.update(0)
throttle_op = sm['carControl'].actuators.gas #[0,1]
brake_op = sm['carControl'].actuators.brake #[0,1]
steer_op = sm['controlsState'].angleSteersDes # degrees [-180,180]
throttle_out = throttle_op
steer_out = steer_op
brake_out = brake_op
steer_out = steer_rate_limit(old_steer, steer_out)
old_steer = steer_out
# OP Exit conditions
# if throttle_out > 0.3:
# cruise_button = CruiseButtons.CANCEL
# is_openpilot_engaged = False
# if brake_out > 0.3:
# cruise_button = CruiseButtons.CANCEL
# is_openpilot_engaged = False
# if steer_out > 0.3:
# cruise_button = CruiseButtons.CANCEL
# is_openpilot_engaged = False
else:
if throttle_out == 0 and old_throttle > 0:
if throttle_ease_out_counter > 0:
throttle_out = old_throttle
throttle_ease_out_counter += -1
else:
throttle_ease_out_counter = REPEAT_COUNTER
old_throttle = 0
if brake_out == 0 and old_brake > 0:
if brake_ease_out_counter > 0:
brake_out = old_brake
brake_ease_out_counter += -1
else:
brake_ease_out_counter = REPEAT_COUNTER
old_brake = 0
if steer_out == 0 and old_steer != 0:
if steer_ease_out_counter > 0:
steer_out = old_steer
steer_ease_out_counter += -1
else:
steer_ease_out_counter = REPEAT_COUNTER
old_steer = 0
# --------------Step 2-------------------------------
steer_carla = steer_out / (max_steer_angle * STEER_RATIO * -1)
steer_carla = np.clip(steer_carla, -1, 1)
steer_out = steer_carla * (max_steer_angle * STEER_RATIO * -1)
old_steer = steer_carla * (max_steer_angle * STEER_RATIO * -1)
vc.throttle = throttle_out / 0.6
vc.steer = steer_carla
vc.brake = brake_out
vehicle.apply_control(vc)
# --------------Step 3-------------------------------
vel = vehicle.get_velocity()
speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) # in m/s
vehicle_state.speed = speed
vehicle_state.angle = steer_out
vehicle_state.cruise_button = cruise_button
vehicle_state.is_engaged = is_openpilot_engaged
if rk.frame % PRINT_DECIMATION == 0:
print("frame: ", "engaged:", is_openpilot_engaged, "; throttle: ",
round(vc.throttle, 3), "; steer(c/deg): ",
round(vc.steer, 3), round(steer_out, 3), "; brake: ",
round(vc.brake, 3))
rk.keep_time()
import os
import random
import sys
import xml.etree.ElementTree as ET
import numpy as np
import carla
from carla import ColorConverter as cc
import pygame
from agents.navigation.basic_agent import BasicAgent
from agents.navigation.global_route_planner import GlobalRoutePlanner
from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO
client = carla.Client('localhost', 2000)
world = client.get_world()
world_map = world.get_map()
pygame.font.init()
myfont = pygame.font.SysFont('Comic Sans MS', 30)
vehicle = None
camera_sensor = None
class surface_holder:
def __init__(self):
self.surface = None
def game_loop(args):
pygame.init()
pygame.font.init()
world = None
timestep = 0.1
"""
class Bunch(object):
def __init__(self, adict):
self.__dict__.update(adict)
args = Bunch({'autopilot':False, 'debug':False, 'filter':'vehicle.tesla.model3', 'height':720, 'host':'127.0.0.1', 'port':2000, 'res':'1280x720', 'rolename':'hero', 'width':1280})
"""
try:
client = carla.Client(args.host, args.port)
client.set_timeout(2.0)
print(args)
# Following line opens up pygame
display = pygame.display.set_mode((args.width, args.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
hud = HUD(args.width, args.height)
#if (client.get_world().get_map().name != "Town03"):
# client.load_world('Town03')
world = World(client.get_world(), hud, args.filter, args.rolename)
world.camera_manager.toggle_recording()
settings = world.world.get_settings()
if not settings.synchronous_mode or settings.fixed_delta_seconds != timestep:
settings.synchronous_mode = True
settings.fixed_delta_seconds = timestep
world.world.apply_settings(settings)
controller = KeyboardControl(world, args.autopilot)
clock = pygame.time.Clock()
coords = list()
for i in range(100):
clock.tick_busy_loop(600)
if controller.parse_events(client, world, clock):
return
world.tick(clock)
world.world.tick()
world.eval_reward()
world.render(display)
pygame.display.flip()
coords.append(
(world.player.get_location().x, world.player.get_location().y))
#time.sleep(0.5)
with open("Coords_dodge", 'wb') as f:
pickle.dump(coords, f)
finally:
if (world and world.recording_enabled):
client.stop_recorder()
if world is not None:
world.destroy()
pygame.quit()
def __init__(self, render=False, carla_port=2000, record=False, record_dir=None, args=None, record_vision=False, reward_type='lane_follow', **kwargs):
self.render_display = render
self.record_display = record
print('[CarlaEnv] record_vision:', record_vision)
self.record_vision = record_vision
self.record_dir = record_dir
self.reward_type = reward_type
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'] # DMC uses this
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', carla_port)
self.client.set_timeout(2.0)
self.world = self.client.get_world()
self.map = self.world.get_map()
# 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")
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 = spaces.Box(low=np.array((low, low)), high=np.array((high, high)))
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)
self.horizon = self.max_episode_steps
self.image_shape = (3, self.vision_size, self.vision_size)
# roaming carla agent
self.count = 0
self.world.tick()
self.reset_init()
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.*")
# town nav
self.route_planner_dao = GlobalRoutePlannerDAO(self.map, sampling_resolution=0.1)
self.route_planner = CustomGlobalRoutePlanner(self.route_planner_dao)
self.route_planner.setup()
self.target_location = carla.Location(x=-13.473097, y=134.311234, z=-0.010433)
# roaming carla agent
# self.agent = None
# self.count = 0
# self.world.tick()
self.reset() # creates self.agent
#from carla.client import make_carla_client
import carla
actor_list = []
try:
client = carla.Client("localhost",2000)
client.set_timeout(2)
world = client.get_world()
print(client.get_available_maps())
print(world)
finally:
for actor in actor_list:
actor.destroy()
print("all actors destroyed!")
def main():
argparser = argparse.ArgumentParser(
description=__doc__)
argparser.add_argument(
'--host',
metavar='H',
default='127.0.0.1',
help='IP of the host server (default: 127.0.0.1)')
argparser.add_argument(
'-p', '--port',
metavar='P',
default=2000,
type=int,
help='TCP port to listen to (default: 2000)')
argparser.add_argument(
'-n', '--number-of-vehicles',
metavar='N',
default=30,
type=int,
help='Number of vehicles (default: 30)')
argparser.add_argument(
'-w', '--number-of-walkers',
metavar='W',
default=10,
type=int,
help='Number of walkers (default: 10)')
argparser.add_argument(
'--safe',
action='store_true',
help='Avoid spawning vehicles prone to accidents')
argparser.add_argument(
'--filterv',
metavar='PATTERN',
default='vehicle.*',
help='Filter vehicle model (default: "vehicle.*")')
argparser.add_argument(
'--generationv',
metavar='G',
default='All',
help='restrict to certain vehicle generation (values: "1","2","All" - default: "All")')
argparser.add_argument(
'--filterw',
metavar='PATTERN',
default='walker.pedestrian.*',
help='Filter pedestrian type (default: "walker.pedestrian.*")')
argparser.add_argument(
'--generationw',
metavar='G',
default='2',
help='restrict to certain pedestrian generation (values: "1","2","All" - default: "2")')
argparser.add_argument(
'--tm-port',
metavar='P',
default=8000,
type=int,
help='Port to communicate with TM (default: 8000)')
argparser.add_argument(
'--asynch',
action='store_true',
help='Activate asynchronous mode execution')
argparser.add_argument(
'--hybrid',
action='store_true',
help='Activate hybrid mode for Traffic Manager')
argparser.add_argument(
'-s', '--seed',
metavar='S',
type=int,
help='Set random device seed and deterministic mode for Traffic Manager')
argparser.add_argument(
'--car-lights-on',
action='store_true',
default=False,
help='Enable car lights')
argparser.add_argument(
'--hero',
action='store_true',
default=False,
help='Set one of the vehicles as hero')
argparser.add_argument(
'--respawn',
action='store_true',
default=False,
help='Automatically respawn dormant vehicles (only in large maps)')
argparser.add_argument(
'--no-rendering',
action='store_true',
default=False,
help='Activate no rendering mode')
args = argparser.parse_args()
logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO)
vehicles_list = []
walkers_list = []
all_id = []
client = carla.Client(args.host, args.port)
client.set_timeout(10.0)
synchronous_master = False
random.seed(args.seed if args.seed is not None else int(time.time()))
try:
world = client.get_world()
traffic_manager = client.get_trafficmanager(args.tm_port)
traffic_manager.set_global_distance_to_leading_vehicle(2.5)
if args.respawn:
traffic_manager.set_respawn_dormant_vehicles(True)
if args.hybrid:
traffic_manager.set_hybrid_physics_mode(True)
traffic_manager.set_hybrid_physics_radius(70.0)
if args.seed is not None:
traffic_manager.set_random_device_seed(args.seed)
settings = world.get_settings()
if not args.asynch:
traffic_manager.set_synchronous_mode(True)
if not settings.synchronous_mode:
synchronous_master = True
settings.synchronous_mode = True
settings.fixed_delta_seconds = 0.05
else:
synchronous_master = False
else:
print("You are currently in asynchronous mode. If this is a traffic simulation, \
you could experience some issues. If it's not working correctly, switch to synchronous \
mode by using traffic_manager.set_synchronous_mode(True)")
if args.no_rendering:
settings.no_rendering_mode = True
world.apply_settings(settings)
blueprints = get_actor_blueprints(world, args.filterv, args.generationv)
blueprintsWalkers = get_actor_blueprints(world, args.filterw, args.generationw)
if args.safe:
blueprints = [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4]
blueprints = [x for x in blueprints if not x.id.endswith('microlino')]
blueprints = [x for x in blueprints if not x.id.endswith('carlacola')]
blueprints = [x for x in blueprints if not x.id.endswith('cybertruck')]
blueprints = [x for x in blueprints if not x.id.endswith('t2')]
blueprints = [x for x in blueprints if not x.id.endswith('sprinter')]
blueprints = [x for x in blueprints if not x.id.endswith('firetruck')]
blueprints = [x for x in blueprints if not x.id.endswith('ambulance')]
blueprints = sorted(blueprints, key=lambda bp: bp.id)
spawn_points = world.get_map().get_spawn_points()
number_of_spawn_points = len(spawn_points)
if args.number_of_vehicles < number_of_spawn_points:
random.shuffle(spawn_points)
elif args.number_of_vehicles > number_of_spawn_points:
msg = 'requested %d vehicles, but could only find %d spawn points'
logging.warning(msg, args.number_of_vehicles, number_of_spawn_points)
args.number_of_vehicles = number_of_spawn_points
# @todo cannot import these directly.
SpawnActor = carla.command.SpawnActor
SetAutopilot = carla.command.SetAutopilot
SetVehicleLightState = carla.command.SetVehicleLightState
FutureActor = carla.command.FutureActor
# --------------
# Spawn vehicles
# --------------
batch = []
hero = args.hero
for n, transform in enumerate(spawn_points):
if n >= args.number_of_vehicles:
break
blueprint = random.choice(blueprints)
if blueprint.has_attribute('color'):
color = random.choice(blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
if blueprint.has_attribute('driver_id'):
driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values)
blueprint.set_attribute('driver_id', driver_id)
if hero:
blueprint.set_attribute('role_name', 'hero')
hero = False
else:
blueprint.set_attribute('role_name', 'autopilot')
# prepare the light state of the cars to spawn
light_state = vls.NONE
if args.car_lights_on:
light_state = vls.Position | vls.LowBeam | vls.LowBeam
# spawn the cars and set their autopilot and light state all together
batch.append(SpawnActor(blueprint, transform)
.then(SetAutopilot(FutureActor, True, traffic_manager.get_port()))
.then(SetVehicleLightState(FutureActor, light_state)))
for response in client.apply_batch_sync(batch, synchronous_master):
if response.error:
logging.error(response.error)
else:
vehicles_list.append(response.actor_id)
# -------------
# Spawn Walkers
# -------------
# some settings
percentagePedestriansRunning = 0.0 # how many pedestrians will run
percentagePedestriansCrossing = 0.0 # how many pedestrians will walk through the road
# 1. take all the random locations to spawn
spawn_points = []
for i in range(args.number_of_walkers):
spawn_point = carla.Transform()
loc = world.get_random_location_from_navigation()
if (loc != None):
spawn_point.location = loc
spawn_points.append(spawn_point)
# 2. we spawn the walker object
batch = []
walker_speed = []
for spawn_point in spawn_points:
walker_bp = random.choice(blueprintsWalkers)
# set as not invincible
if walker_bp.has_attribute('is_invincible'):
walker_bp.set_attribute('is_invincible', 'false')
# set the max speed
if walker_bp.has_attribute('speed'):
if (random.random() > percentagePedestriansRunning):
# walking
walker_speed.append(walker_bp.get_attribute('speed').recommended_values[1])
else:
# running
walker_speed.append(walker_bp.get_attribute('speed').recommended_values[2])
else:
print("Walker has no speed")
walker_speed.append(0.0)
batch.append(SpawnActor(walker_bp, spawn_point))
results = client.apply_batch_sync(batch, True)
walker_speed2 = []
for i in range(len(results)):
if results[i].error:
logging.error(results[i].error)
else:
walkers_list.append({"id": results[i].actor_id})
walker_speed2.append(walker_speed[i])
walker_speed = walker_speed2
# 3. we spawn the walker controller
batch = []
walker_controller_bp = world.get_blueprint_library().find('controller.ai.walker')
for i in range(len(walkers_list)):
batch.append(SpawnActor(walker_controller_bp, carla.Transform(), walkers_list[i]["id"]))
results = client.apply_batch_sync(batch, True)
for i in range(len(results)):
if results[i].error:
logging.error(results[i].error)
else:
walkers_list[i]["con"] = results[i].actor_id
# 4. we put together the walkers and controllers id to get the objects from their id
for i in range(len(walkers_list)):
all_id.append(walkers_list[i]["con"])
all_id.append(walkers_list[i]["id"])
all_actors = world.get_actors(all_id)
# wait for a tick to ensure client receives the last transform of the walkers we have just created
if args.asynch or not synchronous_master:
world.wait_for_tick()
else:
world.tick()
# 5. initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...])
# set how many pedestrians can cross the road
world.set_pedestrians_cross_factor(percentagePedestriansCrossing)
for i in range(0, len(all_id), 2):
# start walker
all_actors[i].start()
# set walk to random point
all_actors[i].go_to_location(world.get_random_location_from_navigation())
# max speed
all_actors[i].set_max_speed(float(walker_speed[int(i/2)]))
print('spawned %d vehicles and %d walkers, press Ctrl+C to exit.' % (len(vehicles_list), len(walkers_list)))
# Example of how to use Traffic Manager parameters
traffic_manager.global_percentage_speed_difference(30.0)
while True:
if not args.asynch and synchronous_master:
world.tick()
else:
world.wait_for_tick()
finally:
if not args.asynch and synchronous_master:
settings = world.get_settings()
settings.synchronous_mode = False
settings.no_rendering_mode = False
settings.fixed_delta_seconds = None
world.apply_settings(settings)
print('\ndestroying %d vehicles' % len(vehicles_list))
client.apply_batch([carla.command.DestroyActor(x) for x in vehicles_list])
# stop walker controllers (list is [controller, actor, controller, actor ...])
for i in range(0, len(all_id), 2):
all_actors[i].stop()
print('\ndestroying %d walkers' % len(walkers_list))
client.apply_batch([carla.command.DestroyActor(x) for x in all_id])
time.sleep(0.5)
def game_loop(args):
pygame.init()
pygame.font.init()
world = None
colliding_agent = None
NPCs = None
rospy.init_node('delta_carla')
try:
client = carla.Client(args.host, args.port)
client.set_timeout(2.0)
display = pygame.display.set_mode((args.width, args.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
spawn_points = list(client.get_world().get_map().get_spawn_points())
if args.scene is not 5:
npc_spawn_points = [
x for x in spawn_points if x is not spawn_points[scene_list[
args.scene]['collision_spawn']]
]
else:
npc_spawn_points = [x for x in spawn_points]
print("{} Spawn Points at: {}".format(len(npc_spawn_points),
npc_spawn_points))
########
NPCs = collision_scenario.NPC(client.get_world(),
scene_list[args.scene]['npc'],
npc_spawn_points)
#########
hud = HUD(args.width, args.height)
world = World(client.get_world(), hud,
spawn_points[scene_list[args.scene]['ego_spawn']],
args.spawn_top_camera)
controller = KeyboardControl(world, args.autopilot)
# Set the scenarios for collision
# scene = 0 # Default 30m crash scenario
# scene = 2 # 150m crash scenario
colliding_agent = collision_scenario.Colliding_Agent(
world.world, world.vehicle, Args.scene)
colliding_agent.create_agent(
spawn_points[scene_list[args.scene]['collision_spawn']])
world_carla = world.world
ego = world.vehicle.attributes['role_name']
# PRINT HERE
clock = pygame.time.Clock()
while not rospy.core.is_shutdown():
clock.tick_busy_loop(60)
if controller.parse_events(world, clock):
return
world.tick(clock)
world.render(display)
pygame.display.flip()
colliding_agent.control_agent()
finally:
if world is not None:
world.destroy()
if colliding_agent is not None:
colliding_agent.destroy()
if NPCs is not None:
NPCs.destroy(client)
pygame.quit()
def game_loop(args):
global X, X_init
actor_list = []
X = readX() # X: coords_motionPlanner.txt
X_init = X
try:
client = carla.Client(args.host, args.port) # get client
client.set_timeout(4.0)
world = client.get_world() # get world
settings = world.get_settings()
print(settings)
# -----------------------------------------
# create our ego car：make; color; name; initial state
# -----------------------------------------
init_state = [X[0][3], X[0][4], X[0][5]]
car_ego_make = 'vehicle.audi.tt'
car_ego_color = '0,0,255'
car_ego_name = 'car_ego'
car_ego = create_car_ego(world, car_ego_make, car_ego_color,
car_ego_name, init_state)
actor_list.append(car_ego)
log.write('ego car is created and its id: %d!\n' % car_ego.id)
log.flush()
print('ego car is created and its id:', car_ego.id)
# -----------------------------------------
# use .log to record running process (video)
# -----------------------------------------
if not os.path.exists(address1 + 'saved/'):
os.mkdir(address1 + 'saved/')
recording = address1 + 'saved/' + 'recording_' + str(round(
time.time())) + '.log'
log.write('recording video filename: %s\n' % recording)
log.flush()
client.start_recorder(recording)
print('recording starts')
# -----------------------------------------
# implement agents: basic_agent.py
# -----------------------------------------
agent_ego = BasicAgent(car_ego)
# -----------------------------------------
# global view: get all information of the world
# -----------------------------------------
world = client.get_world()
vehicles = world.get_actors().filter('vehicle.*')
# -----------------------------------------
# our ego car can move only if other cars exist and their speed > 0
# -----------------------------------------
while len(vehicles) <= 1: # other cars exist
world = client.get_world()
vehicles = world.get_actors().filter('vehicle.*')
log.write('%d cars in the world' % len(vehicles))
log.flush()
for car in vehicles: # other cars' speed > 0
check_car = car
if check_car.attributes.get(
'role_name') != 'car_ego': # find other car
speed_car = math.sqrt(check_car.get_velocity().x**2 +
check_car.get_velocity().y**2)
break
else:
continue
time.sleep(2.)
# -----------------------------------------
# execute task and motion planning
# -----------------------------------------
i = 0 # step
behavior = X[i][1] # 1: merge lane; 0: not merge lane
while i < len(X) - 1:
if behavior == 0: # if not merging lane, skip risk model
# -----------------------------------------
# set the current step
# -----------------------------------------
curr_lane = X[i][2]
next_lane = X[i + 1][2]
curr_step = [X[i + 1][3], X[i + 1][4]]
agent_ego.set_destination((curr_step[0], curr_step[1], 1.2))
log.write('current lane is %d\n' % curr_lane)
log.write('our ego car does not merge lane in current step\n')
log.write(
'our ego car performs current step to get next lane %d\n' %
next_lane)
log.flush()
print('current lane is %d' % curr_lane)
print('our ego car does not merge lane in current step')
print(
'our ego car performs current step to get next lane %d\n' %
next_lane)
# -----------------------------------------
# execute the current step
# -----------------------------------------
ego_loc = car_ego.get_location(
) # current location of our ego car
mini_dis = 15 # a minimal distance to check if our ego car achieves the destination
while math.sqrt((ego_loc.x - curr_step[0])**2 +
(ego_loc.y - curr_step[1])**2) > mini_dis:
if not world.wait_for_tick(
): # as soon as the server is ready continue!
continue
control = agent_ego.run_step()
car_ego.apply_control(control)
control.manual_gear_shift = False
ego_loc = car_ego.get_location()
log.write('our ego car reaches the target lane %d\n' %
X[i + 1][2])
log.flush()
# -----------------------------------------
# ready for the next step
# -----------------------------------------
i = i + 1
behavior = X[i][1]
else: # if our ego car merge lane, apply risk model and compute "p_risk"
# -----------------------------------------
# compute risk value "p_risk"
# -----------------------------------------
curr_lane = X[i][2]
p_risk = fuc_risk(i, car_ego, world, curr_lane)
log.write('current lane is %d\n' % curr_lane)
log.write('p_risk is %3.3f if our ego car merges lane\n' %
p_risk)
log.flush()
print('current lane is %d' % curr_lane)
print('p_risk is %3.3f if our ego car merges lane' % p_risk)
# -----------------------------------------
# if "p_risk" is smaller than "thre_risk", our ego car still merges lane
# if "thre_risk" < 0, it is our TMPUD
# if "thre_risk" > 0, it is one baseline
# -----------------------------------------
thre_risk = 0.0
if p_risk < thre_risk:
# -----------------------------------------
# set the current step
# -----------------------------------------
curr_step = [X[i + 1][3], X[i + 1][4]]
next_lane = X[i + 1][2]
agent_ego.set_destination(
(curr_step[0], curr_step[1], 1.2))
log.write('current lane is %d\n' % curr_lane)
log.write(
'our ego car does not merge lane in current step\n')
log.write(
'our ego car performs current step to get next lane %d\n'
% next_lane)
log.flush()
print('current lane is %d' % curr_lane)
print('our ego car does not merge lane in current step')
print(
'our ego car performs current step to get next lane %d\n'
% next_lane)
# -----------------------------------------
# execute the current step
# -----------------------------------------
ego_loc = car_ego.get_location(
) # current location of our ego car
while math.sqrt((ego_loc.x - curr_step[0])**2 +
(ego_loc.y - curr_step[1])**2) > mini_dis:
if not world.wait_for_tick(
): # as soon as the server is ready continue!
continue
control = agent_ego.run_step()
car_ego.apply_control(control)
control.manual_gear_shift = False
ego_loc = car_ego.get_location()
log.write('our ego car reaches the target lane %d\n' %
X[i + 1][2])
log.flush()
# -----------------------------------------
# ready for the next step
# -----------------------------------------
i = i + 1
behavior = X[i][1]
else:
# -----------------------------------------
# if "p_risk" is bigger than "thre_risk", it does not mean our ego car cannot merge lane definitely
# we need to do task planning again with the updated information
# but if no new plan is generated, our ego car are forced to merge lane
# -----------------------------------------
# log.write('Risk! cannot merge lane, please do task planning again!\n')
# print('Risk! cannot merge lane, please do task planning again!')
# -----------------------------------------
# update the risk value of target lane in lanes_risk.txt
# -----------------------------------------
with open(address1 + 'interaction/lanes_risk.txt',
'r') as f: # read original lane risk
lanes_risk = [line.rstrip() for line in f]
f.close()
init_risk = int(lanes_risk[int(X[i][2] - 1) * 12 + 6 - 1])
updated_risk = int(p_risk * 100)
lanes_risk[int(X[i][2] - 1) * 12 + 6 - 1] = updated_risk
f = open(address1 + 'interaction/lanes_risk.txt',
'w') # write updated lane risk
for item in lanes_risk:
f.write('%s\n' % str(item))
f.flush()
f.close()
log.write('update risk value of lane %d\n' %
int(curr_lane))
log.write('original value is %d, now it is %d!\n' %
(init_risk, updated_risk))
log.flush()
print('update risk value of lane %d\n' % int(curr_lane))
print('original value is %d, now it is %d!\n' %
(init_risk, updated_risk))
# -----------------------------------------
# do task planning again and generate the new X (coords_motionPlanner.txt)
# -----------------------------------------
source = str(round(curr_lane))
dest = str(round(X[-1][2]))
command1 = 'python' + ' ' + address1 + 'task-level' + '/' + 'get_optimal_task_plan.py' + ' ' + source + ' ' + dest
os.system(command1) # get optimal task plan
command2 = 'python' + ' ' + address1 + 'task-level/ground_task_plan.py'
os.system(command2) # ground task plan
X = readX() # read X again
# -----------------------------------------
# if X changes, it means our ego car finds a new task plan
# -----------------------------------------
if not (X == X_init):
i = 0
behavior = X[i][1]
X_init = X
else:
# -----------------------------------------
# if X not change
# -----------------------------------------
behavior = 0 # temporaily do not change
log.write(
'do task task planning again, start and dest lanes are %s and %s, respectively\n'
% (source, dest))
log.write(
'Here to update coordinate for motion planner (X)\n')
log.flush()
log.write('task and motion planning is done!\n\n\n\n')
log.flush()
finally:
for actor in actor_list: # delete our ego car
actor.destroy()
client.stop_recorder()
print("ALL cleaned up!")
def game_loop(args, net, scaler, port, phys_settings, cam_path):
world = None
timestep = 1 / 30
"""
class Bunch(object):
def __init__(self, adict):
self.__dict__.update(adict)
args = Bunch({'autopilot':False, 'debug':False, 'filter':'vehicle.tesla.model3', 'height':720, 'host':'127.0.0.1', 'port':2000, 'res':'1280x720', 'rolename':'hero', 'width':1280})
"""
try:
client = carla.Client(args.host, args.port)
client.set_timeout(20.0)
print(args)
if (client.get_world().get_map().name != "Town03"):
client.load_world('Town03')
world = World(client.get_world(), args.filter, phys_settings, cam_path,
args.rolename)
world.timestep = timestep
world.camera_manager.toggle_recording()
controller = KeyboardControl(world, args.autopilot, net, scaler)
if False:
if track != 3:
with open('reg8.3_data', 'rb') as f:
nbrs_right, rightLane, nbrs_left, leftLane, midLane, center_nbrs = pickle.load(
f)
for i in range(1, len(midLane)):
world.world.debug.draw_line(
carla.Location(midLane[i - 1][0], midLane[i - 1][1],
2),
carla.Location(midLane[i][0], midLane[i][1], 2),
life_time=20,
color=carla.Color(0, 125, 155, 0))
else:
with open('reg8.4_data', 'rb') as f:
nbrs_right, rightLane, nbrs_left, leftLane, midLane, center_nbrs = pickle.load(
f)
for i in range(1, len(midLane)):
world.world.debug.draw_line(
carla.Location(midLane[i - 1][0], midLane[i - 1][1],
2),
carla.Location(midLane[i][0], midLane[i][1], 2),
life_time=25,
color=carla.Color(0, 125, 155, 0))
world.world.debug.draw_line(
carla.Location(leftLane[i - 1][0], leftLane[i - 1][1],
2),
carla.Location(leftLane[i][0], leftLane[i][1], 2),
life_time=25,
color=carla.Color(0, 250, 155, 0))
world.world.debug.draw_line(
carla.Location(rightLane[i - 1][0],
rightLane[i - 1][1], 2),
carla.Location(rightLane[i][0], rightLane[i][1], 2),
life_time=25,
color=carla.Color(205, 125, 155, 0))
data = []
vel_data = []
settings = world.world.get_settings()
if not settings.synchronous_mode or settings.fixed_delta_seconds != timestep:
settings.synchronous_mode = True
settings.fixed_delta_seconds = timestep
world.world.apply_settings(settings)
for i in range(math.ceil(25 / timestep)):
world.world.tick()
result = controller.parse_events(client, world)
data.append(
(world.player.get_location().x, world.player.get_location().y,
world.player.get_transform().rotation.yaw))
data.append(
(world.player.get_velocity().x, world.player.get_velocity().y))
if result == 5:
with open('loc_data', 'wb') as f:
pickle.dump(data, f)
with open('vel_data', 'wb') as f:
pickle.dump(vel_data, f)
return world.f0, world.eval_target_distance_reward()
elif result:
return
world.eval_reward()
current_transform = world.player.get_transform()
pos = current_transform.location
f0 = world.f0
f1 = world.eval_target_distance_reward()
finally:
if (world and world.recording_enabled):
client.stop_recorder()
if world is not None:
world.destroy()
import os
print(os.getcwd())
with open('loc_data', 'wb') as f:
pickle.dump(data, f)
with open('vel_data', 'wb') as f:
pickle.dump(vel_data, f)
return f0, f1
def main():
argparser = argparse.ArgumentParser(description=__doc__)
argparser.add_argument('--host',
metavar='H',
default='127.0.0.1',
help='IP of the host server (default: 127.0.0.1)')
argparser.add_argument('-p',
'--port',
metavar='P',
default=2000,
type=int,
help='TCP port to listen to (default: 2000)')
argparser.add_argument('-n',
'--number-of-vehicles',
metavar='N',
default=10,
type=int,
help='number of vehicles (default: 10)')
argparser.add_argument(
'-d',
'--delay',
metavar='D',
default=2.0,
type=float,
help='delay in seconds between spawns (default: 2.0)')
argparser.add_argument('--safe',
action='store_true',
help='avoid spawning vehicles prone to accidents')
args = argparser.parse_args()
logging.basicConfig(format='%(levelname)s: %(message)s',
level=logging.INFO)
actor_list = []
client = carla.Client(args.host, args.port)
client.set_timeout(2.0)
try:
world = client.get_world()
blueprints = world.get_blueprint_library().filter('vehicle.*')
if args.safe:
blueprints = [
x for x in blueprints
if int(x.get_attribute('number_of_wheels')) == 4
]
blueprints = [x for x in blueprints if not x.id.endswith('isetta')]
blueprints = [
x for x in blueprints if not x.id.endswith('carlacola')
]
spawn_points = world.get_map().get_spawn_points()
number_of_spawn_points = len(spawn_points)
if args.number_of_vehicles < number_of_spawn_points:
random.shuffle(spawn_points)
elif args.number_of_vehicles > number_of_spawn_points:
msg = 'requested %d vehicles, but could only find %d spawn points'
logging.warning(msg, args.number_of_vehicles,
number_of_spawn_points)
args.number_of_vehicles = number_of_spawn_points
# --------------
# Spawn ego vehicle
# --------------
ego_bp = world.get_blueprint_library().find('vehicle.tesla.model3')
ego_bp.set_attribute('role_name', 'ego')
print('\nEgo role_name is set')
ego_color = random.choice(
ego_bp.get_attribute('color').recommended_values)
ego_bp.set_attribute('color', ego_color)
print('\nEgo color is set')
spawn_points = world.get_map().get_spawn_points()
number_of_spawn_points = len(spawn_points)
if 0 < number_of_spawn_points:
random.shuffle(spawn_points)
ego_transform = spawn_points[0]
ego_vehicle = world.spawn_actor(ego_bp, ego_transform)
print('\nEgo is spawned')
else:
logging.warning('Could not found any spawn points')
cam_bp = None
cam_bp = world.get_blueprint_library().find('sensor.camera.rgb')
cam_bp.set_attribute("image_size_x", str(640))
cam_bp.set_attribute("image_size_y", str(480))
cam_bp.set_attribute("fov", str(105))
cam_location = carla.Location(2, 0, 1)
cam_rotation = carla.Rotation(180, 180, 180)
cam_transform = carla.Transform(cam_location, cam_rotation)
ego_cam = world.spawn_actor(cam_bp,
cam_transform,
attach_to=ego_vehicle)
if ego_vehicle.is_at_traffic_light():
traffic_light = ego_vehicle.get_traffic_light()
print(image.frame_number)
if (ego_vehicle.get_traffic_light()
).get_state() == carla.TrafficLightState.Red:
#world.hud.notification("Traffic light changed! Good to go!")
(ego_vehicle.get_traffic_light()).set_state(
carla.TrafficLightState.Green)
ego_cam.listen(lambda image: image.save_to_disk(
'tutorial/output/%.6d.jpg' % image.frame_number))
depth_cam = None
depth_bp = world.get_blueprint_library().find('sensor.camera.depth')
depth_bp.set_attribute("image_size_x", str(640))
depth_bp.set_attribute("image_size_y", str(480))
depth_bp.set_attribute("fov", str(105))
depth_location = carla.Location(2, 0, 1)
depth_rotation = carla.Rotation(180, 180, 180)
depth_transform = carla.Transform(depth_location, depth_rotation)
depth_cam = world.spawn_actor(depth_bp,
depth_transform,
attach_to=ego_vehicle)
depth_cam.listen(lambda image: image.save_to_disk(
'tutorial/new_depth_output/%.6d.jpg' % image.frame_number))
spectator = world.get_spectator()
world_snapshot = world.wait_for_tick()
spectator.set_transform(ego_vehicle.get_transform())
ego_vehicle.set_autopilot(True)
print('\nEgo autopilot enabled')
while True:
world_snapshot = world.wait_for_tick()
if ego_vehicle.is_at_traffic_light():
traffic_light = ego_vehicle.get_traffic_light()
if (ego_vehicle.get_traffic_light()
).get_state() == carla.TrafficLightState.Red:
(ego_vehicle.get_traffic_light()).set_state(
carla.TrafficLightState.Green)
SpawnActor = carla.command.SpawnActor
SetAutopilot = carla.command.SetAutopilot
FutureActor = carla.command.FutureActor
batch = []
for n, transform in enumerate(spawn_points):
if n >= args.number_of_vehicles:
break
blueprint = random.choice(blueprints)
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
blueprint.set_attribute('role_name', 'autopilot')
batch.append(
SpawnActor(blueprint,
transform).then(SetAutopilot(FutureActor, True)))
for response in client.apply_batch_sync(batch):
if response.error:
logging.error(response.error)
else:
actor_list.append(response.actor_id)
print('spawned %d vehicles, press Ctrl+C to exit.' % len(actor_list))
while True:
world.wait_for_tick()
finally:
print('\ndestroying %d actors' % len(actor_list))
client.apply_batch([carla.command.DestroyActor(x) for x in actor_list])
def main(optimalDistance,
followDrivenPath,
chaseMode,
evaluateChasingCar,
driveName='',
record=False,
followMode=False,
resultsName='results',
P=None,
I=None,
D=None,
nOfFramesToSkip=0):
counter = 1
actor_list = []
pygame.init()
carDetector = CarDetector()
position = CarPosition(driveName.split('/')[1])
position.startRecording = True
evaluation = Evaluation()
semantic = SemanticSegmentation()
display = pygame.display.set_mode((800, 600),
pygame.HWSURFACE | pygame.DOUBLEBUF)
font = get_font()
clock = pygame.time.Clock()
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
world = client.get_world()
vehicleToFollowSpawned = False
try:
m = world.get_map()
start_pose = random.choice(m.get_spawn_points())
blueprint_library = world.get_blueprint_library()
vehicle = world.spawn_actor(
random.choice(blueprint_library.filter('jeep')), start_pose)
actor_list.append(vehicle)
vehicle.set_simulate_physics(True)
if followDrivenPath:
evaluation.LoadHistoryFromFile(driveName)
first = evaluation.history[0]
start_pose = carla.Transform(
carla.Location(first[0], first[1], first[2]),
carla.Rotation(first[3], first[4], first[5]))
vehicle.set_transform(start_pose)
collision_sensor = world.spawn_actor(
blueprint_library.find('sensor.other.collision'),
carla.Transform(),
attach_to=vehicle)
collision_sensor.listen(
lambda event: evaluation.CollisionHandler(event))
actor_list.append(collision_sensor)
blueprint = world.get_blueprint_library().find('sensor.camera.rgb')
blueprint.set_attribute('image_size_x', '800')
blueprint.set_attribute('image_size_y', '600')
blueprint.set_attribute('fov', '90')
camera_rgb = world.spawn_actor(
blueprint_library.find('sensor.camera.rgb'),
carla.Transform(carla.Location(x=1.5, z=1.4, y=0.3),
carla.Rotation(pitch=0)), #5,3,0 # -0.3
attach_to=vehicle)
actor_list.append(camera_rgb)
camera_rgb2 = world.spawn_actor(
blueprint_library.find('sensor.camera.rgb'),
carla.Transform(carla.Location(x=1.5, z=1.4, y=-0.3),
carla.Rotation(pitch=0)),
attach_to=vehicle)
actor_list.append(camera_rgb2)
camera_segmentation = world.spawn_actor(
blueprint_library.find('sensor.camera.semantic_segmentation'),
carla.Transform(carla.Location(x=1.5, z=1.4, y=0),
carla.Rotation(pitch=0)), #5,3,0 # -0.3
attach_to=vehicle)
actor_list.append(camera_segmentation)
# Create a synchronous mode context.
with CarlaSyncMode(world,
camera_rgb,
camera_rgb2,
camera_segmentation,
fps=30) as sync_mode:
while True:
if should_quit():
return
clock.tick(30)
# Advance the simulation and wait for the data.
snapshot, img_rgb, image_rgb2, image_segmentation = sync_mode.tick(
timeout=2.0)
line = []
if not vehicleToFollowSpawned and followDrivenPath:
vehicleToFollowSpawned = True
location1 = vehicle.get_transform()
newX, newY = carDetector.CreatePointInFrontOFCar(
location1.location.x, location1.location.y,
location1.rotation.yaw)
diffX = newX - location1.location.x
diffY = newY - location1.location.y
newX = location1.location.x - (diffX * 5)
newY = location1.location.y - (diffY * 5)
start_pose.location.x = newX
start_pose.location.y = newY
vehicle.set_transform(start_pose)
start_pose2 = random.choice(m.get_spawn_points())
bp = blueprint_library.filter('model3')[0]
bp.set_attribute('color', '0,101,189')
vehicleToFollow = world.spawn_actor(bp, start_pose2)
start_pose2 = carla.Transform()
start_pose2.rotation = start_pose.rotation
start_pose2.location.x = start_pose.location.x
start_pose2.location.y = start_pose.location.y
start_pose2.location.z = start_pose.location.z
vehicleToFollow.set_transform(start_pose2)
actor_list.append(vehicleToFollow)
vehicleToFollow.set_simulate_physics(True)
vehicleToFollow.set_autopilot(False)
if followDrivenPath:
if counter >= len(evaluation.history):
break
tmp = evaluation.history[counter]
currentPos = carla.Transform(
carla.Location(tmp[0], tmp[1], tmp[2]),
carla.Rotation(tmp[3], tmp[4], tmp[5]))
vehicleToFollow.set_transform(currentPos)
counter += 1
fps = round(1.0 / snapshot.timestamp.delta_seconds)
location1 = vehicle.get_transform()
location2 = vehicleToFollow.get_transform()
position.SaveCarPosition(location1)
newX, newY = carDetector.CreatePointInFrontOFCar(
location1.location.x, location1.location.y,
location1.rotation.yaw)
angle = carDetector.getAngle(
[location1.location.x, location1.location.y], [newX, newY],
[location2.location.x, location2.location.y])
possibleAngle = 0
carInTheImage = semantic.IsThereACarInThePicture(
image_segmentation)
bbox = carDetector.get3DboundingBox(vehicleToFollow,
camera_rgb, carInTheImage)
# Choose approriate steer and throttle here
steer, throttle = 0, 1
vehicle.apply_control(
carla.VehicleControl(throttle=throttle, steer=steer))
if evaluateChasingCar:
evaluation.AddError(
location1.location.distance(location2.location),
optimalDistance)
velocity1 = vehicle.get_velocity()
velocity2 = vehicleToFollow.get_velocity()
draw_image(display,
image_rgb2,
image_segmentation,
location1,
location2,
record=record,
driveName=driveName,
smazat=line)
display.blit(
font.render('% 5d FPS (real)' % clock.get_fps(), True,
(255, 255, 255)), (8, 10))
display.blit(
font.render('% 5d FPS (simulated)' % fps, True,
(255, 255, 255)), (8, 28))
if len(bbox) != 0:
points = [(int(bbox[i, 0]), int(bbox[i, 1]))
for i in range(8)]
BB_COLOR = (248, 64, 24)
# draw lines
# base
pygame.draw.line(display, BB_COLOR, points[0], points[1])
pygame.draw.line(display, BB_COLOR, points[1], points[2])
pygame.draw.line(display, BB_COLOR, points[2], points[3])
pygame.draw.line(display, BB_COLOR, points[3], points[0])
# top
pygame.draw.line(display, BB_COLOR, points[4], points[5])
pygame.draw.line(display, BB_COLOR, points[5], points[6])
pygame.draw.line(display, BB_COLOR, points[6], points[7])
pygame.draw.line(display, BB_COLOR, points[7], points[4])
# base-top
pygame.draw.line(display, BB_COLOR, points[0], points[4])
pygame.draw.line(display, BB_COLOR, points[1], points[5])
pygame.draw.line(display, BB_COLOR, points[2], points[6])
pygame.draw.line(display, BB_COLOR, points[3], points[7])
real_dist = location1.location.distance(location2.location)
#if chaseMode or followMode:
#myPrint(angle,predicted_angle, possibleAngle,real_dist, predicted_distance,chaseMode)
pygame.display.flip()
except Exception as ex:
print(ex)
finally:
print('Ending')
if evaluateChasingCar:
if not os.path.exists('res'):
os.mkdir('res')
evaluation.WriteIntoFileFinal(os.path.join('res',
resultsName + '.txt'),
driveName=driveName)
position.SaveHistoryToFile()
print('destroying actors.')
for actor in actor_list:
actor.destroy()
pygame.quit()
print('done.')
import random
import math
import numpy as np
from numpy import linalg as la
import matplotlib.pyplot as plot
from .CrosswalkPedestrian import CrosswalkPedestrian
from simple_pid import PID
# to connect to a simulator we need to create a "Client" object
# provide the IP address and port of a running instance of the simulator
_HOST_ = '127.0.0.1'
_PORT_ = 2000
client = carla.Client(_HOST_, _PORT_)
# to connect to a simulator we need to create a "Client" object, to do so
# provide the IP address and port of a running instance of the simulator
_SLEEP_TIME_ = 1
# once the client is configured, directly retrieve the world
world = client.get_world()
# get a list of all pedestrians from the blueprint library
blueprintsWalkers = world.get_blueprint_library().filter("walker.pedestrian.*")
# variables in order to directly retrieve certain methods/classes
import carla
import random
import time
import numpy as np
import cv2
import math
os.system
import PIL
import PIL.Image as Image
import time
import argparse
import pandas as pd
# client creation
client = carla.Client('127.0.0.1', 2000)
client.set_timeout(10.0) # seconds
# change map. note that this replaces the current world (is destroyed) with a new one
# world = client.load_world('Town02')
# connect to the current world
world = client.get_world()
# fixed time-step and synchronous mode ON
#TM needs to be in synchronous mode too
traffic_manager = client.get_trafficmanager(8000)
traffic_manager.set_synchronous_mode(True)
# set weather
# only intervenes with sensor.camera.rgb. Neither affect the actor's physics nor other sensors
def game_loop(args):
global update_cycle
pygame.init()
pygame.font.init()
world = None
vehicles_list = []
try:
client = carla.Client(args.host, args.port)
client.set_timeout(2.0)
display = pygame.display.set_mode((args.width, args.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
hud = HUD(args.width, args.height)
world = World(client.get_world(), hud, args)
controller = KeyboardControl(world)
clock = pygame.time.Clock()
world.player.set_simulate_physics(False)
# Spawn some obstacles to avoid
batch = []
batch.append(SpawnNPC(client, world, args, 30, 1.5))
batch.append(SpawnNPC(client, world, args, 65, -3 * 1.5))
batch.append(SpawnNPC(client, world, args, 110, -1 * 1.5))
for response in carla.Client.apply_batch_sync(client, batch):
if response.error:
logging.error(response.error)
else:
vehicles_list.append(response.actor_id)
start_time = world.hud.simulation_time
while True:
yield from asyncio.sleep(
0.01) # check if any data from the websocket
if controller.parse_events(client, world):
return
if len(way_points) == 0:
player = world.player.get_transform()
way_points.append(player)
v_points.append(0)
sim_time = world.hud.simulation_time - start_time
if update_cycle and (len(way_points) < _update_point_thresh):
update_cycle = False
# print("sending data")
x_points = [point.location.x for point in way_points]
y_points = [point.location.y for point in way_points]
yaw = way_points[0].rotation.yaw * math.pi / 180
waypoint_x, waypoint_y, waypoint_t, waypoint_j = world.get_waypoint(
x_points[-1], y_points[-1])
ws.send(
json.dumps({
'traj_x': x_points,
'traj_y': y_points,
'traj_v': v_points,
'yaw': yaw,
"velocity": velocity,
'time': sim_time,
'waypoint_x': waypoint_x,
'waypoint_y': waypoint_y,
'waypoint_t': waypoint_t,
'waypoint_j': waypoint_j,
'tl_state': _tl_state,
'obst_x': obst_x,
'obst_y': obst_y
}))
clock.tick_busy_loop(60)
world.tick(clock)
world.move(sim_time)
world.render(display)
pygame.display.flip()
finally:
print("key board interrupt, good bye")
if world is not None:
world.destroy()
client.apply_batch(
[carla.command.DestroyActor(x) for x in vehicles_list])
pygame.quit()
def main():
actor_list = []
pygame.init()
display = pygame.display.set_mode(
(800, 600),
pygame.HWSURFACE | pygame.DOUBLEBUF)
font = get_font()
clock = pygame.time.Clock()
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
world = client.get_world()
try:
m = world.get_map()
start_pose = random.choice(m.get_spawn_points())
waypoint = m.get_waypoint(start_pose.location)
blueprint_library = world.get_blueprint_library()
ego_blueprint = random.choice(world.get_blueprint_library().filter("vehicle.audi.etron"))
ego_blueprint.set_attribute('role_name', 'hero')
vehicle = world.spawn_actor(ego_blueprint, start_pose)
actor_list.append(vehicle)
vehicle.set_simulate_physics(False)
camera_top = world.spawn_actor(
blueprint_library.find('sensor.camera.rgb'),
carla.Transform(carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8.0)),
attach_to=vehicle)
actor_list.append(camera_top)
camera_front = world.spawn_actor(
blueprint_library.find('sensor.camera.rgb'),
carla.Transform(carla.Location(x=1.6, z=1.7)),
attach_to=vehicle)
actor_list.append(camera_front)
# Create a synchronous mode context.
with CarlaSyncMode(world, camera_front, fps=30) as sync_mode:
while True:
if should_quit():
return
clock.tick()
# Advance the simulation and wait for the data.
snapshot, image_rgb = sync_mode.tick(timeout=2.0)
# Choose the next waypoint and update the car location.
waypoint = random.choice(waypoint.next(1.5))
vehicle.set_transform(waypoint.transform)
fps = round(1.0 / snapshot.timestamp.delta_seconds)
# Draw the display.
draw_image(display, image_rgb)
display.blit(
font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)),
(8, 10))
display.blit(
font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)),
(8, 28))
pygame.display.flip()
finally:
print('destroying actors.')
for actor in actor_list:
actor.destroy()
pygame.quit()
print('done.')
def main():
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.yamaha.yzf')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.*.*')
global_dict['vehicle'] = vehicle
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
global_dict['max_steer_angle'] = np.deg2rad(
physics_control.wheels[0].max_steer_angle)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'camera:view': {
#'transform':carla.Transform(carla.Location(x=0.0, y=4.0, z=4.0), carla.Rotation(pitch=-30, yaw=-60)),
'transform':
carla.Transform(carla.Location(x=-3.0, y=0.0, z=6.0),
carla.Rotation(pitch=-45)),
#'transform':carla.Transform(carla.Location(x=0.0, y=0.0, z=6.0), carla.Rotation(pitch=-90)),
'callback':
view_image_callback,
},
'lidar': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': lidar_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
#spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
agent = BasicAgent(vehicle, target_speed=args.max_speed)
# prepare map
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
#destination = get_random_destination(spawn_points)
global_dict['plan_map'] = replan(agent, destination,
copy.deepcopy(origin_map))
# start to plan
plan_thread = threading.Thread(target=make_plan, args=())
#visualization_thread = threading.Thread(target = show_traj, args=())
while True:
if (global_dict['img']
is not None) and (global_dict['nav']
is not None) and (global_dict['pcd']
is not None):
plan_thread.start()
break
else:
time.sleep(0.001)
# wait for the first plan result
while not global_dict['start_control']:
time.sleep(0.001)
#visualization_thread.start()
# start to control
print('Start to control')
ctrller = CapacController(world, vehicle, 30)
env = CARLAEnv(world, vehicle)
env.reset()
#env.step()
while True:
# change destination
if close2dest(vehicle, destination):
#destination = get_random_destination(spawn_points)
print('get destination !', time.time())
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
global_dict['plan_map'] = replan(agent, destination,
copy.deepcopy(origin_map))
v = global_dict['vehicle'].get_velocity()
a = global_dict['vehicle'].get_acceleration()
global_dict['vel'] = np.sqrt(v.x**2 + v.y**2 + v.z**2)
global_dict['a'] = np.sqrt(a.x**2 + a.y**2 + a.z**2)
#steer_angle = global_dict['vehicle'].get_control().steer*global_dict['max_steer_angle']
#w = global_vel*np.tan(steer_angle)/2.405
control_time = time.time()
dt = control_time - global_dict['trajectory']['time']
index = int((dt / args.max_t) // args.dt) + 5
if index > 0.99 / args.dt:
continue
"""
transform = vehicle.get_transform()
dx, dy, dyaw = get_diff_tf(transform, global_dict['transform'])
dyaw = -dyaw
_x = global_dict['trajectory']['x'][index] - dx
_y = global_dict['trajectory']['y'][index] - dy
x = _x*np.cos(dyaw) + _y*np.sin(dyaw)
y = _y*np.cos(dyaw) - _x*np.sin(dyaw)
_vx = global_dict['trajectory']['vx'][index]
_vy = global_dict['trajectory']['vy'][index]
vx = _vx*np.cos(dyaw) + _vy*np.sin(dyaw)
vy = _vy*np.cos(dyaw) - _vx*np.sin(dyaw)
_ax = global_dict['trajectory']['ax'][index]
_ay = global_dict['trajectory']['ay'][index]
ax = _ax*np.cos(dyaw) + _ay*np.sin(dyaw)
ay = _ay*np.cos(dyaw) - _ax*np.sin(dyaw)
"""
control = ctrller.run_step(global_dict['trajectory'], index,
global_dict['state0'])
env.step(control)
#vehicle.apply_control(control)
"""
dyaw = np.deg2rad(global_dict['transform'].rotation.yaw)
x = global_dict['trajectory']['x'][index]*np.cos(dyaw) + global_dict['trajectory']['y'][index]*np.sin(dyaw)
y = global_dict['trajectory']['y'][index]*np.cos(dyaw) - global_dict['trajectory']['x'][index]*np.sin(dyaw)
"""
curve = None #show_traj(True)
#visualize(global_dict['view_img'], global_dict['draw_cost_map'], global_dict['nav'], args, curve)
visualize(global_dict, global_dict['draw_cost_map'], args, curve)
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()
def _make_carla_client(self, host, port):
print("_make_carla_client")
self._client = carla.Client(host, port)
self._client.set_timeout(2.0)
def game_loop(args):
global Test_on
pygame.init()
pygame.font.init()
world = None
dis = Distance()
version = 20000
version_name = 'xyhd_all_data_file' + str(version)
model_path = 'model_supervised/train_data/'
model_file = 'xyhd_40000_data_file' + '.h5'
trajectory_path = 'model_supervised/train_data/'
model_name = model_path + model_file
model = load_model(model_name)
trajectory = open(trajectory_path + version_name + '.txt', 'w')
#3 분 3000에포크
print(model_name)
try:
client = carla.Client(args.host, args.port)
client.set_timeout(2.0)
display = pygame.display.set_mode(
(args.width, args.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
hud = HUD(args.width, args.height,trajectory)
world = World(client.get_world(), hud, args.filter, args.rolename)
controller = KeyboardControl(world, args.autopilot)
clock = pygame.time.Clock()
while True:
if Test_on == True:
# print("Test start ! ", Test_on)
clock.tick_busy_loop(60)
if controller.parse_events(client, world, clock):
return
x, y, h = world.tick(clock)
distance = dis.get_d(x, y, h)
x_test = (x, y, h, distance)
# print("x, y, h, d", x, y, h, distance)
x_test = np.reshape(x_test, [1, 4])
action = model.predict(x_test)
print(action)
accel = float(action[0][0])
steer = float(action[0][1])
brake = float(action[0][2])
controller.do_action(world, clock.get_time(), accel, steer, brake)
world.render(display)
pygame.display.flip()
else:
clock.tick_busy_loop(60)
if controller.parse_events(client, world, clock):
return
world.tick(clock)
world.render(display)
pygame.display.flip()
finally:
if (world and world.recording_enabled):
client.stop_recorder()
if world is not None:
world.destroy()
trajectory.close()
pygame.quit()
def __init__(self, host, world_port):
self.client = carla.Client(host, world_port)
self.client.set_timeout(10.0)
self.world = self.client.get_world()
self.blueprint_lib = self.world.get_blueprint_library()
self.car_agent_model = self.blueprint_lib.filter("model3")[0]
def __init__(self, params):
# parameters
self.display_size = params['display_size'] # rendering screen size
self.max_past_step = params['max_past_step']
self.number_of_vehicles = params['number_of_vehicles']
self.number_of_walkers = params['number_of_walkers']
self.dt = params['dt']
self.task_mode = params['task_mode']
self.max_time_episode = params['max_time_episode']
self.max_waypt = params['max_waypt']
self.obs_range = params['obs_range']
self.lidar_bin = params['lidar_bin']
self.d_behind = params['d_behind']
self.obs_size = int(self.obs_range / self.lidar_bin)
self.out_lane_thres = params['out_lane_thres']
self.desired_speed = params['desired_speed']
# Destination
if params['task_mode'] == 'roundabout':
self.dests = [[4.46, -61.46, 0], [-49.53, -2.89, 0],
[-6.48, 55.47, 0], [35.96, 3.33, 0]]
else:
self.dests = None
# action and observation spaces
self.discrete = params['discrete']
self.discrete_act = [params['discrete_acc'],
params['discrete_steer']] # acc, steer
self.n_acc = len(self.discrete_act[0])
self.n_steer = len(self.discrete_act[1])
if self.discrete:
self.action_space = spaces.Discrete(self.n_acc * self.n_steer)
else:
self.action_space = spaces.Box(
np.array([
params['continuous_accel_range'][0],
params['continuous_steer_range'][0]
]),
np.array([
params['continuous_accel_range'][1],
params['continuous_steer_range'][1]
]),
dtype=np.float32) # acc, steer
self.observation_space = spaces.Dict({
'birdeye':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'lidar':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'camera':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8)
})
# Connect to carla server and get world object
print('connecting to Carla server...')
client = carla.Client('localhost', params['port'])
client.set_timeout(10.0)
self.world = client.load_world(params['town'])
print('Carla server connected!')
# Set weather
self.world.set_weather(carla.WeatherParameters.ClearNoon)
# Get spawn points
self.vehicle_spawn_points = list(
self.world.get_map().get_spawn_points())
self.walker_spawn_points = []
for i in range(self.number_of_walkers):
spawn_point = carla.Transform()
loc = self.world.get_random_location_from_navigation()
if (loc != None):
spawn_point.location = loc
self.walker_spawn_points.append(spawn_point)
# Create the ego vehicle blueprint
self.ego_bp = self._create_vehicle_bluepprint(
params['ego_vehicle_filter'], color='49,8,8')
# Collision sensor
self.collision_hist = [] # The collision history
self.collision_hist_l = 1 # collision history length
self.collision_bp = self.world.get_blueprint_library().find(
'sensor.other.collision')
# Lidar sensor
self.lidar_data = None
self.lidar_height = 2.1
self.lidar_trans = carla.Transform(
carla.Location(x=0.0, z=self.lidar_height))
self.lidar_bp = self.world.get_blueprint_library().find(
'sensor.lidar.ray_cast')
self.lidar_bp.set_attribute('channels', '32')
self.lidar_bp.set_attribute('range', '5000')
# Camera sensor
self.camera_img = np.zeros((self.obs_size, self.obs_size, 3),
dtype=np.uint8)
self.camera_trans = carla.Transform(carla.Location(x=0.8, z=1.7))
self.camera_bp = self.world.get_blueprint_library().find(
'sensor.camera.rgb')
# Modify the attributes of the blueprint to set image resolution and field of view.
self.camera_bp.set_attribute('image_size_x', str(self.obs_size))
self.camera_bp.set_attribute('image_size_y', str(self.obs_size))
self.camera_bp.set_attribute('fov', '110')
# Set the time in seconds between sensor captures
self.camera_bp.set_attribute('sensor_tick', '0.02')
# Set fixed simulation step for synchronous mode
self.settings = self.world.get_settings()
self.settings.fixed_delta_seconds = self.dt
# Record the time of total steps and resetting steps
self.reset_step = 0
self.total_step = 0
# Initialize the renderer
self._init_renderer()
def main():
actor_list = []
# In this tutorial script, we are going to add a vehicle to the simulation
# and let it drive in autopilot. We will also create a camera attached to
# that vehicle, and save all the images generated by the camera to disk.
try:
# First of all, we need to create the client that will send the requests
# to the simulator. Here we'll assume the simulator is accepting
# requests in the localhost at port 2000.
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
# Once we have a client we can retrieve the world that is currently
# running.
world = client.get_world()
# The world contains the list blueprints that we can use for adding new
# actors into the simulation.
blueprint_library = world.get_blueprint_library()
# Now let's filter all the blueprints of type 'vehicle' and choose one
# at random.
bp = random.choice(blueprint_library.filter('vehicle'))
# A blueprint contains the list of attributes that define a vehicle
# instance, we can read them and modify some of them. For instance,
# let's randomize its color.
color = random.choice(bp.get_attribute('color').recommended_values)
bp.set_attribute('color', color)
# Now we need to give an initial transform to the vehicle. This is a
# nice spot in Town01.
transform = carla.Transform(carla.Location(x=140.0, y=199.0, z=40.0),
carla.Rotation(yaw=0.0))
# So let's tell the world to spawn the vehicle.
vehicle = world.spawn_actor(bp, transform)
# It is important to note that the actors we create won't be destroyed
# unless we call their "destroy" function. If we fail to call "destroy"
# they will stay in the simulation even after we quit the Python script.
# For that reason, we are storing all the actors we create so we can
# destroy them afterwards.
actor_list.append(vehicle)
print('created %s' % vehicle.type_id)
# Let's put the vehicle to drive around.
vehicle.set_autopilot(True)
# Let's add now a "depth" camera attached to the vehicle. Note that the
# transform we give here is now relative to the vehicle.
camera_bp = blueprint_library.find('sensor.camera.depth')
camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4))
camera = world.spawn_actor(camera_bp,
camera_transform,
attach_to=vehicle)
actor_list.append(camera)
print('created %s' % camera.type_id)
# Now we register the function that will be called each time the sensor
# receives an image. In this example we are saving the image to disk
# converting the pixels to gray-scale.
cc = carla.ColorConverter.LogarithmicDepth
camera.listen(lambda image: image.save_to_disk(
'_out/%06d.png' % image.frame_number, cc))
# Oh wait, I don't like the location we gave to the vehicle, I'm going
# to move it a bit forward.
location = vehicle.get_location()
location.x += 40
vehicle.set_location(location)
print('moved vehicle to %s' % location)
# But the city now is probably quite empty, let's add a few more
# vehicles.
transform.location += carla.Location(x=40, y=-3.2)
transform.rotation.yaw = -180.0
for x in range(0, 10):
transform.location.x += 8.0
bp = random.choice(blueprint_library.filter('vehicle'))
# This time we are using try_spawn_actor. If the spot is already
# occupied by another object, the function will return None.
npc = world.try_spawn_actor(bp, transform)
if npc is not None:
actor_list.append(npc)
npc.set_autopilot()
print('created %s' % npc.type_id)
time.sleep(5)
finally:
print('destroying actors')
for actor in actor_list:
actor.destroy()
print('done.')
def main(n_vehicles, host, world_port, tm_port):
vehicles = []
client = carla.Client(host, world_port)
client.set_timeout(10.0)
try:
world = client.get_world()
traffic_manager = client.get_trafficmanager(tm_port)
traffic_manager.set_global_distance_to_leading_vehicle(1.0)
traffic_manager.set_synchronous_mode(True)
settings = world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = 0.1
world.apply_settings(settings)
# cannot import these directly.
SpawnActor = carla.command.SpawnActor
SetAutopilot = carla.command.SetAutopilot
SetVehicleLightState = carla.command.SetVehicleLightState
FutureActor = carla.command.FutureActor
# spawn vehicles.
batch = []
sensors = []
for i, transform in enumerate(world.get_map().get_spawn_points()):
if i >= n_vehicles:
break
blueprints = world.get_blueprint_library().filter('vehicle.*')
blueprint = random.choice(blueprints)
blueprint.set_attribute('role_name', 'autopilot')
#vehicle = SpawnActor(blueprint, transform).then(SetAutopilot(FutureActor, True, traffic_manager.get_port()))
vehicle = world.spawn_actor(blueprint, transform)
vehicle.set_autopilot(True, traffic_manager.get_port())
batch.append(vehicle)
rgb_cam = world.get_blueprint_library().find("sensor.camera.rgb")
rgb_cam.set_attribute("image_size_x", f"{width}")
rgb_cam.set_attribute("image_size_y", f"{height}")
rgb_cam.set_attribute("fov", f"{fov}")
sensor_pos = carla.Transform(carla.Location(x=2.5, z=0.7))
sensor = world.spawn_actor(rgb_cam, sensor_pos, attach_to=vehicle)
sensors.append(sensor)
sensor.listen(lambda data: process_img(data, iter))
# for response in client.apply_batch_sync(batch, True):
# if response.error:
# print(response.error)
# else:
# vehicles.append(response.actor_id)
# let them run around.
for sample in range(10000):
#print('Tick: %d' % t)
#iter+=1
for i, v in enumerate(world.get_actors().filter('*vehicle*')):
print('Vehicle %d: id=%d, x=%.2f, y=%.2f' %
(i, v.id, v.get_location().x, v.get_location().y))
world.tick()
finally:
settings = world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
world.apply_settings(settings)
client.apply_batch([carla.command.DestroyActor(x) for x in vehicles])
client.apply_batch([carla.command.DestroyActor(x) for x in sensors])
def setUp(self):
self.testing_address = TESTING_ADDRESS
self.client = carla.Client(*TESTING_ADDRESS)
self.client.set_timeout(120.0)
def game_loop():
global world, control
pygame.init()
pygame.font.init()
try:
client = carla.Client(HOST, PORT)
client.set_timeout(TIMEOUT)
display = pygame.display.set_mode((IMG_LENGTH, IMG_WIDTH),
pygame.HWSURFACE | pygame.DOUBLEBUF)
hud = HUD(IMG_LENGTH, IMG_WIDTH)
world = World(client.get_world(), hud, 'vehicle.bmw.grandtourer')
controller = KeyboardControl(world, False)
# get random starting point and destination
spawn_points = world.map.get_spawn_points()
#start_transform = world.player.get_transform()
start_transform = spawn_points[1]
world.player.set_transform(start_transform)
#destination = spawn_points[random.randint(0,len(spawn_points)-1)]
destination = spawn_points[0]
agent = BasicAgent(world.player, target_speed=MAX_SPEED)
agent.set_destination((destination.location.x, destination.location.y,
destination.location.z))
control = agent.run_step()
blueprint_library = world.world.get_blueprint_library()
vehicle = world.player
rgb_camera = add_rgb_camera(blueprint_library, vehicle)
#depth_camera = add_depth_camera(blueprint_library, vehicle)
#semantic_camera = add_semantic_camera(blueprint_library, vehicle)
time.sleep(1)
rgb_camera.listen(lambda image: get_rgb_img(image))
#depth_camera.listen(lambda image: get_depth_img(image))
#semantic_camera.listen(lambda image: get_semantic_img(image))
clock = pygame.time.Clock()
weather = carla.WeatherParameters(
cloudyness=random.randint(0, 80), #0-100
precipitation=0, #random.randint(0,20),#0-100
precipitation_deposits=0, #random.randint(0,20),#0-100
wind_intensity=random.randint(0, 50), #0-100
sun_azimuth_angle=random.randint(60, 90), #0-360
sun_altitude_angle=random.randint(60, 90)) #-90~90
world.world.set_weather(weather)
# put vehicle on starting point
world.player.set_transform(start_transform)
frames = 0
while frames < MAX_FRAMES:
frames += 1
if controller.parse_events(client, world, clock):
return
# as soon as the server is ready continue!
if not world.world.wait_for_tick(TIMEOUT):
continue
world.tick(clock)
world.render(display)
pygame.display.flip()
control = agent.run_step()
control.manual_gear_shift = False
world.player.apply_control(control)
finally:
rgb_camera.stop()
#depth_camera.stop()
#semantic_camera.stop()
if world is not None:
world.destroy()
pygame.quit()
def bridge(q):
# setup CARLA
client = carla.Client("127.0.0.1", 2000)
client.set_timeout(10.0)
world = client.load_world(args.town)
settings = world.get_settings()
settings.synchronous_mode = True # Enables synchronous mode
settings.fixed_delta_seconds = 0.05
world.apply_settings(settings)
world.set_weather(carla.WeatherParameters.ClearSunset)
if args.low_quality:
world.unload_map_layer(carla.MapLayer.Foliage)
world.unload_map_layer(carla.MapLayer.Buildings)
world.unload_map_layer(carla.MapLayer.ParkedVehicles)
world.unload_map_layer(carla.MapLayer.Props)
world.unload_map_layer(carla.MapLayer.StreetLights)
world.unload_map_layer(carla.MapLayer.Particles)
blueprint_library = world.get_blueprint_library()
world_map = world.get_map()
vehicle_bp = blueprint_library.filter('vehicle.tesla.*')[1]
spawn_points = world_map.get_spawn_points()
assert len(spawn_points) > args.num_selected_spawn_point, \
f'''No spawn point {args.num_selected_spawn_point}, try a value between 0 and
{len(spawn_points)} for this town.'''
spawn_point = spawn_points[args.num_selected_spawn_point]
vehicle = world.spawn_actor(vehicle_bp, spawn_point)
max_steer_angle = vehicle.get_physics_control().wheels[0].max_steer_angle
# make tires less slippery
# wheel_control = carla.WheelPhysicsControl(tire_friction=5)
physics_control = vehicle.get_physics_control()
physics_control.mass = 2326
# physics_control.wheels = [wheel_control]*4
physics_control.torque_curve = [[20.0, 500.0], [5000.0, 500.0]]
physics_control.gear_switch_time = 0.0
vehicle.apply_physics_control(physics_control)
transform = carla.Transform(carla.Location(x=0.8, z=1.13))
def create_camera(fov, callback):
blueprint = blueprint_library.find('sensor.camera.rgb')
blueprint.set_attribute('image_size_x', str(W))
blueprint.set_attribute('image_size_y', str(H))
blueprint.set_attribute('fov', str(fov))
blueprint.set_attribute('sensor_tick', '0.05')
camera = world.spawn_actor(blueprint, transform, attach_to=vehicle)
camera.listen(callback)
return camera
camerad = Camerad()
road_camera = create_camera(fov=40, callback=camerad.cam_callback_road)
road_wide_camera = create_camera(
fov=163, callback=camerad.cam_callback_wide_road
) # fov bigger than 163 shows unwanted artifacts
cameras = [road_camera, road_wide_camera]
vehicle_state = VehicleState()
# reenable IMU
imu_bp = blueprint_library.find('sensor.other.imu')
imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle)
imu.listen(lambda imu: imu_callback(imu, vehicle_state))
gps_bp = blueprint_library.find('sensor.other.gnss')
gps = world.spawn_actor(gps_bp, transform, attach_to=vehicle)
gps.listen(lambda gps: gps_callback(gps, vehicle_state))
# launch fake car threads
threads = []
exit_event = threading.Event()
threads.append(
threading.Thread(target=panda_state_function,
args=(
vehicle_state,
exit_event,
)))
threads.append(
threading.Thread(target=peripheral_state_function,
args=(exit_event, )))
threads.append(
threading.Thread(target=fake_driver_monitoring, args=(exit_event, )))
threads.append(
threading.Thread(target=can_function_runner,
args=(
vehicle_state,
exit_event,
)))
for t in threads:
t.start()
# can loop
rk = Ratekeeper(100, print_delay_threshold=0.05)
# init
throttle_ease_out_counter = REPEAT_COUNTER
brake_ease_out_counter = REPEAT_COUNTER
steer_ease_out_counter = REPEAT_COUNTER
vc = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False)
is_openpilot_engaged = False
throttle_out = steer_out = brake_out = 0
throttle_op = steer_op = brake_op = 0
throttle_manual = steer_manual = brake_manual = 0
old_steer = old_brake = old_throttle = 0
throttle_manual_multiplier = 0.7 # keyboard signal is always 1
brake_manual_multiplier = 0.7 # keyboard signal is always 1
steer_manual_multiplier = 45 * STEER_RATIO # keyboard signal is always 1
while True:
# 1. Read the throttle, steer and brake from op or manual controls
# 2. Set instructions in Carla
# 3. Send current carstate to op via can
cruise_button = 0
throttle_out = steer_out = brake_out = 0.0
throttle_op = steer_op = brake_op = 0
throttle_manual = steer_manual = brake_manual = 0.0
# --------------Step 1-------------------------------
if not q.empty():
message = q.get()
m = message.split('_')
if m[0] == "steer":
steer_manual = float(m[1])
is_openpilot_engaged = False
elif m[0] == "throttle":
throttle_manual = float(m[1])
is_openpilot_engaged = False
elif m[0] == "brake":
brake_manual = float(m[1])
is_openpilot_engaged = False
elif m[0] == "reverse":
cruise_button = CruiseButtons.CANCEL
is_openpilot_engaged = False
elif m[0] == "cruise":
if m[1] == "down":
cruise_button = CruiseButtons.DECEL_SET
is_openpilot_engaged = True
elif m[1] == "up":
cruise_button = CruiseButtons.RES_ACCEL
is_openpilot_engaged = True
elif m[1] == "cancel":
cruise_button = CruiseButtons.CANCEL
is_openpilot_engaged = False
elif m[0] == "ignition":
vehicle_state.ignition = not vehicle_state.ignition
elif m[0] == "quit":
break
throttle_out = throttle_manual * throttle_manual_multiplier
steer_out = steer_manual * steer_manual_multiplier
brake_out = brake_manual * brake_manual_multiplier
old_steer = steer_out
old_throttle = throttle_out
old_brake = brake_out
if is_openpilot_engaged:
sm.update(0)
# TODO gas and brake is deprecated
throttle_op = clip(sm['carControl'].actuators.accel / 1.6, 0.0,
1.0)
brake_op = clip(-sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
steer_op = sm['carControl'].actuators.steeringAngleDeg
throttle_out = throttle_op
steer_out = steer_op
brake_out = brake_op
steer_out = steer_rate_limit(old_steer, steer_out)
old_steer = steer_out
else:
if throttle_out == 0 and old_throttle > 0:
if throttle_ease_out_counter > 0:
throttle_out = old_throttle
throttle_ease_out_counter += -1
else:
throttle_ease_out_counter = REPEAT_COUNTER
old_throttle = 0
if brake_out == 0 and old_brake > 0:
if brake_ease_out_counter > 0:
brake_out = old_brake
brake_ease_out_counter += -1
else:
brake_ease_out_counter = REPEAT_COUNTER
old_brake = 0
if steer_out == 0 and old_steer != 0:
if steer_ease_out_counter > 0:
steer_out = old_steer
steer_ease_out_counter += -1
else:
steer_ease_out_counter = REPEAT_COUNTER
old_steer = 0
# --------------Step 2-------------------------------
steer_carla = steer_out / (max_steer_angle * STEER_RATIO * -1)
steer_carla = np.clip(steer_carla, -1, 1)
steer_out = steer_carla * (max_steer_angle * STEER_RATIO * -1)
old_steer = steer_carla * (max_steer_angle * STEER_RATIO * -1)
vc.throttle = throttle_out / 0.6
vc.steer = steer_carla
vc.brake = brake_out
vehicle.apply_control(vc)
# --------------Step 3-------------------------------
vel = vehicle.get_velocity()
speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) # in m/s
vehicle_state.speed = speed
vehicle_state.vel = vel
vehicle_state.angle = steer_out
vehicle_state.cruise_button = cruise_button
vehicle_state.is_engaged = is_openpilot_engaged
if rk.frame % PRINT_DECIMATION == 0:
print("frame: ", "engaged:", is_openpilot_engaged, "; throttle: ",
round(vc.throttle, 3), "; steer(c/deg): ",
round(vc.steer, 3), round(steer_out, 3), "; brake: ",
round(vc.brake, 3))
if rk.frame % 5 == 0:
world.tick()
rk.keep_time()
# Clean up resources in the opposite order they were created.
exit_event.set()
for t in reversed(threads):
t.join()
gps.destroy()
imu.destroy()
for c in cameras:
c.destroy()
vehicle.destroy()
def main(arg):
"""Main function of the script"""
client = carla.Client(arg.host, arg.port)
client.set_timeout(10.0)
client.load_world('Town05')
client.reload_world()
world = client.get_world()
try:
original_settings = world.get_settings()
settings = world.get_settings()
traffic_manager = client.get_trafficmanager(8000)
traffic_manager.set_synchronous_mode(True)
delta = 0.05
settings.fixed_delta_seconds = delta
settings.synchronous_mode = True
settings.no_rendering_mode = arg.no_rendering
world.apply_settings(settings)
blueprint_library = world.get_blueprint_library()
# vehicle_bp = blueprint_library.filter(arg.filter)[0]
# vehicle_transform = random.choice(world.get_map().get_spawn_points())
# vehicle = world.spawn_actor(vehicle_bp, vehicle_transform)
# vehicle.set_autopilot(arg.no_autopilot)
lidar_bp = generate_lidar_bp(arg, world, blueprint_library, delta)
lidar_transform = carla.Transform(carla.Location(x=-65.0, y=3.0,
z=6.0))
lidar = world.spawn_actor(lidar_bp, lidar_transform)
fig = mlab.figure(size=(960, 540), bgcolor=(0.05, 0.05, 0.05))
vis = mlab.points3d(0, 0, 0, 0, mode='point', figure=fig)
mlab.view(distance=25)
buf = {'pts': np.zeros((1, 3)), 'intensity': np.zeros(1)}
# @mlab.animate(delay=100)
def anim():
i = 0
while True:
vis.mlab_source.reset(x=buf['pts'][:, 0],
y=buf['pts'][:, 1],
z=buf['pts'][:, 2],
scalars=buf['intensity'])
mlab.savefig(f'{i%10}.png', figure=fig)
time.sleep(0.1)
i += 1
if arg.semantic:
lidar.listen(lambda data: semantic_lidar_callback(data, buf))
else:
lidar.listen(lambda data: lidar_callback(data, buf))
loopThread = threading.Thread(target=carlaEventLoop,
args=[world],
daemon=True)
loopThread.start()
anim()
# mlab.show()
finally:
world.apply_settings(original_settings)
traffic_manager.set_synchronous_mode(False)
vehicle.destroy()
lidar.destroy()
def game_loop(options_dict):
world = None
try:
client = carla.Client('localhost', 2000)
client.set_timeout(60.0)
print('Changing world to Town 5')
client.load_world('Town05')
world = World(client.get_world())
world = World(client.get_world())
spawn_points = world.world.get_map().get_spawn_points()
vehicle_bp = 'model3'
vehicle_transform = spawn_points[options_dict['spawn_point_indices']
[0]]
vehicle = Car(vehicle_bp, vehicle_transform, world)
agent = agent = BasicAgent(vehicle.vehicle)
destination_point = spawn_points[options_dict['spawn_point_indices']
[1]].location
print('Going to ', destination_point)
agent.set_destination(
(destination_point.x, destination_point.y, destination_point.z))
camera_bp = [
'sensor.camera.rgb', 'sensor.camera.depth', 'sensor.lidar.ray_cast'
]
camera_transform = [
carla.Transform(carla.Location(x=1.5, z=2.4),
carla.Rotation(pitch=-15, yaw=40)),
carla.Transform(carla.Location(x=1.5, z=2.4),
carla.Rotation(pitch=-15, yaw=-40)),
carla.Transform(carla.Location(x=1.5, z=2.4))
]
cam1 = Camera(camera_bp[0], camera_transform[0], vehicle)
cam2 = Camera(camera_bp[0], camera_transform[1], vehicle)
# depth1 = Camera(camera_bp[1], camera_transform[0], vehicle)
# depth2 = Camera(camera_bp[1], camera_transform[1], vehicle)
lidar = Lidar(camera_bp[2], camera_transform[2], vehicle)
prev_location = vehicle.vehicle.get_location()
sp = 2
file = open("control_input.txt", "a")
while True:
world.world.tick()
world_snapshot = world.world.wait_for_tick(60.0)
if not world_snapshot:
continue
control = agent.run_step()
vehicle.vehicle.apply_control(control)
w = str(world_snapshot.frame_count) + ',' + str(
control.steer) + ',' + str(get_speed(vehicle.vehicle)) + '\n'
file.write(w)
current_location = vehicle.vehicle.get_location()
if current_location.distance(
prev_location) <= 0.0 and current_location.distance(
destination_point) <= 10:
print('distance from destination: ',
current_location.distance(destination_point))
if len(options_dict['spawn_point_indices']) <= sp:
break
else:
destination_point = spawn_points[
options_dict['spawn_point_indices'][sp]].location
print('Going to ', destination_point)
agent.set_destination(
(destination_point.x, destination_point.y,
destination_point.z))
sp += 1
prev_location = current_location
finally:
if world is not None:
world.destroy()
def __init__(self, params):
# parameters
print('init mai hai ham')
action_params = params['action_params']
self.path_params = params['path_params']
self.display_size = params['display_size'] # rendering screen size
self.max_past_step = params['max_past_step']
self.number_of_vehicles = params['number_of_vehicles']
self.number_of_walkers = params['number_of_walkers']
self.task_mode = params['task_mode']
self.max_time_episode = params['max_time_episode']
self.max_waypt = params['max_waypt']
self.obs_range = params['obs_range']
self.lidar_bin = params['lidar_bin']
self.d_behind = params['d_behind']
self.obs_size = int(self.obs_range / self.lidar_bin)
self.out_lane_thres = params['out_lane_thres']
self.desired_speed = params['desired_speed']
self.max_ego_spawn_times = params['max_ego_spawn_times']
self.display_route = params['display_route']
control_params = params['control_params']
self.args_lateral_dict = control_params['args_lateral_dict']
self.args_longitudinal_dict = control_params['args_longitudinal_dict']
D_T_S = action_params['d_t_s']
N_S_SAMPLE = action_params['n_s_sample']
MINT = action_params['mint']
MAXT = action_params['maxt']
MAX_ROAD_WIDTH = action_params['max_road_width']
MAX_LAT_VEL = action_params['max_lat_vel']
TARGET_SPEED = self.path_params['TARGET_SPEED']
self.dt = self.path_params['DT'] # time interval between 2 frames
if 'pixor' in params.keys():
self.pixor = params['pixor']
self.pixor_size = params['pixor_size']
else:
self.pixor = False
# Destination
if params['task_mode'] == 'roundabout':
self.dests = [[4.46, -61.46, 0], [-49.53, -2.89, 0],
[-6.48, 55.47, 0], [35.96, 3.33, 0]]
else:
self.dests = None
# action and observation spaces
self.discrete = params['discrete']
self.discrete_act = [params['discrete_acc'],
params['discrete_steer']] # acc, steer
self.n_acc = len(self.discrete_act[0])
self.n_steer = len(self.discrete_act[1])
if self.discrete:
self.action_space = spaces.Discrete(self.n_acc * self.n_steer)
else:
self.action_space = spaces.Box(
low=np.array([
TARGET_SPEED - D_T_S * N_S_SAMPLE, MINT, -MAX_ROAD_WIDTH,
-MAX_LAT_VEL
]),
high=np.array([
TARGET_SPEED + D_T_S * N_S_SAMPLE, MAXT, MAX_ROAD_WIDTH,
MAX_LAT_VEL
]),
dtype=np.float32)
observation_space_dict = {
'camera':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'lidar':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'birdeye':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'state':
spaces.Box(np.array([-2, -1, -5, 0]),
np.array([2, 1, 30, 1]),
dtype=np.float32)
}
if self.pixor: # dont change
observation_space_dict.update({
'roadmap':
spaces.Box(low=0,
high=255,
shape=(self.obs_size, self.obs_size, 3),
dtype=np.uint8),
'vh_clas':
spaces.Box(low=0,
high=1,
shape=(self.pixor_size, self.pixor_size, 1),
dtype=np.float32),
'vh_regr':
spaces.Box(low=-5,
high=5,
shape=(self.pixor_size, self.pixor_size, 6),
dtype=np.float32),
'pixor_state':
spaces.Box(np.array([-1000, -1000, -1, -1, -5]),
np.array([1000, 1000, 1, 1, 20]),
dtype=np.float32)
})
self.observation_space = spaces.Dict(observation_space_dict)
self.observation_space = spaces.Box(
low=-1,
high=1,
shape=(self.obs_size * self.obs_size * 3 + 4, 1),
dtype=np.float32)
# Connect to carla server and get world object
print('connecting to Carla server...')
client = carla.Client('localhost', params['port'])
client.set_timeout(10.0)
self.world = client.load_world(params['town'])
print('Carla server connected!')
# Set weather
self.world.set_weather(carla.WeatherParameters.ClearNoon)
# Get spawn points
self.vehicle_spawn_points = list(
self.world.get_map().get_spawn_points())
self.walker_spawn_points = []
for i in range(self.number_of_walkers):
spawn_point = carla.Transform()
loc = self.world.get_random_location_from_navigation()
if (loc != None):
spawn_point.location = loc
self.walker_spawn_points.append(spawn_point)
# Create the ego vehicle blueprint
self.ego_bp = self._create_vehicle_bluepprint(
params['ego_vehicle_filter'], color='49,8,8')
# Collision sensor
self.collision_hist = [] # The collision history
self.collision_hist_l = 1 # collision history length
self.collision_bp = self.world.get_blueprint_library().find(
'sensor.other.collision')
# Lidar sensor
self.lidar_data = None
self.lidar_height = 2.1 # Note : HEIGHT OF LIDAR
self.lidar_trans = carla.Transform(
carla.Location(x=0.0, z=self.lidar_height))
self.lidar_bp = self.world.get_blueprint_library().find(
'sensor.lidar.ray_cast')
self.lidar_bp.set_attribute('channels', '32')
self.lidar_bp.set_attribute('range', '5000')
# Camera sensor
self.camera_img = np.zeros((self.obs_size, self.obs_size, 3),
dtype=np.uint8)
self.camera_trans = carla.Transform(carla.Location(x=0.8, z=1.7))
self.camera_bp = self.world.get_blueprint_library().find(
'sensor.camera.rgb')
# Modify the attributes of the blueprint to set image resolution and field of view.
self.camera_bp.set_attribute('image_size_x', str(self.obs_size))
self.camera_bp.set_attribute('image_size_y', str(self.obs_size))
self.camera_bp.set_attribute('fov', '110')
# Set the time in seconds between sensor captures
self.camera_bp.set_attribute('sensor_tick', '0.02')
# Set fixed simulation step for synchronous mode
self.settings = self.world.get_settings()
self.settings.fixed_delta_seconds = self.dt
# Record the time of total steps and resetting steps
self.reset_step = 0
self.total_step = 0
# Initialize the renderer
self._init_renderer()
# Get pixel grid points
if self.pixor:
x, y = np.meshgrid(
np.arange(self.pixor_size),
np.arange(self.pixor_size)) # make a canvas with coordinates
x, y = x.flatten(), y.flatten()
self.pixel_grid = np.vstack((x, y)).T