def __init__(self, center: Vector3r, width: Vector3r, height: Vector3r):
""" Create a new rectangle representation in 3D. """
self.center = center
self.half_width = width / 2.0
self.half_height = height / 2.0
self._S = Vector3r(1.0, 1.0, 1.0) # scaling
self._T = Vector3r(0.0, 0.0, 0.0) # translation
def collect_points_6dir(self, tgt):
# must be in CV mode, otherwise the point clouds won't align
scan_config = [0, -1, 2, 1, 4, 5] # front, left, back, right, up, down
points_6dir = np.zeros((0, self.imgwidth, 3), dtype=np.float32)
for k, face in enumerate(scan_config):
if face == 4: # look upwards at tgt
pose = Pose(Vector3r(tgt[0], tgt[1], tgt[2]),
to_quaternion(math.pi / 2, 0, 0)) # up
elif face == 5: # look downwards
pose = Pose(Vector3r(tgt[0], tgt[1], tgt[2]),
to_quaternion(-math.pi / 2, 0,
0)) # down - pitch, roll, yaw
else: # rotate from [-90, 0, 90, 180]
yaw = math.pi / 2 * face
pose = Pose(Vector3r(tgt[0], tgt[1], tgt[2]),
to_quaternion(0, 0, yaw))
self.set_vehicle_pose(pose)
depth_front, _ = self.get_depth_campos()
if depth_front is None:
rospy.logwarn('Missing image {}: {}'.format(k, tgt))
continue
# import ipdb;ipdb.set_trace()
point_array = expo_util.depth_to_point_cloud(depth_front,
self.focal,
self.pu,
self.pv,
mode=k)
points_6dir = np.concatenate((points_6dir, point_array), axis=0)
# reset the pose for fun
pose = Pose(Vector3r(tgt[0], tgt[1], tgt[2]), to_quaternion(0, 0, 0))
self.set_vehicle_pose(pose)
# print 'points:', points_6dir.shape
return points_6dir
def _drop_till_obstacle_detected(self, speed=5.0):
"""Move downward vertically untill an obstacle is detected by lidar sensor."""
current_pos = self._uav.simGetGroundTruthKinematics().position
self._uav.moveByVelocityAsync(0, 0, speed, duration=1000)
while True:
try:
lidar_data = self._uav.getLidarData(
lidar_name="BottomObstacleDetector")
if len(lidar_data.point_cloud) >= 3:
uav_pos = self._uav.simGetGroundTruthKinematics().position
for i in range(0, len(lidar_data.point_cloud), 3):
p = Vector3r(
lidar_data.point_cloud[i],
lidar_data.point_cloud[i + 1],
lidar_data.point_cloud[i + 2],
)
delta_height = p.z_val - uav_pos.z_val
inside_circle = (Vector3r(
p.x_val, p.y_val, 0).distance_to(
Vector3r(uav_pos.x_val, uav_pos.y_val, 0)) < 2)
if delta_height > 0 and inside_circle:
self._uav.moveByVelocityAsync(0,
0,
0,
duration=1.0)
return p.z_val
except AttributeError:
print("AttributeError")
continue
def frustum_plot_list_from_viewport_vectors(
pose: Pose,
eye_to_top_left: np.ndarray,
eye_to_top_right: np.ndarray,
eye_to_bottom_left: np.ndarray,
eye_to_bottom_right: np.ndarray,
scale: float = 2.0
) -> List[Vector3r]:
eye = pose.position
top_left = eye + Vector3r(*(eye_to_top_left * scale))
top_right = eye + Vector3r(*(eye_to_top_right * scale))
bottom_left = eye + Vector3r(*(eye_to_bottom_left * scale))
bottom_right = eye + Vector3r(*(eye_to_bottom_right * scale))
return [
eye, top_left,
eye, top_right,
eye, bottom_left,
eye, bottom_right,
top_left, top_right,
top_right, bottom_right,
bottom_right, bottom_left,
bottom_left, top_left,
]
def quaternion_from_two_vectors(a: Vector3r, b: Vector3r) -> Quaternionr:
""" What rotation (around the intersection of the two vectors) we need to rotate `a` to `b`? """
# ref.: https://gitlab.com/libeigen/eigen/-/blob/master/Eigen/src/Geometry/Quaternion.h (FromTwoVectors)
v0 = a / a.get_length()
v1 = b / b.get_length()
c = v1.dot(v0)
assert c > -1 # FIXME handle the case when the vectors are nearly opposites
s = np.sqrt((1 + c) * 2)
axis = v0.cross(v1) / s
return Quaternionr(axis.x_val, axis.y_val, axis.z_val, w_val=(s / 2))
def fly(client: MultirotorClient, args: argparse.Namespace) -> None:
reset(client)
rect = cast(Rect, args.rect)
initial_pose = cast(Pose, client.simGetVehiclePose())
closest_corner = rect.closest_corner(initial_pose.position)
if args.verbose:
ff.print_pose(initial_pose, airsim.to_eularian_angles)
ff.log_info(f"Closest corner {ff.to_xyz_str(closest_corner)}")
start_pos = Vector3r(
*ff.to_xyz_tuple(closest_corner)) # NOTE make sure we copy it by value
start_pos.z_val += args.z_offset
start_pos.y_val += args.y_offset
start_pos.x_val += args.x_offset
plot = Plotter(client)
plot.flush_persistent()
plot.points([start_pos], Rgba.Red)
if args.teleport:
ff.log(f"Teleporting to {ff.to_xyz_str(start_pos)}...")
new_pose = Pose(start_pos, initial_pose.orientation)
Controller.teleport(client, to=new_pose, ignore_collision=True)
else:
ff.log(f"Flying to {ff.to_xyz_str(start_pos)}...")
client.moveToPositionAsync(*ff.to_xyz_tuple(start_pos),
velocity=5).join()
ff.log("Capturing grid...")
time.sleep(2)
z = Vector3r(0, 0, args.altitude_shift)
path = [
z + Vector3r(*ff.to_xyz_tuple(corner))
for corner in rect.zigzag(lanes=args.zigzags)
]
if AUGMENT_PATHS:
path = Controller.augment_path(path, AUGMENT_PATHS_MAX_DIST)
plot.points(path, Rgba.White)
plot.lines(path, Rgba.Green)
try:
if RECORD:
client.startRecording()
Controller.fly_path(client, path)
except Exception as e:
raise e
finally:
if RECORD:
client.stopRecording()
def from_dump(dump_str: str) -> Rect:
""" Convert `dump_str`, a string representation of a rectangle, back to a `Rect`.
Note: `dump_str` is assumed to have been created by calling the `Rect.to_dump` method.
"""
import json
json_repr = json.loads(dump_str)
return Rect(
Vector3r(*json_repr["center"]),
Vector3r(*[2 * _ for _ in json_repr["half_width"]]),
Vector3r(*[2 * _ for _ in json_repr["half_height"]]),
)
def align_meshroom_to_airsim(meshroom_pose,
meshroom_to_airsim_transform):
# NOTE this transformation is only based on the positions (and not on the orientations)
meshroom_pos = np.append(meshroom_pose.position.to_numpy_array(),
1) # [x, y, z, 1]
airsim_pos = np.matmul(meshroom_to_airsim_transform, meshroom_pos)
return Pose(Vector3r(*airsim_pos), meshroom_pose.orientation)
def convert_2_pose(p, e):
"""
p: A 3-element NumPy array, the position.
q: A 4-element NumPy array, a quaternion witht he last element being w.
"""
return Pose(Vector3r(p[0], p[1], p[2]), to_quaternion(e[0], e[1], e[2]))
def convert_2_pose(a):
"""
a: A 7-element array.
"""
return Pose(Vector3r(a[0], a[1], a[2]),
Quaternionr(a[3], a[4], a[5], a[6]))
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
# Reset the drone
client.reset()
client.enableApiControl(True)
client.armDisarm(True)
# Draw the ROI outline (erasing previous plots)
client.simFlushPersistentMarkers()
if SHOW_PLOTS:
client.simPlotLineStrip(points=args.roi.corners(repeat_first=True),
is_persistent=True)
# Get the first position the drone will fly to
initial_pose = client.simGetVehiclePose()
closest_corner = args.roi.closest_corner(initial_pose.position)
if args.verbose:
ff.print_pose(initial_pose, airsim.to_eularian_angles)
ff.log_info(f"Closest corner {ff.to_xyz_str(closest_corner)}")
start_pos = Vector3r(
*ff.to_xyz_tuple(closest_corner if args.corner else args.roi.center))
# NOTE AirSim uses NED coordinates, so negative Z values are "up" actually
if (z := args.z_offset):
start_pos.z_val -= z # start higher up, to avoid crashing with objects
def quaternion_from_rotation_axis_angle(axis: Vector3r, angle: float, is_degrees: bool = False) -> Quaternionr:
if is_degrees:
angle = np.deg2rad(angle)
half_angle = angle / 2
axis /= axis.get_length() # normalize
axis *= np.sin(half_angle)
return Quaternionr(axis.x_val, axis.y_val, axis.z_val, w_val=np.cos(half_angle))
def data_sampling(self, positions, trajname):
self.init_folders(trajname)
self.randquaternionsampler.init_random_yaw()
start = time.time()
for k, pose in enumerate(positions):
position = Vector3r(pose[0], pose[1], pose[2])
orientation = self.randquaternionsampler.next_quaternion()
dronepose = Pose(position, orientation)
self.imgclient.setpose(dronepose)
time.sleep(0.02)
self.imgclient.simPause(True)
rgblist, depthlist, seglist, camposelist = self.imgclient.readimgs(
)
self.imgclient.simPause(False)
# save images and poses
imgprefix = str(k).zfill(6) + '_'
for w, camind in enumerate(self.camlist):
camname = self.camlist_name[camind]
# save RGB image
if 'Scene' in self.imgtypelist:
img = rgblist[w] # change bgr to rgb
cv2.imwrite(
join(self.imgdirs[w], imgprefix + camname + '.png'),
img, [cv2.IMWRITE_PNG_COMPRESSION, 0])
# save depth image
if 'DepthPlanner' in self.imgtypelist:
depthimg = depthlist[w]
np.save(
join(self.depthdirs[w],
imgprefix + camname + '_depth.npy'), depthimg)
# save segmentation image
if 'Segmentation' in self.imgtypelist:
segimg = seglist[w]
np.save(
join(self.segdirs[w],
imgprefix + camname + '_seg.npy'), segimg)
# write pose to file
self.posenplist[w].append(np.array(camposelist[w]))
# imgshow = np.concatenate((leftimg,rightimg),axis=1)
print(
' {0}, pose {1}, orientation ({2:.2f},{3:.2f},{4:.2f},{5:.2f})'
.format(k, pose, orientation.x_val, orientation.y_val,
orientation.z_val, orientation.w_val))
# cv2.imshow('img',imgshow)
# cv2.waitKey(1)
for w in range(len(self.camlist)):
# save poses into numpy txt
np.savetxt(self.posefilelist[w], self.posenplist[w])
end = time.time()
print('Trajectory sample time {}'.format(end - start))
def fly_zone(client: airsim.MultirotorClient,
zone: Rect,
altitude_shift: float = 0.0) -> None:
path = [
Vector3r(corner.x_val, corner.y_val, corner.z_val - altitude_shift)
for corner in zone.corners(repeat_first=True)
]
if AUGMENT_PATHS:
path = Controller.augment_path(path, max_dist=2)
if SHOW_PLOTS:
client.simPlotPoints(points=path,
is_persistent=True,
color_rgba=Rgba.White,
size=5)
client.simPlotLineStrip(points=path,
is_persistent=True,
color_rgba=Rgba.White)
# Controller.fly_path(client, path) ##client.moveOnPathAsync(path, velocity=2).join()
# XXX testing.. stretching Rect, zigzagging path and flying over it
zz_path = Rect(
Vector3r(0, 0, -altitude_shift) + zone.center,
# Vector3r(0, 0, -altitude_shift) + zone.half_width * 4,
# Vector3r(0, 0, -altitude_shift) + zone.half_height * 4,
zone.half_width * 2,
zone.half_height * 2,
).zigzag(4)
if AUGMENT_PATHS:
zz_path = Controller.augment_path(zz_path, max_dist=2)
if SHOW_PLOTS:
client.simPlotPoints(points=zz_path,
is_persistent=True,
color_rgba=Rgba.White,
size=5)
client.simPlotLineStrip(points=zz_path,
is_persistent=True,
color_rgba=Rgba.Green)
Controller.fly_path(
client, zz_path) ##client.moveOnPathAsync(zz_path, velocity=2).join()
def _move_to(self, target, speed=5.0, yaw=0.0):
"""Due to bugs in controlling the drone(issue 1677 and 1292), the temporal solution is presented."""
target = (Vector3r(target[0], target[1], target[2])
if type(target) == list else target)
self._uav.moveToPositionAsync(
target.x_val,
target.y_val,
target.z_val,
speed,
yaw_mode=airsim.YawMode(False, yaw),
).join()
def explore(self):
"""An offline method of exploration."""
img_dir = self._config["image_dir"]
# Generate flight plan
views = self.generate_explore_views()
# Arm and takeoff
self._uav.armDisarm(True)
self._uav.takeoffAsync().join()
current_pos = self._uav.simGetGroundTruthKinematics().position
self._move_to(Vector3r(current_pos.x_val, current_pos.y_val, -60))
# Fly to each view and take photos
for idx, view in enumerate(views):
self._observe_at_view(view, os.path.join(img_dir, "%s.png" % idx))
def init_vehicle(self):
# Set Drive Commands to zero initially
self._throttle = 0 # Used as a constant gain factor for the action throttle.
self._steering = 0 # Each action lasts this many seconds
self._brake = 0
self.THROTTLE_INC = .10
self.THROTTLE_DEC = -.10
self.BRAKE_INC = .10
self.BRAKE_DEC = -.20
self.STEER_LEFT_INC = -.10
self.STEER_RIGHT_INC = .10
# Connect to the AirSim simulator and begin:
print('Initializing Car Client')
self.client = client.CarClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
print('Initialization DONE!')
print("Setting Camera Views")
orien = Vector3r(0, 0, 0)
self.client.simSetCameraOrientation(0, orien) #radians
orien = Vector3r(0, .12, -np.pi/9)
self.client.simSetCameraOrientation(1, orien)
orien = Vector3r(0, .12, np.pi/9)
self.client.simSetCameraOrientation(2, orien)
# Reset Collion Flags
print("Setting Camera Views DONE!")
# Set up GUI Video Feeder
num_video_feeds = np.sum(np.array(self.image_mask_FC_FR_FL, dtype = np.int)) * self.IMG_STEP
GUIConn, self.simEnvDataConn = multiprocessing.Pipe()
self.app = AirSimGUI.CarGUI(GUIConn, vehicle_names = [self.vehicle_name],
num_video_feeds = num_video_feeds, isRGB = self.isRGB, isNormal = self.isNormal)
def vector_rotated_by_quaternion(v: Vector3r, q: Quaternionr) -> Vector3r:
q /= q.get_length() # normalize
# Extract the vector and scalar parts of q:
u, s = Vector3r(q.x_val, q.y_val, q.z_val), q.w_val
# NOTE the results from these two methods are the same up to 7 decimal places.
# ref.: https://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion
# return u * (2 * u.dot(v)) + v * (s * s - u.dot(u)) + u.cross(v) * (2 * s)
# ref.: https://gitlab.com/libeigen/eigen/-/blob/master/Eigen/src/Geometry/Quaternion.h (_transformVector)
uv = u.cross(v)
uv += uv
return v + uv * s + u.cross(uv)
def __init__(
self,
time_stamp: int,
pos_x: float, pos_y: float, pos_z: float,
q_w: float, q_x: float, q_y: float, q_z: float,
image_file: str,
):
""" Represents an item from AirSim's recording file (i.e. a row in `airsim_rec.txt`).
Reference: https://github.com/microsoft/AirSim/blob/master/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp
"""
# assert os.path.isfile(image_file), image_file
self.time_stamp = time_stamp
self.image_file = image_file
self.position = Vector3r(pos_x, pos_y, pos_z)
self.orientation = Quaternionr(q_x, q_y, q_z, q_w)
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
if args.flush:
client.simFlushPersistentMarkers()
if not args.transformation:
poses = [
Pose(record.position, record.orientation)
for record in args.recording.values()
]
positions = [record.position for record in args.recording.values()]
else:
matrix = np.loadtxt(
args.transformation) # load the 4x4 transformation matrix
print(matrix)
poses = []
positions = []
for record in args.recording.values():
pos = Vector3r(*np.matmul(matrix, np.append(record.position, 1)))
poses.append(Pose(pos, record.orientation))
positions = [pos]
if args.axes:
client.simPlotTransforms(poses,
scale=100.0,
thickness=2.5,
is_persistent=True)
else:
client.simPlotPoints(positions,
args.color,
size=10,
is_persistent=True)
if args.lines:
client.simPlotLineStrip(positions,
args.color,
thickness=2.5,
is_persistent=True)
if args.aim:
line_list = []
for pose in poses:
line_list.append(pose.position)
x_axis, _, _ = AirSimNedTransform.local_axes_frame(pose)
line_list.append(pose.position + x_axis * 100)
client.simPlotLineList(line_list,
args.color,
thickness=2.5,
is_persistent=True)
def explore(self):
if not self._safety_surface["type"] == "circle":
return
img_dir = self._config["image_dir"]
# Arm and takeoff
self._uav.armDisarm(True)
self._uav.takeoffAsync().join()
self._move_to(Vector3r(0, 0, -60))
# Explore center point and points on edge
root = self.generate_explore_views()
DETE.show_explore_path(root)
center = root
triangles = root.dfs()
# all_points = [center.pc, center.childs[0].p2]
print("Center point, counter=1")
self.explore_point(center.pc, center, center.pc,
os.path.join(img_dir, "1.png"))
print("Edge point, counter=2")
photo_counter = 2
self.explore_point(
center.childs[0].p2,
center.childs[0],
center.pc,
os.path.join(img_dir, "2.png"),
)
# Iteration: dividing triangles
for triangle in triangles:
if triangle.parent.is_root:
# all_points.extend([triangle.p3, triangle.pc])
photo_counter += 1
print("Edge point, counter=%d" % photo_counter)
self.explore_point(
triangle.p3,
triangle,
center.pc,
os.path.join(img_dir, "%d.png" % photo_counter),
)
photo_counter += 1
print("Center point, counter=%d" % photo_counter)
self.explore_point(
triangle.pc,
triangle,
center.pc,
os.path.join(img_dir, "%d.png" % photo_counter),
)
def matrix_from_rotation_axis_angle(axis: Vector3r, angle: float, is_degrees: bool = False) -> np.ndarray:
# ref.: https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
if is_degrees:
angle = np.deg2rad(angle)
x, y, z = axis.to_numpy_array()
ca = np.cos(angle)
sa = np.sin(angle)
omca = 1 - ca
return np.array(
[
[ca + (x ** 2) * omca, x * y * omca - z * sa, x * z * omca + y * sa],
[y * x * omca + z * sa, ca + (y ** 2) * omca, y * z * omca - x * sa],
[z * x * omca - y * sa, z * y * omca + x * sa, ca + (z ** 2) * omca],
]
)
def explore(self):
"""Traverse the concentric circles in turn by the strategy of
iteratively halving the radius of the circle
"""
if not self._safety_surface["type"] == "circle":
return
img_dir = self._config["image_dir"]
# Arm and take off
self._uav.armDisarm(True)
self._uav.takeoffAsync().join()
self._move_to(Vector3r(0, 0, -60))
# Explore center point and points on edge
points_per_round = self.generate_explore_views()
# self.show_explore_path(points_per_round)
center = points_per_round[0][0]
root_triangle = Triangle(center, center, center, None, True)
point_counter = 1
print("Center point, counter=1")
self.explore_point(
center,
root_triangle,
root_triangle.pc,
os.path.join(img_dir, "%d.png" % point_counter),
)
for point in points_per_round[-1]:
point_counter += 1
print("round %d, point, counter=%d" %
(len(points_per_round) - 1, point_counter))
self.explore_point(
point,
root_triangle,
root_triangle.pc,
os.path.join(img_dir, "%d.png" % point_counter),
)
"""Explore circle 2, 1, 3 successively"""
point_counter = self.between(0, 4, points_per_round, 2, point_counter,
img_dir)
point_counter = self.between(0, 2, points_per_round, 1, point_counter,
img_dir)
point_counter = self.between(2, 4, points_per_round, 3, point_counter,
img_dir)
def setRandomPose(client, center, offset_range, rotation_range):
random_vector = np.random.random_sample((2, 1))
unit_vector = random_vector / np.linalg.norm(random_vector)
displacement_vector = (offset_range[1] - offset_range[0]) * (
np.random.random_sample()) * unit_vector + offset_range[0]
displacement_vector = displacement_vector.reshape(-1, )
position = Vector3r(center.position.x_val + displacement_vector[0],
center.position.y_val + displacement_vector[1],
center.position.z_val)
rotation = (rotation_range[1] - rotation_range[0]
) * np.random.random_sample() + rotation_range[
0] #2*np.sin(2*np.pi*np.random.random_sample())
orientation = Quaternionr(z_val=rotation)
pose = Pose(position_val=position, orientation_val=orientation)
client.simSetObjectPose("pallet", pose, True)
return pose, displacement_vector, rotation
def explore(self):
"""Explore the place of interest from the outer circle to the inner circle"""
if not self._safety_surface["type"] == "circle":
return
img_dir = self._config["image_dir"]
# Arm and take off
self._uav.armDisarm(True)
self._uav.takeoffAsync().join()
self._move_to(Vector3r(0, 0, -60))
# Explore center point
points_per_round = self.generate_explore_views()
self.show_explore_path(points_per_round)
center = points_per_round[0][0]
root_triangle = Triangle(center, center, center, None, True)
photo_counter = 1
print("Center point, counter=1")
self.explore_point(center, root_triangle, root_triangle.pc,
os.path.join(img_dir, "1.png"))
# Explore the outer circle
for point in points_per_round[-1]:
photo_counter += 1
print("round %d, point, counter=%d" %
(len(points_per_round) - 1, photo_counter))
self.explore_point(
point,
root_triangle,
root_triangle.pc,
os.path.join(img_dir, "%d.png") % photo_counter,
)
# Explore other points from the outer circle to the inner circle
for i in range(len(points_per_round) - 2, 0, -1):
for point in points_per_round[i]:
photo_counter += 1
print("round %d, point, counter=%d" % (i, photo_counter))
tmp_triangle = self.__generate_smallest_triangle(
point, center, points_per_round[i + 1])
self.explore_point(
point,
tmp_triangle,
tmp_triangle.pc,
os.path.join(img_dir, "%d.png" % photo_counter),
)
def _observe_at_view(self, view, img_path):
"""Fly to given position and take a photo at given orientation."""
position = view["position"]
position = (Vector3r(position[0], position[1], position[2])
if type(position) == list else position)
print("Observe (%f, %f, %f) with yaw %f and pitch %f" % (
position.x_val,
position.y_val,
position.z_val,
view["yaw"] / math.pi * 180,
view["pitch"] / math.pi * 180,
))
self._move_to(position, yaw=view["yaw"] / math.pi * 180)
# self._uav.hoverAsync().join()
# time.sleep(5)
self._uav.simSetCameraOrientation(
"0", airsim.to_quaternion(view["pitch"], 0.0, 0.0))
png = self._uav.simGetImage("0", airsim.ImageType.Scene)
ReconstructionNavigator.mkdir(os.path.dirname(img_path))
airsim.write_file(img_path, png)
def capture_LIDAR_depth(self, p, q):
"""
p: A 3-element NumPy array, the position.
q: A 4-element NumPy array, quaternion, w is the last element.
"""
faces = [0, 1, 2, -1] # Front, right, back, left.
depthList = []
q0 = Quaternionr(q[0], q[1], q[2], q[3])
for face in faces:
# Compose a AirSim Pose object.
yaw = np.pi / 2 * face
yq = to_quaternion(0, 0, yaw)
# q = to_quaternion( e[0], e[1], e[2] )
q1 = yq * q0
pose = Pose(Vector3r(p[0], p[1], p[2]), q1)
# Change the vehicle pose.
self.set_vehicle_pose(pose)
# Get the image.
for i in range(self.maxTrial):
depth, _ = self.get_depth_campos()
if depth is None:
print("Fail for trail %d on face %d. " % (i, face))
continue
if (depth is None):
raise Exception("Could not get depth image for face %d. " %
(face))
# Get a valid depth.
depthList.append(depth)
return depthList
def pose_from_meshroom_to_airsim(meshroom_pose):
assert len(meshroom_pose.center) == 3 and len(
meshroom_pose.rotation) == 9, meshroom_pose
xyzw = MeshroomTransform.rotation(meshroom_pose.rotation,
as_xyzw_quaternion=True)
return Pose(Vector3r(*meshroom_pose.center), Quaternionr(*xyzw))
def __init__(self,
vehicle_names,
image_mask_FC_FR_FL=[True, False, False],
reward_function=car_racing_rewarding_function,
mode="rgb_normal"):
self.vehicle_names = vehicle_names
# Set reward function
self.reward_function = reward_function
# Set Environment Return options
self.mode = mode
self.mode
# Set Drive Commands to zero initially
self._throttle = dict.fromkeys(
self.vehicle_names,
0) # Used as a constant gain factor for the action throttle.
self._steering = dict.fromkeys(
self.vehicle_names, 0) # Each action lasts this many seconds
self._brake = dict.fromkeys(self.vehicle_names, 0)
self.THROTTLE_INC = .10
self.THROTTLE_DEC = -.10
self.BRAKE_INC = .10
self.BRAKE_DEC = -.20
self.STEER_LEFT_INC = -.10
self.STEER_RIGHT_INC = .10
# The number of INERTIAL state variables to keep track of
self.count_inertial_state_variables = 15 # Posx, Posy, PosZ, Vx, Vy, Vz, Ax, Ay, Az, AngVelx, AngVely, AngVelz, AngAccx, AngAccy, AngAccz
# Throttle up, throttle down, increase brake, decrease break, left_steer, right_steer, No action
self.count_car_actions = 7
# Initialize the current inertial state to zero
self.current_inertial_states = dict.fromkeys(
self.vehicle_names,
np.array(np.zeros(self.count_inertial_state_variables)))
# Initialize the IMAGE variables -- We Take in Front Center, Right, Left
self.images_rgb = dict.fromkeys(self.vehicle_names, 0)
self.images_rgba = dict.fromkeys(self.vehicle_names, 0)
self.image_mask_rgb = np.array(
[[0 + 3 * i, 1 + 3 * i, 2 + 3 * i]
for m, i in zip(image_mask_FC_FR_FL, range(3)) if m]).reshape(-1)
self.image_mask_rgba = np.array(
[[0 + 4 * i, 1 + 4 * i, 2 + 4 * i]
for m, i in zip(image_mask_FC_FR_FL, range(3)) if m]).reshape(-1)
self.image_mask_FC_FR_FL = image_mask_FC_FR_FL
self.initial_position = dict.fromkeys(self.vehicle_names, 0)
self.initial_velocity = dict.fromkeys(self.vehicle_names, 0)
# Connect to the AirSim simulator and begin:
print('Initializing Car Client')
self.client = client.CarClient()
self.client.confirmConnection()
for vn in self.vehicle_names:
self.client.enableApiControl(True, vn)
orien = Vector3r(0, 0, 0)
self.client.simSetCameraOrientation(0,
orien.to_Quaternionr(),
vehicle_name=vn) #radians
orien = Vector3r(0, .12, -np.pi / 9)
self.client.simSetCameraOrientation(1, orien, vehicle_name=vn)
orien = Vector3r(0, .12, np.pi / 9)
self.client.simSetCameraOrientation(2, orien, vehicle_name=vn)
# Reset Collion Flags
print('Initialization Complete!')
# Timing Operations Initialize
self.dt = 0
self.tic = 0
self.toc = 0
def __init__(self,
vehicle_name="Car1",
action_duration=.08,
image_mask_FC_FR_FL=[True, True, True],
sim_mode="rgb",
IMG_HEIGHT=128,
IMG_WIDTH=128,
IMG_STEP=3):
# Set Environment Return options
self.action_duration = action_duration
self.mode = sim_mode
# Set Drive Commands to zero initially
self._throttle = 0 # Used as a constant gain factor for the action throttle.
self._steering = 0 # Each action lasts this many seconds
self._brake = 0
# Simulator Image setup
self.IMG_HEIGHT = IMG_HEIGHT
self.IMG_WIDTH = IMG_WIDTH
isRGB = False
IMG_CHANNELS = 1
if 'rgb' in self.mode:
isRGB = True
IMG_CHANNELS = 3
isNormal = False
if 'normal' in self.mode:
isNormal = True
self.IMG_CHANNELS = IMG_CHANNELS
self.IMG_STEP = IMG_STEP
self.IMG_VIEWS = np.sum(np.array(image_mask_FC_FR_FL, dtype=np.int))
# Initialize the container that holds the sequence of images from the simulator
self.obs4 = np.zeros(
(self.IMG_HEIGHT, self.IMG_WIDTH,
self.IMG_CHANNELS * self.IMG_STEP * self.IMG_VIEWS))
# The number of INERTIAL state variables to keep track of
self.count_inertial_state_variables = 15 # Posx, Posy, PosZ, Vx, Vy, Vz, Ax, Ay, Az, AngVelx, AngVely, AngVelz, AngAccx, AngAccy, AngAccz
# Throttle up, throttle down, increase brake, decrease break, left_steer, right_steer, No action
self.count_car_actions = 7
# Initialize the current inertial state to zero
self.current_inertial_state = np.array(
np.zeros(self.count_inertial_state_variables))
# Initialize the IMAGE variables -- We Take in Front Center, Right, Left
self.images_rgb = None
self.images_rgba = None
self.image_mask_rgb = np.array(
[[0 + 3 * i, 1 + 3 * i, 2 + 3 * i]
for m, i in zip(image_mask_FC_FR_FL, range(3)) if m]).reshape(-1)
self.image_mask_rgba = np.array(
[[0 + 4 * i, 1 + 4 * i, 2 + 4 * i]
for m, i in zip(image_mask_FC_FR_FL, range(3)) if m]).reshape(-1)
self.image_mask_FC_FR_FL = image_mask_FC_FR_FL
# Connect to the AirSim simulator and begin:
print('Initializing Car Client')
self.client = client.CarClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
print('Initialization DONE!')
print("Setting Camera Views")
orien = Vector3r(0, 0, 0)
self.client.simSetCameraOrientation("0", orien) #radians
orien = Vector3r(0, .12, -np.pi / 9)
self.client.simSetCameraOrientation("1", orien)
orien = Vector3r(0, .12, np.pi / 9)
self.client.simSetCameraOrientation("2", orien)
# Reset Collion Flags
print("Setting Camera Views DONE!")
# Set up GUI Video Feeder
self.gui_data = {'obs': None, 'state': None, 'meta': None}
self.vehicle_name = vehicle_name
num_video_feeds = np.sum(
np.array(self.image_mask_FC_FR_FL, dtype=np.int)) * IMG_STEP
GUIConn, self.simEnvDataConn = multiprocessing.Pipe()
self.app = AirSimGUI.CarGUI(GUIConn,
vehicle_names=[vehicle_name],
num_video_feeds=num_video_feeds,
isRGB=isRGB,
isNormal=isNormal)
# Timing Operations Initialize
self.time_to_do_action = 0
self.time_to_grab_images = 0
self.time_to_grab_states = 0
self.time_to_calc_reward = 0
self.time_to_step = 0
self.extra_metadata = None
