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(
'--data_dir',
metavar='D',
default='lidar_output_def',
help='Directory to save lidar data')
argparser.add_argument(
'-p', '--port',
metavar='P',
default=2000,
type=int,
help='TCP port to listen to (default: 2000)')
argparser.add_argument(
'--save_lidar_data',
default=False,
action='store_true',
help='To save lidar points or not')
argparser.add_argument(
'--save_gt',
default=False,
action='store_true',
help='To save ground truth or not')
argparser.add_argument(
'--town',
default='Town03',
help='Spawn in Town01, Town02 or Town03'
)
args = argparser.parse_args()
shutil.rmtree(args.data_dir, ignore_errors=True)
Path(args.data_dir + '/lidar').mkdir(parents=True, exist_ok=True)
logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO)
# keyboard = Keyboard(0.05)
client = carla.Client(args.host, args.port)
client.set_timeout(10.0)
client.load_world(args.town)
# Setting synchronous mode
# This is essential for proper workiong of sensors
world = client.get_world()
settings = world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = 0.05 # FPS = 1/0.05 = 20
world.apply_settings(settings)
try:
world = client.get_world()
ego_vehicles = []
ego_cam = None
ego_col = None
ego_lane = None
ego_obs = None
ego_gnss = None
ego_imu = None
# --------------
# Start recording
# --------------
"""
client.start_recorder('~/tutorial/recorder/recording01.log')
"""
# --------------
# Spawn ego vehicle
# --------------
# vehicles_list = custom_spawn(world)
num_vehicles = len(ego_transforms)
ego_bps = []
for i in range(num_vehicles):
ego_bps.append(world.get_blueprint_library().find('vehicle.tesla.model3'))
for i in range(num_vehicles):
ego_bps[i].set_attribute('role_name', 'ego')
print('\nEgo role_names are set')
for i in range(num_vehicles):
ego_color = random.choice(ego_bps[i].get_attribute('color').recommended_values)
ego_bps[i].set_attribute('color', ego_color)
print('\nEgo colors are set')
# spawn_points = world.get_map().get_spawn_points()
# number_of_spawn_points = len(spawn_points)
# spawn num_vehicles vehicles
for i in range(num_vehicles):
ego_vehicles.append(world.spawn_actor(ego_bps[i], ego_transforms[i]))
# --------------
# Add a new LIDAR sensor to my ego
# --------------
# Default
# lidar_cam = None
# lidar_bp = world.get_blueprint_library().find('sensor.lidar.ray_cast')
# lidar_bp.set_attribute('channels',str(32))
# lidar_bp.set_attribute('points_per_second',str(90000))
# lidar_bp.set_attribute('rotation_frequency',str(40))
# lidar_bp.set_attribute('range',str(20))
# lidar_location = carla.Location(0,0,2)
# lidar_rotation = carla.Rotation(0,0,0)
# lidar_transform = carla.Transform(lidar_location,lidar_rotation)
# lidar_sen = world.spawn_actor(lidar_bp,lidar_transform,attach_to=ego_vehicle)
# lidar_sen.listen(lambda point_cloud: process_point_cloud(point_cloud, args.save_lidar_data))
# VLP 16
lidar_cam = None
lidar_bp = world.get_blueprint_library().find('sensor.lidar.ray_cast')
lidar_bp.set_attribute('channels',str(16))
lidar_bp.set_attribute('rotation_frequency',str(20)) # Set the fps of simulator same as this
lidar_bp.set_attribute('range',str(50))
lidar_bp.set_attribute('lower_fov', str(-15))
lidar_bp.set_attribute('upper_fov', str(15))
lidar_bp.set_attribute('points_per_second',str(300000))
lidar_bp.set_attribute('dropoff_general_rate',str(0.0))
# lidar_bp.set_attribute('noise_stddev',str(0.173))
# lidar_bp.set_attribute('noise_stddev',str(0.141)) Works in this case
lidar_location = carla.Location(0,0,2)
lidar_rotation = carla.Rotation(0,0,0)
lidar_transform = carla.Transform(lidar_location,lidar_rotation)
lidar_sen = world.spawn_actor(lidar_bp,lidar_transform,attach_to=ego_vehicles[0])
lidar_sen.listen(lambda point_cloud: process_point_cloud(args, point_cloud, args.save_lidar_data))
# --------------
# Enable autopilot for ego vehicle
# --------------
for i in range(num_vehicles):
ego_vehicles[i].set_autopilot(False)
# --------------
# Dummy Actor for spectator
# --------------
dummy_bp = world.get_blueprint_library().find('sensor.camera.rgb')
dummy_transform = carla.Transform(carla.Location(x=-6, z=4), carla.Rotation(pitch=10.0))
dummy = world.spawn_actor(dummy_bp, dummy_transform, attach_to=ego_vehicles[0], attachment_type=carla.AttachmentType.SpringArm)
dummy.listen(lambda image: dummy_function(image))
spectator = world.get_spectator()
spectator.set_transform(dummy.get_transform())
gt_array = []
# --------------
# Game loop. Prevents the script from finishing.
# --------------
count = -1
while True:
# This is for async mode
# world_snapshot = world.wait_for_tick()
# In synchronous mode, the client ticks the world
world.tick()
count+= 1
if count == 0:
start_tf = ego_vehicles[0].get_transform()
start_tf.rotation.yaw -= 3
print('Start TF: ', start_tf)
# T_start = np.array(start_tf.get_inverse_matrix()) Can be used?
# print(start_tf.transform(start_tf.location))
bias_tf = start_tf.transform(ego_vehicles[0].get_transform().location)
[print('Ego Vehicle ID is: ', ego_vehicles[i].id) for i in range(num_vehicles)]
input('\nPress Enter to Continue:')
# print(start_tf.transform(ego_vehicle.get_transform().location))
spectator.set_transform(dummy.get_transform())
if args.save_gt:
vehicle_tf = ego_vehicles[0].get_transform()
vehicle_tf_odom = start_tf.transform(vehicle_tf.location) - bias_tf
vehicle_tf_odom = np.array([vehicle_tf_odom.x, vehicle_tf_odom.y, vehicle_tf_odom.z])
gt_array.append(vehicle_tf_odom)
except Exception as e:
print(e)
finally:
if args.save_gt:
gt_array = np.array(gt_array)
np.savetxt( args.data_dir + '/gt.csv', gt_array, delimiter=',')
# --------------
# Stop recording and destroy actors
# --------------
client.stop_recorder()
if ego_vehicles is not None:
if ego_cam is not None:
ego_cam.stop()
ego_cam.destroy()
if ego_col is not None:
ego_col.stop()
ego_col.destroy()
if ego_lane is not None:
ego_lane.stop()
ego_lane.destroy()
if ego_obs is not None:
ego_obs.stop()
ego_obs.destroy()
if ego_gnss is not None:
ego_gnss.stop()
ego_gnss.destroy()
if ego_imu is not None:
ego_imu.stop()
ego_imu.destroy()
if lidar_sen is not None:
lidar_sen.stop()
lidar_sen.destroy()
if dummy is not None:
dummy.stop()
dummy.destroy()
[ego_vehicles[i].destroy() for i in range(num_vehicles)]
def main_test(self):
restart_time = time.time()
PATH = "/home/a/RL_decision/trained_info.tar"
print(torch.cuda.get_device_name())
clock = pygame.time.Clock()
Keyboardcontrol = KeyboardControl(self, False)
display = pygame.display.set_mode((self.width, self.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
self.lane_start_point = [
carla.Location(x=14.905815, y=-135.747452, z=0.000000),
carla.Location(x=172.745468, y=-364.531799, z=0.000000),
carla.Location(x=382.441040, y=-212.488907, z=0.000000)
]
self.lane_finished_point = [
carla.Location(x=14.631096, y=-205.746918, z=0.000000),
carla.Location(x=232.962860, y=-364.149139, z=0.000000),
carla.Location(x=376.542816, y=-10.352980, z=0.000000)
]
self.lane_change_point = [
carla.Location(x=14.905815, y=-135.747452, z=0.000000),
carla.Location(x=14.631096, y=-205.746918, z=0.000000),
carla.Location(x=172.745468, y=-364.531799, z=0.000000),
carla.Location(x=232.962860, y=-364.149139, z=0.000000),
carla.Location(x=382.441040, y=-212.488907, z=0.000000),
carla.Location(x=376.542816, y=-10.352980, z=0.000000)
]
self.world.debug.draw_string(carla.Location(x=14.905815,
y=-135.747452,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
self.world.debug.draw_string(carla.Location(x=14.631096,
y=-205.746918,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
# ---------------------------#
self.world.debug.draw_string(carla.Location(x=172.745468,
y=-364.531799,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
self.world.debug.draw_string(carla.Location(x=232.962860,
y=-364.149139,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
# ---------------------------#
self.world.debug.draw_string(carla.Location(x=382.441040,
y=-212.488907,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
self.world.debug.draw_string(carla.Location(x=376.542816,
y=-10.352980,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
lane_distance_between_points = []
for i in range(len(self.lane_finished_point)):
lane_distance_between_points.append(
((self.lane_start_point[i].x - self.lane_finished_point[i].x)**
2 + (self.lane_start_point[i].y -
self.lane_finished_point[i].y)**2 +
(self.lane_start_point[i].z - self.lane_finished_point[i].z)**
2)**0.5)
pre_decision = None
while True:
# if time.time()-restart_time > 10:
# print("restart")
# self.restart()
if Keyboardcontrol.parse_events(client, self, clock):
return
self.spectator.set_transform(
carla.Transform(
self.player.get_transform().location +
carla.Location(z=50), carla.Rotation(pitch=-90)))
self.camera_rgb.render(display)
self.hud.render(display)
pygame.display.flip()
## Get max lane ##
# print("start get lane")
if self.section < len(lane_distance_between_points):
self.lane_distance_between_start = (
(self.player.get_transform().location.x -
self.lane_start_point[self.section].x)**2 +
(self.player.get_transform().location.y -
self.lane_start_point[self.section].y)**2)**0.5
self.lane_distance_between_end = (
(self.player.get_transform().location.x -
self.lane_finished_point[self.section].x)**2 +
(self.player.get_transform().location.y -
self.lane_finished_point[self.section].y)**2)**0.5
# print("self.lane_distance_between_start : ",self.lane_distance_between_start,"self.lane_distance_between_end :",self.lane_distance_between_end, "lane_distance_between_points[section]",lane_distance_between_points[self.section],"section :", self.section)
if max(lane_distance_between_points[self.section], self.lane_distance_between_start, self.lane_distance_between_end) == \
lane_distance_between_points[self.section]:
self.max_Lane_num = 3
# world.debug.draw_string(self.player.get_transform().location, 'o', draw_shadow = True,
# color = carla.Color(r=255, g=255, b=0), life_time = 999)
elif max(lane_distance_between_points[self.section], self.lane_distance_between_start, self.lane_distance_between_end) == \
self.lane_distance_between_start and self.max_Lane_num ==3:
self.section += 1
self.max_Lane_num = 4
if self.section >= len(lane_distance_between_points
): # when, section_num = 3
self.section = 0
## finished get max lane ##
# print("finished get lane")
self.search_distance_valid()
# print("distance :" ,,"max_lane_num : ", self.max_Lane_num)
if self.max_Lane_num == 3:
self.world.debug.draw_string(
self.player.get_transform().location,
'o',
draw_shadow=True,
color=carla.Color(r=255, g=255, b=0),
life_time=9999)
if time.time() - self.lane_change_time > 10:
self.lane_change_time = time.time()
if pre_decision is None:
self.decision = 1
self.ego_Lane += 1
pre_decision = 1
elif pre_decision == 1:
pre_decision = -1
self.ego_Lane += -1
self.decision = -1
elif pre_decision == -1:
self.decision = 1
self.ego_Lane += 1
pre_decision = 1
else:
self.decision = 0
self.controller.apply_control(self.decision)
# self.world.wait_for_tick(10.0)
clock.tick(30)
self.hud.tick(self, clock)
def setup_sensors(self, vehicle, debug_mode=False):
"""
Create the sensors defined by the user and attach them to the ego-vehicle
:param vehicle: ego vehicle
:return:
"""
bp_library = CarlaDataProvider.get_world().get_blueprint_library()
for sensor_spec in self._agent.sensors():
# These are the pseudosensors (not spawned)
if sensor_spec['type'].startswith('sensor.opendrive_map'):
# The HDMap pseudo sensor is created directly here
sensor = OpenDriveMapReader(vehicle, sensor_spec['reading_frequency'])
elif sensor_spec['type'].startswith('sensor.speedometer'):
delta_time = CarlaDataProvider.get_world().get_settings().fixed_delta_seconds
frame_rate = 1 / delta_time
sensor = SpeedometerReader(vehicle, frame_rate)
# These are the sensors spawned on the carla world
else:
bp = bp_library.find(str(sensor_spec['type']))
if sensor_spec['type'].startswith('sensor.camera'):
bp.set_attribute('image_size_x', str(sensor_spec['width']))
bp.set_attribute('image_size_y', str(sensor_spec['height']))
bp.set_attribute('fov', str(sensor_spec['fov']))
bp.set_attribute('lens_circle_multiplier', str(3.0))
bp.set_attribute('lens_circle_falloff', str(3.0))
bp.set_attribute('chromatic_aberration_intensity', str(0.5))
bp.set_attribute('chromatic_aberration_offset', str(0))
# Change __call__ in autonomous agent if you change sensor_tick.
bp.set_attribute('sensor_tick', '0.05')
sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
elif sensor_spec['type'].startswith('sensor.lidar'):
bp.set_attribute('range', str(85))
bp.set_attribute('rotation_frequency', str(20))
bp.set_attribute('channels', str(64))
bp.set_attribute('upper_fov', str(10))
bp.set_attribute('lower_fov', str(-30))
bp.set_attribute('points_per_second', str(600000))
# bp.set_attribute('atmosphere_attenuation_rate', str(0.004))
# bp.set_attribute('dropoff_general_rate', str(0.45))
# bp.set_attribute('dropoff_intensity_limit', str(0.8))
# bp.set_attribute('dropoff_zero_intensity', str(0.4))
bp.set_attribute('sensor_tick', '0.05')
sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
elif sensor_spec['type'].startswith('sensor.other.radar'):
bp.set_attribute('horizontal_fov', str(sensor_spec['fov'])) # degrees
bp.set_attribute('vertical_fov', str(sensor_spec['fov'])) # degrees
bp.set_attribute('points_per_second', '1500')
bp.set_attribute('range', '100') # meters
sensor_location = carla.Location(x=sensor_spec['x'],
y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
elif sensor_spec['type'].startswith('sensor.other.gnss'):
bp.set_attribute('noise_alt_stddev', str(0.000005))
bp.set_attribute('noise_lat_stddev', str(0.000005))
bp.set_attribute('noise_lon_stddev', str(0.000005))
bp.set_attribute('noise_alt_bias', str(0.0))
bp.set_attribute('noise_lat_bias', str(0.0))
bp.set_attribute('noise_lon_bias', str(0.0))
sensor_location = carla.Location(x=sensor_spec['x'],
y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation()
elif sensor_spec['type'].startswith('sensor.other.imu'):
bp.set_attribute('noise_accel_stddev_x', str(0.001))
bp.set_attribute('noise_accel_stddev_y', str(0.001))
bp.set_attribute('noise_accel_stddev_z', str(0.015))
bp.set_attribute('noise_gyro_stddev_x', str(0.001))
bp.set_attribute('noise_gyro_stddev_y', str(0.001))
bp.set_attribute('noise_gyro_stddev_z', str(0.001))
sensor_location = carla.Location(x=sensor_spec['x'],
y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
# create sensor
sensor_transform = carla.Transform(sensor_location, sensor_rotation)
sensor = CarlaDataProvider.get_world().spawn_actor(bp, sensor_transform, vehicle)
# setup callback
sensor.listen(CallBack(sensor_spec['id'], sensor_spec['type'], sensor, self._agent.sensor_interface))
self._sensors_list.append(sensor)
# Tick once to spawn the sensors
CarlaDataProvider.get_world().tick()
def __init__(self, parent_actor, width, height, gap_x, diplace_y):
self.save_counter = 0
self.width = width
self.height = height
self.sensor = None
self._surface = None
self._parent = parent_actor
# self._hud = hud
self._recording = False
self.timedata = time.strftime("%Y-%d-%I-%M-%S",
time.localtime(time.time()))
timedata = self.timedata
self.dirpath = "datacollected_{}".format(timedata)
self.cur_dir = os.path.dirname(os.path.realpath(__file__))
self.save_dir = self.cur_dir + "/" + self.dirpath
if not os.path.exists(self.save_dir):
os.makedirs(self.save_dir)
self._camera_transforms = [
carla.Transform(carla.Location(x=0, z=2.8)),
carla.Transform(carla.Location(x=0, z=2.8),
carla.Rotation(yaw=90)),
carla.Transform(carla.Location(x=0, z=2.8),
carla.Rotation(yaw=180)),
carla.Transform(carla.Location(x=0, z=2.8),
carla.Rotation(yaw=270))
]
self._transform_index = 0
self._sensors = [
# ['sensor.camera.rgb', cc.Raw, 'Camera RGB'],
['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)']
# ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'],
# ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'],
# ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'],
# ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 'Camera Semantic Segmentation (CityScapes Palette)'],
# ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']
]
self.fov = 90.0
world = self._parent.get_world()
bp_library = world.get_blueprint_library()
print("setting image")
for item in self._sensors:
bp = bp_library.find(item[0])
if item[0].startswith('sensor.camera'):
bp.set_attribute('image_size_x', str(self.width))
bp.set_attribute('image_size_y', str(self.height))
bp.set_attribute('fov', str(self.fov))
bp.set_attribute('image_size_x', '500')
bp.set_attribute('image_size_y', '420')
print("set image six")
elif item[0].startswith('sensor.lidar'):
bp.set_attribute('range', '5000')
# bp.set_attribute('rotation_frequency', '20')
bp.set_attribute('upper_fov', '15')
bp.set_attribute('lower_fov', '-25')
bp.set_attribute('channels', '64')
bp.set_attribute('points_per_second', '1000000')
item.append(bp)
self.surface_shift_x = gap_x
self.surface_shift_y = diplace_y
self.lidar_points = None
self.depth_image = None
def convert_position_to_transform(position, actor_list=None):
"""
Convert an OpenScenario position into a CARLA transform
Not supported: Road, RelativeRoad, Lane, RelativeLane as the PythonAPI currently
does not provide sufficient access to OpenDrive information
Also not supported is Route. This can be added by checking additional
route information
"""
if position.find('World') is not None:
world_pos = position.find('World')
x = float(world_pos.attrib.get('x', 0))
y = float(world_pos.attrib.get('y', 0))
z = float(world_pos.attrib.get('z', 0))
yaw = math.degrees(float(world_pos.attrib.get('h', 0)))
pitch = math.degrees(float(world_pos.attrib.get('p', 0)))
roll = math.degrees(float(world_pos.attrib.get('r', 0)))
if not OpenScenarioParser.use_carla_coordinate_system:
y = y * (-1.0)
yaw = yaw * (-1.0)
return carla.Transform(
carla.Location(x=x, y=y, z=z),
carla.Rotation(yaw=yaw, pitch=pitch, roll=roll))
elif ((position.find('RelativeWorld') is not None)
or (position.find('RelativeObject') is not None)
or (position.find('RelativeLane') is not None)):
rel_pos = position.find('RelativeWorld') or position.find(
'RelativeObject') or position.find('RelativeLane')
# get relative object and relative position
obj = rel_pos.attrib.get('object')
obj_actor = None
actor_transform = None
if actor_list is not None:
for actor in actor_list:
if actor.rolename == obj:
obj_actor = actor
actor_transform = actor.transform
else:
for actor in CarlaDataProvider.get_world().get_actors():
if 'role_name' in actor.attributes and actor.attributes[
'role_name'] == obj:
obj_actor = actor
actor_transform = obj_actor.get_transform()
break
if obj_actor is None:
raise AttributeError(
"Object '{}' provided as position reference is not known".
format(obj))
# calculate orientation h, p, r
is_absolute = False
if rel_pos.find('Orientation') is not None:
orientation = rel_pos.find('Orientation')
is_absolute = (orientation.attrib.get('type') == "absolute")
dyaw = math.degrees(float(orientation.attrib.get('h', 0)))
dpitch = math.degrees(float(orientation.attrib.get('p', 0)))
droll = math.degrees(float(orientation.attrib.get('r', 0)))
if not OpenScenarioParser.use_carla_coordinate_system:
dyaw = dyaw * (-1.0)
yaw = actor_transform.rotation.yaw
pitch = actor_transform.rotation.pitch
roll = actor_transform.rotation.roll
if not is_absolute:
yaw = yaw + dyaw
pitch = pitch + dpitch
roll = roll + droll
else:
yaw = dyaw
pitch = dpitch
roll = droll
# calculate location x, y, z
# dx, dy, dz
if (position.find('RelativeWorld')
is not None) or (position.find('RelativeObject')
is not None):
dx = float(rel_pos.attrib.get('dx', 0))
dy = float(rel_pos.attrib.get('dy', 0))
dz = float(rel_pos.attrib.get('dz', 0))
if not OpenScenarioParser.use_carla_coordinate_system:
dy = dy * (-1.0)
x = actor_transform.location.x + dx
y = actor_transform.location.y + dy
z = actor_transform.location.z + dz
# dLane, ds, offset
elif position.find('RelativeLane') is not None:
dlane = float(rel_pos.attrib.get('dLane'))
ds = float(rel_pos.attrib.get('ds'))
offset = float(rel_pos.attrib.get('offset'))
carla_map = CarlaDataProvider.get_map()
relative_waypoint = carla_map.get_waypoint(
actor_transform.location)
if dlane == 0:
wp = relative_waypoint
elif dlane == -1:
wp = relative_waypoint.get_left_lane()
elif dlane == 1:
wp = relative_waypoint.get_right_lane()
if wp is None:
raise AttributeError(
"Object '{}' position with dLane={} is not valid".
format(obj, dlane))
wp = wp.next(ds)[-1]
# offset
# Verschiebung von Mittelpunkt
h = math.radians(wp.transform.rotation.yaw)
x_offset = math.sin(h) * offset
y_offset = math.cos(h) * offset
if OpenScenarioParser.use_carla_coordinate_system:
x_offset = x_offset * (-1.0)
y_offset = y_offset * (-1.0)
x = wp.transform.location.x + x_offset
y = wp.transform.location.y + y_offset
z = wp.transform.location.z
return carla.Transform(
carla.Location(x=x, y=y, z=z),
carla.Rotation(yaw=yaw, pitch=pitch, roll=roll))
# Not implemented
elif position.find('Road') is not None:
raise NotImplementedError("Road positions are not yet supported")
elif position.find('RelativeRoad') is not None:
raise NotImplementedError(
"RelativeRoad positions are not yet supported")
elif position.find('Lane') is not None:
raise NotImplementedError("Lane positions are not yet supported")
elif position.find('Route') is not None:
raise NotImplementedError("Route positions are not yet supported")
else:
raise AttributeError("Unknown position")
def ndarray_to_rotation(array: np.ndarray) -> carla.Rotation: # pylint: disable=no-member
"""Converts neural network friendly tensor back to `carla.Rotation`."""
return carla.Rotation(*list(map(float, array))) # pylint: disable=no-member
def RPY_to_carla_rotation(roll, pitch, yaw):
return carla.Rotation(roll=math.degrees(roll),
pitch=-math.degrees(pitch),
yaw=-math.degrees(yaw))
client = carla.Client("localhost", 2000)
client.set_timeout(10.0)
world = client.load_world('Town06')
# set the weather
world = client.get_world()
weather = carla.WeatherParameters(cloudiness=10.0,
precipitation=0.0,
sun_altitude_angle=90.0)
world.set_weather(weather)
# set the spectator position for demo purpose
spectator = world.get_spectator()
spectator.set_transform(
carla.Transform(carla.Location(x=-68.29, y=151.75, z=170.8),
carla.Rotation(pitch=-31.07, yaw=-90.868,
roll=1.595))) # plain ground
env = CARLA_ENV(world)
time.sleep(2) # sleep for 2 seconds, wait the initialization to finish
# spawn a vehicle, here I choose a Tesla model
spawn_point = carla.Transform(
carla.Location(x=-277.08, y=-15.39, z=4.94),
carla.Rotation(pitch=0.000000, yaw=0, roll=0.000000))
model_name = "vehicle.tesla.model3"
model_uniquename = env.spawn_vehicle(
model_name, spawn_point
) # spawn the model and get the uniquename, the CARLA_ENV class will store the vehicle into vehicle actor list
time.sleep(5)
'''
# ======================================================================================
try:
client = carla.Client("localhost", 2000)
client.set_timeout(5.0)
world = client.get_world()
blueprint_library = world.get_blueprint_library()
bp = blueprint_library.filter("model3")[0]
print(bp)
# spawn_point = random.choice(world.get_map().get_spawn_points())
vehicle_spawn_point = carla.Transform(carla.Location(x=50, y=5, z=1),
carla.Rotation(yaw=0.0))
# spawn_point = carla.Transform(carla.Location(x=233, y=-12, z=1), carla.Rotation(yaw=-90))
vehicle = world.spawn_actor(bp, vehicle_spawn_point)
actor_list.append(vehicle)
vehicle.set_autopilot(True)
# vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0))
cam_bp = blueprint_library.find("sensor.camera.rgb")
cam_bp.set_attribute("image_size_x", format(IM_WIDTH))
cam_bp.set_attribute("image_size_y", format(IM_HEIGHT))
cam_bp.set_attribute("fov", "90")
fcam_spawn_point = carla.Transform(carla.Location(x=0.9, z=1.5),
carla.Rotation(yaw=0.0))
def scenario(scenario_params, ctrl_params, carla_synchronous, eval_env,
eval_stg):
ego_vehicle = None
agent = None
spectator_camera = None
other_vehicles = []
record_spectator = False
client, world, carla_map, traffic_manager = carla_synchronous
try:
map_reader = NaiveMapQuerier(world, carla_map, debug=True)
online_config = OnlineConfig(record_interval=10,
node_type=eval_env.NodeType)
# Mock vehicles
spawn_points = carla_map.get_spawn_points()
blueprints = get_all_vehicle_blueprints(world)
spawn_indices = [scenario_params.ego_spawn_idx
] + scenario_params.other_spawn_ids
other_vehicle_ids = []
for k, spawn_idx in enumerate(spawn_indices):
if k == 0:
blueprint = world.get_blueprint_library().find(
'vehicle.audi.a2')
else:
blueprint = np.random.choice(blueprints)
spawn_point = spawn_points[spawn_idx]
spawn_point = shift_spawn_point(carla_map, k,
scenario_params.spawn_shifts,
spawn_point)
# Prevent collision with road.
spawn_point.location += carla.Location(0, 0, 0.5)
vehicle = world.spawn_actor(blueprint, spawn_point)
if k == 0:
ego_vehicle = vehicle
else:
vehicle.set_autopilot(True, traffic_manager.get_port())
if scenario_params.ignore_signs:
traffic_manager.ignore_signs_percentage(vehicle, 100.)
if scenario_params.ignore_lights:
traffic_manager.ignore_lights_percentage(vehicle, 100.)
if scenario_params.ignore_vehicles:
traffic_manager.ignore_vehicles_percentage(vehicle, 100.)
if not scenario_params.auto_lane_change:
traffic_manager.auto_lane_change(vehicle, False)
other_vehicles.append(vehicle)
other_vehicle_ids.append(vehicle.id)
frame = world.tick()
agent = MidlevelAgent(
ego_vehicle,
map_reader,
other_vehicle_ids,
eval_stg,
n_burn_interval=scenario_params.n_burn_interval,
control_horizon=ctrl_params.control_horizon,
prediction_horizon=ctrl_params.prediction_horizon,
step_horizon=ctrl_params.step_horizon,
n_predictions=ctrl_params.n_predictions,
scene_builder_cls=TrajectronPlusPlusSceneBuilder,
turn_choices=scenario_params.turn_choices,
max_distance=scenario_params.max_distance,
plot_boundary=False,
log_agent=False,
log_cplex=False,
plot_scenario=True,
plot_simulation=True,
plot_overapprox=False,
scene_config=online_config)
agent.start_sensor()
assert agent.sensor_is_listening
"""Move the spectator to the ego vehicle.
The positioning is a little off"""
state = agent.get_vehicle_state()
goal = agent.get_goal()
goal_x, goal_y = goal.x, -goal.y
if goal.is_relative:
location = carla.Location(x=state[0] + goal_x / 2.,
y=state[1] - goal_y / 2.,
z=state[2] + 50)
else:
location = carla.Location(x=(state[0] + goal_x) / 2.,
y=(state[1] + goal_y) / 2.,
z=state[2] + 50)
# configure the spectator
world.get_spectator().set_transform(
carla.Transform(location,
carla.Rotation(pitch=-70, yaw=-90, roll=20)))
record_spectator = False
if record_spectator:
# attach camera to spectator
spectator_camera = attach_camera_to_spectator(world, frame)
n_burn_frames = scenario_params.n_burn_interval * online_config.record_interval
if ctrl_params.loop_type == LoopEnum.CLOSED_LOOP:
run_frames = scenario_params.run_interval * online_config.record_interval
else:
run_frames = ctrl_params.control_horizon * online_config.record_interval - 1
for idx in range(n_burn_frames + run_frames):
control = None
for ctrl in scenario_params.controls:
if ctrl.interval[0] <= idx and idx <= ctrl.interval[1]:
control = ctrl.control
break
frame = world.tick()
agent.run_step(frame, control=control)
finally:
if spectator_camera:
spectator_camera.destroy()
if agent:
agent.destroy()
if ego_vehicle:
ego_vehicle.destroy()
for other_vehicle in other_vehicles:
other_vehicle.destroy()
if record_spectator == True:
time.sleep(5)
else:
time.sleep(1)
def reset(self):
self.actor_list = []
self.collision_hist = []
self.depth = 50
self.Obdist = 20
self.ObdistL = 15
self.ObdistR = 15
self.azimuth = 0
self.zrate = 0
self.spawn_point = (self.world.get_map().get_spawn_points())[5]
self.vehicle = self.world.spawn_actor(self.bp, self.spawn_point)
self.vehicle.apply_control(
carla.VehicleControl(throttle=0.0, steer=0.0))
time.sleep(1)
#vehicle.set_autopilot(True) # if you just wanted some NPCs to drive.
self.actor_list.append(self.vehicle)
# https://carla.readthedocs.io/en/latest/cameras_and_sensors
# get the blueprint for this sensor
self.blueprint = self.blueprint_library.find('sensor.other.radar')
self.blueprintOD = self.blueprint_library.find('sensor.other.obstacle')
self.blueprintODside = self.blueprint_library.find(
'sensor.other.obstacle')
self.blueprintCD = self.blueprint_library.find(
'sensor.other.collision')
self.blueprintRGB = self.blueprint_library.find('sensor.camera.rgb')
self.blueprintIMU = self.blueprint_library.find('sensor.other.imu')
# change the dimensions of the image
self.blueprintRGB.set_attribute('image_size_x', f'{self.im_width}')
self.blueprintRGB.set_attribute('image_size_y', f'{self.im_height}')
self.blueprintRGB.set_attribute('fov', '110')
self.blueprint.set_attribute('horizontal_fov', '15')
self.blueprint.set_attribute('vertical_fov', '5')
self.blueprint.set_attribute('range', '15')
self.blueprintOD.set_attribute('distance', '40')
self.blueprintOD.set_attribute('debug_linetrace', 'False')
self.blueprintOD.set_attribute('hit_radius', '2.5')
self.blueprintODside.set_attribute('distance', '25')
self.blueprintODside.set_attribute('debug_linetrace', 'False')
self.blueprintODside.set_attribute('hit_radius', '2')
# Adjust sensor relative to vehicle
spawn_point = carla.Transform(carla.Location(x=2.5, z=0.7))
spawn_pointD = carla.Transform(carla.Location(x=2.5, z=4))
spawn_pointL = carla.Transform(carla.Location(x=2.5, z=3),
carla.Rotation(0, -50, 0))
spawn_pointR = carla.Transform(carla.Location(x=2.5, z=3),
carla.Rotation(0, 50, 0))
spawn_pointcam = carla.Transform(carla.Location(x=0, z=2))
# spawn the sensor and attach to vehicle.
#self.sensor = self.world.spawn_actor(self.blueprint, spawn_point, attach_to=self.vehicle)
self.sensorOD = self.world.spawn_actor(self.blueprintOD,
spawn_pointD,
attach_to=self.vehicle)
self.sensorODR = self.world.spawn_actor(self.blueprintODside,
spawn_pointR,
attach_to=self.vehicle)
self.sensorODL = self.world.spawn_actor(self.blueprintODside,
spawn_pointL,
attach_to=self.vehicle)
self.sensorCD = self.world.spawn_actor(self.blueprintCD,
spawn_point,
attach_to=self.vehicle)
self.sensorRGB = self.world.spawn_actor(self.blueprintRGB,
spawn_pointcam,
attach_to=self.vehicle)
self.sensorIMU = self.world.spawn_actor(self.blueprintIMU,
spawn_point,
attach_to=self.vehicle)
# add sensor to list of actors
#self.actor_list.append(self.sensor)
self.actor_list.append(self.sensorOD)
self.actor_list.append(self.sensorODL)
self.actor_list.append(self.sensorODR)
self.actor_list.append(self.sensorCD)
self.actor_list.append(self.sensorRGB)
self.actor_list.append(self.sensorIMU)
# do something with this sensor
#self.sensor.listen(lambda data: self.process_data(data))
self.sensorOD.listen(lambda dataOD: self.register_obstacle(dataOD))
self.sensorODL.listen(lambda dataODL: self.register_obstacleL(dataODL))
self.sensorODR.listen(lambda dataODR: self.register_obstacleR(dataODR))
self.sensorCD.listen(lambda dataCD: self.register_collision(dataCD))
#self.sensorRGB.listen(lambda dataRGB: self.process_img(dataRGB))
self.sensorIMU.listen(lambda dataIMU: self.process_IMU(dataIMU))
class NoSignalJunctionCrossing(BasicScenario):
"""
Implementation class for
'Non-signalized junctions: crossing negotiation' scenario,
Traffic Scenario 10.
Location : Town03
"""
# ego vehicle parameters
_ego_vehicle_model = 'vehicle.lincoln.mkz2017'
_ego_vehicle_start = carla.Transform(
carla.Location(x=-74.32, y=-50, z=0.5), carla.Rotation(yaw=270))
_ego_vehicle_max_velocity = 20
_ego_vehicle_driven_distance = 105
# other vehicle
_other_vehicle_model = 'vehicle.tesla.model3'
_other_vehicle_start = carla.Transform(
carla.Location(x=-105, y=-136, z=0.5), carla.Rotation(yaw=0))
_other_vehicle_max_brake = 1.0
_other_vehicle_target_velocity = 15
def __init__(self, world, debug_mode=False):
"""
Setup all relevant parameters and create scenario
and instantiate scenario manager
"""
self.other_vehicles = [
setup_vehicle(world, self._other_vehicle_model,
self._other_vehicle_start)
]
self.ego_vehicle = setup_vehicle(world,
self._ego_vehicle_model,
self._ego_vehicle_start,
hero=True)
super(NoSignalJunctionCrossing,
self).__init__(name="NoSignalJunctionCrossing",
town="Town03",
world=world,
debug_mode=debug_mode)
def _create_behavior(self):
"""
After invoking this scenario, it will wait for the user
controlled vehicle to enter the start region,
then make a traffic participant to accelerate
until it is going fast enough to reach an intersection point.
at the same time as the user controlled vehicle at the junction.
Once the user controlled vehicle comes close to the junction,
the traffic participant accelerates and passes through the junction.
After 60 seconds, a timeout stops the scenario.
"""
# Creating leaf nodes
start_other_trigger = InTriggerRegion(self.ego_vehicle, -80, -70, -75,
-60)
sync_arrival = SyncArrival(self.other_vehicles[0], self.ego_vehicle,
carla.Location(x=-74.63, y=-136.34))
pass_through_trigger = InTriggerRegion(self.ego_vehicle, -90, -70,
-124, -119)
keep_velocity_other = KeepVelocity(self.other_vehicles[0],
self._other_vehicle_target_velocity)
stop_other_trigger = InTriggerRegion(self.other_vehicles[0], -45, -35,
-140, -130)
stop_other = StopVehicle(self.other_vehicles[0],
self._other_vehicle_max_brake)
end_condition = InTriggerRegion(self.ego_vehicle, -90, -70, -170, -156)
# Creating non-leaf nodes
root = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
scenario_sequence = py_trees.composites.Sequence()
sync_arrival_parallel = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
keep_velocity_other_parallel = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
# Building tree
root.add_child(scenario_sequence)
scenario_sequence.add_child(start_other_trigger)
scenario_sequence.add_child(sync_arrival_parallel)
scenario_sequence.add_child(keep_velocity_other_parallel)
scenario_sequence.add_child(stop_other)
scenario_sequence.add_child(end_condition)
sync_arrival_parallel.add_child(sync_arrival)
sync_arrival_parallel.add_child(pass_through_trigger)
keep_velocity_other_parallel.add_child(keep_velocity_other)
keep_velocity_other_parallel.add_child(stop_other_trigger)
return root
def _create_test_criteria(self):
"""
A list of all test criteria will be created that is later used
in parallel behavior tree.
"""
criteria = []
# Adding checks for ego vehicle
collision_criterion_ego = CollisionTest(self.ego_vehicle)
driven_distance_criterion = DrivenDistanceTest(
self.ego_vehicle, self._ego_vehicle_driven_distance)
criteria.append(collision_criterion_ego)
criteria.append(driven_distance_criterion)
# Add approriate checks for other vehicles
for vehicle in self.other_vehicles:
collision_criterion = CollisionTest(vehicle)
criteria.append(collision_criterion)
return criteria
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()
vehicle = world.spawn_actor(
random.choice(blueprint_library.filter('vehicle.*')), start_pose)
actor_list.append(vehicle)
vehicle.set_simulate_physics(False)
camera_rgb = 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=vehicle)
actor_list.append(camera_rgb)
camera_semseg = world.spawn_actor(
blueprint_library.find('sensor.camera.semantic_segmentation'),
carla.Transform(carla.Location(x=-5.5, z=2.8),
carla.Rotation(pitch=-15)),
attach_to=vehicle)
actor_list.append(camera_semseg)
# Create a synchronous mode context.
with CarlaSyncMode(world, camera_rgb, camera_semseg) as sync_mode:
while True:
if should_quit():
return
clock.tick()
# Advance the simulation and wait for the data.
snapshot, image_rgb, image_semseg = 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)
image_semseg.convert(carla.ColorConverter.CityScapesPalette)
fps = 1.0 / snapshot.timestamp.delta_seconds
# Draw the display.
draw_image(display, image_rgb)
draw_image(display, image_semseg, blend=True)
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():
global actor_list, flag, frame
client = carla.Client(host, port)
client.set_timeout(5.0)
try:
world = client.get_world()
except:
print("ERROR: Cannot get world !")
import sys
sys.exit()
set_weather(world)
#carla.TrafficLight.set_green_time(float(999))
#carla.TrafficLight.set_red_time(0)
try:
blueprint_library = world.get_blueprint_library()
# add vehicle
vehicle = add_vehicle_component(world, blueprint_library)
# put the vehicle to drive around.
#vehicle.set_autopilot(True)
map = world.get_map()
spawn_points = map.get_spawn_points()
destination = spawn_points[random.randint(0,
len(spawn_points) -
1)].location
agent = BasicAgent(vehicle, target_speed=20)
agent.set_destination((destination.x, destination.y, destination.z))
dao = GlobalRoutePlannerDAO(map)
planner = GlobalRoutePlanner(dao)
planner.setup()
vehicle.set_simulate_physics(False)
my_location = vehicle.get_location()
trace_list = planner.trace_route(my_location, destination)
waypoints = []
for (waypoint, road_option) in trace_list:
waypoints.append(waypoint)
next_point = waypoints[0].transform
camera = add_camera_component(world, blueprint_library, vehicle)
camera.stop()
while True:
vehicle.set_transform(next_point)
my_location = next_point.location
#my_location = vehicle.get_location()
me2destination = my_location.distance(destination)
if me2destination < 50:
destination = spawn_points[random.randint(
0,
len(spawn_points) - 1)].location
#agent.set_destination((destination.x,destination.y,destination.z))
print("destination change !!!")
trace_list = planner.trace_route(my_location, destination)
waypoints = []
for (waypoint, road_option) in trace_list:
waypoints.append(waypoint)
print(len(waypoints))
if len(waypoints) < 5:
print('my_location:', my_location.x, my_location.y,
my_location.z)
print('destination:', destination.x, destination.y,
destination.z)
print('me2destination:', me2destination)
next_point = waypoints[random.randint(0,
min(len(waypoints) - 1,
50))].transform
camera.listen(lambda image: deal_image(image))
while not flag:
sleep(0.01)
camera.stop()
flag = False
get_instruct(waypoints)
for waypoint in waypoints[0:min(len(waypoints) - 1, 30)]:
box_point = carla.Location(waypoint.transform.location.x,
waypoint.transform.location.y,
waypoint.transform.location.z - 0.4)
box = carla.BoundingBox(box_point,
carla.Vector3D(x=2, y=0.1, z=0.5))
rotation = carla.Rotation(
pitch=waypoint.transform.rotation.pitch,
yaw=waypoint.transform.rotation.yaw,
roll=waypoint.transform.rotation.roll)
world.debug.draw_box(box=box,
rotation=rotation,
thickness=1.2,
life_time=0)
sleep(0.3)
camera.listen(lambda image: deal_image(image))
while not flag:
sleep(0.01)
camera.stop()
flag = False
sleep(1.0)
finally:
print('destroying actors')
for actor in actor_list:
actor.destroy()
actor_list = []
print('done.')
def main():
actor_list = []
try:
client = carla.Client("localhost", 2000)
client.set_timeout(10.0)
# Town05, Town07 has highway, parking, standard roads
world = client.get_world()
# blueprint library
blueprint_library = world.get_blueprint_library()
# Agent = Dodge Charge Police
bp = blueprint_library.find('vehicle.dodge_charger.police')
# GNSS Sensor
gnss_bp = world.get_blueprint_library().find('sensor.other.gnss')
gnss_location = carla.Location(0, 0, 0)
gnss_rotation = carla.Rotation(0, 0, 0)
gnss_transform = carla.Transform(gnss_location, gnss_rotation)
gnss_bp.set_attribute(
"sensor_tick",
str(1.0)) # Wait time for sensor to update (1.0 = 1s)
# Spawn Agent
transform = carla.Transform(
carla.Location(
# x=121.61898803710938, y=187.5887451171875, z=1.0), carla.Rotation(yaw=180) # Location in Town02
# x=130.81553649902344, y=65.8092269897461, z=1.0), carla.Rotation(yaw=-0) # Location in Town03
# x = 117.7, y = 62.5, z = 1.0), carla.Rotation(yaw=-0) # Location in Town03
# x = 82.0, y = 8.2, z = 1.0), carla.Rotation(yaw=-0) # Location in Town03
x=157.0,
y=-148.0,
z=1.0),
carla.Rotation(
yaw=-0) # Location in Town03 - Tunnel Entrance (North)
)
vehicle = world.spawn_actor(bp, transform)
actor_list.append(vehicle)
print('created %s' % vehicle.type_id)
# Spawn Sensors
gnss = world.spawn_actor(gnss_bp, gnss_transform, attach_to=vehicle)
actor_list.append(gnss)
print('created %s' % gnss.type_id,
"\n# # # # # # # # # # # # # # # # # # # # # #")
# Get Town Map name
town = str(world.get_map().name)
# Activate GNSS Sensor and save current location in GNSS_DATA.parquet
print(
"Starting GNSS sensor\n# # # # # # # # # # # # # # # # # # # # # #"
)
gnss.listen(lambda event: gnss_live_location(event))
time.sleep(2)
print(
"Current location acquired.\n# # # # # # # # # # # # # # # # # # # # # #"
)
# Current Location - Loads it from GNSS_DATA.parquet
gnss_data = pd.read_parquet("GNSS_DATA.parquet")
current_loc = (gnss_data.loc[0, 'lat'], gnss_data.loc[0, 'lon'],
gnss_data.loc[0, 'alt'])
# Fixed Destination (CARLA Location of type x, y, z)
# destination = (127.02777862548828, 306.4728088378906, 1.0) # Fixed Location in Town02
destination = (-20.639827728271484, -142.1471405029297, 1.0
) # Fixed Location in Town03
# destination = (118.0, -201.5, 1.0) # Fixed Location in Town03
# Random Destination (CARLA Location)
# If choosing this option instead of the fixed destination make sure to
# change dest_fixed=False in the code below
# destination = random.choice(world.world.get_map().get_spawn_points())
# Get shortest path and visualize it
df_carla_path = process_nav_a2b(
world, town, current_loc, destination,
dest_fixed=True) # put dest_fixed=False if random location
# Vehicle autopilot
vehicle.set_autopilot(
False
) # Not activated if False (Needs to be False for teleportation)
# Vehicle physics
vehicle.set_simulate_physics(False) # For teleportation put False
# Gather and send waypoints one by one to vehicle
print("Driving to destination...")
for index, row in df_carla_path.iterrows():
target_x = row["x"]
target_y = row["y"]
target_z = row["z"]
# Find next waypoint 2 meters ahead.
target_waypoint = carla.Transform(
carla.Location(x=target_x, y=target_y,
z=target_z + 1)) # +1 in z for teleport
# Teleport the vehicle.
vehicle.set_transform(target_waypoint)
time.sleep(1)
print("# # # # # # # # # # # # # # # # # # # # # #\n"
"You have arrived at your destination.\n")
finally:
print('\nDestroying actors.')
client.apply_batch([carla.command.DestroyActor(x) for x in actor_list])
if os.path.exists("GNSS_DATA.parquet"):
print('Destroying GNSS_DATA.parquet.')
os.remove("GNSS_DATA.parquet")
print('Done.')
def get_transform(vehicle_location, angle, d=6.4):
a = math.radians(angle)
location = carla.Location(d * math.cos(a), d * math.sin(a),
2.0) + vehicle_location
return carla.Transform(location, carla.Rotation(yaw=180 + angle,
pitch=-15))
def __init__(self, parent_actor, hud):
self.sensor = None
self.surface = None
self._parent = parent_actor
self.hud = hud
self.recording = False
self._camera_transforms = [
carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)),
carla.Transform(carla.Location(x=1.6, z=1.7))]
self.transform_index = 1
self.sensors = [
['sensor.camera.rgb', cc.Raw, 'Camera RGB'],
['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'],
['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'],
['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'],
['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'],
['sensor.camera.semantic_segmentation', cc.CityScapesPalette,
'Camera Semantic Segmentation (CityScapes Palette)'],
['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']]
world = self._parent.get_world()
bp_library = world.get_blueprint_library()
for item in self.sensors:
bp = bp_library.find(item[0])
if item[0].startswith('sensor.camera'):
bp.set_attribute('image_size_x', str(hud.dim[0]))
bp.set_attribute('image_size_y', str(hud.dim[1]))
elif item[0].startswith('sensor.lidar'):
bp.set_attribute('range', '5000')
## rotation freq
bp.set_attribute('rotation_frequency', '20')
item.append(bp)
self.index = None
########################################################################
## create custom sensors for collecting rgb image and Lidar data
#'''
self.sensor_rgb = None
self.sensor_lidar = None
# listen and record control signals on each image
self.control_listener = None
print("init custum rgb and Lidar sensors")
if self.sensor_rgb is not None:
self.sensor_rgb.destroy()
#self.surface = None
self.sensor_rgb = self._parent.get_world().spawn_actor(
self.sensors[0][-1], # rgb image
self._camera_transforms[self.transform_index],
attach_to=self._parent)
# We need to pass the lambda a weak reference to self to avoid
# circular reference.
weak_self = weakref.ref(self)
self.sensor_rgb.listen(lambda image: CameraManager._parse_image_rgblidar(weak_self, image, 0))
#'''
## Lidar
#'''
#print(self.sensors[6])
#['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)', <carla.libcarla.ActorBlueprint object at 0x7f1b038fd570>]
#assert(0)
if self.sensor_lidar is not None:
self.sensor_lidar.destroy()
#self.surface = None
self.sensor_lidar = self._parent.get_world().spawn_actor(
self.sensors[6][-1], # Lidar image
self._camera_transforms[self.transform_index],
attach_to=self._parent)
# We need to pass the lambda a weak reference to self to avoid
# circular reference.
#weak_self = weakref.ref(self)
self.sensor_lidar.listen(lambda image: CameraManager._parse_image_rgblidar(weak_self, image, 6))
#'''
## Control signals
'''
if self.control_listener is not None:
self.control_listener.destroy()
# sync with image input, so use sensors[0]
self.control_listener = self._parent.get_world().spawn_actor(
self.sensors[0][-1], # rgb image
self._camera_transforms[self.transform_index],
attach_to=self._parent)
self.control_listener.listen(lambda image: CameraManager._parse_image_control(weak_self, image, world))
'''
## image saver
self.out_dir = "_out_data"
self.episode_count = 0
#print('%s/ep_%d/' % (self.out_dir, self.episode_count))
#assert(0)
self.saver = [
# BufferedImageSaver(f'{self.out_dir}5/ep_{self.episode_count:03d}/CameraDepth/',
# 1000, WINDOW_HEIGHT, WINDOW_WIDTH, 1),
BufferedImageSaver('%s/ep_%d/' % (self.out_dir, self.episode_count),
100, WINDOW_HEIGHT, WINDOW_WIDTH, 3, 'CameraRGB'),
BufferedImageSaver('%s/ep_%d/' % (self.out_dir, self.episode_count),
100, 8000, 1, 3, 'Lidar')
## TODO: current limit 5000 points, might be more
]
import pygame
except ImportError:
raise RuntimeError('cannot import pygame, make sure pygame package is installed')
try:
import numpy as np
except ImportError:
raise RuntimeError('cannot import numpy, make sure numpy package is installed')
AUTOPILOT_DESTINATION = carla.Location(x=126.097374, y=125.237732, z=0.342617)
# Enable this to place the camera on dirvers view.
DRIVERVIEW_CAMERA = False
MAIN_CAMERA_TRANSFORM = carla.Transform(carla.Location(x=1, z=1), carla.Rotation(pitch=-50, yaw=180))
if DRIVERVIEW_CAMERA == True:
MAIN_CAMERA_TRANSFORM = carla.Transform(carla.Location(x=5.3, y=0.35, z=0.9), carla.Rotation(yaw=180, pitch=-5))
MIRROR_W = 350
MIRROR_H = 200
DISPLAY_W = 1200
DISPLAY_H = 1000
SCREEN_W = MIRROR_W*2+DISPLAY_W
SCREEN_H = max(DISPLAY_H, MIRROR_H)
FRONT_W = 300
FRONT_H = 60
def main():
actor_list = []
verboseIsEnabled = None
try:
"""
Section for starting the client and connecting to the server
"""
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
for arg in sys.argv:
if (arg == '--verbose'):
verboseIsEnabled = True
if verboseIsEnabled:
print('client version: %s' % client.get_client_version())
print('server version: %s' % client.get_server_version())
print('client to server connection status: {}'.format(client.get_server_version()))
print('Retrieving the world data from server...')
world = client.get_world()
if verboseIsEnabled:
print('{} \n'.format(world))
"""
Section for retrieving the blueprints and spawn the actors
"""
blueprint_library = world.get_blueprint_library()
if verboseIsEnabled:
print('\nRetrieving CARLA blueprint library...')
print('\nobject: %s\n\nblueprint methods: %s\n\nblueprint list:' % (type(blueprint_library), dir(blueprint_library)) )
for blueprint in blueprint_library:
print(blueprint)
audi_blueprint = blueprint_library.find('vehicle.audi.tt')
print('\n%s\n' % audi_blueprint)
color = '191,191,191'
audi_blueprint.set_attribute('color', color)
# Place measured in map 'carissma_scenario' 32
transform = carla.Transform(
carla.Location(x=32.4, y=76.199997, z=39.22),
carla.Rotation(yaw=0.0))
vehicleEgo = world.spawn_actor(audi_blueprint, transform)
actor_list.append(vehicleEgo)
print('created %s' % vehicleEgo.type_id)
transform = carla.Transform(
carla.Location(
x=88.2, y=93.5,
z=38.98),carla.Rotation(yaw=-90.0)
)
color = random.choice(audi_blueprint.get_attribute('color').recommended_values)
audi_blueprint.set_attribute('color', color)
vehicleObservable = world.spawn_actor(audi_blueprint, transform)
actor_list.append(vehicleObservable)
print('created %s' % vehicleObservable.type_id)
'''
time.sleep(1.5)
vehicleEgo.apply_control(carla.VehicleControl(throttle=.3))
elapsed_time = 0
time_increment = 0.1
'''
# Find the blueprint of the sensor.
blueprint = world.get_blueprint_library().find('sensor.camera.rgb')
# Modify the attributes of the blueprint to set image resolution and field of view.
blueprint.set_attribute('image_size_x', '1920')
blueprint.set_attribute('image_size_y', '1080')
blueprint.set_attribute('fov', '110')
# Provide the position of the sensor relative to the vehicle.
# transform = carla.Transform(carla.Location(x=0.8, z=1.7))
transformFrontCam = carla.Transform(carla.Location(x=0.8, z=1.5), carla.Rotation(yaw=0))
transformRearCam = carla.Transform(carla.Location(x=-1.8, z=1), carla.Rotation(yaw=180))
# Tell the world to spawn the sensor, don't forget to attach it to your vehicle actor.
frontCam = world.spawn_actor(blueprint, transformFrontCam, attach_to=vehicleEgo)
rearCam = world.spawn_actor(blueprint, transformRearCam , attach_to=vehicleEgo)
# Subscribe to the sensor stream by providing a callback function, this function is
# called each time a new image is generated by the sensor.
frontCam.listen(lambda image: image.save_to_disk('output/front/%06d.png' % image.frame_number))
rearCam.listen(lambda image: image.save_to_disk('output/rear/%06d.png' % image.frame_number))
actor_list.append(frontCam)
actor_list.append(rearCam)
time.sleep(5)
vehicleEgo.apply_control(carla.VehicleControl(throttle=.7))
time.sleep(3.8)
vehicleObservable.apply_control(carla.VehicleControl(throttle=1))
time.sleep(2.6)
vehicleEgo.apply_control(carla.VehicleControl(hand_brake=True))
time.sleep(3.5)
finally:
print('destroying actors')
for actor in actor_list:
actor.destroy()
print('done.')
def main(args):
actor_list = []
sensor_list = []
pygame.init()
display = pygame.display.set_mode(
(SCREEN_W, SCREEN_H),
pygame.HWSURFACE | pygame.DOUBLEBUF)
fill_rect = pygame.Rect((0,0,MIRROR_W, SCREEN_H/2))
pygame.display.set_caption('CARLA Scenarios')
font = get_font()
font_big = get_font(font_size=30)
clock = pygame.time.Clock()
client = carla.Client('localhost', 2000)
client.set_timeout(5.0)
world = client.get_world()
world = client.reload_world()
config = read_json_config(args.spawn_config)
# This agent will drive the autopilot to certain destination
behaviour_agent = None
is_agent_controlled = False
prev_agent_autopilot_enabled = False
scenario_instance = None
scenario_class = None
try:
m = world.get_map()
sx, sy, sz = float(config['ego_actor']['x']), float(config['ego_actor']['y']), float(config['ego_actor']['z'])
syaw, spitch, sroll = float(config['ego_actor']['yaw']), float(config['ego_actor']['pitch']), float(config['ego_actor']['roll'])
s_loc = carla.Location(sx, sy, sz+1)
s_rot = carla.Rotation(yaw=syaw, pitch=spitch, roll=sroll)
start_pose = carla.Transform(location=s_loc, rotation=s_rot)
#spawn car
blueprint_library = world.get_blueprint_library()
car_bp = blueprint_library.find(config['ego_actor']['actor_type'])
#car_bp.set_attribute('color','255,20,147')
#car_bp.set_attribute('color','158,255,0')
car_bp.set_attribute('role_name', 'hero')
vehicle = world.spawn_actor(
car_bp,
start_pose)
#print(vehicle.attributes.get('role_name'))
actor_list.append(vehicle)
#spawn spectator cam
cam_bp = blueprint_library.find('sensor.camera.rgb')
cam_bp.set_attribute('image_size_x', str(SCREEN_W))
cam_bp.set_attribute('image_size_y', str(SCREEN_H))
#cam_bp.set_attribute('sensor_tick',str(FREQ))
camera_rgb = world.spawn_actor(
cam_bp,
MAIN_CAMERA_TRANSFORM,
attach_to=vehicle,
attachment_type=carla.AttachmentType.SpringArm)
#every sensor must be registered by
#appending to actor list AND sensor list
actor_list.append(camera_rgb)
sensor_list.append(camera_rgb)
#rearview cam
#camera_front = spawn.spawn_rgb(vehicle, world, img_x=FRONT_W, img_y=FRONT_H, hz=FREQ, pitch=-12)
cam_bp.set_attribute('image_size_x', str(FRONT_W))
cam_bp.set_attribute('image_size_y', str(FRONT_H))
cam_bp.set_attribute('fov', str(60))
mirror_rear = world.spawn_actor(
cam_bp,
carla.Transform(carla.Location(x=-2, z=1.3), carla.Rotation(yaw=-180)),
attach_to=vehicle)
actor_list.append(mirror_rear)
sensor_list.append(mirror_rear)
#spawn left mirror
cam_bp.set_attribute('image_size_x', str(MIRROR_W))
cam_bp.set_attribute('image_size_y', str(MIRROR_H))
cam_bp.set_attribute('fov', str(70))
mirror_left = world.spawn_actor(
cam_bp,
carla.Transform(carla.Location(y=-1.1, z=1.0), carla.Rotation(yaw=-160)),
attach_to=vehicle)
#every sensor must be registered by
#appending to actor list AND sensor list
actor_list.append(mirror_left)
sensor_list.append(mirror_left)
#spawn right mirror
cam_bp.set_attribute('image_size_x', str(MIRROR_W))
cam_bp.set_attribute('image_size_y', str(MIRROR_H))
cam_bp.set_attribute('fov', str(70))
mirror_right = world.spawn_actor(
cam_bp,
carla.Transform(carla.Location(y=1.1, z=1.0), carla.Rotation(yaw=160)),
attach_to=vehicle)
#every sensor must be registered by
#appending to actor list AND sensor list
actor_list.append(mirror_right)
sensor_list.append(mirror_right)
world.player = vehicle
controller = DualControl(vehicle, world=world, start_in_autopilot=False, agent_controlled=True)
if args.scenario == True:
scenario_config_json = read_json_config(args.scenario_config)
scenario_class = scenario_config_json["class"]
if scenario_class == "BikeCrossing":
scenario_instance = BikeCrossing()
elif scenario_class == "CarCrashScenario":
scenario_instance = CarCrashScenario()
elif scenario_class == "PedestrianCrossing":
scenario_instance = PedestrianCrossing()
scenario_instance.load_config(args.scenario_config)
scenario_instance.spawn_npcs()
trigger_distances = scenario_instance.get_distances()
beep = pygame.mixer.Sound('assets/sounds/bell.wav')
wheel_icon = pygame.image.load('assets/icons/steering wheel.png').convert_alpha()
sound_time = 0
flash_time = 0
#initial scenario stage
flash_on = False
stage = 0
#initial hlc
hlc = 2
if DRIVERVIEW_CAMERA == True:
camera_rgb.set_transform(MAIN_CAMERA_TRANSFORM)
# Create a synchronous mode context.
#SENSORS SHOULD BE PASSED IN THE SAME ORDER AS IN ACTOR_LIST
with CarlaSyncMode(world, vehicle, m, *sensor_list, fps=FREQ, record=args.record, scenario=args.scenario) as sync_mode:
record_start_flag = False
yaw_prev = None
while True:
if should_quit():
return
#print(sync_mode._queues)
clock.tick()
# Advance the simulation and wait for the data.
tick_data = sync_mode.tick(timeout=2.0, hlc=hlc)
snapshot = tick_data[0]
image_rgb = tick_data[1]
image_front = tick_data[2]
image_mirror_left = tick_data[3]
image_mirror_right = tick_data[4]
#parse for control input and run vehicle
hlc = controller.parse_events(vehicle, clock)
control_states = vehicle.get_control()
#draw trail based on hlc
#sync_mode.draw_trail(hlc)
v = vehicle.get_velocity()
trans = vehicle.get_transform()
#print('{:.3f}, {:.3f}'.format(trans.location.x, trans.location.y))
closest_wp = m.get_waypoint(trans.location)
draw_loc = closest_wp.transform.location+carla.Location(z=2)
#sync_mode.world.debug.draw_point(draw_loc, life_time=0.05)
#print(closest_wp.is_junction, len(closest_wp.next(0.5)))
#track SECOND waypoint in queue as first does not shift with lc command
second_wp = sync_mode.waypoint_queue[1]
heading_error = second_wp.transform.rotation.yaw - trans.rotation.yaw
heading_error = util.angle_wrap(heading_error)
delta_x, delta_y = util.car_frame_deltas(trans, second_wp.transform.location)
vx, vy = util.measure_forward_velocity(v, trans.rotation, return_both=True)
vx_kph = vx*3.6
curvature = util.curvature(sync_mode.waypoint_queue[1].transform, sync_mode.waypoint_queue[2].transform)
#lateral accel
#r = 1/(curvature + 1e-6)
#print(vx**2/r)
if len(sync_mode.vehicles_close)>0:
#print(sync_mode.vehicles_close[0][0], sync_mode.vehicles_close[0][1].__str__())
dist_to_car = max(sync_mode.vehicles_close[0][0] - 4.5, 0)
else:
dist_to_car = 50.0
if len(sync_mode.pedestrians_close)>0:
#print(sync_mode.vehicles_close[0][0], sync_mode.vehicles_close[0][1].__str__())
dist_to_walker = max(sync_mode.pedestrians_close[0][0] - 4.5, 0)
else:
dist_to_walker = 50.0
affordance = (heading_error, delta_y, curvature, dist_to_car, dist_to_walker)
'''
behaviour agent begin
'''
if controller._agent_autopilot_enabled == True:
if prev_agent_autopilot_enabled == False:
# Init the agent
behaviour_agent = BehaviorAgent(vehicle, behavior="normal")
# Set agent's destination
behaviour_agent.set_destination(behaviour_agent.vehicle.get_location(), AUTOPILOT_DESTINATION)
print ("Autopilot is controlled by BehaviourAgent to destination: {}".format(AUTOPILOT_DESTINATION))
behaviour_agent.update_information()
# print("Autopilot driving {} more waypoints till destination is reached".format(len(behaviour_agent.get_local_planner().waypoints_queue)))
if len(behaviour_agent.get_local_planner().waypoints_queue) <= 0: # For destination precision change this value
print("Target almost reached, mission accomplished...")
controller._agent_autopilot_enabled = False
behaviour_agent.set_destination(behaviour_agent.vehicle.get_location(), behaviour_agent.vehicle.get_location())
else:
input_control = behaviour_agent.run_step()
world.player.apply_control(input_control)
prev_agent_autopilot_enabled = controller._agent_autopilot_enabled
'''
behaviour agent end
'''
'''
scenario logic
'''
if sync_mode.scenario and (scenario_class == "BikeCrossing"):
# print ("sync_mode_scenario stage {}".format(stage))
#check distance to bike
bike_dist = scenario_instance.check_distance(trans.location)
#if distance is below threshold, do something
#first stage: flash warning
#2nd stage: bike starts crossing
if stage==0:
if bike_dist < trigger_distances[0]:
#stage 0 to 1 or 0 to 3 transition
if controller._agent_autopilot_enabled:
stage = 1
else:
stage = 3
elif stage==1:
#stage 1: play warning sound
if snapshot.timestamp.elapsed_seconds - sound_time > 3:
beep.play()
sound_time = snapshot.timestamp.elapsed_seconds - 0.75
if snapshot.timestamp.elapsed_seconds - flash_time > 1.5:
flash_on = not flash_on
flash_time = snapshot.timestamp.elapsed_seconds - 0.75
if bike_dist < trigger_distances[1]:
flash_on = False
#stage one to two transition
scenario_instance.begin()
#disable autopilot
controller._agent_autopilot_enabled = False
print('autopilot toggled: {}'.format(controller._agent_autopilot_enabled))
sync_mode.car.set_autopilot(controller._agent_autopilot_enabled)
stage = 2
elif controller._agent_autopilot_enabled == False:
#stage 1 to 3 transition
flash_on = False
stage = 3
elif stage==2:
#delta, throttle, brake = driver.drive(heading_error, delta_y, vx, curvature, 28, min(dist_to_car, dist_to_walker))
vc = carla.VehicleControl(throttle=0, steer=0, brake=0.01)
sync_mode.car.apply_control(vc)
v = vehicle.get_velocity()
vx, vy = util.measure_forward_velocity(v, trans.rotation, return_both=True)
if vx <= 0.1 :
stage = 3
elif stage == 3:
#stage 3, bike crossing still gets triggered
#but driver stays in control
if bike_dist > trigger_distances[1]:
scenario_instance.begin()
stage = 4
elif controller._agent_autopilot_enabled:
stage = 1
elif stage==4:
#nothing happens in stage 4
# print ("Scenario done, close it")
# scenario_instance.kill_npcs()
sync_mode.scenario = False
pass
if sync_mode.scenario and (scenario_class == "CarCrashScenario"):
# print ("sync_mode_scenario stage {}".format(stage))
#check distance to cars
cars_dist = scenario_instance.check_distance(trans.location)
#if distance is below threshold, do something
#first stage: flash warning
#2nd stage: none
if stage==0:
if cars_dist < trigger_distances[0]:
scenario_instance.begin()
#stage 0 to 1 or 0 to 3 transition
if controller._agent_autopilot_enabled:
stage = 1
else:
stage = 3
elif stage==1:
#stage 1: play warning sound
if snapshot.timestamp.elapsed_seconds - sound_time > 3:
beep.play()
sound_time = snapshot.timestamp.elapsed_seconds - 0.75
if snapshot.timestamp.elapsed_seconds - flash_time > 1.5:
flash_on = not flash_on
flash_time = snapshot.timestamp.elapsed_seconds - 0.75
if cars_dist < trigger_distances[1]:
flash_on = False
#stage one to two transition
scenario_instance.begin()
#disable autopilot
controller._agent_autopilot_enabled = False
print('autopilot toggled: {}'.format(controller._agent_autopilot_enabled))
sync_mode.car.set_autopilot(controller._agent_autopilot_enabled)
stage = 2
elif controller._agent_autopilot_enabled == False:
#stage 1 to 3 transition
flash_on = False
stage = 3
elif stage==2:
#delta, throttle, brake = driver.drive(heading_error, delta_y, vx, curvature, 28, min(dist_to_car, dist_to_walker))
vc = carla.VehicleControl(throttle=0, steer=0, brake=0.01)
sync_mode.car.apply_control(vc)
v = vehicle.get_velocity()
vx, vy = util.measure_forward_velocity(v, trans.rotation, return_both=True)
if vx <= 0.1 :
stage = 3
elif stage == 3:
#stage 3, car crash still gets triggered
#but driver stays in control
if cars_dist > trigger_distances[1]:
scenario_instance.begin()
stage = 4
elif controller._agent_autopilot_enabled:
stage = 1
elif stage==4:
#nothing happens in stage 4
# print ("Scenario done, close it")
# scenario_instance.kill_npcs()
sync_mode.scenario = False
pass
if sync_mode.scenario and (scenario_class == "PedestrianCrossing"):
# print ("sync_mode_scenario stage {}".format(stage))
#check distance to pedestrian
pedestrian_dist = scenario_instance.check_distance(trans.location)
#if distance is below threshold, do something
#first stage: flash warning
#2nd stage: pedestrian starts crossing
if stage==0:
if pedestrian_dist < trigger_distances[0]:
scenario_instance.begin()
#stage 0 to 1 or 0 to 3 transition
if controller._agent_autopilot_enabled:
stage = 1
else:
stage = 3
elif stage==1:
#stage 1: play warning sound
if snapshot.timestamp.elapsed_seconds - sound_time > 3:
beep.play()
sound_time = snapshot.timestamp.elapsed_seconds - 0.75
if snapshot.timestamp.elapsed_seconds - flash_time > 1.5:
flash_on = not flash_on
flash_time = snapshot.timestamp.elapsed_seconds - 0.75
if pedestrian_dist < trigger_distances[1]:
flash_on = False
#stage one to two transition
# scenario_instance.begin()
#disable autopilot
controller._agent_autopilot_enabled = False
print('autopilot toggled: {}'.format(controller._agent_autopilot_enabled))
sync_mode.car.set_autopilot(controller._agent_autopilot_enabled)
stage = 2
elif controller._agent_autopilot_enabled == False:
#stage 1 to 3 transition
flash_on = False
stage = 3
elif stage==2:
#delta, throttle, brake = driver.drive(heading_error, delta_y, vx, curvature, 28, min(dist_to_car, dist_to_walker))
vc = carla.VehicleControl(throttle=0, steer=0, brake=0.01)
sync_mode.car.apply_control(vc)
v = vehicle.get_velocity()
vx, vy = util.measure_forward_velocity(v, trans.rotation, return_both=True)
if vx <= 0.1 :
stage = 3
elif stage == 3:
#stage 3, pedestrian crossing still gets triggered
#but driver stays in control
if pedestrian_dist<trigger_distances[1]:
scenario_instance.begin()
stage = 4
elif controller._agent_autopilot_enabled:
stage = 1
elif stage==4:
#nothing happens in stage 4
# print ("Scenario done, close it")
# scenario_instance.kill_npcs()
sync_mode.scenario = False
pass
'''end of scenario code'''
#record
if vx > 0.01:
record_start_flag = True
if sync_mode.record and record_start_flag:
#sync_mode.record_image(tick_data[1:])
sync_mode.record_frame(snapshot, trans, v, control_states, affordance, second_wp, hlc, stage)
#image_semseg.convert(carla.ColorConverter.CityScapesPalette)
fps = round(1.0 / snapshot.timestamp.delta_seconds)
# Draw the display.
display.fill((0,0,0), rect=fill_rect)
# draw_image(display, image_rgb)
draw_cam(display, image_rgb, 0, 0, False)
draw_cam(display, image_front, (SCREEN_W/2)-FRONT_W/2, 0)
draw_cam(display, image_mirror_left, 0, SCREEN_H - MIRROR_H)
draw_cam(display, image_mirror_right, MIRROR_W+DISPLAY_W, SCREEN_H - MIRROR_H)
if flash_on:
display.blit(
font_big.render('Please takeover control of the vehicle', True, (255,0,0)),
(50 + SCREEN_W//2-265, SCREEN_H//2))
display.blit(wheel_icon, (SCREEN_W * 0.46,SCREEN_H//3 + 64))
display.blit(
font.render('steer: {0:2.2f}, brake: {1:2.2f}, gas: {2:2.2f}'.format(control_states.steer,control_states.brake,control_states.throttle), False, (255, 255, 255)),
(8, 20))
display.blit(
font.render('% 5d Gear' % control_states.gear, False, (255, 255, 255)),
(8, 40))
display.blit(
font_big.render('Speed: % 5d km/h' % vx_kph, False, (50, 87, 168)),
(SCREEN_W * 0.45, SCREEN_H - 205))
autopilot_str_val = 'OFF'
if controller._agent_autopilot_enabled == True:
autopilot_str_val = 'ON'
display.blit(
font_big.render(' Autopilot: ' + autopilot_str_val, True, (139, 61, 136)),
(SCREEN_W * 0.45, SCREEN_H - 240))
pygame.display.flip()
except Exception as exception:
print (str(exception))
finally:
print('destroying actors.')
if args.scenario:
if scenario_instance is not None:
scenario_instance.kill_npcs()
for actor in actor_list:
actor.destroy()
pygame.quit()
print('done.')
def create_environment(world, sensors, n_vehicles, n_walkers, spawn_points,
client):
global sensors_callback
sensors_ret = []
blueprint_library = world.get_blueprint_library()
# setup sensors
for sensor_spec in sensors:
bp = blueprint_library.find(sensor_spec['type'])
if sensor_spec['type'].startswith('sensor.camera'):
bp.set_attribute('image_size_x', str(sensor_spec['width']))
bp.set_attribute('image_size_y', str(sensor_spec['height']))
bp.set_attribute('fov', str(sensor_spec['fov']))
sensor_location = carla.Location(x=sensor_spec['x'],
y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
elif sensor_spec['type'].startswith('sensor.lidar'):
bp.set_attribute('range', '200')
bp.set_attribute('rotation_frequency', '10')
bp.set_attribute('channels', '32')
bp.set_attribute('upper_fov', '15')
bp.set_attribute('lower_fov', '-30')
bp.set_attribute('points_per_second', '500000')
sensor_location = carla.Location(x=sensor_spec['x'],
y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
elif sensor_spec['type'].startswith('sensor.other.gnss'):
sensor_location = carla.Location(x=sensor_spec['x'],
y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation()
# create sensor
sensor_transform = carla.Transform(sensor_location, sensor_rotation)
sensor = world.spawn_actor(bp, sensor_transform)
# add callbacks
sc = CallBack()
sensor.listen(sc)
sensors_callback.append(sc)
sensors_ret.append(sensor)
vehicles_list = []
walkers_list = []
all_id = []
blueprint = world.get_blueprint_library().filter('vehicle.audi.a2')[0]
walker_bp = world.get_blueprint_library().filter(
"walker.pedestrian.0001")[0]
# @todo cannot import these directly.
SpawnActor = carla.command.SpawnActor
SetAutopilot = carla.command.SetAutopilot
FutureActor = carla.command.FutureActor
# --------------
# Spawn vehicles
# --------------
batch = []
for num, transform in enumerate(spawn_points):
if num >= n_vehicles:
break
blueprint.set_attribute('role_name', 'autopilot')
batch.append(
SpawnActor(blueprint,
transform).then(SetAutopilot(FutureActor, True)))
for response in client.apply_batch_sync(batch, False):
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(n_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:
# 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'):
# walking
walker_speed.append(
walker_bp.get_attribute('speed').recommended_values[1])
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 altogether 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
world.wait_for_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.' %
(len(vehicles_list), len(walkers_list)))
return vehicles_list, walkers_list, all_id, all_actors, sensors_ret
world = client.load_world('Town06') # highway town simple
else: # otherwise, continue using Town06
world = client.get_world()
worldmap = world.get_map()
blueprint_library = world.get_blueprint_library()
bp = blueprint_library.filter('model3')[0] # blueprint for Tesla Model 3
bp.set_attribute('color', '0,0,255') # blue
bp2 = blueprint_library.filter('prius')[
0] # blueprint for red Toyota Prius
bp3 = blueprint_library.filter('mustang')[
0] # blueprint for light blue Ford Mustang
print(bp)
global vehicle
vehicle = None
spawn_point = carla.Transform(carla.Location(151.071, 147.458, 2.5),
carla.Rotation(0, 0.234757, 0))
spawn_point_2 = carla.Transform(carla.Location(124.071, 150.58, 2.5),
carla.Rotation(0, 0.234757, 0))
spawn_point_3 = carla.Transform(carla.Location(154.071, 150.458, 2.5),
carla.Rotation(0, 0.234757, 0))
#spawn_point = random.choice(world.get_map().get_spawn_points())
print(str(spawn_point.location) + str(spawn_point.rotation))
vehicle = world.try_spawn_actor(bp, spawn_point) # spawn the car (actor)
vehicle_2 = world.try_spawn_actor(bp2,
spawn_point_2) # spawn the car (actor)
vehicle_3 = world.try_spawn_actor(bp3,
spawn_point_3) # spawn the car (actor)
# get the blueprint for this sensor
blueprint = blueprint_library.find('sensor.camera.rgb')
#blueprint.set_attribute('post_processing', 'SemanticSegmentation')
# change the dimensions of the image
def get_transform(angle, d=6.5):
a = math.radians(angle)
location = carla.Location(d * math.cos(a), d * math.sin(a), 2.0) + LOCATION
return carla.Transform(location, carla.Rotation(yaw=180 + angle,
pitch=-15))
def convert_position_to_transform(position):
"""
Convert an OpenScenario position into a CARLA transform
Not supported: Road, RelativeRoad, Lane, RelativeLane as the PythonAPI currently
does not provide sufficient access to OpenDrive information
Also not supported is Route. This can be added by checking additional
route information
"""
if position.find('World') is not None:
world_pos = position.find('World')
x = float(world_pos.attrib.get('x', 0))
y = float(world_pos.attrib.get('y', 0))
z = float(world_pos.attrib.get('z', 0))
yaw = float(world_pos.attrib.get('h', 0))
pitch = float(world_pos.attrib.get('p', 0))
roll = float(world_pos.attrib.get('r', 0))
return carla.Transform(
carla.Location(x=x, y=y, z=z),
carla.Rotation(yaw=yaw, pitch=pitch, roll=roll))
elif (position.find('RelativeWorld')
is not None) or (position.find('RelativeObject') is not None):
rel_pos = position.find('RelativeWorld')
obj = float(rel_pos.attrib.get('object'))
obj_actor = None
for actor in CarlaDataProvider.get_world().get_actors():
if actor.attributes['role_name'] == obj:
obj_actor = actor
break
if obj_actor is None:
raise AttributeError(
"Object '{}' provided as position reference is not known".
format(obj))
dx = float(rel_pos.attrib.get('dx', 0))
dy = float(rel_pos.attrib.get('dy', 0))
dz = float(rel_pos.attrib.get('dz', 0))
is_absolute = False
if rel_pos.find('Orientation') is not None:
orientation = rel_pos.find('Orientation')
is_absolute = (orientation.attrib.get('type') == "absolute")
dyaw = float(orientation.attrib.get('h', 0))
dpitch = float(orientation.attrib.get('p', 0))
droll = float(orientation.attrib.get('r', 0))
x = obj_actor.get_location().x + dx
y = obj_actor.get_location().y + dy
z = obj_actor.get_location().z + dz
yaw = obj_actor.get_transform().rotation.yaw
pitch = obj_actor.get_transform().rotation.pitch
roll = obj_actor.get_transform().rotation.roll
if not is_absolute:
yaw = yaw + dyaw
pitch = pitch + dpitch
roll = roll + droll
else:
yaw = dyaw
pitch = dpitch
roll = droll
return carla.Transform(
carla.Location(x=x, y=y, z=z),
carla.Rotation(yaw=yaw, pitch=pitch, roll=roll))
elif position.find('Road') is not None:
raise NotImplementedError("Road positions are not yet supported")
elif position.find('RelativeRoad') is not None:
raise NotImplementedError(
"RelativeRoad positions are not yet supported")
elif position.find('Lane') is not None:
raise NotImplementedError("Lane positions are not yet supported")
elif position.find('RelativeLane') is not None:
raise NotImplementedError(
"RelativeLane positions are not yet supported")
elif position.find('Route') is not None:
raise NotImplementedError("Route positions are not yet supported")
else:
raise AttributeError("Unknown position")
def restart(self):
self.decision = None
self.simul_time = time.time()
self.lane_change_time = time.time()
self.max_Lane_num = 4
self.ego_Lane = 2
self.controller = None
self.accumulated_reward = 0
self.section = 0
self.episode_start = time.time()
self.pre_max_Lane_num = self.max_Lane_num
self.index = 0
print('start destroying actors.')
if len(self.actor_list) != 0:
# print('destroying actors.')
# print("actor 제거 :", self.actor_list)
for actor in self.actor_list:
# print("finally 에서 actor 제거 :", self.actor_list)
if actor.is_alive:
actor.destroy()
print("finshed actor destroy")
# for x in self.actor_list:
# try:
# client.apply_batch([carla.command.DestroyActors(x.id)])
# except:
# continue
if len(self.extra_list) != 0:
# client.apply_batch([carla.command.DestoryActors(x.id) for x in self.extra_list])
# print("extra 제거 :", self.extra_list)
for x in self.extra_list:
try:
client.apply_batch([carla.command.DestroyActor(x.id)])
except:
continue
# for extra in self.extra_list:
# # print("finally 에서 actor 제거 :", self.extra_list)
# print(extra.is_alive)
# if extra.is_alive:
# extra.destroy()
print("finshed extra destroy")
# for x in self.extra_list:
# try:
# client.apply_batch([carla.command.DestroyActor(x.id)])
# except:
# continue
print('finished destroying actors.')
self.actor_list = []
self.extra_list = []
self.ROI_extra_list = []
self.extra_controller_list = []
self.extra_dl_list = []
self.ROI_extra_dl_list = []
blueprint_library = self.world.get_blueprint_library()
# start_pose = random.choice(self.map.get_spawn_points())
start_pose = self.map.get_spawn_points()[102]
##spawn points 시뮬레이션 상 출력##
# print(start_pose)
# for n, x in enumerate(self.map.get_spawn_points()):
# world.debug.draw_string(x.location, 'o', draw_shadow=True,
# color=carla.Color(r=0, g=255, b=255), life_time=30)
# self.load_traj()
self.waypoint = self.map.get_waypoint(start_pose.location,
lane_type=carla.LaneType.Driving)
self.end_point = self.waypoint.next(400)[0].transform.location
# print(self.waypoint.transform)
## Ego vehicle의 global Route 출력##
world.debug.draw_string(self.waypoint.next(400)[0].transform.location,
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=255),
life_time=100)
# print(start_pose)
# print(self.waypoint.transform)
# self.controller = MPCController.Controller
self.player = world.spawn_actor(
random.choice(blueprint_library.filter('vehicle.bmw.grandtourer')),
start_pose)
# print(self.player.bounding_box) # ego vehicle length
self.actor_list.append(self.player)
self.camera_rgb = RGBSensor(self.player, self.hud)
self.actor_list.append(self.camera_rgb.sensor)
# self.camera_depth =DepthCamera(self.player, self.hud)
# self.actor_list.append(self.camera_depth.sensor)
# self.camera_semseg = SegmentationCamera(self.player,self.hud)
# self.actor_list.append(self.camera_semseg.sensor)
self.collision_sensor = CollisionSensor(self.player,
self.hud) # 충돌 여부 판단하는 센서
self.actor_list.append(self.collision_sensor.sensor)
self.lane_invasion_sensor = LaneInvasionSensor(
self.player, self.hud) # lane 침입 여부 확인하는 센서
self.actor_list.append(self.lane_invasion_sensor.sensor)
self.gnss_sensor = GnssSensor(self.player)
self.actor_list.append(self.gnss_sensor.sensor)
# --------------
# Spawn Surrounding vehicles
# --------------
print("Generate Extra")
spawn_points = []
blueprints = self.world.get_blueprint_library().filter('vehicle.*')
blueprints = [
x for x in blueprints
if int(x.get_attribute('number_of_wheels')) == 4
]
# print(*blueprints)
for i in range(10, 10 * (self.extra_num) + 1, 10):
dl = random.choice([-1, 0, 1])
self.extra_dl_list.append(dl)
spawn_point = None
if dl == -1:
spawn_point = self.waypoint.next(
i)[0].get_left_lane().transform
elif dl == 0:
spawn_point = self.waypoint.next(i)[0].transform
elif dl == 1:
spawn_point = self.waypoint.next(
i)[0].get_right_lane().transform
else:
print("Except ")
spawn_point = carla.Transform(
(spawn_point.location + carla.Location(z=1)),
spawn_point.rotation)
spawn_points.append(spawn_point)
# print(blueprint_library.filter('vehicle.bmw.grandtourer'))
# blueprint = random.choice(blueprint_library.filter('vehicle.bmw.grandtourer'))
blueprint = random.choice(blueprints)
# print(blueprint.has_attribute('color'))
if blueprint.has_attribute('color'):
# color = random.choice(blueprint.get_attribute('color').recommended_values)
# print(blueprint.get_attribute('color').recommended_values)
color = '255,255,255'
blueprint.set_attribute('color', color)
extra = self.world.spawn_actor(blueprint, spawn_point)
self.extra_list.append(extra)
print('Extra Genration Finished')
self.spectator.set_transform(
carla.Transform(
self.player.get_transform().location + carla.Location(z=100),
carla.Rotation(pitch=-90)))
ROI = 1000
# extra_target_velocity = 10
port = 8000
traffic_manager = client.get_trafficmanager(port)
traffic_manager.set_global_distance_to_leading_vehicle(100.0)
# traffic_manager.set_synchronous_mode(True) # for std::out_of_range eeror
tm_port = traffic_manager.get_port()
for extra in self.extra_list:
extra.set_autopilot(True, tm_port)
# Pure_puresuit_controller(extra, self.waypoint, None, 50) # km/h
# target_velocity = 30 #random.randrange(10, 40) # km/h
# extra.enable_constant_velocity(extra.get_trzansform().get_right_vector() * target_velocity/3.6)
traffic_manager.auto_lane_change(extra, False)
# self.player.set_autopilot(True,tm_port)
# traffic_manager.auto_lane_change(self.player, False)
self.controller = Pure_puresuit_controller(self.player, self.waypoint,
self.extra_list, 70) # km/h
# target_velocity = 60 / 3.6
# forward_vec = self.player.get_transform().get_forward_vector()
# print(forward_vec)
# velocity_vec = target_velocity*forward_vec
# self.player.set_target_velocity(velocity_vec)
# print(velocity_vec)
# print(velocity_vec)
# client.get_trafficmanager.auto_lane_change(extra, False)
###Test####
# clock = pygame.time.Clock()
# Keyboardcontrol = KeyboardControl(self, False)
# display = pygame.display.set_mode(
# (self.width, self.height),
# pygame.HWSURFACE | pygame.DOUBLEBUF)
# while True:
# if Keyboardcontrol.parse_events(client, self, clock):
# return
# self.spectator.set_transform(
# carla.Transform(self.player.get_transform().location + carla.Location(z=50),
# carla.Rotation(pitch=-90)))
# self.camera_rgb.render(display)
# self.hud.render(display)
# pygame.display.flip()
#
# self.controller.apply_control()
# # self.world.wait_for_tick(10.0)
# clock.tick(30)
#
# self.hud.tick(self, clock)
#### Test Finished #####
#### Test2 #####
# cnt=0
# clock = pygame.time.Clock()
# while True:
# # print(self.waypoint.lane_id)
# self.spectator.set_transform(
# carla.Transform(self.player.get_transform().location + carla.Location(z=100),
# carla.Rotation(pitch=-90)))
# cnt += 1
# if cnt == 100:
# print('수해유 ㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠ')
# decision = 1
# self.controller.apply_control(decision)
# else:
# self.controller.apply_control()
# clock.tick(30)
#### Test2 Finished #####
# self.input_size = (self.extra_num)*4 + 1
self.input_size = 4 #dr dv da dl
self.output_size = 3
# for response in client.apply_batch_sync(batch):
# if response.error:
# logging.error(response.error)
# else:
# self.extra_list.append(response.actor_id)
print(5)
def scenario(scenario_params, agent_constructor, ctrl_params,
carla_synchronous, eval_env, eval_stg):
ego_vehicle = None
agent = None
other_vehicles = []
client, world, carla_map, traffic_manager = carla_synchronous
try:
map_reader = NaiveMapQuerier(world, carla_map, debug=True)
online_config = OnlineConfig(node_type=eval_env.NodeType)
# Mock vehicles
spawn_points = carla_map.get_spawn_points()
blueprints = get_all_vehicle_blueprints(world)
spawn_indices = [scenario_params.ego_spawn_idx
] + scenario_params.other_spawn_ids
other_vehicle_ids = []
for k, spawn_idx in enumerate(spawn_indices):
if k == 0:
blueprint = world.get_blueprint_library().find(
'vehicle.audi.a2')
else:
blueprint = np.random.choice(blueprints)
spawn_point = spawn_points[spawn_idx]
spawn_point = shift_spawn_point(carla_map, k,
scenario_params.spawn_shifts,
spawn_point)
# Prevent collision with road.
spawn_point.location += carla.Location(0, 0, 0.5)
vehicle = world.spawn_actor(blueprint, spawn_point)
if k == 0:
ego_vehicle = vehicle
else:
vehicle.set_autopilot(True, traffic_manager.get_port())
other_vehicles.append(vehicle)
other_vehicle_ids.append(vehicle.id)
world.tick()
agent = agent_constructor(
ego_vehicle,
map_reader,
other_vehicle_ids,
eval_stg,
n_burn_interval=scenario_params.n_burn_interval,
control_horizon=ctrl_params.control_horizon,
prediction_horizon=ctrl_params.prediction_horizon,
n_predictions=ctrl_params.n_predictions,
n_coincide=ctrl_params.n_coincide,
scene_builder_cls=TrajectronPlusPlusSceneBuilder,
log_agent=False,
log_cplex=False,
plot_scenario=True,
plot_simulation=True,
scene_config=online_config)
agent.start_sensor()
assert agent.sensor_is_listening
if scenario_params.goal:
agent.set_goal(scenario_params.goal.x,
scenario_params.goal.y,
is_relative=scenario_params.goal.is_relative)
"""Move the spectator to the ego vehicle.
The positioning is a little off"""
state = agent.get_vehicle_state()
goal = agent.get_goal()
if goal.is_relative:
location = carla.Location(x=state[0] + goal.x,
y=state[1] - goal.y,
z=state[2] + 50)
else:
location = carla.Location(x=state[0] + (state[0] - goal.x) / 2.,
y=state[1] - (state[1] + goal.y) / 2.,
z=state[2] + 50)
world.get_spectator().set_transform(
carla.Transform(location, carla.Rotation(pitch=-90)))
n_burn_frames = scenario_params.n_burn_interval * online_config.record_interval
if scenario_params.loop_type == LoopEnum.CLOSED_LOOP:
run_frames = scenario_params.run_interval * online_config.record_interval
elif isinstance(agent, OAAgent):
run_frames = ctrl_params.control_horizon * online_config.record_interval - 1
else:
run_frames = ctrl_params.n_coincide * online_config.record_interval - 1
for idx in range(n_burn_frames + run_frames):
control = None
for ctrl in scenario_params.controls:
if ctrl.interval[0] <= idx and idx <= ctrl.interval[1]:
control = ctrl.control
break
frame = world.tick()
agent.run_step(frame, control=control)
finally:
if agent:
agent.destroy()
if ego_vehicle:
ego_vehicle.destroy()
for other_vehicle in other_vehicles:
other_vehicle.destroy()
time.sleep(1)
def main(self):
PATH = "/home/a/RL_decision/trained_info.tar"
print(torch.cuda.get_device_name())
clock = pygame.time.Clock()
Keyboardcontrol = KeyboardControl(self, False)
display = pygame.display.set_mode((self.width, self.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
self.agent = decision_driving_Agent(self.input_size, self.output_size,
True, 1000, self.extra_num)
self.lane_start_point = [
carla.Location(x=14.905815, y=-135.747452, z=0.000000),
carla.Location(x=172.745468, y=-364.531799, z=0.000000),
carla.Location(x=382.441040, y=-212.488907, z=0.000000)
]
self.lane_finished_point = [
carla.Location(x=14.631096, y=-205.746918, z=0.000000),
carla.Location(x=232.962860, y=-364.149139, z=0.000000),
carla.Location(x=376.542816, y=-10.352980, z=0.000000)
]
self.lane_change_point = [
carla.Location(x=14.905815, y=-135.747452, z=0.000000),
carla.Location(x=14.631096, y=-205.746918, z=0.000000),
carla.Location(x=172.745468, y=-364.531799, z=0.000000),
carla.Location(x=232.962860, y=-364.149139, z=0.000000),
carla.Location(x=382.441040, y=-212.488907, z=0.000000),
carla.Location(x=376.542816, y=-10.352980, z=0.000000)
]
self.world.debug.draw_string(carla.Location(x=14.905815,
y=-135.747452,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
self.world.debug.draw_string(carla.Location(x=14.631096,
y=-205.746918,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
# ---------------------------#
self.world.debug.draw_string(carla.Location(x=172.745468,
y=-364.531799,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
self.world.debug.draw_string(carla.Location(x=232.962860,
y=-364.149139,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
# ---------------------------#
self.world.debug.draw_string(carla.Location(x=376.542816,
y=-10.352980,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
self.world.debug.draw_string(carla.Location(x=382.441040,
y=-212.488907,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
lane_distance_between_points = []
for i in range(len(self.lane_finished_point)):
lane_distance_between_points.append(
(self.lane_start_point[i].x -
self.lane_finished_point[i].x)**2 +
(self.lane_start_point[i].y -
self.lane_finished_point[i].y)**2 +
(self.lane_start_point[i].z -
self.lane_finished_point[i].z)**2)
epoch_init = 0
# if(os.path.exists(PATH)):
# print("저장된 가중치 불러옴")
# checkpoint = torch.load(PATH)
#
# self.agent.model.load_state_dict(checkpoint['model_state_dict'])
# device = torch.device('cuda')
# self.agent.model.to(device)
# self.agent.optimizer.load_state_dict(checkpoint['optimizer_state_dict'])
# self.agent.buffer = checkpoint['memorybuffer']
# epoch_init = checkpoint['epoch']
# self.agent.buffer.load_state_dict(self.save_dir['memorybuffer'])
# self.is_first_time = False
# controller = VehicleControl(self, True)
# self.player.apply_control(carla.Vehicle
# Control(throttle=1.0, steer=0.0, brake=0.0))
# vehicles = self.world.get_actors().filter('vehicle.*')
# for i in vehicles:
# print(i.id)
try:
n_iters = 150
is_error = 0
for epoch in range(epoch_init, n_iters):
# cnt = 0
first_time_print = True
[state, x_static] = self.get_next_state() #초기 상태 s0 초기화
while (True):
if Keyboardcontrol.parse_events(client, self, clock):
return
self.camera_rgb.render(display)
self.hud.render(display)
pygame.display.flip()
self.spectator.set_transform(
carla.Transform(
self.player.get_transform().location +
carla.Location(z=100), carla.Rotation(pitch=-90)))
## Get max lane ##
if self.section < len(lane_distance_between_points):
self.lane_distance_between_start = (
(self.player.get_transform().location.x -
self.lane_start_point[self.section].x)**2 +
(self.player.get_transform().location.y -
self.lane_start_point[self.section].y)**2)
self.lane_distance_between_end = (
(self.player.get_transform().location.x -
self.lane_finished_point[self.section].x)**2 +
(self.player.get_transform().location.y -
self.lane_finished_point[self.section].y)**2)
# print("self.lane_distance_between_start : ",self.lane_distance_between_start,"self.lane_distance_between_end :",self.lane_distance_between_end, "lane_distance_between_points[self.section]",lane_distance_between_points[self.section],"self.section :", self.section)
if max(lane_distance_between_points[self.section], self.lane_distance_between_start,
self.lane_distance_between_end) == \
lane_distance_between_points[self.section]:
self.max_Lane_num = 3
# world.debug.draw_string(self.player.get_transform().location, 'o', draw_shadow = True,
# color = carla.Color(r=255, g=255, b=0), life_time = 999)
elif max(lane_distance_between_points[self.section], self.lane_distance_between_start,
self.lane_distance_between_end) == \
self.lane_distance_between_start and self.max_Lane_num == 3:
self.section += 1
self.max_Lane_num = 4
if self.section >= len(lane_distance_between_points
): # when, section_num = 3
self.section = 0
## finished get max lane ##
##visualize when, max lane ==3 ##
if self.max_Lane_num == 3:
self.world.debug.draw_string(
self.waypoint.transform.location,
'o',
draw_shadow=True,
color=carla.Color(r=255, g=255, b=255),
life_time=9999)
# [self.extra_list, self.extra_dl_list] = self.agent.search_extra_in_ROI(self.extra_list,self.player,self.extra_dl_list)
## finished to visualize ##
if self.agent.is_training:
##dqn 과정##
# 가중치 초기화 (pytroch 내부)
# 입실론-그리디 행동 탐색 (act function)
# 메모리 버퍼에 MDP 튜플 얻기 ㅡ (step function)
# 메모리 버퍼에 MDP 튜플 저장 ㅡ
# optimal Q 추정 ㅡ (learning function)
# Loss 계산 ㅡ
# 가중치 업데이트 ㅡ
if epoch % 10 == 0:
# [w, b] = self.agent.model.parameters() # unpack parameters
self.save_dir = torch.save(
{
'epoch':
epoch,
'model_state_dict':
self.agent.model.state_dict(),
'optimizer_state_dict':
self.agent.optimizer.state_dict(),
'memorybuffer':
self.agent.buffer
}, PATH)
# print(clock.get_fps())
# if time.time() - self.simul_time >10 and clock.get_fps() < 15:
# self.restart()
# time.sleep(3)
# continue
if self.decision is not None:
[next_state, next_x_static] = self.get_next_state()
sample = [
state, x_static, self.decision, reward,
next_state, next_x_static, done
]
self.agent.buffer.append(sample)
# print("befor act")
self.decision = self.agent.act(state, x_static)
# print("after act")
# print(decision)
# if self.decision ==1 and self.max_Lane_num==self.ego_Lane:
# print( " ")
# print("befroe safte check/ 판단 :", self.decision, "최대 차선 :", self.max_Lane_num, "ego lane :",self.ego_Lane)
self.decision = self.safety_check(self.decision)
# print("판단 :", self.decision, "최대 차선 :", self.max_Lane_num, "ego lane :",self.ego_Lane)
# print("before")
is_error = self.controller.apply_control(self.decision)
# print("after")
# print("extra_controller 개수 :", len(self.extra_controller_list))
# for i in range(len(self.extra_controller_list)):
# self.extra_controller_list[i].apply_control()
# print("before step")
[state, x_static, decision, reward, __, _,
done] = self.step(self.decision)
# print("after step")
if done:
print("epsilon :", self.agent.epsilon)
print("epoch : ", epoch, "누적 보상 : ",
self.accumulated_reward)
writer.add_scalar('누적 보상 ',
self.accumulated_reward, epoch)
if len(self.agent.buffer.size()
) > self.agent.batch_size:
# print("start learning")
for i in range(300):
self.agent.learning()
# print("finished learning")
client.set_timeout(10)
self.restart()
break
else:
self.agent.act(state, x_static)
clock.tick(40)
self.hud.tick(self, clock)
finally:
print('\ndestroying %d vehicles' % len(self.extra_list))
# client.apply_batch([carla.command.DestroyActor(x) for x in self.extra_list])
print('destroying actors.')
# client.apply_batch([carla.command.DestroyActors(x.id) for x in self.actor_list])
# client.apply_batch([carla.command.DestroyActors(x.id) for x in self.extra_list])
for actor in self.actor_list:
# print("finally 에서 actor 제거 :", self.actor_list)
if actor.is_alive:
actor.destroy()
for extra in self.extra_list:
# print("finally 에서 actor 제거 :", self.extra_list)
if extra.is_alive:
extra.destroy()
# pygame.quit()
print('done.')
int(box[1][0]-box[0][0]), #width
int(box[1][1] - box[0][1]), #height
linewidth=1,edgecolor='r',facecolor='none')
PLT_IMG_AX.add_patch(rect)
print('successfully connected to Carla')
world = client.get_world()
settings = world.get_settings()
world.apply_settings(settings)
map = world.get_map()
bp = world.get_blueprint_library().find('sensor.camera.rgb')
bp.set_attribute('image_size_x', str(IM_WIDTH))
bp.set_attribute('image_size_y', str(IM_HEIGHT))
bp.set_attribute('fov', '110')
# Set the time in seconds between sensor captures
bp.set_attribute('sensor_tick', '0.2')
transform = carla.Transform(carla.Location(x=-65.0, y=3.0, z=6.0), carla.Rotation(yaw=180.0, pitch=-30.0))
camera = world.spawn_actor(bp, transform)
camera.listen(processImg)
dbp = world.get_blueprint_library().find('sensor.camera.depth')
dbp.set_attribute('image_size_x', str(IM_WIDTH))
dbp.set_attribute('image_size_y', str(IM_HEIGHT))
dbp.set_attribute('fov', '110')
dcamera = world.spawn_actor(dbp, transform)
dcamera.listen(processDepthImg)
_, PLT_IMG_AX = plt.subplots(1)
ani = FuncAnimation(plt.gcf(), imgUpdate, interval=50)
plt.show()
# image.raw_data
# # Default format (depends on the camera PostProcessing but always a numpy array).
def __init__(self, *args, **kwargs): #parent_actor, hud, gamma_correction
self.sensor = None
self.surface = None
self._parent = args[0]
self.hud = args[1]
self.recording = False
bound_y = 0.5 + self._parent.bounding_box.extent.y
Attachment = carla.AttachmentType
self._camera_transforms = [
(carla.Transform(carla.Location(x=-5.5, z=2.5),
carla.Rotation(pitch=8.0)), Attachment.SpringArm),
(carla.Transform(carla.Location(x=1.6, z=1.7)), Attachment.Rigid),
(carla.Transform(carla.Location(x=5.5, y=1.5,
z=1.5)), Attachment.SpringArm),
(carla.Transform(carla.Location(x=-8.0, z=6.0),
carla.Rotation(pitch=6.0)), Attachment.SpringArm),
(carla.Transform(carla.Location(x=-1, y=-bound_y,
z=0.5)), Attachment.Rigid)
]
self.transform_index = 1
self.sensors = [
['sensor.camera.rgb', cc.Raw, 'Camera RGB', {}],
['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)', {}],
['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)', {}],
[
'sensor.camera.depth', cc.LogarithmicDepth,
'Camera Depth (Logarithmic Gray Scale)', {}
],
[
'sensor.camera.semantic_segmentation', cc.Raw,
'Camera Semantic Segmentation (Raw)', {}
],
[
'sensor.camera.semantic_segmentation', cc.CityScapesPalette,
'Camera Semantic Segmentation (CityScapes Palette)', {}
],
[
'sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)', {
'range': '50'
}
], ['sensor.camera.dvs', cc.Raw, 'Dynamic Vision Sensor', {}],
[
'sensor.camera.rgb', cc.Raw, 'Camera RGB Distorted', {
'lens_circle_multiplier': '3.0',
'lens_circle_falloff': '3.0',
'chromatic_aberration_intensity': '0.5',
'chromatic_aberration_offset': '0'
}
]
]
world = self._parent.get_world()
bp_library = world.get_blueprint_library()
for item in self.sensors:
bp = bp_library.find(item[0])
if item[0].startswith('sensor.camera'):
bp.set_attribute('image_size_x', str(self.hud.dim[0]))
bp.set_attribute('image_size_y', str(self.hud.dim[1]))
if bp.has_attribute('gamma'):
bp.set_attribute('gamma', str(args[2]))
for attr_name, attr_value in item[3].items():
bp.set_attribute(attr_name, attr_value)
elif item[0].startswith('sensor.lidar'):
self.lidar_range = 50
for attr_name, attr_value in item[3].items():
bp.set_attribute(attr_name, attr_value)
if attr_name == 'range':
self.lidar_range = float(attr_value)
item.append(bp)
self.index = None
# ]
# ego_transforms = [
# carla.Transform(carla.Location(x=187.220924, y=198.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578)),
# carla.Transform(carla.Location(x=187.220924, y=195.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578)),
# carla.Transform(carla.Location(x=187.220924, y=201.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578)),
# carla.Transform(carla.Location(x=182.220924, y=198.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578)),
# carla.Transform(carla.Location(x=182.220924, y=195.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578)),
# carla.Transform(carla.Location(x=182.220924, y=201.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578)),
# carla.Transform(carla.Location(x=192.220924, y=198.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578)),
# carla.Transform(carla.Location(x=192.220924, y=195.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578)),
# carla.Transform(carla.Location(x=192.220924, y=201.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578))
# ]
ego_transforms = [
carla.Transform(carla.Location(x=187.220924, y=198.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578)),
# carla.Transform(carla.Location(x=167.220924, y=195.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578)),
# carla.Transform(carla.Location(x=167.220924, y=201.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578)),
# carla.Transform(carla.Location(x=162.220924, y=195.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578)),
# carla.Transform(carla.Location(x=162.220924, y=201.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578))
# carla.Transform(carla.Location(x=192.220924, y=195.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578)),
# carla.Transform(carla.Location(x=192.220924, y=201.343231, z=3.553675), carla.Rotation(pitch=-1.277402, yaw=-179.359268, roll=-0.017578))
]
def process_point_cloud(args, point_cloud_carla, save_lidar_data):
if save_lidar_data:
point_cloud_carla.save_to_disk(args.data_dir + '/lidar' +'/%.6d.ply' % point_cloud_carla.frame)
# Creating a numpy array as well. To be used later
pcd = np.copy(np.frombuffer(point_cloud_carla.raw_data, dtype=np.dtype('float32')))
pcd = np.reshape(pcd, (int(pcd.shape[0] / 4), 4))
