class Car:
im_width = 640
im_height = 480
fov = 110
actor_list = []
front_camera = None
front_camera_intrinsics = None
front_camera_depth = None
front_camera_depth_old = None
front_camera_semantic = None
front_camera_semantic_old = None
imu_sensor = None
def __init__(self):
self.client = carla.Client("localhost", 2000)
self.client.set_timeout(2.0)
self.world = self.client.get_world()
blueprint_library = self.world.get_blueprint_library()
self.model_3 = blueprint_library.filter("model3")[0]
self.model_3.set_attribute('role_name', 'hero')
self.agent = None
settings = self.world.get_settings()
settings.synchronous_mode = False # Disables synchronous mode
settings.fixed_delta_seconds = 0
self.world.apply_settings(settings)
def reset(self):
actors = self.world.get_actors()
self.client.apply_batch([
carla.command.DestroyActor(x) for x in actors
if "vehicle" in x.type_id
])
self.client.apply_batch([
carla.command.DestroyActor(x) for x in actors
if "sensor" in x.type_id
])
for a in actors.filter("vehicle*"):
if a.is_alive:
a.destroy()
for a in actors.filter("sensor*"):
if a.is_alive:
a.destroy()
print("Scene init done!")
self.actor_list = []
self.transform = self.world.get_map().get_spawn_points()[0]
self.transform.location.z += 1
self.vehicle = self.world.spawn_actor(self.model_3, self.transform)
# self.vehicle.set_autopilot()
##### Setup Agent #####
self.agent = BehaviorAgent(self.vehicle, behavior="normal")
destination_location = self.world.get_map().get_spawn_points(
)[50].location
self.agent.set_destination(self.vehicle.get_location(),
destination_location,
clean=True)
self.agent.update_information(self.world)
########################
self.actor_list.append(self.vehicle)
self.other_vehicles = []
self.other_agents = []
spawn_points = self.world.get_map().get_spawn_points()
for _ in range(50):
print("Agent", _, "spawned!")
tmp_transform = random.choice(spawn_points)
tmp_transform.location.z += 1
tmp_vehicle = self.world.spawn_actor(self.model_3, tmp_transform)
tmp_agent = RoamingAgent(tmp_vehicle)
# tmp_destination_location = random.choice(spawn_points).location
# tmp_agent.set_destination(tmp_vehicle.get_location(), tmp_destination_location, clean=True)
# tmp_agent.update_information(self.world)
self.other_vehicles.append(tmp_vehicle)
self.other_agents.append(tmp_agent)
self.actor_list.append(tmp_vehicle)
self.rgb_cam = self.world.get_blueprint_library().find(
"sensor.camera.rgb")
self.rgb_cam.set_attribute("image_size_x", f"{self.im_width}")
self.rgb_cam.set_attribute("image_size_y", f"{self.im_height}")
self.rgb_cam.set_attribute("fov", "110")
fx = self.im_width / (2 * np.tan(self.fov * np.pi / 360))
fy = self.im_height / (2 * np.tan(self.fov * np.pi / 360))
self.front_camera_intrinsics = np.array([[fx, 0, self.im_width / 2],
[0, fy, self.im_height / 2],
[0, 0, 1]])
self.depth_cam = self.world.get_blueprint_library().find(
"sensor.camera.depth")
self.depth_cam.set_attribute("image_size_x", f"{self.im_width}")
self.depth_cam.set_attribute("image_size_y", f"{self.im_height}")
self.depth_cam.set_attribute("fov", "110")
self.semantic_cam = self.world.get_blueprint_library().find(
"sensor.camera.depth")
self.semantic_cam.set_attribute("image_size_x", f"{self.im_width}")
self.semantic_cam.set_attribute("image_size_y", f"{self.im_height}")
self.semantic_cam.set_attribute("fov", "110")
transform = carla.Transform(carla.Location(x=2.5, z=1))
self.sensor_rgb = self.world.spawn_actor(self.rgb_cam,
transform,
attach_to=self.vehicle)
self.actor_list.append(self.sensor_rgb)
self.sensor_rgb.listen(lambda data: self.process_img(data))
self.sensor_depth = self.world.spawn_actor(self.depth_cam,
transform,
attach_to=self.vehicle)
self.actor_list.append(self.sensor_depth)
self.sensor_depth.listen(lambda data: self.process_img_depth(data))
self.sensor_semantic = self.world.spawn_actor(self.semantic_cam,
transform,
attach_to=self.vehicle)
self.actor_list.append(self.sensor_semantic)
self.sensor_semantic.listen(
lambda data: self.process_img_semantic(data))
self.imu_sensor = IMUSensor(self.vehicle)
while self.front_camera is None:
time.sleep(0.01)
self.episode_start = time.time()
return self.front_camera
def process_img(self, image):
i = np.array(image.raw_data)
i2 = i.reshape((self.im_height, self.im_width, 4))
i3 = i2[:, :, :3]
self.front_camera = i3
def process_img_depth(self, image):
i = np.array(image.raw_data)
i2 = i.reshape((self.im_height, self.im_width, 4))
i2 = i2[:, :, :3]
i3 = np.add(
np.add(i2[:, :, 2], np.multiply(i2[:, :, 1], 256)),
np.multiply(i2[:, :, 0], 256 * 256),
)
i3 = np.divide(i3, 256**3 - 1)
self.front_camera_depth_old = self.front_camera_depth
self.front_camera_depth = i3
def process_img_semantic(self, image):
image.convert(carla.ColorConverter.CityScapesPalette)
i = np.array(image.raw_data)
i2 = i.reshape((self.im_height, self.im_width, 4))
self.front_camera_semantic_old = self.front_camera_semantic
self.front_camera_semantic = i2
def game_loop(args):
""" Main loop for agent"""
pygame.init()
pygame.font.init()
world = None
vis = None
lidar = None
original_settings = None
tot_target_reached = 0
num_min_waypoints = 21
try:
client = carla.Client(args.host, args.port)
client.set_timeout(4.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)
# make synchronous and disable keyboard controller BEGIN
logging.info("make synchronous and disable keyboard controller")
original_settings = world.world.get_settings()
settings = world.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
world.world.apply_settings(settings)
# END
controller = KeyboardControl(world)
agent = BehaviorAgent(world.player, behavior=args.behavior)
spawn_points = world.map.get_spawn_points()
random.shuffle(spawn_points)
if spawn_points[0].location != agent.vehicle.get_location():
destination = spawn_points[0].location
else:
destination = spawn_points[1].location
agent.set_destination(agent.vehicle.get_location(), destination, clean=True)
# open3d_lidar setup BEGIN
logging.info("open3d_lidar setup")
blueprint_library = world.world.get_blueprint_library()
lidar_bp = generate_lidar_bp(args, world.world, blueprint_library, delta)
user_offset = carla.Location(args.x, args.y, args.z)
lidar_transform = carla.Transform(carla.Location(x=-0.5, z=1.8) + user_offset)
lidar = world.world.spawn_actor(lidar_bp,
lidar_transform, attach_to=world.player)
point_list = o3d.geometry.PointCloud()
lidar.listen(lambda data: lidar_callback(data, point_list))
vis = o3d.visualization.Visualizer()
vis.create_window(
window_name='Carla Lidar',
width=960,
height=540,
left=480,
top=270)
vis.get_render_option().background_color = [0.05, 0.05, 0.05]
vis.get_render_option().point_size = 1
vis.get_render_option().show_coordinate_frame = True
frame = 0
# open3d_lidar setup END
clock = pygame.time.Clock()
logging.info("entering loop")
while True:
# open3d_lidar render BEGIN
# print("open3d_lidar render")
if frame == 2:
vis.add_geometry(point_list)
vis.update_geometry(point_list)
vis.poll_events()
vis.update_renderer()
# time.sleep(0.005)
world.world.tick()
frame += 1
# open3d_lidar render END
# move self.world.on_tick(hud.on_world_tick) here BEGIN
# print("HUD tick")
# hud.on_world_tick(
# world.world.get_snapshot().timestamp)
# move self.world.on_tick(hud.on_world_tick) here END
if controller.parse_events():
return
# As soon as the server is ready continue!
# if not world.world.wait_for_tick(10.0):
# continue
agent.update_information()
# display.fill("red")
world.tick(clock)
clock.tick_busy_loop(50)
world.render(display)
pygame.display.flip()
# Set new destination when target has been reached
if len(agent.get_local_planner().waypoints_queue) < num_min_waypoints:
agent.reroute(spawn_points)
tot_target_reached += 1
world.hud.notification("The target has been reached " +
str(tot_target_reached) + " times.", seconds=4.0)
elif len(agent.get_local_planner().waypoints_queue) == 0:
print("Target reached, mission accomplished...")
break
speed_limit = world.player.get_speed_limit()
agent.get_local_planner().set_speed(speed_limit)
control = agent.run_step()
world.player.apply_control(control)
finally:
if vis is not None:
vis.destroy_window()
if lidar is not None:
lidar.destroy()
if original_settings is not None \
and world is not None:
world.world.apply_settings(original_settings)
if world is not None:
world.destroy()
pygame.quit()
def game_loop(args):
""" Main loop for agent"""
prev_timestamp = 0
pygame.init()
skip_first_frame = True
pygame.font.init()
world = None
tot_target_reached = 0
num_min_waypoints = 21
try:
start_timestamp = 0
client = carla.Client(args.host, args.port)
client.set_timeout(20.0)
client.load_world('Town01')
client.reload_world()
#measurement_data, sensor_data = client.read_data()
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)
# Set Sensors
#RGB camera
agent = BehaviorAgent(world.player, behavior="normal")
spawn_points = world.map.get_spawn_points()
random.shuffle(spawn_points)
if spawn_points[0].location != agent.vehicle.get_location():
destination = spawn_points[0].location
else:
destination = spawn_points[1].location
agent.set_destination(agent.vehicle.get_location(), destination, clean=True)
waypoints_file = WAYPOINTS_FILENAME
waypoints_filepath =\
os.path.join(os.path.dirname(os.path.realpath(__file__)),
WAYPOINTS_FILENAME)
waypoints_np = None
with open(waypoints_filepath) as waypoints_file_handle:
waypoints = list(csv.reader(waypoints_file_handle,
delimiter=',',
quoting=csv.QUOTE_NONNUMERIC))
waypoints_np = np.array(waypoints)
print(waypoints_np.shape)
wp_goal_index = 0
local_waypoints = None
path_validity = np.zeros((NUM_PATHS, 1), dtype=bool)
controller = resources.controller2d.Controller2D(waypoints)
bp = resources.behavioural_planner.BehaviouralPlanner(BP_LOOKAHEAD_BASE)
lp = resources.local_planner.LocalPlanner(NUM_PATHS,
PATH_OFFSET,
CIRCLE_OFFSETS,
CIRCLE_RADII,
PATH_SELECT_WEIGHT,
TIME_GAP,
A_MAX,
SLOW_SPEED,
STOP_LINE_BUFFER)
clock = pygame.time.Clock()
frame = 0
current_timestamp = start_timestamp
reached_the_end = False
while True:
frame = frame +1
clock.tick_busy_loop(60)
agent.update_information(world)
world.tick(clock)
world.render(display)
if not world.world.wait_for_tick(10.0):
continue
pygame.display.flip()
transform = world.player.get_transform()
vel = world.player.get_velocity()
print("Vel", vel)
prev_timestamp = current_timestamp
current_timestamp = hud.simulation_time
control = world.player.get_control()
current_x = transform.location.x
current_y = transform.location.y
current_yaw = np.radians(transform.rotation.yaw)
current_speed = math.sqrt(vel.x**2 + vel.y**2 )
print("Variables: " , current_x,current_y,current_yaw,current_speed)
open_loop_speed = lp._velocity_planner.get_open_loop_speed(current_timestamp - prev_timestamp)
#open_loop_speed = lp._velocity_planner.get_open_loop_speed(current_timestamp - prev_timestamp)
print("Current Timestamp", current_timestamp)
print("prev_timestamp",prev_timestamp)
if frame % 5 == 0:
#Lane detection
ego_state = [current_x, current_y, current_yaw, open_loop_speed]
bp.set_lookahead(BP_LOOKAHEAD_BASE + BP_LOOKAHEAD_TIME * open_loop_speed)
print("Ego_State",ego_state)
bp.transition_state(waypoints,waypoints, ego_state, current_speed)
goal_state_set = lp.get_goal_state_set(bp._goal_index, bp._goal_state, waypoints, ego_state)
paths, path_validity = lp.plan_paths(goal_state_set)
paths = resources.local_planner.transform_paths(paths, ego_state)
best_index = lp._collision_checker.select_best_path_index(paths, bp._goal_state_hyp)
try:
if best_index == None:
best_path = lp._prev_best_path
else:
best_path = paths[best_index]
lp._prev_best_path = best_path
except:
continue
desired_speed = bp._goal_state[2]
decelerate_to_stop = False
local_waypoints = lp._velocity_planner.compute_velocity_profile(best_path, desired_speed, ego_state, current_speed)
if local_waypoints != None:
wp_distance = [] # distance array
local_waypoints_np = np.array(local_waypoints)
for i in range(1, local_waypoints_np.shape[0]):
wp_distance.append(
np.sqrt((local_waypoints_np[i, 0] - local_waypoints_np[i-1, 0])**2 +
(local_waypoints_np[i, 1] - local_waypoints_np[i-1, 1])**2))
wp_distance.append(0)
wp_interp = []
for i in range(local_waypoints_np.shape[0] - 1):
wp_interp.append(list(local_waypoints_np[i]))
num_pts_to_interp = int(np.floor(wp_distance[i] /\
float(INTERP_DISTANCE_RES)) - 1)
wp_vector = local_waypoints_np[i+1] - local_waypoints_np[i]
wp_uvector = wp_vector / np.linalg.norm(wp_vector[0:2])
for j in range(num_pts_to_interp):
next_wp_vector = INTERP_DISTANCE_RES * float(j+1) * wp_uvector
wp_interp.append(list(local_waypoints_np[i] + next_wp_vector))
wp_interp.append(list(local_waypoints_np[-1]))
controller.update_waypoints(wp_interp)
pass
if local_waypoints != None and local_waypoints != []:
controller.update_values(current_x, current_y, current_yaw,
current_speed,
current_timestamp, frame)
controller.update_controls()
cmd_throttle, cmd_steer, cmd_brake = controller.get_commands()
else:
cmd_throttle = 0.0
cmd_steer = 0.0
cmd_brake = 0.0
if skip_first_frame and frame == 0:
pass
elif local_waypoints == None:
pass
else:
## if i % 5 == 0:
wp_interp_np = np.array(wp_interp)
path_indices = np.floor(np.linspace(0,
wp_interp_np.shape[0]-1,
INTERP_MAX_POINTS_PLOT))
#if len(agent.get_local_planner().waypoints_queue) < num_min_waypoints and args.loop:
# agent.reroute(spawn_points)
# tot_target_reached += 1
# world.hud.notification("The target has been reached " +
# str(tot_target_reached) + " times.", seconds=4.0)
#elif len(agent.get_local_planner().waypoints_queue) == 0 and not args.loop:
# print("Target reached, mission accomplished...")
# break
#speed_limit = world.player.get_speed_limit()
#agent.get_local_planner().set_speed(speed_limit)
#cmd_throttle = 0.4
#cmd_steer = 0
#cmd_brake = 0
dist_to_last_waypoint = np.linalg.norm(np.array([
waypoints[-1][0] - current_x,
waypoints[-1][1] - current_y]))
if dist_to_last_waypoint < DIST_THRESHOLD_TO_LAST_WAYPOINT:
reached_the_end = True
if reached_the_end:
cmd_steer = np.fmax(np.fmin(0, 1.0), -1.0)
cmd_throttle = np.fmax(np.fmin(0, 1.0), 0)
cmd_brake = np.fmax(np.fmin(0.1, 1.0), 0)
break
cmd_steer = np.fmax(np.fmin(cmd_steer, 1.0), -1.0)
cmd_throttle = np.fmax(np.fmin(cmd_throttle, 1.0), 0)
cmd_brake = np.fmax(np.fmin(cmd_brake, 1.0), 0)
world.player.apply_control(carla.VehicleControl(throttle=cmd_throttle,
steer=cmd_steer,
brake=cmd_brake))
if reached_the_end:
print("Reached the end of path. Writing to controller_output...")
else:
print("Exceeded assessment time. Writing to controller_output...")
finally:
if world is not None:
world.destroy()
pygame.quit()
def setup_scenario(world,
client,
synchronous_master=False,
subject_behavior="normal"):
# @todo cannot import these directly.
SpawnActor = carla.command.SpawnActor
SetAutopilot = carla.command.SetAutopilot
FutureActor = carla.command.FutureActor
batch = []
ego_batch = []
vehicles_list = []
blueprints = world.get_blueprint_library()
blueprints = [x for x in blueprints if x.id.endswith("model3")]
all_waypoints = world.get_map().generate_waypoints(3)
# waypoint_list = filter_waypoints(road_id, lane_id)
left_most_lane = filter_waypoints(all_waypoints, 15, -3)
middle_lane = filter_waypoints(all_waypoints, 15, -5)
right_lane = filter_waypoints(all_waypoints, 15, -6)
sub_spawn_point = middle_lane[1].transform
manual_spawn_point = left_most_lane[1].transform
ego_spawn_point = right_lane[1].transform
sub_spawn_point = carla.Transform(
Location(x=sub_spawn_point.location.x,
y=sub_spawn_point.location.y,
z=0.5),
Rotation(yaw=sub_spawn_point.rotation.yaw),
)
ego_spawn_point = carla.Transform(
Location(x=ego_spawn_point.location.x,
y=ego_spawn_point.location.y,
z=0.5),
Rotation(yaw=ego_spawn_point.rotation.yaw),
)
manual_spawn_point = carla.Transform(
Location(x=manual_spawn_point.location.x,
y=manual_spawn_point.location.y,
z=0.5),
Rotation(yaw=manual_spawn_point.rotation.yaw),
)
# --------------
# Spawn vehicles
# --------------
# Manual Vehicle
blueprint = random.choice(blueprints)
color = "0,255,0"
blueprint.set_attribute("color", color)
blueprint.set_attribute("role_name", "hero")
batch.append(
SpawnActor(blueprint,
manual_spawn_point).then(SetAutopilot(FutureActor, False)))
# Subject Vehicle Details
blueprint = random.choice(blueprints)
color = "0,0,255"
blueprint.set_attribute("color", color)
blueprint.set_attribute("role_name", "autopilot")
batch.append(
SpawnActor(blueprint,
sub_spawn_point).then(SetAutopilot(FutureActor, True)))
# Ego Vehicle Details
color = "255,0,0"
blueprint.set_attribute("color", color)
blueprint.set_attribute("role_name", "ego")
ego_batch.append(
SpawnActor(blueprint,
ego_spawn_point).then(SetAutopilot(FutureActor, True)))
# Spawn
ego_vehicle_id = None
for response in client.apply_batch_sync(ego_batch, synchronous_master):
if response.error:
print("Response Error while applying ego batch!")
else:
# self.vehicles_list.append(response.actor_id)
ego_vehicle_id = response.actor_id
print("Ego vehicle id: ------------------------------", ego_vehicle_id)
for response in client.apply_batch_sync(batch, synchronous_master):
if response.error:
print("Response Error while applying batch!")
else:
vehicles_list.append(response.actor_id)
manual_vehicle = world.get_actors(vehicles_list)[0]
subject_vehicle = world.get_actors(vehicles_list)[1]
ego_vehicle = world.get_actors([ego_vehicle_id])[0]
print("Warm start initiated...")
warm_start_curr = 0
update_spectator(world, ego_vehicle)
while warm_start_curr < 3:
warm_start_curr += world.get_settings().fixed_delta_seconds
if synchronous_master:
world.tick()
else:
world.wait_for_tick()
client.apply_batch_sync([SetAutopilot(ego_vehicle, False)],
synchronous_master)
client.apply_batch_sync([SetAutopilot(subject_vehicle, False)],
synchronous_master)
if subject_behavior == "manual":
manual_agent = Agent(manual_vehicle)
else:
subject_agent = BehaviorAgent(subject_vehicle,
behavior=subject_behavior)
destination = carla.Location(x=240.50791931152344,
y=45.247249603271484,
z=0.0)
subject_agent.set_destination(subject_agent.vehicle.get_location(),
destination,
clean=True)
subject_agent.update_information(world)
print("Warm start finished...")
## Get current lane waypoints
ego_vehicle_location = ego_vehicle.get_location()
nearest_waypoint = world.get_map().get_waypoint(ego_vehicle_location,
project_to_road=True)
current_lane_waypoints = nearest_waypoint.next_until_lane_end(1)
if subject_behavior == "manual":
return (ego_vehicle, manual_vehicle, current_lane_waypoints,
manual_agent)
else:
return (ego_vehicle, subject_vehicle, current_lane_waypoints,
subject_agent)
class World(object):
""" Class representing the simulation environment. """
def __init__(self,
carla_world: carla.World,
traffic_manager: carla.TrafficManager,
config: dict,
spawn_point: carla.Transform = None):
"""
Constructor method.
If spawn_point not given, choose random spawn point recommended by the map.
"""
self.carla_world = carla_world
self.tm = traffic_manager
self.spectator = carla_world.get_spectator()
try:
self.map = self.carla_world.get_map()
except RuntimeError as error:
print('RuntimeError: {}'.format(error))
print(' The server could not send the OpenDRIVE (.xodr) file:')
print(
' Make sure it exists, has the same name of your town, and is correct.'
)
sys.exit(1)
self._weather_presets = find_weather_presets()
self._weather_index = config['world']['weather']
self.carla_world.set_weather(
self._weather_presets[self._weather_index][0])
self.ego_veh = None
# Containers for managing carla sensors
self.carla_sensors = {}
# This dict will store references to all sensor's data container.
# It is to facilitate the recording, so the recorder only needs to query this one-stop container.
# When a CarlaSensor is added via add_carla_sensor(), its data container is registered automatically.
# When sensor data are updated, the content in this dict is updated automatically since they are just pointers.
self.all_sensor_data = {}
# Ground truth extractor
self.ground_truth = None
# Start simuation
self.restart(config, spawn_point)
# Tick the world to bring the ego vehicle actor into effect
self.carla_world.tick()
# Placeholder for the behavior agent
# See set_behavior_agent() method.
self._behavior_agent = None
# Placeholder for goals of behavior agent
self._agent_goals = None
def restart(self, config, spawn_point=None):
"""
Start the simulation with the configuration arguments.
It spawns the actors including ego vehicle and sensors. If the ego vehicle exists already,
it respawns the vehicle either at the same location or at the designated location.
"""
# Set up carla engine using config
settings = self.carla_world.get_settings()
settings.no_rendering_mode = config['world']['no_rendering']
settings.synchronous_mode = config['world']['sync_mode']
settings.fixed_delta_seconds = config['world']['delta_seconds']
self.carla_world.apply_settings(settings)
# Spawn a Mustang as the ego vehicle (not stolen from John Wick, don't panic)
ego_veh_bp = self.carla_world.get_blueprint_library().find(
'vehicle.mustang.mustang')
if self.ego_veh:
if spawn_point is None:
print("Respawning ego vehicle.")
spawn_point = self.ego_veh.get_transform()
else:
print("Respawning ego vehicle at assigned point.")
# Destroy previously spawned actors
self.destroy()
spawn_point.location.z += 2.0
spawn_point.rotation.roll = 0.0
spawn_point.rotation.pitch = 0.0
self.ego_veh = self.carla_world.try_spawn_actor(
ego_veh_bp, spawn_point)
if self.ego_veh is None:
print('Chosen spawn point failed.')
else:
if spawn_point:
print("Spawning new ego vehicle at assigned point.")
spawn_point.location.z += 2.0
self.ego_veh = self.carla_world.try_spawn_actor(
ego_veh_bp, spawn_point)
while self.ego_veh is None:
if not self.map.get_spawn_points():
print('There are no spawn points available in your map/town.')
sys.exit(1)
print("Spawning new ego vehicle at a random point.")
spawn_points = self.map.get_spawn_points()
spawn_point = random.choice(
spawn_points) if spawn_points else carla.Transform()
self.ego_veh = self.carla_world.try_spawn_actor(
ego_veh_bp, spawn_point)
# Point the spectator to the ego vehicle
self.see_ego_veh()
# Ground truth extractor
debug = False
if __debug__:
debug = True
self.ground_truth = GroundTruthExtractor(self.ego_veh, config['gt'],
debug)
def add_carla_sensor(self, carla_sensor: CarlaSensor):
"""
Add a CarlaSensor.
This sensor will be added to the carla_sensors list, and all_sensor_data will add a new key-value pair,
where the key is the same as the carla_sensor's name and the value is the reference to carla_sensor's data.
"""
if carla_sensor.name in self.carla_sensors.keys():
raise RuntimeError(
'Trying to add a CarlaSensor with a duplicate name.')
# Add the CarlaSensor
self.carla_sensors[carla_sensor.name] = carla_sensor
# Register the CarlaSensor's data to all_sensor_data
self.all_sensor_data[carla_sensor.name] = carla_sensor.data
def set_ego_autopilot(self, active, autopilot_config=None):
"""
Set traffic manager and register ego vehicle to it.
This makes use of the traffic manager provided by Carla to control the ego vehicle.
See https://carla.readthedocs.io/en/latest/adv_traffic_manager/ for more info.
"""
if autopilot_config:
self.tm.auto_lane_change(self.ego_veh,
autopilot_config['auto_lane_change'])
self.tm.ignore_lights_percentage(
self.ego_veh, autopilot_config['ignore_lights_percentage'])
self.tm.vehicle_percentage_speed_difference(
self.ego_veh,
autopilot_config['vehicle_percentage_speed_difference'])
self.ego_veh.set_autopilot(active, self.tm.get_port())
def force_lane_change(self, to_left):
"""
Force ego vehicle to change the lane regardless collision with other vehicles.
It only allows lane changes in the possible direction.
Carla's traffic manager doesn't seem to always respect this command.
Input:
to_left: boolean to indicate the direction of lane change.
"""
# carla uses true for right
self.tm.force_lane_change(self.ego_veh, not to_left)
def set_behavior_agent(self, agent_config):
"""
Set up a BehaviorAgent object to control the ego vehicle to follow a set of waypoints.
Warning: BehaviorAgent class directly copied from Carla's repository under PythonAPI/carla.
It is used in several carla example codes to control the car. However the whole agent package
is not officially documented and the its use here is based on the examination of the example codes.
Input:
agent_config: dict storing configurations for behavior agent.
"""
self._behavior_agent = BehaviorAgent(
self.ego_veh, agent_config['ignore_traffic_light'],
agent_config['behavior'])
self._agent_goals = deque()
for goal in agent_config['goals']:
self._agent_goals.append(
carla.Location(goal['x'], goal['y'], goal['z']))
# Set the first goal
self._behavior_agent.set_destination(self.ego_veh.get_location(),
self._agent_goals.popleft())
def run_agent_step(self):
"""
Run the behavior agent at the current step.
It first checks if it should add the next goal into the internal waypoints to follow, then
update and apply the control signal.
Output:
bool to indicate if should keep running.
"""
# Set a new goal if about to reach the current goal
if len(self._behavior_agent.get_local_planner().waypoints_queue
) < 21 and self._agent_goals:
new_start = self._behavior_agent.get_local_planner(
).waypoints_queue[-1][0].transform.location
new_goal = self._agent_goals.popleft()
self._behavior_agent.set_destination(new_start,
new_goal,
clean=False)
print("New target: {}".format(new_goal))
self._behavior_agent.update_information(self.ego_veh)
# If debug set to True, imminent waypoints will be drawn
debug = False
if __debug__:
debug = True
control, keep_running = self._behavior_agent.run_step(debug=debug)
self.ego_veh.apply_control(control)
return keep_running
def step_forward(self):
"""
Tick carla world to take simulation one step forward.
Output:
bool to indicate if should keep running.
"""
keep_running = True
self.carla_world.tick()
# Run behavior agent if used
if self._behavior_agent:
keep_running = self.run_agent_step()
# Update CarlaSensors' data
for carla_sensor in self.carla_sensors.values():
carla_sensor.update()
self.ground_truth.update()
return keep_running
def see_ego_veh(self, following_dist=5, height=5, tilt_ang=-30):
""" Aim the spectator down to the ego vehicle. """
spect_location = carla.Location(x=-following_dist)
self.ego_veh.get_transform().transform(
spect_location) # it modifies passed-in location
ego_rotation = self.ego_veh.get_transform().rotation
self.spectator.set_transform(
carla.Transform(
spect_location + carla.Location(z=height),
carla.Rotation(pitch=tilt_ang, yaw=ego_rotation.yaw)))
def allow_free_run(self):
""" Allow carla engine to run asynchronously and freely. """
settings = self.carla_world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = 0.0
self.carla_world.apply_settings(settings)
def destroy(self):
""" Destroy spawned actors in carla world. """
if self.ego_veh:
print("Destroying the ego vehicle.")
self.ego_veh.destroy()
self.ego_veh = None
for carla_sensor in self.carla_sensors.values():
carla_sensor.destroy()
self.carla_sensors.clear()
def main():
global global_nav, global_control, global_pos, global_vel, global_acceleration, global_angular_velocity
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
#world = client.get_world()
world = client.load_world('Town01')
"""
weather = carla.WeatherParameters(
cloudiness=random.randint(0,80),
precipitation=0,
sun_altitude_angle=random.randint(40,90)
)
set_weather(world, weather)
"""
world.set_weather(carla.WeatherParameters.MidRainSunset)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
_world = World(vehicle, world)
sensor_dict = {
'camera': {
'transform': carla.Transform(
carla.Location(x=0.5, y=0.0, z=2.5)
), #'transform':carla.Transform(carla.Location(x=-3.0, y=0.0, z=2.5), carla.Rotation(pitch=-20)),
'callback': 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()
time.sleep(0.3)
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=MAX_SPEED)
agent = BehaviorAgent(vehicle,
ignore_traffic_light=True,
behavior='normal')
_destination = carla.Transform()
destination = world.get_random_location_from_navigation()
_destination.location = destination
#port = 8000
#tm = client.get_trafficmanager(port)
#tm.ignore_lights_percentage(vehicle,100)
#destination = get_random_destination(spawn_points)
plan_map = replan2(agent, _destination, copy.deepcopy(origin_map))
#agent.set_destination(agent.vehicle.get_location(), destination, clean=True)
#for cnt in tqdm(range(args.num)):
for cnt in range(args.num):
if close2dest(vehicle, _destination):
_destination = carla.Transform()
destination = world.get_random_location_from_navigation()
_destination.location = destination
plan_map = replan2(agent, _destination, copy.deepcopy(origin_map))
#agent.set_destination(agent.vehicle.get_location(), destination, clean=True)
if vehicle.is_at_traffic_light():
traffic_light = vehicle.get_traffic_light()
if traffic_light.get_state() == carla.TrafficLightState.Red:
traffic_light.set_state(carla.TrafficLightState.Green)
agent.update_information(_world)
speed_limit = vehicle.get_speed_limit()
agent.get_local_planner().set_speed(speed_limit)
control = agent.run_step()
control.manual_gear_shift = False
global_control = control
vehicle.apply_control(control)
nav = get_nav(vehicle, plan_map)
global_nav = nav
global_pos = vehicle.get_transform()
global_vel = vehicle.get_velocity()
global_acceleration = vehicle.get_acceleration()
global_angular_velocity = vehicle.get_angular_velocity()
#yaw = global_pos.rotation.yaw
#_ax = global_acceleration.x
#_ay = global_acceleration.y
#ax = _ax*np.cos(yaw) + _ay*np.sin(yaw)
#ay = _ay*np.cos(yaw) - _ax*np.sin(yaw)
#cv2.imshow('Nav', nav)
cv2.imshow('Vision', global_img)
cv2.waitKey(10)
index = str(time.time())
#save_data(index)
cmd_file.close()
pos_file.close()
vel_file.close()
acc_file.close()
angular_vel_file.close()
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()