def main():
argparser = argparse.ArgumentParser(description=__doc__)
argparser.add_argument('-m',
'--milestone-number',
metavar='M',
default=1,
type=int,
help='Milestone number (default: 1)')
args = argparser.parse_args()
actor_list = []
try:
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
world = client.get_world()
blueprints = world.get_blueprint_library().filter('vehicle.*')
blueprints = [
x for x in blueprints
if int(x.get_attribute('number_of_wheels')) == 4
]
blueprints = [x for x in blueprints if not x.id.endswith('isetta')]
def try_spawn_random_vehicle_at(transform, recursion=0):
blueprint = random.choice(blueprints)
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
blueprint.set_attribute('role_name', 'autopilot')
vehicle = world.try_spawn_actor(blueprint, transform)
if vehicle is not None:
actor_list.append(vehicle)
print('spawned %r at %s' %
(vehicle.type_id, transform.location))
else:
if recursion > 20:
print('WARNING: vehicle not spawned, NONE returned')
else:
return try_spawn_random_vehicle_at(transform,
recursion + 1)
return vehicle
# Defining positions
ex1 = [
carla.Vector3D(42.5959, -4.3443, 1.8431),
carla.Vector3D(22, -4, 1.8431),
carla.Vector3D(9, -22, 1.8431)
]
ex2 = [
carla.Vector3D(42.5959, -4.3443, 1.8431),
carla.Vector3D(-30, 167, 1.8431)
]
ex3 = [
carla.Vector3D(42.5959, -4.3443, 1.8431),
carla.Vector3D(22, -4, 1.8431),
carla.Vector3D(9, -22, 1.8431)
]
#ex4 = [ carla.Vector3D(42.5959,-4.3443,1.8431), carla.Vector3D(134,-3,1.8431)]
#kzs2 = carla.Vector3D(-85,-23,1.8431)
milestones = [ex1, ex2, ex3]
ms = max(0, min(args.milestone_number - 1, len(milestones) - 1))
ex = milestones[ms]
end = ex[len(ex) - 1]
destination = ex[1]
# Getting waypoint to spawn
start = get_start_point(world, ex[0])
# Spawning
vehicle = try_spawn_random_vehicle_at(start.transform)
if vehicle == None:
return
# Setting autopilot
def route_finished(autopilot):
pos = autopilot.get_vehicle().get_transform().location
print("Vehicle arrived at destination: ", pos)
if pos.distance(carla.Location(end)) < 5.0:
print("Excercise route finished")
running = False
else:
autopilot.set_destination(end)
autopilot = ai.Autopilot(vehicle)
autopilot.set_destination(destination)
autopilot.set_route_finished_callback(route_finished)
if ms == 2:
spawn = start.get_right_lane()
kamikaze = try_spawn_random_vehicle_at(spawn.transform)
bp = world.get_blueprint_library().find('sensor.other.collision')
sensor = world.spawn_actor(bp,
carla.Transform(),
attach_to=kamikaze)
def _on_collision(self, event):
if not self:
return
print('Collision with: ', event.other_actor.type_id)
if event.other_actor.type_id.split('.')[0] == 'vehicle':
print("Test FAILED")
kamikaze.destroy()
sensor.destroy()
sensor.listen(lambda event: _on_collision(kamikaze, event))
control = carla.VehicleControl()
control.throttle = 1.0
control.steer = -0.07
control.brake = 0.0
control.hand_brake = False
kamikaze.apply_control(control)
ctr = 0
running = True
while running:
status = autopilot.update()
if status == ai.data.Status.ARRIVED:
ctr += 1
if ctr > 3:
running = False
else:
ctr = 0
#print "distance: ", vehicle.get_transform().location.distance(carla.Location(ex1[2]))
time.sleep(0.5)
finally:
print('destroying actors')
for actor in actor_list:
actor.destroy()
print('done.')
pygame.quit()
exit()
# Create behaviour tree
scenario = LaneCutIn(world, "LaneCutInDemo", vehicle, [traffic_vehicle])
root = scenario.create_tree()
behaviour_tree = py_trees.trees.BehaviourTree(root=root)
behaviour_tree.tick()
# Set sensor
bp_library = world.get_blueprint_library()
bp = bp_library.find('sensor.camera.rgb')
bp.set_attribute('image_size_x', str(1920 // 2))
bp.set_attribute('image_size_y', str(1080 // 2))
camera_transform = carla.Transform(carla.Location(x=-5.5, z=2.8),
carla.Rotation(pitch=-15))
camera_sensor = world.spawn_actor(bp, camera_transform, attach_to=vehicle)
bp = bp_library.find('sensor.lidar.ray_cast')
bp.set_attribute('range', '5000')
lidar_transform = carla.Transform(carla.Location(z=3),
carla.Rotation(pitch=0))
lidar_sensor = world.spawn_actor(bp, lidar_transform, attach_to=vehicle)
# Camera callback funtion
def parse_image(image):
image.convert(cc.Raw)
array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8"))
array = np.reshape(array, (image.height, image.width, 4))
def main():
actor_list = []
# In this tutorial script, we are going to add a vehicle to the simulation
# and let it drive in autopilot. We will also create a camera attached to
# that vehicle, and save all the images generated by the camera to disk.
try:
# First of all, we need to create the client that will send the requests
# to the simulator. Here we'll assume the simulator is accepting
# requests in the localhost at port 2000.
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
# Once we have a client we can retrieve the world that is currently
# running.
world = client.get_world()
# The world contains the list blueprints that we can use for adding new
# actors into the simulation.
blueprint_library = world.get_blueprint_library()
# Now let's filter all the blueprints of type 'vehicle' and choose one
# at random.
bp = random.choice(blueprint_library.filter('vehicle'))
# A blueprint contains the list of attributes that define a vehicle
# instance, we can read them and modify some of them. For instance,
# let's randomize its color.
color = random.choice(bp.get_attribute('color').recommended_values)
bp.set_attribute('color', color)
# Now we need to give an initial transform to the vehicle. We choose a
# random transform from the list of recommended spawn points of the map.
transform = random.choice(world.get_map().get_spawn_points())
# So let's tell the world to spawn the vehicle.
vehicle = world.spawn_actor(bp, transform)
# It is important to note that the actors we create won't be destroyed
# unless we call their "destroy" function. If we fail to call "destroy"
# they will stay in the simulation even after we quit the Python script.
# For that reason, we are storing all the actors we create so we can
# destroy them afterwards.
actor_list.append(vehicle)
print('created %s' % vehicle.type_id)
# Let's put the vehicle to drive around.
vehicle.set_autopilot(True)
# Let's add now a "depth" camera attached to the vehicle. Note that the
# transform we give here is now relative to the vehicle.
camera_bp = blueprint_library.find('sensor.camera.depth')
camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4))
camera = world.spawn_actor(camera_bp,
camera_transform,
attach_to=vehicle)
actor_list.append(camera)
print('created %s' % camera.type_id)
# Now we register the function that will be called each time the sensor
# receives an image. In this example we are saving the image to disk
# converting the pixels to gray-scale.
cc = carla.ColorConverter.LogarithmicDepth
camera.listen(lambda image: image.save_to_disk(
'_out/%06d.png' % image.frame_number, cc))
# Oh wait, I don't like the location we gave to the vehicle, I'm going
# to move it a bit forward.
location = vehicle.get_location()
location.x += 40
vehicle.set_location(location)
print('moved vehicle to %s' % location)
# But the city now is probably quite empty, let's add a few more
# vehicles.
transform.location += carla.Location(x=40, y=-3.2)
transform.rotation.yaw = -180.0
for x in range(0, 10):
transform.location.x += 8.0
bp = random.choice(blueprint_library.filter('vehicle'))
# This time we are using try_spawn_actor. If the spot is already
# occupied by another object, the function will return None.
npc = world.try_spawn_actor(bp, transform)
if npc is not None:
actor_list.append(npc)
npc.set_autopilot()
print('created %s' % npc.type_id)
time.sleep(5)
finally:
print('destroying actors')
for actor in actor_list:
actor.destroy()
print('done.')
def main():
actor_list = []
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
try:
world = client.get_world()
blueprints = world.get_blueprint_library().filter('vehicle.*')
blueprints = [
x for x in blueprints
if int(x.get_attribute('number_of_wheels')) == 4
]
blueprints = [x for x in blueprints if not x.id.endswith('isetta')]
def draw_forward_vector():
start_transform = world.get_map().get_waypoint(
actor_list[0].get_location()).transform
start_location = start_transform.location
f_vect = start_transform.get_forward_vector()
raiser = carla.Location(y=0.5)
line_end = start_location + carla.Location(x=f_vect.x * 5,
y=f_vect.y * 5)
world.debug.draw_line(start_location + raiser,
line_end + raiser,
thickness=0.3,
life_time=10.0)
def label_spawn_points():
wps = world.get_map().get_spawn_points()
for i in range(len(wps)):
world.debug.draw_string(wps[i].location, str(i))
# This is definition of a callback function that will be called when the autopilot arrives at destination
def route_finished(autopilot):
print("Vehicle arrived at destination")
# Function to spawn new vehicles
def try_spawn_random_vehicle_at(transform, destination):
blueprint = random.choice(blueprints)
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
blueprint.set_attribute('role_name', 'autopilot')
vehicle = world.try_spawn_actor(blueprint, transform)
if vehicle is not None:
actor_list.append(vehicle)
# Instead of setting default autopilot, we create our own and append it to the list of autopilots
autopilot = ai.Autopilot(vehicle)
autopilot.set_destination(destination.location)
# We also register callback to know when the vehicle has arrived at it's destination
autopilot.set_route_finished_callback(route_finished)
print('spawned %r at %s' %
(vehicle.type_id, transform.location))
return autopilot
return False
spawn_points = list(world.get_map().get_spawn_points())
print('found %d spawn points.' % len(spawn_points))
start = spawn_points[124]
end = spawn_points[36]
ex2 = [
carla.Transform(location=carla.Location(42.5959, -4.3443, 1.8431),
rotation=carla.Rotation(yaw=180)),
carla.Transform(location=carla.Location(x=-30, y=167, z=1.8431))
]
start, end = ex2
end = world.get_map().get_waypoint(end.location).transform
controller = try_spawn_random_vehicle_at(start, end)
# Infinite loop to update car status
while True:
status = controller.update()
render(controller.knowledge.retrieve_data('rgb_camera'))
from pygame.locals import K_ESCAPE
from pygame.locals import K_l
for event in pygame.event.get():
if event.type == pygame.QUIT:
return True
elif event.type == pygame.KEYUP:
if event.key == K_ESCAPE:
return True
finally:
print('\ndestroying %d actors' % len(actor_list))
client.apply_batch(
[carla.command.DestroyActor(x.id) for x in actor_list])
pygame.quit()
def restart(self):
"""
(Re)spawns the vehicle
Either at a given actor_spawnpoint or at a random Carla spawnpoint
:return:
"""
# Get vehicle blueprint.
blueprint = secure_random.choice(
self.world.get_blueprint_library().filter(self.actor_filter))
blueprint.set_attribute('role_name', "{}".format(self.role_name))
if blueprint.has_attribute('color'):
color = secure_random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
# Spawn the vehicle.
if not rospy.get_param('~spawn_ego_vehicle'):
actors = self.world.get_actors().filter(self.actor_filter)
for actor in actors:
if actor.attributes['role_name'] == self.role_name:
self.player = actor
break
else:
if self.actor_spawnpoint:
spawn_point = carla.Transform()
spawn_point.location.x = self.actor_spawnpoint.position.x
spawn_point.location.y = -self.actor_spawnpoint.position.y
spawn_point.location.z = self.actor_spawnpoint.position.z + \
2 # spawn 2m above ground
quaternion = (self.actor_spawnpoint.orientation.x,
self.actor_spawnpoint.orientation.y,
self.actor_spawnpoint.orientation.z,
self.actor_spawnpoint.orientation.w)
_, _, yaw = euler_from_quaternion(quaternion)
spawn_point.rotation.yaw = -math.degrees(yaw)
rospy.loginfo("Spawn {} at x={} y={} z={} yaw={}".format(
self.role_name, spawn_point.location.x,
spawn_point.location.y, spawn_point.location.z,
spawn_point.rotation.yaw))
if self.player is not None:
self.player.set_transform(spawn_point)
while self.player is None:
self.player = self.world.try_spawn_actor(
blueprint, spawn_point)
self.player_created = True
else:
if self.player is not None:
spawn_point = self.player.get_transform()
spawn_point.location.z += 2.0
spawn_point.rotation.roll = 0.0
spawn_point.rotation.pitch = 0.0
self.player.set_transform(spawn_point)
while self.player is None:
spawn_points = self.world.get_map().get_spawn_points()
spawn_point = secure_random.choice(
spawn_points) if spawn_points else carla.Transform()
self.player = self.world.try_spawn_actor(
blueprint, spawn_point)
self.player_created = True
if self.player_created:
# Read sensors from file
if not os.path.exists(self.sensor_definition_file):
raise RuntimeError(
"Could not read sensor-definition from {}".format(
self.sensor_definition_file))
json_sensors = None
with open(self.sensor_definition_file) as handle:
json_sensors = json.loads(handle.read())
# Set up the sensors
self.sensor_actors = self.setup_sensors(json_sensors["sensors"])
self.player_created = False
def main():
argparser = argparse.ArgumentParser(description=__doc__)
argparser.add_argument('--host',
metavar='H',
default='127.0.0.1',
help='IP of the host server (default: 127.0.0.1)')
argparser.add_argument('-p',
'--port',
metavar='P',
default=2000,
type=int,
help='TCP port to listen to (default: 2000)')
argparser.add_argument('-n',
'--number-of-vehicles',
metavar='N',
default=10,
type=int,
help='number of vehicles (default: 10)')
argparser.add_argument('-w',
'--number-of-walkers',
metavar='W',
default=50,
type=int,
help='number of walkers (default: 50)')
argparser.add_argument('--safe',
action='store_true',
help='avoid spawning vehicles prone to accidents')
argparser.add_argument('--filterv',
metavar='PATTERN',
default='vehicle.*',
help='vehicles filter (default: "vehicle.*")')
argparser.add_argument(
'--filterw',
metavar='PATTERN',
default='walker.pedestrian.*',
help='pedestrians filter (default: "walker.pedestrian.*")')
argparser.add_argument('-tm_p',
'--tm_port',
metavar='P',
default=8000,
type=int,
help='port to communicate with TM (default: 8000)')
argparser.add_argument('--sync',
action='store_true',
help='Synchronous mode execution')
args = argparser.parse_args()
logging.basicConfig(format='%(levelname)s: %(message)s',
level=logging.INFO)
vehicles_list = []
walkers_list = []
all_id = []
vehicle_locations = []
client = carla.Client(args.host, args.port)
client.set_timeout(10.0)
try:
traffic_manager = client.get_trafficmanager(args.tm_port)
traffic_manager.set_global_distance_to_leading_vehicle(2.0)
world = client.get_world()
synchronous_master = False
if args.sync:
settings = world.get_settings()
traffic_manager.set_synchronous_mode(True)
if not settings.synchronous_mode:
synchronous_master = True
settings.synchronous_mode = True
settings.fixed_delta_seconds = 0.05
world.apply_settings(settings)
else:
synchronous_master = False
blueprints = world.get_blueprint_library().filter(args.filterv)
blueprintsWalkers = world.get_blueprint_library().filter(args.filterw)
if args.safe:
blueprints = [
x for x in blueprints
if int(x.get_attribute('number_of_wheels')) == 4
]
blueprints = [x for x in blueprints if not x.id.endswith('isetta')]
blueprints = [
x for x in blueprints if not x.id.endswith('carlacola')
]
blueprints = [
x for x in blueprints if not x.id.endswith('cybertruck')
]
blueprints = [x for x in blueprints if not x.id.endswith('t2')]
spawn_points = world.get_map().get_spawn_points()
number_of_spawn_points = len(spawn_points)
if args.number_of_vehicles < number_of_spawn_points:
random.shuffle(spawn_points)
elif args.number_of_vehicles > number_of_spawn_points:
msg = 'requested %d vehicles, but could only find %d spawn points'
logging.warning(msg, args.number_of_vehicles,
number_of_spawn_points)
args.number_of_vehicles = number_of_spawn_points
# @todo cannot import these directly.
SpawnActor = carla.command.SpawnActor
SetAutopilot = carla.command.SetAutopilot
FutureActor = carla.command.FutureActor
# --------------
# Spawn vehicles
# --------------
batch = []
for n, transform in enumerate(spawn_points):
if n >= args.number_of_vehicles:
break
blueprint = random.choice(blueprints)
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
if blueprint.has_attribute('driver_id'):
driver_id = random.choice(
blueprint.get_attribute('driver_id').recommended_values)
blueprint.set_attribute('driver_id', driver_id)
blueprint.set_attribute('role_name', 'autopilot')
print("transform =", transform)
batch.append(
SpawnActor(blueprint,
transform).then(SetAutopilot(FutureActor, True)))
for response in client.apply_batch_sync(batch, synchronous_master):
if response.error:
logging.error(response.error)
else:
vehicles_list.append(response.actor_id)
vehicles_ids = vehicles_list
vehicles_actor = world.get_actors(vehicles_ids)
print(vehicles_actor)
# -------------
# Spawn Walkers
# -------------
# some settings
percentagePedestriansRunning = 0.0 # how many pedestrians will run
percentagePedestriansCrossing = 0.0 # how many pedestrians will walk through the road
# 1. take all the random locations to spawn
spawn_points = []
for i in range(args.number_of_walkers):
spawn_point = carla.Transform()
loc = world.get_random_location_from_navigation()
if (loc != None):
spawn_point.location = loc
spawn_points.append(spawn_point)
# 2. we spawn the walker object
batch = []
walker_speed = []
for spawn_point in spawn_points:
walker_bp = random.choice(blueprintsWalkers)
# set as not invincible
if walker_bp.has_attribute('is_invincible'):
walker_bp.set_attribute('is_invincible', 'false')
# set the max speed
if walker_bp.has_attribute('speed'):
if (random.random() > percentagePedestriansRunning):
# walking
walker_speed.append(
walker_bp.get_attribute('speed').recommended_values[1])
else:
# running
walker_speed.append(
walker_bp.get_attribute('speed').recommended_values[2])
else:
print("Walker has no speed")
walker_speed.append(0.0)
batch.append(SpawnActor(walker_bp, spawn_point))
results = client.apply_batch_sync(batch, True)
walker_speed2 = []
for i in range(len(results)):
if results[i].error:
logging.error(results[i].error)
else:
walkers_list.append({"id": results[i].actor_id})
walker_speed2.append(walker_speed[i])
walker_speed = walker_speed2
# 3. we spawn the walker controller
batch = []
walker_controller_bp = world.get_blueprint_library().find(
'controller.ai.walker')
for i in range(len(walkers_list)):
batch.append(
SpawnActor(walker_controller_bp, carla.Transform(),
walkers_list[i]["id"]))
results = client.apply_batch_sync(batch, True)
for i in range(len(results)):
if results[i].error:
logging.error(results[i].error)
else:
walkers_list[i]["con"] = results[i].actor_id
# 4. we put 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
if not args.sync or not synchronous_master:
world.wait_for_tick()
else:
world.tick()
# 5. initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...])
# set how many pedestrians can cross the road
world.set_pedestrians_cross_factor(percentagePedestriansCrossing)
for i in range(0, len(all_id), 2):
# start walker
all_actors[i].start()
# set walk to random point
all_actors[i].go_to_location(
world.get_random_location_from_navigation())
# max speed
all_actors[i].set_max_speed(float(walker_speed[int(i / 2)]))
print('spawned %d vehicles and %d walkers, press Ctrl+C to exit.' %
(len(vehicles_list), len(walkers_list)))
# example of how to use parameters
traffic_manager.global_percentage_speed_difference(30.0)
# testList=[]
a = Location(x=325.350983, y=-254.661804, z=-38.016857)
dead_points = [a]
for i in vehicles_actor:
vehicle_locations.append(0)
while True:
if args.sync and synchronous_master:
world.tick()
else:
world.wait_for_tick()
# time.sleep(1)
for i in range(len(vehicles_actor)):
vehicle_locations[i] = vehicles_actor[i].get_location()
# testVar = vehicle_locations[0]
# print(testVar)
for i in range(len(vehicle_locations)):
x_now = vehicle_locations[i].x
y_now = vehicle_locations[i].y
print("Actor {}: x = {}, y = {}".format(i, x_now, y_now))
if (x_now < -6 and y_now < -317) or (x_now > 96 and y_now > 430) or (x_now <-230 and y_now > 345 or (x_now < -118 and y_now < -290)) or (x_now > 255 and \
-116 < y_now < -108) or (290 < x_now < 302 and y_now < -280) or (312 < x_now < 338 and y_now < -270):
print("El actor {} se va".format(vehicles_actor[i]))
temp_transform = random.choice(
world.get_map().get_spawn_points())
temp_location = temp_transform.location
vehicles_actor[i].set_location(temp_location)
print("Teleported to safety")
# it_is_dead = vehicles_actor[i].destroy()
# vehicles_actor.remove(vehicles_actor[i])
# vehi
# if it_is_dead:
# print("Actor fuera de límites destruido")
finally:
if args.sync and synchronous_master:
settings = world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
world.apply_settings(settings)
print('\ndestroying %d vehicles' % len(vehicles_list))
client.apply_batch(
[carla.command.DestroyActor(x) for x in vehicles_list])
# stop walker controllers (list is [controller, actor, controller, actor ...])
for i in range(0, len(all_id), 2):
all_actors[i].stop()
print('\ndestroying %d walkers' % len(walkers_list))
client.apply_batch([carla.command.DestroyActor(x) for x in all_id])
time.sleep(0.5)
def 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 main():
#Add all actors to this list to easily disable all actors when closing down script.
actor_list = []
pygame.init()
client = carla.Client(
'localhost',
2000) #Host, port, threads. If threads = 0, use all available.
client.set_timeout(
5.0
) #Set the timeout in seconds allowed to block when doing networking calls.
world = client.get_world()
test = client.get_server_version()
print('enabling synchronous mode.')
settings = world.get_settings(
) #synchronous mode(bool), no_rendering_mode(bool), fixed_delta_seconds(float)
settings.synchronous_mode = True
print(settings)
world.apply_settings(settings)
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()
#Create vehicle
#print(blueprint_library.filter('vehicle')) print out all vehicles
vehicle = world.spawn_actor(
random.choice(
blueprint_library.filter('vehicle.mercedes-benz.coupe')),
start_pose)
actor_list.append(vehicle)
vehicle.set_simulate_physics(False)
#Create camera
camera_bp = blueprint_library.find('sensor.camera.rgb')
camera_bp.set_attribute('image_size_x', '1280')
camera_bp.set_attribute('image_size_y', '720')
camera_bp.set_attribute('fov', '75')
transform = carla.Transform(carla.Location(x=1, z=1.7),
carla.Rotation(pitch=-3))
camera = world.spawn_actor(camera_bp, transform, attach_to=vehicle)
'''
camera = world.spawn_actor(
blueprint_library.find('sensor.camera.rgb'),
carla.Transform(carla.Location(x=-5.5, z=1.3), carla.Rotation(pitch=0)),
attach_to=vehicle)
'''
actor_list.append(camera)
# Make sync queue for sensor data.
image_queue = queue.Queue()
camera.listen(image_queue.put)
frame = None
#Display window for viewing camera
display = pygame.display.set_mode((1280, 720),
pygame.HWSURFACE | pygame.DOUBLEBUF)
font = get_font()
clock = pygame.time.Clock()
while True:
if should_quit():
return
clock.tick()
world.tick()
ts = world.wait_for_tick()
if frame is not None:
if ts.frame_count != frame + 1:
logging.warning('frame skip!')
frame = ts.frame_count
while True:
image = image_queue.get()
if image.frame_number == ts.frame_count:
break
logging.warning(
'wrong image time-stampstamp: frame=%d, image.frame=%d',
ts.frame_count, image.frame_number)
fps = 30
distance = velocity() / fps
waypoint = random.choice(waypoint.next(distance))
vehicle.set_transform(waypoint.transform)
draw_image(display, image)
text_surface = font.render('% 5d FPS' % clock.get_fps(), True,
(255, 255, 255))
display.blit(text_surface, (8, 10))
pygame.display.flip()
finally:
print('\ndisabling synchronous mode.')
settings = world.get_settings()
settings.synchronous_mode = False
world.apply_settings(settings)
print('destroying actors.')
for actor in actor_list:
actor.destroy()
pygame.quit()
print('done.')
def main():
actor_list = []
verboseIsOn = 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'):
verboseIsOn = True
if verboseIsOn:
print('client version: %s' % client.get_client_version())
print('server version: %s' % client.get_server_version())
print('client to server connection status: {}'.format(
client.ping()))
print('\nRetrieving the world data from server...')
world = client.get_world()
if verboseIsOn:
print('{} \n'.format(world))
"""
Section for retrieving the blueprints and spawn the actors
"""
blueprint_library = world.get_blueprint_library()
if verboseIsOn:
print('Retrieving CARLA blueprint library...')
print('object: %s\nblueprint methods: %s\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
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=1.0))
time.sleep(3.5)
vehicleEgo.apply_control(carla.VehicleControl(brake=1.0))
time.sleep(1.5)
vehicleEgo.apply_control(carla.VehicleControl(throttle=.5, steer=-.5))
time.sleep(1.5)
vehicleEgo.apply_control(carla.VehicleControl(throttle=.25, steer=.5))
time.sleep(3)
vehicleEgo.apply_control(carla.VehicleControl(hand_brake=True))
time.sleep(3)
'''
'''
vehicle.set_autopilot()
time.sleep(18)
'''
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():
try:
client = carla.Client("localhost", 2000)
client.set_timeout(10.0)
world = client.load_world('Town05')
# set the weather
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=-190, y=1.29, z=75.0),
carla.Rotation(pitch=-88.0, yaw=-1.85,
roll=1.595))) # top view of intersection
env = CARLA_ENV(world)
time.sleep(2) # sleep for 2 seconds, wait the initialization to finish
# get traffic light list
traffic_light_list = get_traffic_lights(world.get_actors())
# get intersection list
intersection_list = create_intersections(env, 4, traffic_light_list)
# edit intersection
# these should be done with the help of the front end gui
init_intersection = intersection_list[0]
normal_intersections = intersection_list[1:]
init_intersection.add_ego_vehicle(safety_distance=15.0,
stop_choice="abrupt",
vehicle_color='255,255,255')
init_intersection.add_follow_vehicle(follow_distance=20.0,
stop_choice="penetrate",
penetrate_distance=2.0)
init_intersection.add_lead_vehicle(lead_distance=20.0,
stop_choice="abrupt")
init_intersection.add_vehicle(choice="left",
stop_choice="abrupt",
vehicle_color='255,255,255')
init_intersection.add_vehicle(choice="right", command="left")
# test edit settings
name1 = init_intersection.add_vehicle(choice="ahead", command="left")
name2 = init_intersection.add_vehicle(choice="ahead", command="right")
init_intersection.edit_vehicle_settings(name1,
choice="ahead",
vehicle_color='128,128,128')
init_intersection.edit_vehicle_settings(name2,
choice="ahead",
gap=15.0,
vehicle_color='128,128,128')
init_intersection.edit_traffic_light("subject")
init_intersection.edit_traffic_light("left",
red_start=40.0,
red_end=60.0,
yellow_start=30.0,
yellow_end=40.0,
green_start=0.0,
green_end=30.0)
init_intersection.edit_traffic_light("right",
red_start=0.0,
red_end=10.0,
yellow_start=10.0,
yellow_end=20.0,
green_start=20.0,
green_end=40.0)
init_intersection.edit_traffic_light("ahead",
red_start=20.0,
red_end=40.0,
yellow_start=10.0,
yellow_end=20.0,
green_start=0.0,
green_end=10.0)
intersection_list[1].add_vehicle(choice="ahead")
intersection_list[1].add_vehicle(choice="left", command="left")
intersection_list[1].add_vehicle(choice="right", command="left")
intersection_list[1].add_vehicle(choice="right", command="right")
intersection_list[1]._shift_vehicles(-10, choice="right", index=0)
#intersection_list[1].edit_traffic_light("left")
intersection_list[2].add_vehicle(choice="ahead")
intersection_list[2].add_vehicle(choice="left", command="left")
intersection_list[2].add_vehicle(choice="right", command="left")
intersection_list[2].add_vehicle(choice="right", command="right")
#intersection_list[2].edit_traffic_light("right")
intersection_list[3].add_vehicle(command="left")
intersection_list[3].add_vehicle()
intersection_list[3].edit_traffic_light("ahead")
# test import/export
init_setting = init_intersection.export_settings()
intersection_list[3].import_settings(init_setting)
intersection_list[3].add_vehicle(command="left")
intersection_list[3].add_vehicle()
third_setting = intersection_list[3].export_settings()
write_intersection_settings("third_intersection_setting",
third_setting)
new_third_setting = read_intersection_settings(
'third_intersection_setting')
intersection_list[2].import_settings(new_third_setting)
IntersectionBackend(env, intersection_list)
finally:
time.sleep(10)
env.destroy_actors()
def go():
client = carla.Client("127.0.0.1", 2000)
client.set_timeout(5.0)
world = client.load_world('Town03')
settings = world.get_settings()
settings.fixed_delta_seconds = 0.05
world.apply_settings(settings)
weather = carla.WeatherParameters(cloudyness=0.0,
precipitation=0.0,
precipitation_deposits=0.0,
wind_intensity=0.0,
sun_azimuth_angle=0.0,
sun_altitude_angle=0.0)
world.set_weather(weather)
blueprint_library = world.get_blueprint_library()
"""
for blueprint in blueprint_library.filter('sensor.*'):
print(blueprint.id)
exit(0)
"""
world_map = world.get_map()
vehicle_bp = random.choice(blueprint_library.filter('vehicle.bmw.*'))
vehicle = world.spawn_actor(vehicle_bp,
random.choice(world_map.get_spawn_points()))
#vehicle.set_autopilot(True)
blueprint = blueprint_library.find('sensor.camera.rgb')
blueprint.set_attribute('image_size_x', str(W))
blueprint.set_attribute('image_size_y', str(H))
blueprint.set_attribute('fov', '70')
blueprint.set_attribute('sensor_tick', '0.05')
transform = carla.Transform(carla.Location(x=0.8, z=1.45))
camera = world.spawn_actor(blueprint, transform, attach_to=vehicle)
camera.listen(cam_callback)
# TODO: wait for carla 0.9.7
imu_bp = blueprint_library.find('sensor.other.imu')
imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle)
imu.listen(imu_callback)
def destroy():
print("clean exit")
imu.destroy()
camera.destroy()
vehicle.destroy()
print("done")
atexit.register(destroy)
threading.Thread(target=health_function).start()
threading.Thread(target=fake_driver_monitoring).start()
# can loop
sendcan = messaging.sub_sock('sendcan')
rk = Ratekeeper(100)
steer_angle = 0
while 1:
vel = vehicle.get_velocity()
speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)
can_function(pm, speed, steer_angle, rk.frame, rk.frame % 500 == 499)
if rk.frame % 5 == 0:
throttle, brake, steer = sendcan_function(sendcan)
steer_angle += steer / 10000.0 # torque
vc = carla.VehicleControl(throttle=throttle,
steer=steer_angle,
brake=brake)
vehicle.apply_control(vc)
print(speed, steer_angle, vc)
rk.keep_time()
def setup_scenario(world,
client,
synchronous_master=False,
subject_behavior="normal"):
# @todo cannot import these directly.
SpawnActor = carla.command.SpawnActor
SetAutopilot = carla.command.SetAutopilot
FutureActor = carla.command.FutureActor
batch = []
ego_batch = []
vehicles_list = []
blueprints = world.get_blueprint_library()
blueprints = [x for x in blueprints if x.id.endswith("model3")]
all_waypoints = world.get_map().generate_waypoints(3)
# waypoint_list = filter_waypoints(road_id, lane_id)
left_most_lane = filter_waypoints(all_waypoints, 15, -3)
middle_lane = filter_waypoints(all_waypoints, 15, -5)
right_lane = filter_waypoints(all_waypoints, 15, -6)
sub_spawn_point = middle_lane[1].transform
manual_spawn_point = left_most_lane[1].transform
ego_spawn_point = right_lane[1].transform
sub_spawn_point = carla.Transform(
Location(x=sub_spawn_point.location.x,
y=sub_spawn_point.location.y,
z=0.5),
Rotation(yaw=sub_spawn_point.rotation.yaw),
)
ego_spawn_point = carla.Transform(
Location(x=ego_spawn_point.location.x,
y=ego_spawn_point.location.y,
z=0.5),
Rotation(yaw=ego_spawn_point.rotation.yaw),
)
manual_spawn_point = carla.Transform(
Location(x=manual_spawn_point.location.x,
y=manual_spawn_point.location.y,
z=0.5),
Rotation(yaw=manual_spawn_point.rotation.yaw),
)
# --------------
# Spawn vehicles
# --------------
# Manual Vehicle
blueprint = random.choice(blueprints)
color = "0,255,0"
blueprint.set_attribute("color", color)
blueprint.set_attribute("role_name", "hero")
batch.append(
SpawnActor(blueprint,
manual_spawn_point).then(SetAutopilot(FutureActor, False)))
# Subject Vehicle Details
blueprint = random.choice(blueprints)
color = "0,0,255"
blueprint.set_attribute("color", color)
blueprint.set_attribute("role_name", "autopilot")
batch.append(
SpawnActor(blueprint,
sub_spawn_point).then(SetAutopilot(FutureActor, True)))
# Ego Vehicle Details
color = "255,0,0"
blueprint.set_attribute("color", color)
blueprint.set_attribute("role_name", "ego")
ego_batch.append(
SpawnActor(blueprint,
ego_spawn_point).then(SetAutopilot(FutureActor, True)))
# Spawn
ego_vehicle_id = None
for response in client.apply_batch_sync(ego_batch, synchronous_master):
if response.error:
print("Response Error while applying ego batch!")
else:
# self.vehicles_list.append(response.actor_id)
ego_vehicle_id = response.actor_id
print("Ego vehicle id: ------------------------------", ego_vehicle_id)
for response in client.apply_batch_sync(batch, synchronous_master):
if response.error:
print("Response Error while applying batch!")
else:
vehicles_list.append(response.actor_id)
manual_vehicle = world.get_actors(vehicles_list)[0]
subject_vehicle = world.get_actors(vehicles_list)[1]
ego_vehicle = world.get_actors([ego_vehicle_id])[0]
print("Warm start initiated...")
warm_start_curr = 0
update_spectator(world, ego_vehicle)
while warm_start_curr < 3:
warm_start_curr += world.get_settings().fixed_delta_seconds
if synchronous_master:
world.tick()
else:
world.wait_for_tick()
client.apply_batch_sync([SetAutopilot(ego_vehicle, False)],
synchronous_master)
client.apply_batch_sync([SetAutopilot(subject_vehicle, False)],
synchronous_master)
if subject_behavior == "manual":
manual_agent = Agent(manual_vehicle)
else:
subject_agent = BehaviorAgent(subject_vehicle,
behavior=subject_behavior)
destination = carla.Location(x=240.50791931152344,
y=45.247249603271484,
z=0.0)
subject_agent.set_destination(subject_agent.vehicle.get_location(),
destination,
clean=True)
subject_agent.update_information(world)
print("Warm start finished...")
## Get current lane waypoints
ego_vehicle_location = ego_vehicle.get_location()
nearest_waypoint = world.get_map().get_waypoint(ego_vehicle_location,
project_to_road=True)
current_lane_waypoints = nearest_waypoint.next_until_lane_end(1)
if subject_behavior == "manual":
return (ego_vehicle, manual_vehicle, current_lane_waypoints,
manual_agent)
else:
return (ego_vehicle, subject_vehicle, current_lane_waypoints,
subject_agent)
def main():
argparser = argparse.ArgumentParser(description=__doc__)
argparser.add_argument('--host',
metavar='H',
default='127.0.0.1',
help='IP of the host server (default: 127.0.0.1)')
argparser.add_argument('-p',
'--port',
metavar='P',
default=2000,
type=int,
help='TCP port to listen to (default: 2000)')
argparser.add_argument('-n',
'--number-of-vehicles',
metavar='N',
default=30,
type=int,
help='number of vehicles (default: 10)')
argparser.add_argument('-w',
'--number-of-walkers',
metavar='W',
default=80,
type=int,
help='number of walkers (default: 50)')
argparser.add_argument('--safe',
action='store_true',
help='avoid spawning vehicles prone to accidents')
argparser.add_argument('--filterv',
metavar='PATTERN',
default='vehicle.*',
help='vehicles filter (default: "vehicle.*")')
argparser.add_argument(
'--filterw',
metavar='PATTERN',
default='walker.pedestrian.*',
help='pedestrians filter (default: "walker.pedestrian.*")')
argparser.add_argument('--tm-port',
metavar='P',
default=8000,
type=int,
help='port to communicate with TM (default: 8000)')
argparser.add_argument('--sync',
action='store_true',
help='Synchronous mode execution')
argparser.add_argument('--hybrid', action='store_true', help='Enanble')
argparser.add_argument('-s',
'--seed',
metavar='S',
type=int,
help='Random device seed')
argparser.add_argument('--car-lights-on',
action='store_true',
default=False,
help='Enable car lights')
argparser.add_argument('--query_object',
default='trashbag',
help='Query an object')
args = argparser.parse_args()
logging.basicConfig(format='%(levelname)s: %(message)s',
level=logging.INFO)
vehicles_list = []
walkers_list = []
all_id = []
client = carla.Client(args.host, args.port)
client.set_timeout(10.0)
synchronous_master = False
random.seed(args.seed if args.seed is not None else int(time.time()))
try:
world = client.get_world()
traffic_manager = client.get_trafficmanager(args.tm_port)
traffic_manager.set_global_distance_to_leading_vehicle(1.0)
if args.hybrid:
traffic_manager.set_hybrid_physics_mode(True)
if args.seed is not None:
traffic_manager.set_random_device_seed(args.seed)
if args.sync:
settings = world.get_settings()
traffic_manager.set_synchronous_mode(True)
if not settings.synchronous_mode:
synchronous_master = True
settings.synchronous_mode = True
settings.fixed_delta_seconds = 0.05
world.apply_settings(settings)
else:
synchronous_master = False
blueprints = world.get_blueprint_library().filter(args.filterv)
blueprintsWalkers = world.get_blueprint_library().filter(args.filterw)
if args.safe:
blueprints = [
x for x in blueprints
if int(x.get_attribute('number_of_wheels')) == 4
]
blueprints = [x for x in blueprints if not x.id.endswith('isetta')]
blueprints = [
x for x in blueprints if not x.id.endswith('carlacola')
]
blueprints = [
x for x in blueprints if not x.id.endswith('cybertruck')
]
blueprints = [x for x in blueprints if not x.id.endswith('t2')]
blueprints = sorted(blueprints, key=lambda bp: bp.id)
spawn_points = world.get_map().get_spawn_points()
number_of_spawn_points = len(spawn_points)
if args.number_of_vehicles < number_of_spawn_points:
random.shuffle(spawn_points)
elif args.number_of_vehicles > number_of_spawn_points:
msg = 'requested %d vehicles, but could only find %d spawn points'
logging.warning(msg, args.number_of_vehicles,
number_of_spawn_points)
args.number_of_vehicles = number_of_spawn_points
# @todo cannot import these directly.
SpawnActor = carla.command.SpawnActor
SetAutopilot = carla.command.SetAutopilot
SetVehicleLightState = carla.command.SetVehicleLightState
FutureActor = carla.command.FutureActor
SetPhysics = carla.command.SetSimulatePhysics
# --------------
# Spawn vehicles
# --------------
batch = []
for n, transform in enumerate(spawn_points):
if n >= args.number_of_vehicles:
break
blueprint = random.choice(blueprints)
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
if blueprint.has_attribute('driver_id'):
driver_id = random.choice(
blueprint.get_attribute('driver_id').recommended_values)
blueprint.set_attribute('driver_id', driver_id)
blueprint.set_attribute('role_name', 'autopilot')
# prepare the light state of the cars to spawn
light_state = vls.NONE
if args.car_lights_on:
light_state = vls.Position | vls.LowBeam | vls.LowBeam
# spawn the cars and set their autopilot and light state all together
batch.append(
SpawnActor(blueprint, transform).then(
SetAutopilot(FutureActor, True,
traffic_manager.get_port())).then(
SetVehicleLightState(
FutureActor, light_state)))
for response in client.apply_batch_sync(batch, synchronous_master):
if response.error:
logging.error(response.error)
else:
vehicles_list.append(response.actor_id)
# -------------
# Spawn Walkers
# -------------
# some settings
percentagePedestriansRunning = 0.0 # how many pedestrians will run
percentagePedestriansCrossing = 0.0 # how many pedestrians will walk through the road
# 1. take all the random locations to spawn
spawn_points = []
for i in range(args.number_of_walkers):
spawn_point = carla.Transform()
loc = world.get_random_location_from_navigation()
if (loc != None):
spawn_point.location = loc
spawn_points.append(spawn_point)
# 2. we spawn the walker object
batch = []
walker_speed = []
for spawn_point in spawn_points:
walker_bp = random.choice(blueprintsWalkers)
# set as not invincible
if walker_bp.has_attribute('is_invincible'):
walker_bp.set_attribute('is_invincible', 'false')
# set the max speed
if walker_bp.has_attribute('speed'):
if (random.random() > percentagePedestriansRunning):
# walking
walker_speed.append(
walker_bp.get_attribute('speed').recommended_values[1])
else:
# running
walker_speed.append(
walker_bp.get_attribute('speed').recommended_values[2])
else:
print("Walker has no speed")
walker_speed.append(0.0)
batch.append(SpawnActor(walker_bp, spawn_point))
results = client.apply_batch_sync(batch, True)
walker_speed2 = []
for i in range(len(results)):
if results[i].error:
logging.error(results[i].error)
else:
walkers_list.append({"id": results[i].actor_id})
walker_speed2.append(walker_speed[i])
walker_speed = walker_speed2
# 3. we spawn the walker controller
batch = []
walker_controller_bp = world.get_blueprint_library().find(
'controller.ai.walker')
for i in range(len(walkers_list)):
batch.append(
SpawnActor(walker_controller_bp, carla.Transform(),
walkers_list[i]["id"]))
results = client.apply_batch_sync(batch, True)
for i in range(len(results)):
if results[i].error:
logging.error(results[i].error)
else:
walkers_list[i]["con"] = results[i].actor_id
# 4. we put 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
if not args.sync or not synchronous_master:
world.wait_for_tick()
else:
world.tick()
# 5. initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...])
# set how many pedestrians can cross the road
world.set_pedestrians_cross_factor(percentagePedestriansCrossing)
for i in range(0, len(all_id), 2):
# start walker
all_actors[i].start()
# set walk to random point
all_actors[i].go_to_location(
world.get_random_location_from_navigation())
# max speed
all_actors[i].set_max_speed(float(walker_speed[int(i / 2)]))
print('spawned %d vehicles and %d walkers, press Ctrl+C to exit.' %
(len(vehicles_list), len(walkers_list)))
# example of how to use parameters
traffic_manager.global_percentage_speed_difference(30.0)
batch = []
object_spawns = []
try:
object_bp = random.choice(
world.get_blueprint_library().filter('*' + args.query_object +
'*'))
except:
raise RuntimeError('invalid query object')
for car_id in vehicles_list:
car = world.get_actor(car_id)
car_transform = car.get_transform()
car_loc = carla.Location(car_transform.location)
for i in range(5):
perturb = random.rand(3) / 1.5
car_loc.x += perturb[0] / 4.
car_loc.y += perturb[1] / 4.
car_loc.z += (1.0 + perturb[2])
batch.append(
SpawnActor(
object_bp,
carla.Transform(car_loc, car_transform.rotation)).then(
SetPhysics(FutureActor, True)))
for response in client.apply_batch_sync(batch, synchronous_master):
if response.error:
logging.error(response.error)
else:
object_spawns.append(response.actor_id)
while True:
if args.sync and synchronous_master:
world.tick()
else:
world.wait_for_tick()
finally:
if args.sync and synchronous_master:
settings = world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
world.apply_settings(settings)
print('\ndestroying %d vehicles' % len(vehicles_list))
client.apply_batch(
[carla.command.DestroyActor(x) for x in vehicles_list])
# stop walker controllers (list is [controller, actor, controller, actor ...])
for i in range(0, len(all_id), 2):
all_actors[i].stop()
print('\ndestroying %d walkers' % len(walkers_list))
client.apply_batch([carla.command.DestroyActor(x) for x in all_id])
print('\ndestroying %d objects' % len(object_spawns))
client.apply_batch(
[carla.command.DestroyActor(x) for x in object_spawns])
time.sleep(0.5)
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_vehicle = None
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)
ego_bp = world.get_blueprint_library().find('vehicle.tesla.model3')
ego_bp.set_attribute('role_name', 'ego')
print('\nEgo role_name is set')
ego_color = random.choice(
ego_bp.get_attribute('color').recommended_values)
ego_bp.set_attribute('color', ego_color)
print('\nEgo color is set')
spawn_points = world.get_map().get_spawn_points()
number_of_spawn_points = len(spawn_points)
# Near a cross road
# ego_transform = carla.Transform(carla.Location(x=-78.116066, y=-81.958496, z=-0.696164),
# carla.Rotation(pitch=1.174273, yaw=-90.156158, roll=0.000019))
# Transform(Location(x=166.122238, y=106.114136, z=0.221694), Rotation(pitch=0.000000, yaw=-177.648560, roll=0.000014))
ego_transform = carla.Transform(
carla.Location(x=166.122238, y=106.114136, z=0.821694),
carla.Rotation(pitch=0.000000, yaw=-177.648560, roll=0.000014))
ego_vehicle = world.spawn_actor(ego_bp, ego_transform)
# --------------
# 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_vehicle)
lidar_sen.listen(lambda point_cloud: process_point_cloud(
args, point_cloud, args.save_lidar_data))
# --------------
# Enable autopilot for ego vehicle
# --------------
ego_vehicle.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_vehicle,
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:
for i in range(10):
world.tick(
) # Sometimes the vehicle is spawned at a height
start_tf = ego_vehicle.get_transform()
start_tf = ego_vehicle.get_transform()
tf_matrix = np.array(start_tf.get_matrix())
print(tf_matrix)
start_vec = np.array([
start_tf.location.x, start_tf.location.y,
start_tf.location.z, 1
]).reshape(-1, 1)
print('Start Vec:', start_vec)
print('After TF:\n', np.linalg.pinv(tf_matrix) @ start_vec)
print('\nEgo Vehicle ID is: ', ego_vehicle.id)
# print(start_tf.transform(ego_vehicle.get_transform().location))
spectator.set_transform(dummy.get_transform())
if args.save_gt:
vehicle_tf = ego_vehicle.get_transform()
vehicle_tf_loc = np.array([
vehicle_tf.location.x, vehicle_tf.location.y,
vehicle_tf.location.z, 1
]).reshape(-1, 1)
vehicle_tf_odom = np.linalg.pinv(tf_matrix) @ vehicle_tf_loc
gt_array.append(vehicle_tf_odom.flatten()[:-1])
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_vehicle 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_vehicle.destroy()
def state2transform(state, z=0.):
return carla.Transform(carla.Location(x=state[0], y=state[1], z=z),
carla.Rotation(yaw=np.degrees(state[2])))
def __init__(self):
self.actor_list = []
pygame.init()
self.display = pygame.display.set_mode(
(IMAGE_WIDTH, IMAGE_HEIGHT), pygame.HWSURFACE | pygame.DOUBLEBUF)
self.font = get_font()
self.clock = pygame.time.Clock()
self.client = carla.Client('localhost', 2000)
self.client.set_timeout(30.0)
self.world = self.client.load_world(cfg.CARLA_TOWN)
self.map = self.world.get_map()
# Hide all objects on the map and show street only
# self.world.unload_map_layer(carla.MapLayer.All)
self.lanemarkings = LaneMarkings()
self.vehiclemanager = VehicleManager()
self.imagesaver = BufferedImageSaver(
f'{cfg.output_directory}/{cfg.CARLA_TOWN}/', cfg.number_of_images,
IMAGE_HEIGHT, IMAGE_WIDTH, 3, '')
self.labelsaver = LabelSaver(cfg.train_gt)
self.image_counter = 0
if (cfg.isThirdPerson):
self.camera_transforms = [
carla.Transform(carla.Location(x=-4.5, z=2.2),
carla.Rotation(pitch=-14.5)),
carla.Transform(carla.Location(x=-4.0, z=2.2),
carla.Rotation(pitch=-18.0))
]
else:
self.camera_transforms = [
carla.Transform(carla.Location(x=0.0, z=3.2),
carla.Rotation(pitch=-19.5)), # camera 1
carla.Transform(carla.Location(x=0.0, z=2.8),
carla.Rotation(pitch=-18.5)), # camera 2
carla.Transform(carla.Location(x=0.3, z=2.4),
carla.Rotation(pitch=-15.0)), # camera 3
carla.Transform(carla.Location(x=1.1, z=2.0),
carla.Rotation(pitch=-16.5)), # camera 4
carla.Transform(carla.Location(x=1.0, z=2.0),
carla.Rotation(pitch=-18.5)), # camera 5
carla.Transform(carla.Location(x=1.4, z=1.2),
carla.Rotation(pitch=-13.5)), # camera 6
carla.Transform(carla.Location(x=1.8, z=1.2),
carla.Rotation(pitch=-14.5)), # camera 7
carla.Transform(carla.Location(x=2.17, z=0.9),
carla.Rotation(pitch=-14.5)), # camera 8
carla.Transform(carla.Location(x=2.2, z=0.7),
carla.Rotation(pitch=-11.5))
] # camera 9
def create_npcs(client, args):
vehicles_list = []
walkers_list = []
all_id = []
client = carla.Client(args.host, args.port)
client.set_timeout(10.0)
synchronous_master = False
np.random.seed(args.seed if args.seed is not None else int(time.time()))
# spawn_npc.py try block BEGIN
world = client.get_world()
traffic_manager = client.get_trafficmanager(8000)
traffic_manager.set_global_distance_to_leading_vehicle(1.0)
if args.hybrid:
traffic_manager.set_hybrid_physics_mode(True)
if args.seed is not None:
traffic_manager.set_random_device_seed(args.seed)
# always sync
blueprints = world.get_blueprint_library().filter(args.filterv)
blueprintsWalkers = world.get_blueprint_library().filter(args.filterw)
if args.safe:
blueprints = [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4]
blueprints = [x for x in blueprints if not x.id.endswith('isetta')]
blueprints = [x for x in blueprints if not x.id.endswith('carlacola')]
blueprints = [x for x in blueprints if not x.id.endswith('cybertruck')]
blueprints = [x for x in blueprints if not x.id.endswith('t2')]
blueprints = sorted(blueprints, key=lambda bp: bp.id)
spawn_points = world.get_map().get_spawn_points()
number_of_spawn_points = len(spawn_points)
if args.number_of_vehicles < number_of_spawn_points:
np.random.shuffle(spawn_points)
elif args.number_of_vehicles > number_of_spawn_points:
msg = 'requested %d vehicles, but could only find %d spawn points'
logging.warning(msg, args.number_of_vehicles, number_of_spawn_points)
args.number_of_vehicles = number_of_spawn_points
# @todo cannot import these directly.
SpawnActor = carla.command.SpawnActor
SetAutopilot = carla.command.SetAutopilot
SetVehicleLightState = carla.command.SetVehicleLightState
FutureActor = carla.command.FutureActor
# --------------
# Spawn vehicles
# --------------
batch = []
for n, transform in enumerate(spawn_points):
if n >= args.number_of_vehicles:
break
blueprint = np.random.choice(blueprints)
if blueprint.has_attribute('color'):
color = np.random.choice(blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
if blueprint.has_attribute('driver_id'):
driver_id = np.random.choice(blueprint.get_attribute('driver_id').recommended_values)
blueprint.set_attribute('driver_id', driver_id)
blueprint.set_attribute('role_name', 'autopilot')
# prepare the light state of the cars to spawn
light_state = vls.NONE
if args.car_lights_on:
light_state = vls.Position | vls.LowBeam | vls.LowBeam
# spawn the cars and set their autopilot and light state all together
batch.append(SpawnActor(blueprint, transform)
.then(SetAutopilot(FutureActor, True, traffic_manager.get_port()))
.then(SetVehicleLightState(FutureActor, light_state)))
for response in client.apply_batch_sync(batch, synchronous_master):
if response.error:
logging.error(response.error)
else:
vehicles_list.append(response.actor_id)
# -------------
# Spawn Walkers
# -------------
# some settings
percentagePedestriansRunning = 0.0 # how many pedestrians will run
percentagePedestriansCrossing = 0.0 # how many pedestrians will walk through the road
# 1. take all the random locations to spawn
spawn_points = []
for i in range(args.number_of_walkers):
spawn_point = carla.Transform()
loc = world.get_random_location_from_navigation()
if (loc != None):
spawn_point.location = loc
spawn_points.append(spawn_point)
# 2. we spawn the walker object
batch = []
walker_speed = []
for spawn_point in spawn_points:
walker_bp = np.random.choice(blueprintsWalkers)
# set as not invincible
if walker_bp.has_attribute('is_invincible'):
walker_bp.set_attribute('is_invincible', 'false')
# set the max speed
if walker_bp.has_attribute('speed'):
if (np.random.random() > percentagePedestriansRunning):
# walking
walker_speed.append(walker_bp.get_attribute('speed').recommended_values[1])
else:
# running
walker_speed.append(walker_bp.get_attribute('speed').recommended_values[2])
else:
print("Walker has no speed")
walker_speed.append(0.0)
batch.append(SpawnActor(walker_bp, spawn_point))
results = client.apply_batch_sync(batch, True)
walker_speed2 = []
for i in range(len(results)):
if results[i].error:
logging.error(results[i].error)
else:
walkers_list.append({"id": results[i].actor_id})
walker_speed2.append(walker_speed[i])
walker_speed = walker_speed2
# 3. we spawn the walker controller
batch = []
walker_controller_bp = world.get_blueprint_library().find('controller.ai.walker')
for i in range(len(walkers_list)):
batch.append(SpawnActor(walker_controller_bp, carla.Transform(), walkers_list[i]["id"]))
results = client.apply_batch_sync(batch, True)
for i in range(len(results)):
if results[i].error:
logging.error(results[i].error)
else:
walkers_list[i]["con"] = results[i].actor_id
# 4. we put 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.tick()
# 5. initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...])
# set how many pedestrians can cross the road
world.set_pedestrians_cross_factor(percentagePedestriansCrossing)
for i in range(0, len(all_id), 2):
# start walker
all_actors[i].start()
# set walk to random point
all_actors[i].go_to_location(world.get_random_location_from_navigation())
# max speed
all_actors[i].set_max_speed(float(walker_speed[int(i/2)]))
print('spawned %d vehicles and %d walkers, press Ctrl+C to exit.' % (len(vehicles_list), len(walkers_list)))
# example of how to use parameters
traffic_manager.global_percentage_speed_difference(30.0)
# spawn_npc.py try block END
return vehicles_list, all_actors, all_id, walkers_list
def main(args):
""" The main function that orchestrates the setup of the world, connection
to the scenario and the subsequent logging of the frames for computation
of the mIoU.
Args:
args: The argparse.Namespace instance that is retrieved from parsing
the arguments.
"""
# Setup the world.
world = setup_world(args.host, args.port, args.delta)
# Retrieve the ego-vehicle from the simulation.
ego_vehicle = None
while ego_vehicle is None:
print("Waiting for the scenario to be ready ...")
sleep(1)
ego_vehicle = retrieve_actor(world, 'vehicle.*', 'hero')
world.tick()
# Transform of the cameras.
camera_transform = carla.Transform(location=carla.Location(1.5, 0.0, 1.4),
rotation=carla.Rotation(0, 0, 0))
# Connect the RGB camera to the vehicle.
rgb_camera = spawn_camera('sensor.camera.rgb', camera_transform,
ego_vehicle, *args.res.split('x'))
# Connect the Semantic segmentation camera to the vehicle.
semantic_camera = spawn_camera('sensor.camera.semantic_segmentation',
camera_transform, ego_vehicle,
*args.res.split('x'))
# Connect the depth camera to the vehicle.
depth_camera = spawn_camera('sensor.camera.depth', camera_transform,
ego_vehicle, *args.res.split('x'))
# Open the CSV file for writing.
csv_file = open(args.output, 'w')
csv_writer = csv.writer(csv_file)
# Create the cleanup function.
global CLEANUP_FUNCTION
CLEANUP_FUNCTION = functools.partial(
cleanup_function,
world=world,
cameras=[rgb_camera, semantic_camera, depth_camera],
csv_file=csv_file)
# Create a PyGame surface for debugging purposes.
width, height = map(int, args.res.split('x'))
surface = None
if args.visualize:
surface = pygame.display.set_mode((width, height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
# Register a callback function with the camera.
rgb_camera.listen(process_rgb_images)
semantic_camera.listen(process_semantic_images)
depth_camera_setup = DepthCameraSetup(
"depth_camera", width, height,
Transform.from_carla_transform(camera_transform))
depth_camera.listen(
functools.partial(process_depth_images,
depth_camera_setup=depth_camera_setup,
ego_vehicle=ego_vehicle,
speed=args.speed,
csv=csv_writer,
surface=surface,
visualize=args.visualize))
try:
# To keep the thread alive so that the images can be processed.
while True:
pass
except KeyboardInterrupt:
CLEANUP_FUNCTION()
sys.exit(0)
# print(tm)
transformed = (tm @ xyz.T).T
return transformed
# pre-define positions
#lvp1 = carla.Transform(carla.Location(x=92.3, y=-156.9, z=10.03), carla.Rotation(pitch=0, yaw=120, roll=0))
#lvp2 = carla.Transform(carla.Location(x=74.8, y=-155.6, z=10.17), carla.Rotation(pitch=0, yaw=120, roll=0))
#lvp3 = carla.Transform(carla.Location(x=72.6, y=-114.8, z=10.08), carla.Rotation(pitch=0, yaw=120, roll=0))
#lvp1 = carla.Transform(carla.Location(x=86, y=-140, z=10.03), carla.Rotation(pitch=0, yaw=120, roll=0))
#lvp2 = carla.Transform(carla.Location(x=80, y=-140, z=10.17), carla.Rotation(pitch=0, yaw=30, roll=0))
#lvp3 = carla.Transform(carla.Location(x=80, y=-132, z=10.08), carla.Rotation(pitch=0, yaw=120, roll=0))
#lvp1 = carla.Transform(carla.Location(x=92.3, y=-156.9, z=10.03), carla.Rotation(pitch=1.9, yaw=10, roll=1))
#lvp2 = carla.Transform(carla.Location(x=74.8, y=-155.6, z=10.17), carla.Rotation(pitch=3, yaw=55, roll=0.3))
#lvp3 = carla.Transform(carla.Location(x=72.6, y=-114.8, z=10.08), carla.Rotation(pitch=1.1, yaw=190, roll=0.5))
lvp1 = carla.Transform(carla.Location(x=92.3, y=-156.9, z=10),
carla.Rotation(pitch=3.9, yaw=10, roll=3.3))
lvp2 = carla.Transform(carla.Location(x=74.8, y=-155.6, z=10),
carla.Rotation(pitch=6, yaw=55, roll=4.3))
lvp3 = carla.Transform(carla.Location(x=72.6, y=-114.8, z=10),
carla.Rotation(pitch=5.1, yaw=190, roll=3.5))
lvp_list = [lvp1, lvp2, lvp3]
pkl_file = open('sensor_data7.pkl', 'rb')
data = pkl.load(pkl_file)
pkl_file.close()
def integrate_point_cloud(data_list, transform_list):
xyz_full = np.zeros((0, 3))
for data, lidar_transform in zip(data_list, lvp_list):
dx = -lidar_transform.location.x
def main():
argparser = argparse.ArgumentParser(description=__doc__)
argparser.add_argument('--host',
metavar='H',
default='127.0.0.1',
help='IP of the host server (default: 127.0.0.1)')
argparser.add_argument('-p',
'--port',
metavar='P',
default=2000,
type=int,
help='TCP port to listen to (default: 2000)')
argparser.add_argument('--fps',
metavar='N',
default=30,
type=int,
help='set fixed FPS, zero for variable FPS')
argparser.add_argument('--vertical-fov',
metavar='N',
default=30.0,
type=float,
help='radar\'s vertical FOV (default: 30.0)')
argparser.add_argument('--horizontal-fov',
metavar='N',
default=30.0,
type=float,
help='radar\'s horizontal FOV (default: 30.0)')
argparser.add_argument(
'--points-per-second',
metavar='N',
default=1500,
type=int,
help='radar\'s total points per second (default: 1500)')
argparser.add_argument(
'--range',
metavar='D',
default=100.0,
type=float,
help='radar\'s maximum range in meters (default: 100.0)')
argparser.add_argument('--time',
metavar='S',
default=10,
type=float,
help='time to run in seconds (default: 10)')
args = argparser.parse_args()
start_timestamp = int(time.time())
try:
client = carla.Client(args.host, args.port)
client.set_timeout(2.0)
world = client.get_world()
settings = world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = (1.0 /
args.fps) if args.fps > 0.0 else 0.0
world.apply_settings(settings)
bp_lb = world.get_blueprint_library()
start_transform = random.choice(world.get_map().get_spawn_points())
sensors_transform = carla.Transform(carla.Location(x=1.6, z=1.7))
# Car
mustang_bp = bp_lb.filter('vehicle.ford.mustang')[0]
mustang = world.spawn_actor(mustang_bp, start_transform)
# mustang.set_autopilot(True)
# Radar
radar_bp = bp_lb.filter('sensor.other.radar')[0]
# Set radar attributes
radar_bp.set_attribute('horizontal_fov',
str(args.horizontal_fov)) # degrees
radar_bp.set_attribute('vertical_fov',
str(args.vertical_fov)) # degrees
radar_bp.set_attribute('points_per_second',
str(args.points_per_second))
radar_bp.set_attribute('range', str(args.range)) # meters
# Spawn it
radar = world.spawn_actor(radar_bp, sensors_transform, mustang)
# Display Radar attributes
print(f"{'- Radar Information ':-<45}")
print(f" - {'ID':<23}{radar_bp.id}")
for attr in radar_bp:
print(f" - {attr.id+':':<23}{get_correct_type(attr)}")
print(f"{'':-<45}")
# Camera
camera_bp = bp_lb.filter('sensor.camera.rgb')[0]
# Set camera attributes
image_size = (1920, 1080)
camera_bp.set_attribute('image_size_x', str(image_size[0]))
camera_bp.set_attribute('image_size_y', str(image_size[1]))
# Spawn it
camera = world.spawn_actor(camera_bp, sensors_transform, mustang)
# Ploting variables
p_depth = []
p_azimuth = []
p_altitude = []
p_velocity = []
p_images = []
def radar_callback(weak_radar, sensor):
self = weak_radar()
if not self:
return
current_depth = []
current_azimuth = []
current_altitude = []
current_velocity = []
for i in sensor:
current_depth.append(i.depth)
current_azimuth.append(i.azimuth)
current_altitude.append(i.altitude)
current_velocity.append(i.velocity)
p_depth.append(current_depth)
p_azimuth.append(current_azimuth)
p_altitude.append(current_altitude)
p_velocity.append(current_velocity)
weak_radar = weakref.ref(radar)
radar.listen(lambda sensor: radar_callback(weak_radar, sensor))
def camera_callback(weak_camera, image):
self = weak_camera()
if not self:
return
# p_images.append(image)
# image.save_to_disk(f'_out_{start_timestamp}/{image.frame:08}')
weak_camera = weakref.ref(camera)
camera.listen(lambda image: camera_callback(weak_camera, image))
time_to_run = args.time # in seconds
initial_time = time.time()
close_time = initial_time + time_to_run
while time.time() < close_time:
sys.stdout.write(
f"\rElapsed time: {time.time() - initial_time:4.1f}/{time_to_run} s"
)
sys.stdout.flush()
world.tick()
# world.wait_for_tick()
# time.sleep(1)
finally:
print('')
try:
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
world.apply_settings(settings)
except NameError:
pass
try:
radar.destroy()
except NameError:
pass
try:
camera.destroy()
except NameError:
pass
sys.exit()
try:
mustang.destroy()
except NameError:
pass
print("0")
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import animation
print("1")
p_depth = np.array([np.array(i) for i in p_depth])
p_azimuth = np.array([np.array(i) for i in p_azimuth])
p_altitude = np.array([np.array(i) for i in p_altitude])
p_velocity = np.array([np.array(i) for i in p_velocity])
p_images = np.array([np.array(i.raw_data) for i in p_images])
# im = np.frombuffer(p_images[5], dtype=np.dtype("uint8"))
# im = im.reshape((image_size[1], image_size[0], 4))
# im = im[:, :, :3]
# im = im[:, :, ::-1]
# plt.imshow(im)
# plt.show()
# sys.exit(0)
# Animation ##################################################
fig, (ax_depth, ax_vel) = plt.subplots(nrows=1,
ncols=2,
figsize=(14, 6))
half_hor_fov_to_deg = math.radians(args.horizontal_fov) / 2.0
half_ver_fov_to_deg = math.radians(args.vertical_fov) / 2.0
horizontal_axes_limits = (-half_hor_fov_to_deg, half_hor_fov_to_deg)
vertical_axes_limits = (-half_ver_fov_to_deg, half_ver_fov_to_deg)
# - Depth Axes ----------------------------------
ax_depth.set(xlim=horizontal_axes_limits, ylim=vertical_axes_limits)
ax_depth.set_title('Depth (m)')
ax_depth.set_xlabel('Azimuth (rad)')
ax_depth.set_ylabel('Altitude (rad)')
# Initialize the depth scater plot without data
scat_depth = ax_depth.scatter(
np.array([]),
np.array([]),
c=np.array([]), # dot intensity/color
s=20, # dot size
vmin=0.0, # colorize from 0 m
vmax=args.range, # to Radar's maximum disance (far)
cmap='viridis') # viridis, plasma, gray...
fig.colorbar(scat_depth,
ax=ax_depth,
extend='max',
shrink=1.0,
pad=0.01)
# - Velocity Axes -------------------------------
ax_vel.set(xlim=horizontal_axes_limits, ylim=vertical_axes_limits)
ax_vel.set_title('Velocity (m/s)')
ax_vel.set_xlabel('Azimuth (rad)')
ax_vel.set_ylabel('Altitude (rad)')
# Initialize the velocity scater plot without data
scat_vel = ax_vel.scatter(
np.array([]),
np.array([]),
c=np.array([]), # dot intensity/color
s=20, # dot size
vmin=-10.0, # colorize from -10 m/s
vmax=10.0, # to +10 m/s
cmap='RdBu') # RdBu, Spectral, coolwarm...
fig.colorbar(scat_vel, ax=ax_vel, extend='both', shrink=1.0, pad=0.01)
# Animation initialization callback
def acc_init():
scat_depth.set_array(p_depth[0])
scat_vel.set_array(p_velocity[0])
scat_depth.set_offsets(np.c_[p_azimuth[0], p_altitude[0]])
scat_vel.set_offsets(np.c_[p_azimuth[0], p_altitude[0]])
return scat_depth, scat_vel
# Animation update callback
def radar_anim_update(i):
scat_depth.set_array(p_depth[i])
scat_vel.set_array(p_velocity[i])
scat_depth.set_offsets(np.c_[p_azimuth[i], p_altitude[i]])
scat_vel.set_offsets(np.c_[p_azimuth[i], p_altitude[i]])
return scat_depth, scat_vel
# Do the animation
radar_animation = animation.FuncAnimation(
fig,
radar_anim_update,
init_func=acc_init,
frames=len(p_azimuth),
interval=(1.0 / args.fps) * 1000 if args.fps > 0.0 else 0.0,
blit=True,
repeat=True)
plt.show()
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,
fps=30) 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 = round(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 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 setup_sensors(self, sensors):
"""
Create the sensors defined by the user and attach them to the ego-vehicle
:param sensors: list of sensors
:return:
"""
actors = []
bp_library = self.world.get_blueprint_library()
sensor_names = []
for sensor_spec in sensors:
try:
sensor_name = str(sensor_spec['type']) + "/" + str(
sensor_spec['id'])
if sensor_name in sensor_names:
rospy.logfatal(
"Sensor rolename '{}' is only allowed to be used once."
.format(sensor_spec['id']))
raise NameError(
"Sensor rolename '{}' is only allowed to be used once."
.format(sensor_spec['id']))
sensor_names.append(sensor_name)
bp = bp_library.find(str(sensor_spec['type']))
bp.set_attribute('role_name', str(sensor_spec['id']))
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']))
try:
bp.set_attribute('sensor_tick',
str(sensor_spec['sensor_tick']))
except KeyError:
pass
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'])
if sensor_spec['type'].startswith('sensor.camera.rgb'):
bp.set_attribute('gamma', str(sensor_spec['gamma']))
bp.set_attribute('shutter_speed',
str(sensor_spec['shutter_speed']))
bp.set_attribute('iso', str(sensor_spec['iso']))
bp.set_attribute('fstop', str(sensor_spec['fstop']))
bp.set_attribute('min_fstop',
str(sensor_spec['min_fstop']))
bp.set_attribute('blade_count',
str(sensor_spec['blade_count']))
bp.set_attribute('exposure_mode',
str(sensor_spec['exposure_mode']))
bp.set_attribute(
'exposure_compensation',
str(sensor_spec['exposure_compensation']))
bp.set_attribute(
'exposure_min_bright',
str(sensor_spec['exposure_min_bright']))
bp.set_attribute(
'exposure_max_bright',
str(sensor_spec['exposure_max_bright']))
bp.set_attribute('exposure_speed_up',
str(sensor_spec['exposure_speed_up']))
bp.set_attribute(
'exposure_speed_down',
str(sensor_spec['exposure_speed_down']))
bp.set_attribute(
'calibration_constant',
str(sensor_spec['calibration_constant']))
bp.set_attribute('focal_distance',
str(sensor_spec['focal_distance']))
bp.set_attribute('blur_amount',
str(sensor_spec['blur_amount']))
bp.set_attribute('blur_radius',
str(sensor_spec['blur_radius']))
bp.set_attribute(
'motion_blur_intensity',
str(sensor_spec['motion_blur_intensity']))
bp.set_attribute(
'motion_blur_max_distortion',
str(sensor_spec['motion_blur_max_distortion']))
bp.set_attribute(
'motion_blur_min_object_screen_size',
str(sensor_spec[
'motion_blur_min_object_screen_size']))
bp.set_attribute('slope', str(sensor_spec['slope']))
bp.set_attribute('toe', str(sensor_spec['toe']))
bp.set_attribute('shoulder',
str(sensor_spec['shoulder']))
bp.set_attribute('black_clip',
str(sensor_spec['black_clip']))
bp.set_attribute('white_clip',
str(sensor_spec['white_clip']))
bp.set_attribute('temp', str(sensor_spec['temp']))
bp.set_attribute('tint', str(sensor_spec['tint']))
bp.set_attribute(
'chromatic_aberration_intensity',
str(sensor_spec['chromatic_aberration_intensity']))
bp.set_attribute(
'chromatic_aberration_offset',
str(sensor_spec['chromatic_aberration_offset']))
bp.set_attribute(
'enable_postprocess_effects',
str(sensor_spec['enable_postprocess_effects']))
bp.set_attribute(
'lens_circle_falloff',
str(sensor_spec['lens_circle_falloff']))
bp.set_attribute(
'lens_circle_multiplier',
str(sensor_spec['lens_circle_multiplier']))
bp.set_attribute('lens_k', str(sensor_spec['lens_k']))
bp.set_attribute('lens_kcube',
str(sensor_spec['lens_kcube']))
bp.set_attribute('lens_x_size',
str(sensor_spec['lens_x_size']))
bp.set_attribute('lens_y_size',
str(sensor_spec['lens_y_size']))
elif sensor_spec['type'].startswith('sensor.lidar'):
bp.set_attribute('range', str(sensor_spec['range']))
bp.set_attribute('rotation_frequency',
str(sensor_spec['rotation_frequency']))
bp.set_attribute('channels', str(sensor_spec['channels']))
bp.set_attribute('upper_fov',
str(sensor_spec['upper_fov']))
bp.set_attribute('lower_fov',
str(sensor_spec['lower_fov']))
bp.set_attribute('points_per_second',
str(sensor_spec['points_per_second']))
try:
bp.set_attribute('sensor_tick',
str(sensor_spec['sensor_tick']))
except KeyError:
pass
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()
bp.set_attribute('noise_alt_stddev',
str(sensor_spec['noise_alt_stddev']))
bp.set_attribute('noise_lat_stddev',
str(sensor_spec['noise_lat_stddev']))
bp.set_attribute('noise_lon_stddev',
str(sensor_spec['noise_lon_stddev']))
bp.set_attribute('noise_alt_bias',
str(sensor_spec['noise_alt_bias']))
bp.set_attribute('noise_lat_bias',
str(sensor_spec['noise_lat_bias']))
bp.set_attribute('noise_lon_bias',
str(sensor_spec['noise_lon_bias']))
elif sensor_spec['type'].startswith('sensor.other.imu'):
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'])
bp.set_attribute('noise_accel_stddev_x',
str(sensor_spec['noise_accel_stddev_x']))
bp.set_attribute('noise_accel_stddev_y',
str(sensor_spec['noise_accel_stddev_y']))
bp.set_attribute('noise_accel_stddev_z',
str(sensor_spec['noise_accel_stddev_z']))
bp.set_attribute('noise_gyro_stddev_x',
str(sensor_spec['noise_gyro_stddev_x']))
bp.set_attribute('noise_gyro_stddev_y',
str(sensor_spec['noise_gyro_stddev_y']))
bp.set_attribute('noise_gyro_stddev_z',
str(sensor_spec['noise_gyro_stddev_z']))
bp.set_attribute('noise_gyro_bias_x',
str(sensor_spec['noise_gyro_bias_x']))
bp.set_attribute('noise_gyro_bias_y',
str(sensor_spec['noise_gyro_bias_y']))
bp.set_attribute('noise_gyro_bias_z',
str(sensor_spec['noise_gyro_bias_z']))
elif sensor_spec['type'].startswith('sensor.other.radar'):
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'])
bp.set_attribute('horizontal_fov',
str(sensor_spec['horizontal_fov']))
bp.set_attribute('vertical_fov',
str(sensor_spec['vertical_fov']))
bp.set_attribute('points_per_second',
str(sensor_spec['points_per_second']))
bp.set_attribute('range', str(sensor_spec['range']))
except KeyError as e:
rospy.logfatal(
"Sensor will not be spawned, because sensor spec is invalid: '{}'"
.format(e))
raise e
# create sensor
sensor_transform = carla.Transform(sensor_location,
sensor_rotation)
sensor = self.world.spawn_actor(bp,
sensor_transform,
attach_to=self.player)
actors.append(sensor)
return actors
def main():
vehicle = None
data = None
vehicle1 = None
vehicle2 = None
vehicle3 = None
vehicle4 = None
vehicle5 = None
actor_list = []
sensors = []
argparser = argparse.ArgumentParser(description=__doc__)
argparser.add_argument('--host',
metavar='H',
default='127.0.0.1',
help='IP of the host server (default: 127.0.0.1)')
argparser.add_argument('-p',
'--port',
metavar='P',
default=2000,
type=int,
help='TCP port to listen to (default: 2000)')
args = argparser.parse_args()
try:
client = carla.Client(args.host, args.port)
client.set_timeout(1.0)
world = client.get_world()
ourMap = world.get_map()
spectator = world.get_spectator()
#*************** deffinition of sensors ****************************
lidar_blueprint = world.get_blueprint_library().find(
'sensor.lidar.ray_cast')
camera_blueprint = world.get_blueprint_library().find(
'sensor.camera.rgb')
obstacleSensor_blueprint = world.get_blueprint_library().find(
'sensor.other.obstacle')
#*********** definition of blueprints for vehicles *******************
blueprint = world.get_blueprint_library().find(
'vehicle.audi.tt') #for main(ego) vehicle
blueprint.set_attribute('role_name', 'hero')
#*********** for non-player vehicles *********************************
non_playerBlueprint1 = random.choice(
world.get_blueprint_library().filter('vehicle.bmw.grandtourer'))
non_playerBlueprint2 = random.choice(
world.get_blueprint_library().filter('vehicle.tesla.*'))
non_playerBlueprint4 = random.choice(
world.get_blueprint_library().filter('vehicle.nissan.micra'))
non_playerBlueprint3 = random.choice(
world.get_blueprint_library().filter('vehicle.toyota.*'))
#******************************* set weather **********************************
weather = carla.WeatherParameters(cloudyness=0.0,
precipitation=0.0,
sun_altitude_angle=70.0,
sun_azimuth_angle=50.0)
world.set_weather(weather)
#************************ spawn non-player vehicles ******************************
class spawn_vehicle:
def __init__(self, blueprint, x, y):
self.vehicle = None
spawn_points = ourMap.get_spawn_points()
spawn_point = spawn_points[30]
spawn_point.location.x += x
spawn_point.location.y += y
self.vehicle = world.spawn_actor(blueprint, spawn_point)
# first non-player vehicle
data = spawn_vehicle(non_playerBlueprint2, -12, 1.0)
vehicle1 = data.vehicle
actor_list.append(vehicle1)
# 2nd non-player vehicle
data = spawn_vehicle(non_playerBlueprint1, -20, 1.0)
vehicle2 = data.vehicle
actor_list.append(vehicle2)
# 3rd non-player vehicle
data = spawn_vehicle(non_playerBlueprint3, 2.0, -1.5)
vehicle3 = data.vehicle
actor_list.append(vehicle3)
#4th nonplayer vehicle
data = spawn_vehicle(non_playerBlueprint4, 10.0, 1.0)
vehicle4 = data.vehicle
actor_list.append(vehicle4)
#********************** spawn main vehicle *****************************
data = spawn_vehicle(blueprint, -24.0, -1.5)
vehicle = data.vehicle
actor_list.append(vehicle)
#************************ camera-sensor settings ***********************
camera_location = carla.Transform(carla.Location(x=1.2, z=1.7))
camera_blueprint.set_attribute('sensor_tick', '0.4')
camera_sensor = world.spawn_actor(camera_blueprint,
camera_location,
attach_to=vehicle)
#=======================================obstacle sensors for maneuver==============================================================
class ObstacleSensor(object):
def __init__(self, parent_actor, x, y, z, angle):
self.sensor = None
self._parent = parent_actor
self.actor = 0.0
self.distance = 0.0
self.x = x
self.y = y
self.z = z
self.angle = angle
world = self._parent.get_world()
bp = world.get_blueprint_library().find(
'sensor.other.obstacle')
bp.set_attribute('debug_linetrace', 'true')
self.sensor = world.spawn_actor(
bp,
carla.Transform(
carla.Location(x=self.x, y=self.y, z=self.z),
carla.Rotation(yaw=self.angle)),
attach_to=self._parent)
sensors.append(self.sensor)
weak_self = weakref.ref(self)
self.sensor.listen(
lambda event: ObstacleSensor._on_event(weak_self, event))
@staticmethod
def _on_event(weak_self, event):
self = weak_self()
if not self:
return
self.actorId = event.other_actor.id
self.actorName = event.other_actor.type_id
self.distance = event.distance
forward_sensor = ObstacleSensor(vehicle,
x=1.5,
y=0.0,
z=0.6,
angle=180)
rear_sensor = ObstacleSensor(vehicle, x=-1.3, y=0.9, z=0.6, angle=90)
center_sensor = ObstacleSensor(vehicle, x=0.0, y=0.9, z=0.6, angle=90)
back_sensor = ObstacleSensor(vehicle, x=-1.3, y=0.0, z=0.6, angle=180)
def control():
if hasattr(rear_sensor, 'actorId') and rear_sensor.distance < 0.5:
print('stop')
'''angularVel=vehicle.get_angular_velocity();
angularVel.x = angularVel.y = angularVel.z = 0;
vehicle.set_angular_velocity(angularVel);
vel=vehicle.get_velocity();
vel.x = vel.y = vel.z = 0;
vehicle.set_velocity(vel);'''
vehicle.apply_control(
carla.VehicleControl(throttle=0.0, brake=1.0))
control = vehicle.get_control()
throttle = control.throttle
brake = control.brake
print('throttle', throttle, 'brake', brake)
print('rear_sensor info:', rear_sensor.actorId,
rear_sensor.actorName, rear_sensor.distance)
else:
#print('go!');
vehicle.apply_control(carla.VehicleControl(throttle=0.5))
if hasattr(back_sensor, 'actorId'):
print('back_sensor info:', back_sensor.actorId,
back_sensor.actorName, back_sensor.distance)
#control();
#set simulator view to the location of the vehicle
while True:
time.sleep(0.1)
control()
spectator.set_transform(get_transform(vehicle.get_location(),
-180))
finally:
print('\ndestroying %d actors' % len(actor_list))
for actor in actor_list:
if actor is not None:
actor.destroy()
for sensor in sensors:
if sensor is not None:
sensor.destroy()
#vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0))
#vehicle.set_autopilot(True) # if you just wanted some NPCs to drive.
actor_list.append(vehicle)
# https://carla.readthedocs.io/en/latest/cameras_and_sensors
# get the blueprint for this sensor
blueprint = blueprint_library.find('sensor.camera.semantic_segmentation')
# change the dimensions of the image
blueprint.set_attribute('image_size_x', f'{IM_WIDTH}')
blueprint.set_attribute('image_size_y', f'{IM_HEIGHT}')
blueprint.set_attribute('fov', '110')
#blueprint.set_attribute('sensor_tick', '0.2')
# Adjust sensor relative to vehicle
spawn_point = carla.Transform(carla.Location(x=2.5, z=0.7))
# spawn the sensor and attach to vehicle.
sensor = world.spawn_actor(blueprint, spawn_point, attach_to=vehicle)
# add sensor to list of actors
actor_list.append(sensor)
# Init display
pygame.init()
display_surface = pygame.display.set_mode(
(640, 480), pygame.HWSURFACE | pygame.DOUBLEBUF)
pygame.display.set_caption('CARLA image')
# do something with this sensor
sensor.listen(lambda data: process_img(data))
spawn_point = spawn_points[0]
spawn_point.location.y -= 30
spawn_point.location.x += 2
vehicle = world.spawn_actor(vehicle_bp, spawn_point)
spawn_point.location.y += 30
spawn_point.location.x += 2
vehicle2 = world.spawn_actor(vehicle_bp, spawn_point)
spawn_point.location.x -= 3
spawn_point.location.y += 5
vehicle3 = world.spawn_actor(vehicle_bp, spawn_point)
camera_spawn_point = carla.Transform(
carla.Location(x=vehicle.bounding_box.extent.x,
z=vehicle.bounding_box.extent.z / 2))
seg_cam = world.spawn_actor(seg_cam_bp, camera_spawn_point, attach_to=vehicle)
seg_cam.listen(
lambda sensor_data: sensor_callback(queue, 'segmentation', sensor_data))
depth_cam = world.spawn_actor(depth_cam_bp,
camera_spawn_point,
attach_to=vehicle)
depth_cam.listen(
lambda sensor_data: sensor_callback(queue, 'depth', sensor_data))
world.tick()
queue = Queue()
world.tick()
from lane_image_functions import * # lane detection image processing functions
from laneDetect import * # lane detection algorithm
""" Init Controller """
# init pygame
pygame.init()
# init joysticks functionality
pygame.joystick.init()
joystick = pygame.joystick.Joystick(0) # get joystick
joystick.init() # init joystick
controller = DS2_Controller(joystick)
""" CONSTANTS """
IM_WIDTH = 1280
IM_HEIGHT = 720
GLOBAL_FONT = cv2.FONT_HERSHEY_SIMPLEX
CAR_SPAWN_POINT = carla.Transform(carla.Location(x=-30, y=207.5, z=1))
WRITE_VID = False
# set up video writer
if WRITE_VID:
out = cv2.VideoWriter('outpy.avi',
cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'), 10,
(IM_WIDTH, IM_HEIGHT))
# list to store actor in the simulation
actor_list = []
# car control object
car_control = carla.VehicleControl()
# create pygame screen
def add_actors(client, num_actors, str_actor_type="walker.pedestrian.*"):
# 0. Choose a blueprint fo the walkers
world = client.get_world()
blueprintsWalkers = world.get_blueprint_library().filter(str_actor_type)
walker_bp = random.choice(blueprintsWalkers)
# 1. Take all the random locations to spawn
spawn_points = []
for i in range(num_actors):
spawn_point = carla.Transform()
spawn_point.location = world.get_random_location_from_navigation()
if (spawn_point.location != None):
spawn_points.append(spawn_point)
# 2. Build the batch of commands to spawn the pedestrians
batch = []
for spawn_point in spawn_points:
walker_bp = random.choice(blueprintsWalkers)
batch.append(carla.command.SpawnActor(walker_bp, spawn_point))
# 2.1 apply the batch
walkers_list = []
results = client.apply_batch_sync(batch, True)
for i in range(len(results)):
if results[i].error:
logging.error(results[i].error, results[i].actor_id)
else:
walkers_list.append({"id": results[i].actor_id})
# 3. Spawn walker AI controllers for each walker
batch = []
walker_controller_bp = world.get_blueprint_library().find(
'controller.ai.walker')
for i in range(len(walkers_list)):
batch.append(
carla.command.SpawnActor(walker_controller_bp, carla.Transform(),
walkers_list[i]["id"]))
# 3.1 apply the batch
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
all_id = []
# 4. Put altogether the walker and controller ids
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 [controller, actor, controller, actor ...])
for i in range(0, len(all_actors), 2):
# start walker
all_actors[i].start()
# set walk to random point
all_actors[i].go_to_location(
world.get_random_location_from_navigation())
# random max speed
all_actors[i].set_max_speed(
1 +
random.random()) # max speed between 1 and 2 (default is 1.4 m/s)
def main():
argparser = argparse.ArgumentParser(description=__doc__)
argparser.add_argument('--host',
metavar='H',
default='127.0.0.1',
help='IP of the host server (default: 127.0.0.1)')
argparser.add_argument('-p',
'--port',
metavar='P',
default=2000,
type=int,
help='TCP port to listen to (default: 2000)')
args = argparser.parse_args()
logging.basicConfig(format='%(levelname)s: %(message)s',
level=logging.INFO)
client = carla.Client(args.host, args.port)
client.set_timeout(10.0)
# ID for world.on_tick callback. Defined here so that it will be defined
# during the finally block below if something goes wrong before we define
# it in the try block.
bsd_id = 0
try:
world = client.get_world()
ego_vehicle = None
ego_cam = None
ego_depth = None
ego_sem = None
ego_col = None
ego_lane = None
ego_obs = None
ego_gnss = None
ego_imu = None
# Define the transform for the RGBd camera
cam_location = carla.Location(2, 0, 1)
cam_rotation = carla.Rotation(-10, 180, 0)
cam_transform = carla.Transform(cam_location, cam_rotation)
# Create the BackseatDriver
# No need to specify hazard labels; the defaults avoid cars,
# pedestrians, buildings, walls, and poles
my_backseat_driver = BackseatDriver(cam_transform,
update_rate=1,
debug=True)
# Register a simple "drive straight for a bit" trajectory.
# Backseat driver will ignore waypoints with a time in the past, so
# to force it to consider all waypoints, just add very large times
trajectory = np.array([
# t x y theta
[np.inf, 0, 0, 0],
[np.inf, 5, 0, 0],
[np.inf, 10, 0, 0],
[np.inf, 15, 0, 0],
[np.inf, 20, 0, 0],
[np.inf, 25, 0, 0]
])
my_backseat_driver.update_planned_trajectory(trajectory)
# --------------
# Start recording
# --------------
"""
client.start_recorder('~/tutorial/recorder/recording01.log')
"""
# --------------
# Spawn ego vehicle
# --------------
ego_bp = world.get_blueprint_library().find('vehicle.tesla.model3')
ego_bp.set_attribute('role_name', 'ego')
print('\nEgo role_name is set')
ego_color = random.choice(
ego_bp.get_attribute('color').recommended_values)
ego_bp.set_attribute('color', ego_color)
print('\nEgo color is set')
spawn_points = world.get_map().get_spawn_points()
number_of_spawn_points = len(spawn_points)
if 0 < number_of_spawn_points:
random.shuffle(spawn_points)
ego_transform = spawn_points[0]
ego_vehicle = world.spawn_actor(ego_bp, ego_transform)
print('\nEgo is spawned')
else:
logging.warning('Could not found any spawn points')
# --------------
# Add a RGB camera sensor to ego vehicle.
# --------------
cam_bp = None
cam_bp = world.get_blueprint_library().find('sensor.camera.rgb')
cam_bp.set_attribute("image_size_x", str(1920))
cam_bp.set_attribute("image_size_y", str(1080))
cam_bp.set_attribute("fov", str(105))
ego_cam = world.spawn_actor(
cam_bp,
cam_transform,
attach_to=ego_vehicle,
attachment_type=carla.AttachmentType.SpringArm)
# Wire up the RGB camera to the backseat_driver callback
ego_cam.listen(my_backseat_driver.semantic_segmentation_callback)
print("rgb camera up")
# --------------
# Add a depth camera sensor to ego vehicle.
# --------------
depth_bp = None
depth_bp = world.get_blueprint_library().find('sensor.camera.depth')
depth_bp.set_attribute("image_size_x", str(1920))
depth_bp.set_attribute("image_size_y", str(1080))
depth_bp.set_attribute("fov", str(105))
depth_location = carla.Location(2, 0, 1)
depth_rotation = carla.Rotation(-10, 180, 0)
depth_transform = carla.Transform(depth_location, depth_rotation)
ego_depth = world.spawn_actor(
depth_bp,
depth_transform,
attach_to=ego_vehicle,
attachment_type=carla.AttachmentType.SpringArm)
# Wire up the depth camera to the backseat_driver callback
ego_depth.listen(my_backseat_driver.depth_callback)
print("depth camera up")
# --------------
# Enable autopilot for ego vehicle
# --------------
ego_vehicle.set_autopilot(True)
# --------------
# Add the backseat driver to be called on every simulation tick
# --------------
bsd_id = world.on_tick(my_backseat_driver.get_safety_estimate)
# --------------
# Game loop. Prevents the script from finishing.
# --------------
while True:
world_snapshot = world.wait_for_tick()
finally:
# --------------
# Stop recording and destroy actors
# --------------
print("Cleaning up...")
world.remove_on_tick(bsd_id)
client.stop_recorder()
if ego_vehicle 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 ego_sem is not None:
ego_sem.stop()
ego_sem.destroy()
if ego_depth is not None:
ego_depth.stop()
ego_depth.destroy()
ego_vehicle.destroy()
def main(self, case):
try:
# initialize one painter
try:
self.painter = CarlaPainter('localhost', 8089)
except Exception as e:
print("NO PAINTER")
print(e)
self.painter = None
self.client = carla.Client('localhost', 2000)
self.client.set_timeout(10.0)
self.world = self.client.get_world()
# set synchronous mode
previous_settings = self.world.get_settings()
self.world.apply_settings(
carla.WorldSettings(synchronous_mode=True,
fixed_delta_seconds=TIMESTEP_DELTA))
self.tm = self.client.get_trafficmanager()
self.tm.set_synchronous_mode(True)
self.tm_port = self.tm.get_port()
print('tm port is: ', self.tm_port)
# create the specific case
results, callback = case(self.tm_port,
self.client.apply_batch_sync, self.world)
self.callback = callback
print('results', results)
self.vehicles.append(results[1])
if not results[0].error:
self.ego_vehicle = self.world.get_actor(results[0].actor_id)
self.ego_id = results[0].actor_id
else:
print('spawn ego error, exit')
self.ego_vehicle = None
return
self.timestep = 0
# batch = [carla.command.SpawnActor(blueprints_vehicles[0], ego_transform).then(carla.command.SetAutopilot(carla.command.FutureActor, False))]
# results = self.client.apply_batch_sync(batch, False)
# if not results[0].error:
# self.ego_vehicle = self.world.get_actor(results[0].actor_id)
# else:
# print('spawn ego error, exit')
# self.ego_vehicle = None
# return
# attach a camera and a lidar to the ego vehicle
blueprint_camera = self.world.get_blueprint_library().find(
'sensor.camera.rgb')
#blueprint_camera.set_attribute('image_size_x', '640')
#blueprint_camera.set_attribute('image_size_y', '480')
blueprint_camera.set_attribute('fov', '110')
#blueprint_camera.set_attribute('sensor_tick', '0.1')
transform_camera = carla.Transform(carla.Location(x=.0, z=1.8))
self.camera = self.world.spawn_actor(blueprint_camera,
transform_camera,
attach_to=self.ego_vehicle)
self.camera.listen(lambda data: self.camera_listener(data))
blueprint_lidar = self.world.get_blueprint_library().find(
'sensor.lidar.ray_cast')
# these specs follow the velodyne vlp32 spec
blueprint_lidar.set_attribute('range', '200')
#blueprint_lidar.set_attribute('range', '30')
blueprint_lidar.set_attribute('rotation_frequency', '10')
blueprint_lidar.set_attribute('channels', '32')
blueprint_lidar.set_attribute('lower_fov', '-25')
blueprint_lidar.set_attribute('upper_fov', '15')
blueprint_lidar.set_attribute('points_per_second', '578560')
self.blueprint_lidar = blueprint_lidar
transform_lidar = carla.Transform(carla.Location(x=0.0, z=1.8))
self.lidar = self.world.spawn_actor(blueprint_lidar,
transform_lidar,
attach_to=self.ego_vehicle)
self.lidar.listen(lambda data: self.lidar_listener(data))
# tick to generate these actors in the game world
self.world.tick()
# save vehicles' trajectories to draw in the frontend
trajectories = [[]]
self.obstacle = None
#self.obstacle = self.spawn_obstacle()
while (True):
self.world.tick()
strs = []
locs = []
loc = self.ego_vehicle.get_location()
strs.append("{:.2f}, ".format(loc.x) + "{:.2f}".format(loc.y) \
+ ", {:.2f}".format(loc.z))
locs.append([loc.x, loc.y, loc.z + 10.0])
# strs.append( "{:.2f}, ".format(loc.x-self.certificate_distance) + "{:.2f}".format(loc.y) \
# + ", {:.2f}".format(loc.z))
strs.append('closest point: ' + str(self.closest_point)[:4])
locs.append(
[loc.x - self.certificate_distance, loc.y, loc.z + 10.0])
strs.append("Certificate GOOD" if self.
certificate_result else "Certificate BAD")
if not self.certificate_result:
print('brake at ', self.timestep)
# vs = [self.world.get_actor(x.actor_id) for x in self.vehicles]
# print(self.world.get_actor(self.ego_id).get_velocity().x)
self.world.get_actor(self.ego_id).apply_control(
carla.VehicleControl(brake=1.0))
locs.append([loc.x - 5, loc.y - 5, loc.z + 20.0])
self.painter.draw_texts(strs, locs, size=20)
##@trajectories[0].append([ego_location.x, ego_location.y, ego_location.z])
## draw trajectories
#painter.draw_polylines(trajectories)
## draw ego vehicle's velocity just above the ego vehicle
#ego_velocity = ego_vehicle.get_velocity()
#velocity_str = "{:.2f}, ".format(ego_velocity.x) + "{:.2f}".format(ego_velocity.y) \
# + ", {:.2f}".format(ego_velocity.z)
self.callback(self.world, self.timestep)
self.timestep += 1
if self.timestep == 100:
print('now')
finally:
if previous_settings is not None:
self.world.apply_settings(previous_settings)
if self.lidar is not None:
self.lidar.stop()
self.lidar.destroy()
if self.camera is not None:
self.camera.stop()
self.camera.destroy()
if self.ego_vehicle is not None:
self.ego_vehicle.destroy()
if self.obstacle is not None:
self.obstacle.destroy()
self.client.apply_batch([
carla.command.DestroyActor(x.actor_id) for x in self.vehicles
])
