def __init__(self,
location: Location = None,
rotation: Rotation = None,
matrix=None):
if matrix is not None:
self.matrix = matrix
self.location = Location(matrix[0, 3], matrix[1, 3], matrix[2, 3])
# Forward vector is retrieved from the matrix.
self.forward_vector = \
Vector3D(self.matrix[0, 0], self.matrix[1, 0],
self.matrix[2, 0])
pitch_r = math.asin(np.clip(self.forward_vector.z, -1, 1))
yaw_r = math.acos(
np.clip(self.forward_vector.x / math.cos(pitch_r), -1, 1))
roll_r = math.asin(
np.clip(matrix[2, 1] / (-1 * math.cos(pitch_r)), -1, 1))
self.rotation = Rotation(math.degrees(pitch_r),
math.degrees(yaw_r), math.degrees(roll_r))
else:
self.location, self.rotation = location, rotation
self.matrix = Transform._create_matrix(self.location,
self.rotation)
# Forward vector is retrieved from the matrix.
self.forward_vector = \
Vector3D(self.matrix[0, 0], self.matrix[1, 0],
self.matrix[2, 0])
def main(argv):
global pixels_to_check
target_vehicle_transform = Transform(Location(242, 131.24, 0))
sensor_transform = Transform(Location(237.7, 132.24, 1.3))
pixels_to_check = [(200, 370)]
run_scenario(target_vehicle_transform, sensor_transform)
target_vehicle_transform = Transform(Location(2, 12, 0))
sensor_transform = Transform(Location(0, 18, 1.4),
Rotation(pitch=0, yaw=-90, roll=0))
pixels_to_check = [(500, 400), (600, 400), (500, 350), (600, 350)]
run_scenario(target_vehicle_transform, sensor_transform)
def use_sample(self, sample):
print('USE_SAMPLE Sample:', sample)
weather = carla.WeatherParameters(**{k: sample.params._asdict()[k] for k in WEATHER_PARAMS})
self.world.world.set_weather(weather)
for obj in sample.objects:
spawn = Transform(self.snap_to_ground(Location(x=obj.position[0],
y=-obj.position[1], z=1)),
Rotation(yaw=-obj.heading * 180 / math.pi - 90))
attrs = dict()
if 'color' in obj._fields:
color = str(int(obj.color.r * 255)) + ',' \
+ str(int(obj.color.g * 255)) + ',' + str(int(obj.color.b * 255))
attrs['color'] = color
if 'blueprint' in obj._fields:
attrs['blueprint_filter'] = obj.blueprint
agent = BrakeAgent
if 'agent' in obj._fields:
agent = AGENTS[obj.agent]
if obj.type in ['Vehicle', 'Car', 'Truck', 'Bicycle', 'Motorcycle']:
self.world.add_vehicle(agent,
spawn=spawn,
has_collision_sensor=False,
has_lane_sensor=False,
ego=obj is sample.objects[0],
**attrs)
elif obj.type == 'Pedestrian':
self.world.add_pedestrian(spawn=spawn, **attrs)
elif obj.type in ['Prop', 'Trash', 'Cone']:
self.world.add_prop(spawn=spawn, **attrs)
else:
print('Unsupported object type:', obj.type)
def spawn_vehicles(self):
self.bp_lib = self.world.get_blueprint_library()
ego_bp = self.bp_lib.filter('vehicle.tesla.model3')[0]
spawn_point = Transform(Location(x=392.1, y=10.0, z=0.02), Rotation(yaw=89.6))
self.ego_vehicle = self.world.spawn_actor(ego_bp, spawn_point)
self.setup_sensors(self.ego_vehicle)
self.actor.append(self.ego_vehicle)
self.ego_vehicle.get_world()
leading_bp = self.bp_lib.filter('vehicle.audi.a2')[0]
#leading_bp = self.bp_lib.filter('vehicle.audi.etron')[0]
#leading_bp = self.bp_lib.filter('vehicle.audi.tt')[0]
#leading_bp = random.choice(self.bp_lib.filter('vehicle.audi.*'))
spawn_point_leading = Transform(Location(x=392.1, y=320.0, z=0.0), Rotation(yaw=90))
self.leading_vehicle = self.world.try_spawn_actor(leading_bp, spawn_point_leading)
self.leading_vehicle.get_world()
self.actor.append(self.leading_vehicle)
def add_agent(self,
agent: ip.Agent,
rolename: str = None,
blueprint: carla.ActorBlueprint = None):
""" Add a vehicle to the simulation. Defaults to an Audi A2 for blueprints if not explicitly given.
Args:
agent: Agent to add.
rolename: Unique name for the actor to spawn.
blueprint: Optional blueprint defining the properties of the actor.
Returns:
The newly added actor.
"""
if blueprint is None:
blueprint_library = self.__world.get_blueprint_library()
blueprint = blueprint_library.find('vehicle.audi.a2')
if rolename is not None:
blueprint.set_attribute('role_name', rolename)
state = agent.state
yaw = np.rad2deg(-state.heading)
transform = Transform(Location(x=state.position[0], y=-state.position[1], z=0.1), Rotation(yaw=yaw))
actor = self.__world.spawn_actor(blueprint, transform)
actor.set_target_velocity(Vector3D(state.velocity[0], -state.velocity[1], 0.))
carla_agent = ip.carla.CarlaAgentWrapper(agent, actor)
self.agents[carla_agent.agent_id] = carla_agent
def main(argv):
client, world = get_world(FLAGS.simulator_host, FLAGS.simulator_port,
FLAGS.simulator_timeout)
# Replayer time factor is only available in > 0.9.5.
client.set_replayer_time_factor(0.1)
print(
client.replay_file(FLAGS.replay_file, FLAGS.replay_start_time,
FLAGS.replay_duration, FLAGS.replay_id))
# Sleep a bit to allow the server to start the replay.
time.sleep(1)
vehicle = world.get_actors().find(FLAGS.replay_id)
if vehicle is None:
raise ValueError("There was an issue finding the vehicle.")
# Install the camera.
camera_blueprint = world.get_blueprint_library().find('sensor.camera.rgb')
camera_blueprint.set_attribute('image_size_x',
str(FLAGS.camera_image_width))
camera_blueprint.set_attribute('image_size_y',
str(FLAGS.camera_image_height))
transform = Transform(Location(2.0, 0.0, 1.4),
Rotation(pitch=0, yaw=0, roll=0))
camera = world.spawn_actor(camera_blueprint, transform, attach_to=vehicle)
# Register the callback on the camera.
camera.listen(process_images)
time.sleep(20)
def reset(self, attempt=0):
logger.info('Resetting ...')
self._in_carla_autopilot = False
self._in_reverse = False
self._destroy()
#
blueprint_library = self._world.get_blueprint_library()
vehicle_bp = blueprint_library.find('vehicle.tesla.model3')
spawn_points = self._world.get_map().get_spawn_points()
spawn_id = self._spawn_preferred_id if attempt == 0 and self._spawn_preferred_id >= 0 else np.random.randint(
len(spawn_points))
spawn_point = spawn_points[spawn_id]
try:
self._actor = self._world.spawn_actor(vehicle_bp, spawn_point)
except RuntimeError as e:
if attempt < 4:
self.reset(attempt + 1)
else:
raise e
logger.info("Spawn point is '{}' with id {}.".format(
spawn_point, spawn_id))
# Attach the camera's - defaults at https://carla.readthedocs.io/en/latest/cameras_and_sensors/.
# Provide the position of the sensor relative to the vehicle.
# Tell the world to spawn the sensor, don't forget to attach it to your vehicle actor.
camera_front = self._world.spawn_actor(self._create_camera(),
Transform(
Location(x=1.25, z=1.4)),
attach_to=self._actor)
camera_rear = self._world.spawn_actor(self._create_camera(),
Transform(
Location(x=-1.0, z=2.0),
Rotation(pitch=180,
roll=180)),
attach_to=self._actor)
self._sensors.append(camera_front)
self._sensors.append(camera_rear)
# Subscribe to the sensor stream by providing a callback function,
# this function is called each time a new image is generated by the sensor.
camera_front.listen(lambda data: self._on_camera(data, camera=0))
camera_rear.listen(lambda data: self._on_camera(data, camera=1))
self._traffic_manager.ignore_lights_percentage(self._actor, 100.)
self._set_weather(use_preset=True)
# The tracker need recreation through the actor dependency.
if self._geo_tracker is not None:
self._geo_tracker.quit()
self._geo_tracker = GeoTrackerThread(self._world, self._actor)
self._geo_tracker.start()
def as_simulator_location(self):
"""Retrieves the location as a simulator location instance.
Returns:
An instance of the simulator class representing the location.
"""
from carla import Location
return Location(self.x, self.y, self.z)
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()
# Connect the segmentation camera to the vehicle.
segmentation_camera_transform = Transform(location=Location(1.0, 0.0, 1.8),
rotation=Rotation(0, 0, 0))
segmentation_camera, camera_setup = spawn_camera(
'sensor.camera.semantic_segmentation', segmentation_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=[segmentation_camera],
csv_file=csv_file)
# Register a callback function with the camera.
segmentation_camera.listen(
functools.partial(process_segmentation_images,
camera_setup=camera_setup,
ego_vehicle=ego_vehicle,
speed=args.speed,
csv=csv_writer,
dump=args.dump))
try:
# To keep the thread alive so that the images can be processed.
while True:
pass
except KeyboardInterrupt:
CLEANUP_FUNCTION()
sys.exit(0)
def as_simulator_transform(self):
"""Converts the transform to a simulator transform.
Returns:
An instance of the simulator class representing the Transform.
"""
from carla import Location, Rotation, Transform
return Transform(
Location(self.location.x, self.location.y, self.location.z),
Rotation(pitch=self.rotation.pitch,
yaw=self.rotation.yaw,
roll=self.rotation.roll))
def restart(self):
# Keep same camera config if the camera manager exists.
cam_index = self.camera_manager.index if self.camera_manager is not None else 0
cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0
# Get a random blueprint.
# blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter))
blueprint = random.choice(self.world.get_blueprint_library().filter('toyota'))
blueprint.set_attribute('role_name', self.actor_role_name)
if blueprint.has_attribute('color'):
# color = random.choice(blueprint.get_attribute('color').recommended_values)
# color = "255,255,255"
color = self.color
blueprint.set_attribute('color', color)
# Spawn the player.
if self.player is not None:
print("this is the playert 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.destroy()
self.player = self.world.try_spawn_actor(blueprint, spawn_point)
while self.player is None:
# def ego_spawn_point(points):
# for pt in points:
# if pt.location.x < 69 and pt.location.x > 68 and pt.location.y > 6 and pt.location.y < 7:
# return pt
# def hero_spawn_point(points):
# for pt in points:
# if pt.location.x < 69 and pt.location.x > 68 and pt.location.y > 7.5 and pt.location.y < 10:
# return pt
# spawn_points = self.map.get_spawn_points()
# spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform()
# if self.actor_role_name == "hero":
# spawn_point = hero_spawn_point(spawn_points)
# elif self.actor_role_name == "ego_vehicle":
# spawn_point = ego_spawn_point(spawn_points)
# print(" ========================================= ")
# for i in range(10):
# print(" these are all the available spawn points ", spawn_points[i])
# spawn_point = spawn_points[179]
spawn_point = Transform(Location(x=self.init_positionx, y=self.init_positiony, z=2.5), Rotation(pitch=1, yaw=180, roll=1))
self.player = self.world.try_spawn_actor(blueprint, spawn_point)
# Set up the sensors.
self.collision_sensor = CollisionSensor(self.player, self.hud)
self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud)
self.gnss_sensor = GnssSensor(self.player)
self.camera_manager = CameraManager(self.player, self.hud)
self.camera_manager.transform_index = cam_pos_index
self.camera_manager.set_sensor(cam_index, notify=False)
actor_type = get_actor_display_name(self.player)
self.hud.notification(actor_type)
def from_simulator_transform(cls, transform):
"""Creates a pylot transform from a simulator transform.
Args:
transform: A simulator transform.
Returns:
:py:class:`.Transform`: An instance of a pylot transform.
"""
from carla import Transform
if not isinstance(transform, Transform):
raise ValueError('transform should be of type Transform')
return cls(Location.from_simulator_location(transform.location),
Rotation.from_simulator_rotation(transform.rotation))
def routing_callback(self, msg):
if msg.status.error_code == ErrorCode.OK:
self.planned_trajectory = None
distance_threshold = 0.5
ego_location = self.carla_actor.get_transform().location
x0 = ego_location.x
y0 = -ego_location.y
apollo_wp = msg.routing_request.waypoint[0]
x1 = apollo_wp.pose.x
y1 = apollo_wp.pose.y
dist = np.linalg.norm([x1 - x0, y1 - y0])
if dist > distance_threshold:
carla_wp = self.parent.map.get_waypoint(Location(x1, -y1, 0))
self.carla_actor.set_transform(carla_wp.transform)
def transform_locations(self, locations):
"""Transforms the given set of locations (specified in the coordinate
space of the current transform) to be in the world coordinate space.
This method has the same functionality as transform_points, and
is provided for convenience; when dealing with a large number of
points, it is advised to use transform_points to avoid the slow
conversion between a numpy array and list of locations.
Args:
points (list(:py:class:`.Location`)): List of locations.
Returns:
list(:py:class:`.Location`): List of transformed points.
"""
points = np.array([loc.as_numpy_array() for loc in locations])
transformed_points = self.__transform(points, self.matrix)
return [Location(x, y, z) for x, y, z in transformed_points]
def main():
global world
client = Client('localhost', 2000)
world = client.get_world()
settings = world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = 1.0 / 10
world.apply_settings(settings)
# Spawn the vehicle.
vehicle = spawn_driving_vehicle(client, world)
# Spawn the camera and register a function to listen to the images.
camera = spawn_rgb_camera(world, Location(x=2.0, y=0.0, z=1.8),
Rotation(roll=0, pitch=0, yaw=0), vehicle)
camera.listen(process_images)
world.tick()
return vehicle, camera, world
def get_lane_marker_linestring_from_right_lane_road_and_lane_id(
world, road_id, lane_id):
all_waypoints = world.get_map().generate_waypoints(3)
filtered_waypoints = []
for wp in all_waypoints:
if wp.lane_id == lane_id and wp.road_id == road_id:
filtered_waypoints.append(wp)
first_wp = filtered_waypoints[0]
next_wps = []
for i in range(150):
next_wps.append(first_wp.next(3)[0])
first_wp = first_wp.next(3)[0]
lane_marker_locations = []
for wp in next_wps:
right_vector = wp.lane_width / 2 * getRightVector(
wp.transform.rotation)
lane_marker_location = Location(
x=wp.transform.location.x + right_vector.x,
y=wp.transform.location.y - right_vector.y,
z=wp.transform.location.z,
)
lane_marker_locations.append(lane_marker_location)
for loc in lane_marker_locations:
world.debug.draw_string(
loc,
"o",
draw_shadow=False,
color=carla.Color(r=0, g=255, b=0),
life_time=10,
)
linestring = LineString([[loc.x, loc.y] for loc in lane_marker_locations])
return linestring
def get_traffic_light_waypoints(self, traffic_light):
"""
get area of a given traffic light
"""
base_transform = traffic_light.get_transform()
base_rot = base_transform.rotation.yaw
area_loc = base_transform.transform(
traffic_light.trigger_volume.location)
# Discretize the trigger box into points
area_ext = traffic_light.trigger_volume.extent
x_values = np.arange(-0.9 * area_ext.x, 0.9 * area_ext.x,
1.0) # 0.9 to avoid crossing to adjacent lanes
area = []
for x in x_values:
point = Vector3D(
x, 0, area_ext.z).rotate(base_rot).as_simulator_vector()
point_location = area_loc + Location(x=point.x, y=point.y)
area.append(point_location)
# Get the waypoints of these points, removing duplicates
ini_wps = []
for pt in area:
wpx = self._map.get_waypoint(pt)
# As x_values are arranged in order, only the last one has
# to be checked
if not ini_wps or ini_wps[-1].road_id != wpx.road_id or ini_wps[
-1].lane_id != wpx.lane_id:
ini_wps.append(wpx)
# Advance them until the intersection
wps = []
for wpx in ini_wps:
while not wpx.is_intersection:
next_wp = wpx.next(0.5)[0]
if next_wp and not next_wp.is_intersection:
wpx = next_wp
else:
break
wps.append(wpx)
return area_loc, wps
def setup_sensors(self, ego_vehicle):
camera_bp = self.bp_lib.find('sensor.camera.rgb')
camera_bp.set_attribute('image_size_x', '800')
camera_bp.set_attribute('image_size_y', '600')
camera_bp.set_attribute('fov', '100')
self.camera = self.world.try_spawn_actor(
camera_bp,
Transform(Location(x=0.8,y=0.0,z=1.7), Rotation(yaw=0.0)),
attach_to=ego_vehicle)
self.actor.append(self.camera)
collision_bp = self.bp_lib.find('sensor.other.collision')
self.collision = self.world.try_spawn_actor(
collision_bp,
Transform(),
attach_to=self.ego_vehicle)
self.actor.append(self.collision)
self.image_queue = queue.Queue()
self.camera.listen(self.image_queue.put)
self.collision_queue = queue.Queue()
self.collision.listen(self.collision_queue.put)
def __init__(self, parent_actor, name, directory, H, W):
"""Constructor method"""
# self.sensor = None
self._parent = parent_actor
self.recording = False
self.directory = directory
self.name = name
self.BEV = np.zeros((3, H, W), dtype=np.uint8)
world = self._parent.get_world()
blueprint = world.get_blueprint_library().find(
'sensor.camera.semantic_segmentation') #semantic_segmentation
blueprint.set_attribute('image_size_x', str(W))
blueprint.set_attribute('image_size_y', str(H))
blueprint.set_attribute('fov', '90')
# Set the time in seconds between sensor captures
blueprint.set_attribute('sensor_tick', '0')
transform = Transform(Location(z=15), Rotation(pitch=-90))
self.sensor = world.spawn_actor(blueprint,
transform,
attach_to=self._parent)
# We need to pass the lambda a weak reference to
# self to avoid circular reference.
weak_self = weakref.ref(self)
self.sensor.listen(lambda image: Camera._parse_image(weak_self, image))
def restart(self):
# Keep same camera config if the camera manager exists.
cam_index = self.camera_manager.index if self.camera_manager is not None else 0
cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0
# Get a random blueprint.
# ******I have made this random.choice just have one choice --> vehicle.bmw.grandtourer
blueprint = random.choice(self.world.get_blueprint_library().filter(
self._actor_filter))
# print("blue print")
# print(self.world.get_blueprint_library().filter(self._actor_filter))
blueprint.set_attribute('role_name', 'hero')
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
# print("color")
# print(blueprint.get_attribute('color').recommended_values)
# ****** I have set the blueprint being white as '255,255,255'
blueprint.set_attribute('color', '255,255,255')
batch = []
blueprint2 = random.choice(self.world.get_blueprint_library().filter(
self._actor_filter))
blueprint2.set_attribute('role_name', 'hero2')
if blueprint2.has_attribute('color'):
blueprint2.set_attribute('color', '0,0,0')
# batch.append(SpawnActor(blueprint, transform))
# Spawn the player.
# ******************* change positions ****************************************
while self.player1 is None:
# spawn_points = self.map.get_spawn_points()
# print("length of sapwn_point is %d" % len(spawn_points) + ",just choose one") # 257 just choose one
x_rand = random.randint(18000, 23000)
x_rand_v2 = random.randint(x_rand + 1000, 25000)
x_rand = x_rand / 100.0
x_rand_v2 = x_rand_v2 / 100.0
y_rand = random.randint(12855, 13455)
y_rand = y_rand / 100.0
x_speed_randon_v2 = random.randint(100, 3000)
if x_speed_randon_v2 - 1000 > 0:
x_speed_player = x_speed_randon_v2 - 100
else:
x_speed_player = 0
x_speed_player = x_speed_player / 100
x_speed_randon_v2 = x_speed_randon_v2 / 100
spawn_point1 = Transform(
Location(x=x_rand, y=y_rand, z=1.320625),
Rotation(pitch=0.000000, yaw=179.999756, roll=0.000000))
# ********************************* end *****************************************
self.player1 = self.world.try_spawn_actor(blueprint, spawn_point1)
spawn_point2 = Transform(
Location(x=x_rand_v2, y=y_rand, z=1.320625),
Rotation(pitch=0.000000, yaw=179.999756, roll=0.000000))
if self.vehicle2 is None:
# print("*****************")
self.vehicle2 = self.world.try_spawn_actor(
blueprint2, spawn_point2)
self.vehicle2.set_velocity(
carla.Vector3D(x=-x_speed_randon_v2, y=0.00000,
z=0.000000))
self.player1.set_velocity(
carla.Vector3D(x=-x_speed_player, y=0.00000, z=0.00000))
# Set up the sensors.
self.collision_sensor = CollisionSensor(self.player1, self.hud)
self.lane_invasion_sensor = LaneInvasionSensor(self.player1, self.hud)
self.gnss_sensor = GnssSensor(self.player1)
self.camera_manager = CameraManager(self.player1, self.hud)
self.camera_manager.transform_index = cam_pos_index
self.camera_manager.set_sensor(cam_index, notify=False)
actor_type = get_actor_display_name(self.player1)
self.hud.notification(actor_type)
def main(x,y,z,pitch,yaw,roll,sensor_id,port,global_file):
img_size = np.asarray([960,540],dtype=np.int)
#pygame.init()
#display = pygame.display.set_mode((VIEW_WIDTH, VIEW_HEIGHT), pygame.HWSURFACE | pygame.DOUBLEBUF)
#font = get_font()
#clock = pygame.time.Clock()
client = carla.Client('localhost', 2000,1)
client.set_timeout(20.0)
# cam_subset=1
#client.reload_world(False)
#world = client.get_world()
world = client.load_world('Town03')
# weather = carla.WeatherParameters(
# cloudiness=0.0,
# precipitation=0.0,
# sun_altitude_angle=50.0)
#world.set_weather(all_weather[weather_num])
world.set_weather(getattr(carla.WeatherParameters, "ClearNoon"))
# 加入其他车辆
#Spawn_the_vehicles(world,client,car_num[0])
###########相机参数
# cam_type='cam1'
# with open("/media/wuminghu/hard/hard/carla/cam.txt") as csv_file:
# reader = csv.reader(csv_file, delimiter=' ')
# for line, row in enumerate(reader):
# if row[0] in cam_type:
# w_x,w_y,w_yaw,cam_H,cam_pitch=[float(i) for i in row[1:]]
# break
###########相机参数
# w_x,w_y,w_yaw,cam_H,cam_pitch=-116,100,100,4,90
actor_list = []
rgb_list = []
camera_bp = world.get_blueprint_library().find('sensor.camera.rgb')
sizex = str(960)
sizey = str(540)
fovcfg = str(90)
camera_bp.set_attribute('image_size_x',sizex )
camera_bp.set_attribute('image_size_y', sizey)
camera_bp.set_attribute('fov', fovcfg)
semseg_bp = world.get_blueprint_library().find('sensor.camera.semantic_segmentation')
semseg_bp.set_attribute('image_size_x', sizex)
semseg_bp.set_attribute('image_size_y', sizey)
semseg_bp.set_attribute('fov', fovcfg)
out_path="/home/ubuntu/xwp/datasets/multi_view_dataset/new_test"
if not os.path.exists(out_path):
os.makedirs(out_path)
cam_point = Transform(Location(x=x, y=y, z=z),
Rotation(pitch=pitch, yaw=yaw, roll=roll))
camera_rgb = world.spawn_actor(camera_bp,cam_point)
camera_seg = world.spawn_actor(semseg_bp,cam_point)
camera_rgb.sensor_name = sensor_id
camera_seg.sensor_name = sensor_id + "_seg"
actor_list.append(camera_rgb)
actor_list.append(camera_seg)
rgb_list .append(camera_rgb)
print('spawned: ',camera_rgb.sensor_name)
vehicles_actor = Spawn_the_vehicles(world,client,car_num[0],port)
print('spawned {} vehicles'.format(len(vehicles_actor)))
######################################################file
with CarlaSyncMode(world, *actor_list, fps=20) as sync_mode:
count=0
k=0
while count<60:
# while count<100:
count+=1
#clock.tick()
# Advance the simulation and wait for the data.
#snapshot, image_rgb,image_semseg = sync_mode.tick(timeout=2.0)
blobs = sync_mode.tick(timeout=2.0)
if count%10!=0:
continue
k+=1
print(k)
# img.append(image_rgb)
#image=image_rgb
# image1=image_semseg.convert(ColorConverter.CityScapesPalette)
# import pdb; pdb.set_trace()
all_images = blobs[1:]
#images = all_images
images = all_images[::2]
semseg_images = all_images[1::2]
snapshot = blobs[0]
fps = round(1.0 / snapshot.timestamp.delta_seconds)
world_snapshot = world.get_snapshot()
actual_actor=[world.get_actor(actor_snapshot.id) for actor_snapshot in world_snapshot]
got_vehicles=[actor for actor in actual_actor if actor.type_id.find('vehicle')!=-1]
vehicles_list = []
for camera_rgb in rgb_list:
vehicles=[vehicle for vehicle in got_vehicles if vehicle.get_transform().location.distance(camera_rgb.get_transform().location)<65]
vehicles_list.append(vehicles)
cam_path = out_path + "/{}".format(camera_rgb.sensor_name)
if not os.path.exists(cam_path):
os.makedirs(cam_path)
os.makedirs(cam_path+"/label_2")
os.makedirs(cam_path+"/image_2")
os.makedirs(cam_path+"/calib")
# debug = world.debug
# for vehicle in vehicles:
# debug.draw_box(carla.BoundingBox(vehicle.get_transform().location+vehicle.bounding_box.location, vehicle.bounding_box.extent), vehicle.get_transform().rotation, 0.05, carla.Color(255,0,0,0), life_time=0.05)
# import pdb; pdb.set_trace()
all_vehicles_list = []
for i,(camera_rgb, vehicles,image,osemseg) in enumerate(zip(rgb_list,vehicles_list,images,semseg_images)):
v=[] #filtered_vehicles
# Convert semseg to cv.image
semseg = np.frombuffer(osemseg.raw_data, dtype=np.dtype("uint8"))
semseg = np.reshape(semseg, (osemseg.height, osemseg.width, 4))
semseg = semseg[:, :, :3]
semseg = semseg[:, :, ::-1]
# BGR
# Done
label_files=open("{}/{}/label_2/{:0>6d}.txt".format(out_path,camera_rgb.sensor_name,k), "w")
car_2d_bbox=[]
for car in vehicles:
extent = car.bounding_box.extent
###############location
car_location =car.bounding_box.location
# car_location1=car.get_transform().location
cords = np.zeros((1, 4))
cords[0, :]=np.array([car_location.x,car_location.y,car_location.z, 1])
cords_x_y_z = ClientSideBoundingBoxes._vehicle_to_sensor(cords, car, camera_rgb)[:3, :]
cords_y_minus_z_x = np.concatenate([cords_x_y_z[1, :], -cords_x_y_z[2, :], cords_x_y_z[0, :]])
###############location
bbox = np.transpose(np.dot(camera_coordinate(camera_rgb), cords_y_minus_z_x))
camera_bbox = (np.concatenate([bbox[:, 0] / bbox[:, 2], bbox[:, 1] / bbox[:, 2], bbox[:, 2]], axis=1))
# pdb.set_trace()
camera_bbox_e = camera_bbox[0]
# if camera_bbox_e[:,0]<0 or camera_bbox_e[:,1]<0 or camera_bbox_e[:,2]<0:
# continue
if camera_bbox_e[:,2]<0:
continue
#bboxe = camera_bbox
xmin,ymin,xmax,ymax=0,0,0,0
bboxe = ClientSideBoundingBoxes.get_bounding_boxes([car], camera_rgb)
if len(bboxe)==0:
continue
bboxe=bboxe[0]
bbox_in_image = (np.min(bboxe[:,0]), np.min(bboxe[:,1]), np.max(bboxe[:,0]), np.max(bboxe[:,1]))
bbox_crop = tuple(max(0, b) for b in bbox_in_image)
bbox_crop = (min(img_size[0], bbox_crop[0]),
min(img_size[1], bbox_crop[1]),
min(img_size[0], bbox_crop[2]),
min(img_size[1], bbox_crop[3]))
if bbox_crop[0] >= bbox_crop[2] or bbox_crop[1] >= bbox_crop[3]:
continue
#t_points = [(int(bboxe[i, 0]), int(bboxe[i, 1])) for i in range(8)]
# width_x=[int(bboxe[i, 0]) for i in range(8)]
# high_y=[int(bboxe[i, 1]) for i in range(8)]
# xmin,ymin,xmax,ymax=min(width_x),min(high_y),max(width_x),max(high_y)
xmin,ymin,xmax,ymax=bbox_crop
# x_cen=(xmin+xmax)/2
# y_cen=(ymin+ymax)/2
# if x_cen<0 or y_cen<0 or x_cen>VIEW_WIDTH or y_cen>VIEW_HEIGHT:
# continue
car_type, truncated, occluded, alpha= 'Car', 0, 0,0
dh, dw,dl=extent.z*2,extent.y*2,extent.x*2
cords_y_minus_z_x=np.array(cords_y_minus_z_x)
ly, lz,lx=cords_y_minus_z_x[0][0],cords_y_minus_z_x[1][0],cords_y_minus_z_x[2][0]
lz=lz+dh
ry=(car.get_transform().rotation.yaw-camera_rgb.get_transform().rotation.yaw+90)*np.pi/180
check_box=False
ofs = 10
for one_box in car_2d_bbox:
xmi,ymi,xma,yma=one_box
if xmin>xmi-ofs and ymin>ymi-ofs and xmax<xma+ofs and ymax<yma+ofs:
check_box=True
if check_box or np.sqrt(ly**2+lx**2)<2:
continue
# bbox_crop = tuple(max(0, b) for b in [xmin,ymin,xmax,ymax])
# bbox_crop = (min(img_size[0], bbox_crop[0]),
# min(img_size[1], bbox_crop[1]),
# min(img_size[0], bbox_crop[2]),
# min(img_size[1], bbox_crop[3]))
# Use segment image to determine whether the vehicle is occluded.
# See https://carla.readthedocs.io/en/0.9.11/ref_sensors/#semantic-segmentation-camera
# print('seg: ',semseg[int((ymin+ymax)/2),int((xmin+xmax)/2),0])
# print('x: ',int((ymin+ymax)/2),'y: ',int((xmin+xmax)/2))
if semseg[int((ymin+ymax)/2),int((xmin+xmax)/2),0] != 10:
continue
car_2d_bbox.append(bbox_crop)
v.append(car)
if car not in all_vehicles_list:
all_vehicles_list.append(car)
if ry>np.pi:
ry-=np.pi
if ry<-np.pi:
ry+=np.pi
# pdb.set_trace()
txt="{} {} {} {} {} {} {} {} {:.4f} {:.4f} {:.4f} {:.4f} {:.4f} {:.4f} {} {}\n".format(car_type, truncated, occluded, alpha, xmin, ymin, xmax, ymax, dh, dw,
dl,ly, lz, lx, ry,car.id)
label_files.write(txt)
label_files.close()
#print(cam_type,cam_subset,k,len(v))
#bounding_boxes = ClientSideBoundingBoxes.get_bounding_boxes(v, camera_rgb)
######################################################file
# pygame.image.save(display,'%s/image_1/%06d.png' % (out_path,k))
image.save_to_disk('{}/{}/image_2/{:0>6d}.png'.format (out_path,camera_rgb.sensor_name,k))
global_file_path = out_path + '/global_label_2'
if not os.path.exists(global_file_path):
os.makedirs(global_file_path)
global_vehicle_file = open("{}/{:0>6d}.txt".format(global_file_path,k), "w")
for vehicle in all_vehicles_list:
extent = vehicle.bounding_box.extent
car_type, truncated, occluded, alpha,xmin,ymin,xmax,ymax= 'Car', 0, 0,0,0,0,0,0
dh, dw,dl=extent.z*2,extent.y*2,extent.x*2
location = vehicle.get_transform().location
ly,lz,lx = location.x,location.y,location.z
ry = vehicle.get_transform().rotation.yaw * np.pi / 180
txt="{} {} {} {} {} {} {} {} {:.4f} {:.4f} {:.4f} {:.4f} {:.4f} {:.4f} {} {}\n".format(car_type, truncated, occluded, alpha, xmin, ymin, xmax, ymax, dh, dw,
dl,ly, lz, lx, ry_filter(ry),vehicle.id)
global_vehicle_file.write(txt)
global_vehicle_file.close()
for camera_rgb in rgb_list:
calib_files=open("{}/{}/calib/{:0>6d}.txt".format(out_path,camera_rgb.sensor_name,0), "w")
K=camera_coordinate(camera_rgb)
txt="P2: {} {} {} {} {} {} {} {} {}\n".format( K[0][0],K[0][1],K[0][2],
K[1][0],K[1][1],K[1][2],
K[2][0],K[2][1],K[2][2])
calib_files.write(txt)
calib_files.close()
print('destroying actors.')
for actor in actor_list:
actor.destroy()
# world_snapshot = world.get_snapshot()
# vehicles_actors = []
# for vehicle in vehicles_actor:
# if world_snapshot.find(vehicle) is not None:
# vehicles_actors.append(vehicle)
# for vehicle in vehicles_actor:
#client.apply_batch([carla.command.DestroyActor(vehicle) for vehicle in vehicles_actors])
#pygame.quit()
return
def __update_spectactor_pose(self):
# Set the world simulation view with respect to the vehicle.
v_pose = self._ego_vehicle.get_transform()
v_pose.location -= 10 * Location(v_pose.get_forward_vector())
v_pose.location.z = 5
self._spectator.set_transform(v_pose)
def Create_actors(self,world, blueprint_library):
ego_list = []
npc_list = []
obstacle_list = []
sensor_list = []
# ego车辆设置---------------------------------------------------------------
ego_bp = blueprint_library.find(id='vehicle.lincoln.mkz2017')
# 坐标建立
ego_transform = Transform(Location(x=30, y=-4.350967, z=0.1),
Rotation(pitch=-0.348271, yaw=180, roll=-0.000000))
# 车辆从蓝图定义以及坐标生成
ego = world.spawn_actor(ego_bp, ego_transform)
ego_list.append(ego)
print('created %s' % ego.type_id)
# 视角设置------------------------------------------------------------------
spectator = world.get_spectator()
spec_transform = Transform(Location(x=30, y=-4.350967, z=0),
Rotation(pitch=-0.348271, yaw=180, roll=-0.000000))
spec_transform.location += carla.Location(x=-15,z=50)
spec_transform.rotation = carla.Rotation(pitch=-90,yaw=0,roll=-0.000000)
spectator.set_transform(spec_transform)
# npc设置--------------------------------------------------------------------
npc_transform = Transform(Location(x=30, y=-4.350967, z=0.1),
Rotation(pitch=-0.348271, yaw=180, roll=-0.000000))
for i in range(1):
npc_transform.location += carla.Location(x=-11,y=6)
npc_transform.rotation = carla.Rotation(pitch=0.348271,yaw=275, roll=-0.000000)
npc_bp = blueprint_library.find(id='vehicle.lincoln.mkz2017')
# print(npc_bp.get_attribute('color').recommended_values)
npc_bp.set_attribute('color', '229,28,0')
npc = world.try_spawn_actor(npc_bp, npc_transform)
if npc is None:
print('%s npc created failed' % i)
else:
npc_list.append(npc)
print('created %s' % npc.type_id)
# 障碍物设置------------------------------------------------------------------
obsta_bp = blueprint_library.find(id='static.prop.streetbarrier')
#障碍物1
obstacle_transform1 =Transform(Location(x=30, y=-4.350967, z=0),
Rotation(pitch=-0.348271, yaw=180, roll=-0.000000))
obstacle_transform1.location += carla.Location(x=14,y=1.8)
obstacle_transform1.rotation = carla.Rotation(pitch=0, yaw=0, roll=0.000000)
for i in range(10):
obstacle1 = world.try_spawn_actor(obsta_bp, obstacle_transform1)
obstacle_transform1.location += carla.Location(x=-2.5,y=-0.04)
obstacle_list.append(obstacle1)
#障碍物2
obstacle_transform2 =Transform(Location(x=30, y=-4.350967, z=0),
Rotation(pitch=-0.348271, yaw=180, roll=-0.000000))
obstacle_transform2.location += carla.Location(x=-8.5,y=1.82)
obstacle_transform2.rotation = carla.Rotation(pitch=0, yaw=275, roll=0.000000)
for i in range(4):
obstacle2 = world.try_spawn_actor(obsta_bp, obstacle_transform2)
obstacle_transform2.location += carla.Location(x=-0.1,y=2.5)
obstacle_list.append(obstacle2)
#障碍物3
obstacle_transform3 =Transform(Location(x=30, y=-4.350967, z=0),
Rotation(pitch=-0.348271, yaw=180, roll=-0.000000))
obstacle_transform3.location += carla.Location(x=13,y=-1.8)
obstacle_transform3.rotation = carla.Rotation(pitch=0, yaw=0, roll=0.000000)
for i in range(9):
obstacle3 = world.try_spawn_actor(obsta_bp, obstacle_transform3)
obstacle_transform3.location += carla.Location(x=-2.5,y=-0.1)
obstacle_list.append(obstacle3)
#障碍物4
obstacle_transform4 =Transform(Location(x=30, y=-4.350967, z=0),
Rotation(pitch=-0.348271, yaw=180, roll=-0.000000))
obstacle_transform4.location += carla.Location(x=-24,y=-52)
obstacle_transform4.rotation = carla.Rotation(pitch=0, yaw=270, roll=0.000000)
for i in range(15):
obstacle4 = world.try_spawn_actor(obsta_bp, obstacle_transform4)
obstacle_transform4.location += carla.Location(x=-0.05,y=2.5)
obstacle_list.append(obstacle4)
#障碍物5
obstacle_transform5 =Transform(Location(x=30, y=-4.350967, z=0),
Rotation(pitch=-0.348271, yaw=180, roll=-0.000000))
obstacle_transform5.location += carla.Location(x=-20.5,y=-52)
obstacle_transform5.rotation = carla.Rotation(pitch=0, yaw=270, roll=0.000000)
for i in range(13):
obstacle5 = world.try_spawn_actor(obsta_bp, obstacle_transform5)
obstacle_transform5.location += carla.Location(x=-0.05,y=2.5)
obstacle_list.append(obstacle5)
#障碍物6
obstacle_transform6 =Transform(Location(x=30, y=-4.350967, z=0),
Rotation(pitch=-0.348271, yaw=180, roll=-0.000000))
obstacle_transform6.location += carla.Location(x=-20,y=-21)
obstacle_transform6.rotation = carla.Rotation(pitch=0, yaw=45, roll=0.000000)
for i in range(8):
obstacle6 = world.try_spawn_actor(obsta_bp, obstacle_transform6)
obstacle_transform6.location += carla.Location(x=1.7,y=2.4)
obstacle_list.append(obstacle6)
# 传感器设置-------------------------------------------------------------------
ego_collision = SS.CollisionSensor(ego)
npc_collision = SS.CollisionSensor(npc)
ego_invasion = SS.LaneInvasionSensor(ego)
npc_invasion = SS.LaneInvasionSensor(npc)
sensor_list.extend([[ego_collision,ego_invasion],[npc_collision,npc_invasion]])
return ego_list,npc_list,obstacle_list,sensor_list
def Create_actors(self, world, blueprint_library):
ego_list = []
npc_list = []
obstacle_list = []
sensor_list = []
# ego车辆设置---------------------------------------------------------------
ego_bp = blueprint_library.find(id='vehicle.lincoln.mkz2017')
# 坐标建立
ego_transform = Transform(
Location(x=160.341522, y=-371.640472, z=0.281942),
Rotation(pitch=0.000000, yaw=0.500910, roll=0.000000))
# 车辆从蓝图定义以及坐标生成
ego = world.spawn_actor(ego_bp, ego_transform)
if ego is None:
print('ego created failed')
else:
ego_list.append(ego)
print('created %s' % ego.type_id)
# 视角设置------------------------------------------------------------------
spectator = world.get_spectator()
# spec_transform = ego.get_transform()
spec_transform = Transform(
Location(x=140.341522, y=-375.140472, z=15.281942),
Rotation(pitch=0.000000, yaw=0.500910, roll=0.000000))
spec_transform.location += carla.Location(x=60, z=45)
spec_transform.rotation = carla.Rotation(pitch=-90, yaw=90)
spectator.set_transform(spec_transform)
# npc设置--------------------------------------------------------------------
npc_transform = ego_transform
for i in range(1):
npc_transform.location += carla.Location(x=-15, y=-3.5)
npc_bp = blueprint_library.find(id='vehicle.lincoln.mkz2017')
npc_bp.set_attribute('color', '229,28,0')
npc = world.try_spawn_actor(npc_bp, npc_transform)
if npc is None:
print('No.%s npc created failed' % i)
else:
npc_list.append(npc)
print('created %s' % npc.type_id)
# 障碍物设置------------------------------------------------------------------
obstacle_transform = ego_transform
for i in range(28):
if i == 0:
obsta_bp = blueprint_library.find(
id='vehicle.mercedes-benz.coupe')
obstacle_transform.location += carla.Location(x=95, y=3.8)
obstacle = world.try_spawn_actor(obsta_bp, obstacle_transform)
obstacle_transform.location += carla.Location(x=0, y=-5.3)
if obstacle is None:
print('%s obstacle created failed' % i)
else:
obstacle_list.append(obstacle)
print('created %s' % obstacle.type_id)
else:
obsta_bp = blueprint_library.find(
id='static.prop.streetbarrier')
obstacle_transform.location += carla.Location(x=-3.5, y=7.4)
obstacle1 = world.try_spawn_actor(obsta_bp, obstacle_transform)
obstacle_list.append(obstacle1)
obstacle_transform.location += carla.Location(y=-7.4)
obstacle2 = world.try_spawn_actor(obsta_bp, obstacle_transform)
obstacle_list.append(obstacle2)
# 传感器设置-------------------------------------------------------------------
ego_collision = SS.CollisionSensor(ego)
npc_collision = SS.CollisionSensor(npc)
# lane_invasion = SS.LaneInvasionSensor(ego)
sensor_list.append(ego_collision)
sensor_list.append(npc_collision)
return ego_list, npc_list, obstacle_list, sensor_list
def __contains__(self, loc: carla.Location) -> bool:
return loc.distance(self.center) <= self.radius
import math
import carla
import numpy as np
import pandas as pd
from scipy.interpolate import splprep, splev
from carla import Transform, Location, Rotation
#Easy selfexplaining lambdas
from config import IMAGE_SIZE
numpy_to_transform = lambda point: Transform(
Location(point[0], point[1], point[2]),
Rotation(yaw=point[3], pitch=0, roll=0))
transform_to_numpy = lambda transform: np.array([
transform.location.x, transform.location.y, transform.location.z, transform
.rotation.yaw
])
numpy_to_location = lambda point: Location(point[0], point[1], point[2])
location_to_numpy = lambda location: np.array(
[location.x, location.y, location.z])
velocity_to_kmh = lambda v: float(3.6 * np.math.sqrt(v.x**2 + v.y**2 + v.z**2))
numpy_to_velocity_vec = lambda v: carla.Vector3D(x=v[0], y=v[1], z=v[2])
def df_to_spawn_points(data: pd.DataFrame,
n: int = 10000,
invert: bool = False) -> np.array:
'''
Method converting spawnpoints loaded from DataFrame into equally placed points on the map.
def _distance(self, wp: msg.WaypointWithSpeedLimit,
loc: carla.Location) -> float:
wp_loc = carla.Location(wp.x, wp.y)
return loc.distance(wp_loc)
def Create_actors(self, world, blueprint_library):
ego_list = []
npc_list = []
obstacle_list = []
sensor_list = []
# ego车辆设置---------------------------------------------------------------
ego_bp = blueprint_library.find(id='vehicle.lincoln.mkz2017')
# 坐标建立
ego_transform = Transform(Location(x=9, y=-110.350967, z=0.1),
Rotation(pitch=0, yaw=-90, roll=-0.000000))
# 车辆从蓝图定义以及坐标生成
ego = world.spawn_actor(ego_bp, ego_transform)
ego_list.append(ego)
print('created %s' % ego.type_id)
# 视角设置------------------------------------------------------------------
spectator = world.get_spectator()
# spec_transform = ego.get_transform()
spec_transform = Transform(Location(x=9, y=-115.350967, z=0),
Rotation(pitch=0, yaw=180, roll=-0.000000))
spec_transform.location += carla.Location(x=-5, z=60)
spec_transform.rotation = carla.Rotation(pitch=-90,
yaw=1.9,
roll=-0.000000)
spectator.set_transform(spec_transform)
# npc设置--------------------------------------------------------------------
npc_transform = Transform(Location(x=9, y=-110.350967, z=0.1),
Rotation(pitch=0, yaw=-90, roll=-0.000000))
for i in range(1):
npc_transform.location += carla.Location(x=-18, y=-24)
npc_transform.rotation = carla.Rotation(pitch=0,
yaw=0,
roll=-0.000000)
npc_bp = blueprint_library.find(id='vehicle.lincoln.mkz2017')
# print(npc_bp.get_attribute('color').recommended_values)
npc_bp.set_attribute('color', '229,28,0')
npc = world.try_spawn_actor(npc_bp, npc_transform)
if npc is None:
print('%s npc created failed' % i)
else:
npc_list.append(npc)
print('created %s' % npc.type_id)
# 障碍物设置------------------------------------------------------------------
obsta_bp = blueprint_library.find(id='static.prop.streetbarrier')
# 障碍物1
obstacle_transform1 = Transform(
Location(x=9, y=-110.350967, z=0),
Rotation(pitch=0, yaw=-90, roll=-0.000000))
obstacle_transform1.location += carla.Location(x=50, y=-27, z=3)
obstacle_transform1.rotation = carla.Rotation(pitch=0,
yaw=0,
roll=0.000000)
for i in range(30):
obstacle1 = world.try_spawn_actor(obsta_bp, obstacle_transform1)
obstacle_transform1.location += carla.Location(x=-2.5,
y=-0.05,
z=-0.12)
obstacle_list.append(obstacle1)
# 障碍物2
# obstacle_transform2 = Transform(Location(x=9, y=-110.350967,z=0),
# Rotation(pitch=0, yaw=-90, roll=-0.000000))
# obstacle_transform2.location += carla.Location(x=-2.8,y=-18.5)
# obstacle_transform2.rotation = carla.Rotation(pitch=0, yaw=0, roll=0.000000)
# for i in range(7):
# obstacle2 = world.try_spawn_actor(obsta_bp, obstacle_transform2)
# obstacle_transform2.location += carla.Location(x=-2.5)
# obstacle_list.append(obstacle2)
# # 障碍物3
# obstacle_transform3 = Transform(Location(x=9, y=-110.350967,z=0),
# Rotation(pitch=0, yaw=-90, roll=-0.000000))
# obstacle_transform3.location += carla.Location(x=-1.65,y=-17.5)
# obstacle_transform3.rotation = carla.Rotation(pitch=0, yaw=90, roll=0.000000)
# for i in range(10):
# obstacle3 = world.try_spawn_actor(obsta_bp, obstacle_transform3)
# obstacle_transform3.location += carla.Location(x=0.006,y=2.5)
# obstacle_list.append(obstacle3)
# # 障碍物4
# obstacle_transform4 = Transform(Location(x=9, y=-110.350967,z=0),
# Rotation(pitch=0, yaw=-90, roll=-0.000000))
# obstacle_transform4.location += carla.Location(x=2.45,y=-15)
# obstacle_transform4.rotation = carla.Rotation(pitch=0, yaw=90, roll=0.000000)
# for i in range(10):
# obstacle4 = world.try_spawn_actor(obsta_bp, obstacle_transform4)
# obstacle_transform4.location += carla.Location(y=2.5)
# obstacle_list.append(obstacle4)
# 传感器设置-------------------------------------------------------------------
ego_collision = SS.CollisionSensor(ego)
npc_collision = SS.CollisionSensor(npc)
ego_invasion = SS.LaneInvasionSensor(ego)
npc_invasion = SS.LaneInvasionSensor(npc)
sensor_list.extend([[ego_collision, ego_invasion],
[npc_collision, npc_invasion]])
return ego_list, npc_list, obstacle_list, sensor_list
def get_top_down_transform(transform, top_down_camera_altitude):
# Calculation relies on the fact that the camera's FOV is 90.
top_down_location = (transform.location +
Location(0, 0, top_down_camera_altitude))
return Transform(top_down_location, Rotation(-90, 0, 0))
class Transform(object):
"""A class that stores the location and rotation of an obstacle.
It can be created from a simulator transform, defines helper functions
needed in Pylot, and makes the simulator transform serializable.
A transform object is instantiated with either a location and a rotation,
or using a matrix.
Args:
location (:py:class:`.Location`, optional): The location of the object
represented by the transform.
rotation (:py:class:`.Rotation`, optional): The rotation (in degrees)
of the object represented by the transform.
matrix: The transformation matrix used to convert points in the 3D
coordinate space with respect to the location and rotation of the
given object.
Attributes:
location (:py:class:`.Location`): The location of the object
represented by the transform.
rotation (:py:class:`.Rotation`): The rotation (in degrees) of the
object represented by the transform.
forward_vector (:py:class:`.Vector3D`): The forward vector of the
object represented by the transform.
matrix: The transformation matrix used to convert points in the 3D
coordinate space with respect to the location and rotation of the
given object.
"""
def __init__(self,
location: Location = None,
rotation: Rotation = None,
matrix=None):
if matrix is not None:
self.matrix = matrix
self.location = Location(matrix[0, 3], matrix[1, 3], matrix[2, 3])
# Forward vector is retrieved from the matrix.
self.forward_vector = \
Vector3D(self.matrix[0, 0], self.matrix[1, 0],
self.matrix[2, 0])
pitch_r = math.asin(np.clip(self.forward_vector.z, -1, 1))
yaw_r = math.acos(
np.clip(self.forward_vector.x / math.cos(pitch_r), -1, 1))
roll_r = math.asin(
np.clip(matrix[2, 1] / (-1 * math.cos(pitch_r)), -1, 1))
self.rotation = Rotation(math.degrees(pitch_r),
math.degrees(yaw_r), math.degrees(roll_r))
else:
self.location, self.rotation = location, rotation
self.matrix = Transform._create_matrix(self.location,
self.rotation)
# Forward vector is retrieved from the matrix.
self.forward_vector = \
Vector3D(self.matrix[0, 0], self.matrix[1, 0],
self.matrix[2, 0])
@classmethod
def from_simulator_transform(cls, transform):
"""Creates a pylot transform from a simulator transform.
Args:
transform: A simulator transform.
Returns:
:py:class:`.Transform`: An instance of a pylot transform.
"""
from carla import Transform
if not isinstance(transform, Transform):
raise ValueError('transform should be of type Transform')
return cls(Location.from_simulator_location(transform.location),
Rotation.from_simulator_rotation(transform.rotation))
@staticmethod
def _create_matrix(location, rotation):
"""Creates a transformation matrix to convert points in the 3D world
coordinate space with respect to the object.
Use the transform_points function to transpose a given set of points
with respect to the object.
Args:
location (:py:class:`.Location`): The location of the object
represented by the transform.
rotation (:py:class:`.Rotation`): The rotation of the object
represented by the transform.
Returns:
A 4x4 numpy matrix which represents the transformation matrix.
"""
matrix = np.identity(4)
cy = math.cos(np.radians(rotation.yaw))
sy = math.sin(np.radians(rotation.yaw))
cr = math.cos(np.radians(rotation.roll))
sr = math.sin(np.radians(rotation.roll))
cp = math.cos(np.radians(rotation.pitch))
sp = math.sin(np.radians(rotation.pitch))
matrix[0, 3] = location.x
matrix[1, 3] = location.y
matrix[2, 3] = location.z
matrix[0, 0] = (cp * cy)
matrix[0, 1] = (cy * sp * sr - sy * cr)
matrix[0, 2] = -1 * (cy * sp * cr + sy * sr)
matrix[1, 0] = (sy * cp)
matrix[1, 1] = (sy * sp * sr + cy * cr)
matrix[1, 2] = (cy * sr - sy * sp * cr)
matrix[2, 0] = (sp)
matrix[2, 1] = -1 * (cp * sr)
matrix[2, 2] = (cp * cr)
return matrix
def __transform(self, points, matrix):
"""Internal function to transform the points according to the
given matrix. This function either converts the points from
coordinate space relative to the transform to the world coordinate
space (using self.matrix), or from world coordinate space to the
space relative to the transform (using inv(self.matrix))
Args:
points: An n by 3 numpy array, where each row is the
(x, y, z) coordinates of a point.
matrix: The matrix of the transformation to apply.
Returns:
An n by 3 numpy array of transformed points.
"""
# Needed format: [[X0,..Xn],[Y0,..Yn],[Z0,..Zn]].
# So let's transpose the point matrix.
points = points.transpose()
# Add 1s row: [[X0..,Xn],[Y0..,Yn],[Z0..,Zn],[1,..1]]
points = np.append(points, np.ones((1, points.shape[1])), axis=0)
# Point transformation (depends on the given matrix)
points = np.dot(matrix, points)
# Get all but the last row in array form.
points = np.asarray(points[0:3].transpose()).astype(np.float16)
return points
def transform_points(self, points):
"""Transforms the given set of points (specified in the coordinate
space of the current transform) to be in the world coordinate space.
For example, if the transform is at location (3, 0, 0) and the
location passed to the argument is (10, 0, 0), this function will
return (13, 0, 0) i.e. the location of the argument in the world
coordinate space.
Args:
points: A (number of points) by 3 numpy array, where each row is
the (x, y, z) coordinates of a point.
Returns:
An n by 3 numpy array of transformed points.
"""
return self.__transform(points, self.matrix)
def inverse_transform_points(self, points):
"""Transforms the given set of points (specified in world coordinate
space) to be relative to the given transform.
For example, if the transform is at location (3, 0, 0) and the location
passed to the argument is (10, 0, 0), this function will return
(7, 0, 0) i.e. the location of the argument relative to the given
transform.
Args:
points: A (number of points) by 3 numpy array, where each row is
the (x, y, z) coordinates of a point.
Returns:
An n by 3 numpy array of transformed points.
"""
return self.__transform(points, np.linalg.inv(self.matrix))
def transform_locations(self, locations):
"""Transforms the given set of locations (specified in the coordinate
space of the current transform) to be in the world coordinate space.
This method has the same functionality as transform_points, and
is provided for convenience; when dealing with a large number of
points, it is advised to use transform_points to avoid the slow
conversion between a numpy array and list of locations.
Args:
points (list(:py:class:`.Location`)): List of locations.
Returns:
list(:py:class:`.Location`): List of transformed points.
"""
points = np.array([loc.as_numpy_array() for loc in locations])
transformed_points = self.__transform(points, self.matrix)
return [Location(x, y, z) for x, y, z in transformed_points]
def inverse_transform_locations(self, locations):
"""Transforms the given set of locations (specified in world coordinate
space) to be relative to the given transform.
This method has the same functionality as inverse_transform_points,
and is provided for convenience; when dealing with a large number of
points, it is advised to use inverse_transform_points to avoid the slow
conversion between a numpy array and list of locations.
Args:
points (list(:py:class:`.Location`)): List of locations.
Returns:
list(:py:class:`.Location`): List of transformed points.
"""
points = np.array([loc.as_numpy_array() for loc in locations])
transformed_points = self.__transform(points,
np.linalg.inv(self.matrix))
return [Location(x, y, z) for x, y, z in transformed_points]
def as_simulator_transform(self):
"""Converts the transform to a simulator transform.
Returns:
An instance of the simulator class representing the Transform.
"""
from carla import Location, Rotation, Transform
return Transform(
Location(self.location.x, self.location.y, self.location.z),
Rotation(pitch=self.rotation.pitch,
yaw=self.rotation.yaw,
roll=self.rotation.roll))
def get_angle_and_magnitude(self, target_loc):
"""Computes relative angle between the transform and a target location.
Args:
target_loc (:py:class:`.Location`): Location of the target.
Returns:
Angle in radians and vector magnitude.
"""
target_vec = target_loc.as_vector_2D() - self.location.as_vector_2D()
magnitude = target_vec.magnitude()
if magnitude > 0:
forward_vector = Vector2D(
math.cos(math.radians(self.rotation.yaw)),
math.sin(math.radians(self.rotation.yaw)))
angle = target_vec.get_angle(forward_vector)
else:
angle = 0
return angle, magnitude
def is_within_distance_ahead(self, dst_loc: Location,
max_distance: float) -> bool:
"""Checks if a location is within a distance.
Args:
dst_loc (:py:class:`.Location`): Location to compute distance to.
max_distance (:obj:`float`): Maximum allowed distance.
Returns:
bool: True if other location is within max_distance.
"""
d_angle, norm_dst = self.get_angle_and_magnitude(dst_loc)
# Return if the vector is too small.
if norm_dst < 0.001:
return True
# Return if the vector is greater than the distance.
if norm_dst > max_distance:
return False
return d_angle < 90.0
def inverse_transform(self):
"""Returns the inverse transform of this transform."""
new_matrix = np.linalg.inv(self.matrix)
return Transform(matrix=new_matrix)
def __mul__(self, other):
new_matrix = np.dot(self.matrix, other.matrix)
return Transform(matrix=new_matrix)
def __repr__(self):
return self.__str__()
def __str__(self):
if self.location:
return "Transform(location: {}, rotation: {})".format(
self.location, self.rotation)
else:
return "Transform({})".format(str(self.matrix))