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 next_quaternion(self, ):
if self.oriind >= self.SmoothCount: # sample a new orientation
rand_ori = self.random_quaternion()
new_ori = rand_ori * self.orientation
new_qtn = Quaternionr(new_ori.x, new_ori.y, new_ori.z, new_ori.w)
(pitch, roll, yaw) = to_eularian_angles(new_qtn)
# print ' %.2f, %.2f, %.2f' %(pitch, roll, yaw )
pitch = np.clip(pitch, self.MinPitch, self.MaxPitch)
roll = np.clip(roll, self.MinRoll, self.MaxRoll)
yaw = np.clip(yaw, self.MinYaw, self.MaxYaw)
# print ' %.2f, %.2f, %.2f' %(pitch, roll, yaw )
new_qtn_clip = to_quaternion(pitch, roll, yaw)
new_ori_clip = Quaternionpy(new_qtn_clip.w_val, new_qtn_clip.x_val,
new_qtn_clip.y_val, new_qtn_clip.z_val)
qtnlist = Quaternionpy.intermediates(self.orientation,
new_ori_clip,
self.SmoothCount - 2,
include_endpoints=True)
self.orientation = new_ori_clip
self.orilist = list(qtnlist)
self.oriind = 1
# print "sampled new", new_ori, ', after clip', self.orientation #, 'list', self.orilist
next_qtn = self.orilist[self.oriind]
self.oriind += 1
# print " return next", next_qtn
return Quaternionr(next_qtn.x, next_qtn.y, next_qtn.z, next_qtn.w)
def init_random_yaw(self):
self.reset()
randomyaw = np.random.uniform(self.MinYaw, self.MaxYaw)
print('Random yaw {}'.format(randomyaw))
qtn = to_quaternion(0., 0., randomyaw)
self.orientation = Quaternionpy(qtn.w_val, qtn.x_val, qtn.y_val,
qtn.z_val)
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 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
for x, y in zip(np.linspace(0, 5, 5), np.linspace(0, 0, 5))
],
scale=1,
color_rgba=[1.0, 1.0, 1.0, 1.0],
duration=1200.0)
# client.simPlotTransforms(poses = [Pose(position_val=Vector3r(x,y,0), orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=yaw)) for x, y, yaw in zip(np.linspace(0,10,10), np.linspace(0,0,10), np.linspace(0,np.pi,10))],
# scale = 35, thickness = 5, duration = 1200.0, is_persistent = False)
# client.simPlotTransforms(poses = [Pose(position_val=Vector3r(x,y,0), orientation_val=to_quaternion(pitch=0.0, roll=roll, yaw=0.0)) for x, y, roll in zip(np.linspace(0,10,10), np.linspace(1,1,10), np.linspace(0,np.pi,10))],
# scale = 35, thickness = 5, duration = 1200.0, is_persistent = False)
client.simPlotTransformsWithNames(
poses=[
Pose(position_val=Vector3r(x, y, 0),
orientation_val=to_quaternion(pitch=0.0, roll=0.0, yaw=yaw))
for x, y, yaw in zip(np.linspace(0, 10, 10), np.linspace(0, 0, 10),
np.linspace(0, np.pi, 10))
],
names=["tf_yaw_" + str(idx) for idx in range(10)],
tf_scale=35,
tf_thickness=5,
text_scale=1,
text_color_rgba=[1.0, 1.0, 1.0, 1.0],
duration=1200.0)
client.simPlotTransformsWithNames(
poses=[
Pose(position_val=Vector3r(x, y, 0),
orientation_val=to_quaternion(pitch=0.0, roll=roll, yaw=0.0))
for x, y, roll in zip(np.linspace(0, 10, 10), np.linspace(1, 1, 10),