def OrbitAnimal(cx, cy, radius, speed, altitude, camera_angle, animal):
"""
@param cx: The x position of our orbit starting location
@param cy: The x position of our orbit starting location
@param radius: The radius of the orbit circle
@param speed: The speed the drone should more, it's hard to take photos when flying fast
@param altitude: The altidude we want to fly at, dont fly too high!
@param camera_angle: The angle of the camera
@param animal: The name of the animal, used to prefix the photos
"""
x = cx - radius
y = cy
# set camera angle
client.simSetCameraOrientation(0, airsim.to_quaternion(
camera_angle * math.pi / 180, 0, 0)) # radians
# move the drone to the requested location
print("moving to position...")
client.moveToPositionAsync(
x, y, z, 1, 60, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, yaw_mode=airsim.YawMode(False, 0)).join()
pos = client.getMultirotorState().kinematics_estimated.position
dx = x - pos.x_val
dy = y - pos.y_val
yaw = airsim.to_eularian_angles(
client.getMultirotorState().kinematics_estimated.orientation)[2]
# keep the drone on target, it's windy out there!
print("correcting position and yaw...")
while abs(dx) > 1 or abs(dy) > 1 or abs(yaw) > 0.1:
client.moveToPositionAsync(
x, y, z, 0.25, 60, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, yaw_mode=airsim.YawMode(False, 0)).join()
pos = client.getMultirotorState().kinematics_estimated.position
dx = x - pos.x_val
dy = y - pos.y_val
yaw = airsim.to_eularian_angles(
client.getMultirotorState().kinematics_estimated.orientation)[2]
print("yaw is {}".format(yaw))
print("location is off by {},{}".format(dx, dy))
o = airsim.to_eularian_angles(
client.getMultirotorState().kinematics_estimated.orientation)
print("yaw is {}".format(o[2]))
# let's orbit around the animal and take some photos
nav = drone_orbit.OrbitNavigator(photo_prefix=animal, radius=radius, altitude=altitude, speed=speed, iterations=1, center=[
cx - pos.x_val, cy - pos.y_val], snapshots=30, image_dir="./drone_images/")
nav.start()
def get_relloc_camera(camera_id='1', mode='rot_mat'):
"""
Function to get position of human relative to the camera
Parameters
----------
camera_id: ID of the camera from which we want to observe the environment
mode: if 'angle', differece in angles (degrees) is returned
if 'rot_mat' rotation matrix is returned. C.dot(T) = O
Returns
-------
rel_pos: Relative position (x, y, z). Numpy array
rel_orient: if mode == angle: Relative orientation (roll, pitch, yaw). Numpy array
if mode == rot_mat: Reotation matix is returned
"""
# Get human's pose
human_pose = client.simGetObjectPose(HUMAN_ID)
# Get camera's pose
camera_pose = client.simGetCameraInfo(camera_id).pose
drone_pose = client.simGetVehiclePose()
# Get relative position
rel_pos = (human_pose.position - camera_pose.position).to_numpy_array()
if mode == 'angle':
# Get relative orientation
human_rpy = np.array(airsim.to_eularian_angles(human_pose.orientation)) # quaternion to Eularian
camera_rpy = np.array(airsim.to_eularian_angles(camera_pose.orientation)) # quaternion to Eularian
rel_orient = human_rpy - camera_rpy
if mode == 'rot_mat':
# Get rotation matrix
human_rot = R.from_quat(human_pose.orientation.to_numpy_array()).as_matrix()
camera_rot = R.from_quat(camera_pose.orientation.to_numpy_array()).as_matrix()
# Calculate transformation/rotation matrix
camera_rot_inv = np.linalg.inv(camera_rot)
rel_orient = camera_rot_inv.dot(human_rot)
rel_pos = camera_rot_inv.dot(rel_pos)
com_rot = np.array([[0, 1, 0], [0, 0, 1], [1, 0, 0]])
rel_orient = com_rot.dot(rel_orient.dot(com_rot.T))
rel_pos = com_rot.dot(rel_pos)
rot = R.from_matrix(rel_orient)
rel_orient = rot.as_euler('zyx', degrees=True)
return rel_pos, rel_orient
def calculate_camera(self, v_identifier):
"""
Calculate camera heading
"""
current_pos_state = self.client.getMultirotorState(
).kinematics_estimated
current_pos_local = np.asarray([
current_pos_state.position.x_val, current_pos_state.position.y_val,
current_pos_state.position.z_val
])
# print("now:"+str(current_pos_local))
camera_angle = self.calculate_camera_to_center(current_pos_local)
global g_current_orientation_pitch, g_current_orientation_roll, g_current_orientation_yaw
g_current_orientation_pitch = camera_angle[0]
g_current_orientation_roll = 0
g_current_orientation_yaw = camera_angle[
2] - airsim.to_eularian_angles(current_pos_state.orientation)[2]
#g_current_orientation_yaw = camera_angle[2]
# log_file("camera",
# v_identifier+":desired orientation:" + str(g_current_orientation_pitch) + ","
# + str(g_current_orientation_yaw))
self.client.simSetCameraOrientation(
"front_center",
airsim.to_quaternion(g_current_orientation_pitch,
g_current_orientation_roll,
g_current_orientation_yaw))
def compute_observation(self):
# retrieve visual observation
image_type = ImageInfo[self.image_type]
responses = self.client.simGetImages([airsim.ImageRequest(0, image_type.index, image_type.as_float, False)])
response = responses[0]
if image_type.as_float:
img1d = np.array(response.image_data_float, dtype=np.float)
img1d = 255 / np.maximum(np.ones(img1d.size), img1d)
img2d = np.reshape(img1d, (response.height, response.width))
image = np.expand_dims(Image.fromarray(img2d).convert('L'), axis=2)
else:
# get numpy array
img1d = np.fromstring(response.image_data_uint8, dtype=np.uint8)
image = img1d.reshape(response.height, response.width, image_type.channel_size)
image = np.ascontiguousarray(image, dtype=np.uint8)
# retrieve poses for both human and robot
pose = self.client.simGetVehiclePose()
r = self._distance_to_goal(pose.position)
yaw = airsim.to_eularian_angles(pose.orientation)[2]
phi = np.arctan2(self.goal_position[1] - pose.position.y_val, self.goal_position[0] - pose.position.x_val) - yaw
goal = Goal(r, phi)
logging.debug('Goal distance: {:.2f}, relative angle: {:.2f}'.format(r, np.rad2deg(phi)))
observation = Observation(image, goal)
return observation
def animate(i, xs, ys, client):
# Aquire and parse data from serial port
vehiclePose = client.simGetVehiclePose()
position = vehiclePose.position
orientation = airsim.to_eularian_angles(vehiclePose.orientation)
print(orientation[0], orientation[1], orientation[2])
# Add x and y to lists
xs.append(position.x_val)
ys.append(position.y_val)
# rs.append(0.5)
# Limit x and y lists to 20 items
# xs = xs[-20:]
# ys = ys[-20:]
# Draw x and y lists
ax.clear()
ax.plot(xs, ys, label="Experimental Probability")
# ax.quiver
yaw = orientation[2]
new_x = np.cos(yaw)
new_y = np.sin(yaw)
ax.quiver(position.x_val, position.y_val, new_x, new_y)
# ax.plot(xs, rs, label="Theoretical Probability")
ax.set_ylim([-200, 200])
ax.set_xlim([-200, 200])
# ax.set_ylim(np.min(ys) - np.std(ys),np.max(ys) + np.std(ys))
# ax.set_xlim(np.min(xs) - np.std(xs), np.max(xs) + np.std(xs))
# Format plot
plt.xticks(rotation=45, ha='right')
plt.subplots_adjust(bottom=0.30)
plt.title('This is how I roll...')
plt.ylabel('Relative frequency')
plt.legend()
def getParamExtrinsicFollow():
# http://ksimek.github.io/2012/08/22/extrinsic/
global state, posIndex
uavX = state[posIndex][0].kinematics_estimated.position.x_val
uavY = state[posIndex][0].kinematics_estimated.position.y_val
uavZ = state[posIndex][0].kinematics_estimated.position.z_val
uavPitch, uavRoll, uavYaw = airsim.to_eularian_angles(
state[posIndex][0].kinematics_estimated.orientation)
# rotation = R.from_euler('xyz', [90, 0, 180], degrees=True)
rotationX = 90 + np.degrees(uavPitch)
rotationY = np.degrees(uavRoll)
rotationZ = 90 + np.degrees(uavYaw)
rotation = R.from_euler('xyz', [rotationX, rotationY, rotationZ],
degrees=True)
Rc = rotation.as_dcm()
C = np.array([uavX, uavY, uavZ])
t = np.dot(-Rc.T, C)
# Note that Rc here should be Transpose, but I can not figure out how to do it
# in numpy ... (fail me ...)
params = np.vstack((Rc, [t])).T
# to have a square matrix
params = np.vstack((params, [[0, 0, 0, 1]]))
return params
def animate(i, xs, ys, client):
throttle, steering = SimpleDrive(client)
# Aquire and parse data from serial port
vehiclePose = client.simGetVehiclePose()
position = vehiclePose.position
orientation = airsim.to_eularian_angles(vehiclePose.orientation)
# Add x and y to lists
xs.append(position.x_val)
ys.append(position.y_val)
steerings.append(steering)
throttles.append(throttle)
# rs.append(0.5)
# Limit x and y lists to 20 items
# xs = xs[-20:]
# ys = ys[-20:]
# Draw x and y lists
ax[0].clear()
ax[1].clear()
# ax.plot(xs, ys, label="Experimental Probability")
# ax.quiver
yaw = orientation[2]
new_x = np.cos(yaw)
new_y = np.sin(yaw)
# ax.quiver(position.x_val, position.y_val, new_x, new_y)
ff = simpleDriver.get_objective_func()
x = np.linspace(0, 1000)
ax[0].plot(x, list(map(ff, x)))
# Steering
ax[1].plot(steerings)
def reset(client, scene=None):
"""
Called from the reset method in AirsimEnv. This method resets the AirSim simulation, respawns the UAV etc.
:param client: AirsimEnv object
:param scene: The AirSim scene
:return: None
"""
client.reset()
if scene is not None:
time.sleep(0.2)
pose = client.simGetVehiclePose()
start_pos = valid_spawn(scene)
pose.position.x_val = start_pos[0]
pose.position.y_val = start_pos[1]
pose.position.z_val = 0
pitch, roll, yaw = airsim.to_eularian_angles(pose.orientation)
yaw = np.random.rand() * 2 * np.pi
pose.orientation = airsim.to_quaternion(pitch, roll, yaw)
client.simSetVehiclePose(pose, True)
time.sleep(0.2)
client.enableApiControl(True)
client.armDisarm(True)
hover(client)
custom_takeoff(client)
hover(client)
def vectorTo3D(pixelX, pixelY, camInfo, depthImage, maxDistView = None, vectorDistances = None):
"""From image pixels (2D) to relative(!) 3D coordinates"""
if isinstance(maxDistView, float) or isinstance(maxDistView, int):
depthImage[ depthImage > maxDistView] = maxDistView
height, width = depthImage.shape
# print(f"Image size: width:{width} -- height:{height}")
halfWidth = width/2
halfHeight= height/2
camPitch, camRoll,camYaw = airsim.to_eularian_angles(camInfo.pose.orientation)
# to rads (its on degrees now)
hFoV = np.radians(camInfo.fov)
vFoV = (height/width)*hFoV
pointsH = np.array(pixelY, dtype=int)
pointsW = np.array(pixelX, dtype=int)
pixelPitch = ((pointsH-halfHeight)/halfHeight) * (vFoV/2)
pixelYaw = ((pointsW-halfWidth)/halfWidth) * (hFoV/2)
theta = (np.pi/2) - pixelPitch
# turn
phi = pixelYaw
r = vectorDistances
x = r*np.sin(theta)*np.cos(phi)
y = r*np.sin(theta)*np.sin(phi)
z = r*np.cos(theta)
return x, y, z
def angluarVelocityThrottleController(self,
cnet,
q,
vcy,
R_cb=np.array([[0, 0, 1], [1, 0, 0],
[0, 1, 0]])):
"""
- function: 2526中角速率控制方法
- params:
- cnet: 目标在图像上的坐标
- q: 飞机在世界坐标系的四元数
- vcy: 飞机竖直方向上速度
- R_cb: 相机系到机体系旋转矩阵。默认光轴与机头方向一致，图像平面水平为xc，光轴为zc，机体系为FRD
- return:
- cmd: [roll_rate, pitch_rate, yaw_rate, throttle], 三轴角速度和油门, FRD
"""
ex, ey = cent[0] - self.x0, cent[1] - self.y0
print("ex: {}, ey: {}".format(ex, ey))
pitch, roll, yaw = airsim.to_eularian_angles(q)
print("pitch: {}, roll: {}, yaw: {}, vcy: {}".format(
pitch, roll, yaw, vcy))
wcx = -self.k2 * ey - self.k3 * (pitch - self.theta_d)
print(
"-self.k2*ey: {}, self.theta_d: {}, -self.k3*(pitch-self.theta_d): {}"
.format(-self.k2 * ey, self.theta_d,
-self.k3 * (pitch - self.theta_d)))
wcy = saturation(self.k5 * ex, 1)
wcz = self.k6 * (roll - self.phi_d)
print("wcx: {}, wcy: {}, wcz: {}".format(wcx, wcy, wcz))
wbx, wby, wbz = R_cb.dot(np.array([wcx, wcy, wcz]))
throttle = self.hover * (self.k4 / self.g *
(vcy - self.k1 * ey) + 1) / np.cos(pitch)
return wbx, wby, wbz, throttle
def takeAction(self, action):
position = self.client.simGetVehiclePose().position
yaw = airsim.to_eularian_angles(
self.client.simGetVehiclePose().orientation)[2]
if action == 0:
yaw_change = 0.0
elif action == 1:
yaw_change = 30.0
elif action == 2:
yaw_change = -30.0
action_yaw = yaw + yaw_change
vx = math.cos(action_yaw)
vy = math.sin(action_yaw)
v_norm = np.linalg.norm([vx, vy])
vx = vx / v_norm * self.MOVE_RATE
vy = vy / v_norm * self.MOVE_RATE
new_x = position.x_val + vx
new_y = position.y_val + vy
new_pose = airsim.Pose(airsim.Vector3r(new_x, new_y, self.HEIGHT),
airsim.utils.to_quaternion(0, 0, action_yaw))
self.client.simSetVehiclePose(new_pose, False)
# time.sleep(self.CV_SLEEP_TIME)
collided = self.checkCollision()
return collided
def AE(self, goal, vehicle_name=''):
pos = self.simGetGroundTruthKinematics(vehicle_name="").position
xdistance = (goal[0] - (pos.x_val))
ydistance = (goal[1] - (pos.y_val))
zdistance = (0 - (pos.z_val))
#elevation=math.atan((zdistance)/np.sqrt(np.power(xdistance,2)+np.power(ydistance,2)))
#elevation=((math.degrees(elevation) - 90) % 360) - 90
elevation = math.atan2(
(zdistance),
np.sqrt(np.power(xdistance, 2) + np.power(ydistance, 2)))
elevation = (math.degrees(elevation))
pitch, roll, yaw = airsim.to_eularian_angles(
client.simGetVehiclePose(vehicle_name="").orientation)
yaw = math.degrees(yaw)
pos_angle = math.atan2(goal[1] - pos.y_val, goal[0] - pos.x_val)
pos_angle = math.degrees(pos_angle) % 360
track = math.radians(pos_angle - yaw)
#track=((track - math.pi) % 2*math.pi) - math.pi
track = ((math.degrees(track) - 180) % 360) - 180
#track=reMap(track,180,-180,1,-1)
AE = np.array([track, elevation])
return AE
def collisionCorrection(timeOutCollision=300):
global controllers
wayPointDict = {}
for ctrl in controllers:
wayPointDict[ctrl.getName()] = 0
time.sleep(timeOutCollision)
running = True
while running:
time.sleep(timeOutCollision)
for ctrl in controllers:
if ctrl.getWayPoint() == wayPointDict[ctrl.getName()]:
print(
f"{ctrl.getName()} is at the same spot after {timeOutCollision}[sec]. Check collision"
)
try:
state = ctrl.getState()
print(f"{ctrl.getName()} got state")
except:
print(f"{ctrl.getName()} could not get state and failed")
try:
if state.collision.has_collided:
print(f"{ctrl.getName()} has collided ...")
stateList = ctrl.getStateList()
x = stateList[-2].kinecmatics_estimated.position.x_val
y = stateList[-2].kinecmatics_estimated.position.y_val
z = stateList[-2].kinecmatics_estimated.position.z_val
_, _, yaw = airsim.to_eularian_angles(
stateList[-3].kinematics_estimated.orientation)
task = ctrl.moveToPositionYawModeAsync(x, y, z, 1, yaw)
task.join()
print(
f"{ctrl.getName()}moved successfully to previous position"
)
except:
print(
f"{ctrl.getName()} failed at moving drone. Debug ...")
# import pdb; pdb.set_trace()
else:
print(
f"{ctrl.getName()} has changed spot. Update wayPointDict")
wayPointDict[ctrl.getName()] = ctrl.getWayPoint()
if ctrl.getWayPoint() >= (ctrl.wayPointSize - 2):
running = False
def get_orientation(client):
"""
Makes call to AirSim and returns orientation (yaw angle)
:param client: AirsimEnv object
:return: Orientation (yaw) of agent in radians.
"""
q = client.simGetGroundTruthKinematics().orientation
return airsim.to_eularian_angles(q)[2]
def getCarOrientation(car_state):
"""将弧度转换成0-360度，注意原来的弧度是-3.14到3.14，
且y坐标的正向朝下，角度正方向为顺时针方向
return: 0-360
"""
_, _, arc = airsim.to_eularian_angles(
car_state.kinematics_estimated.orientation)
return arc
def straight(self, duration, speed):
pitch, roll, yaw = airsim.to_eularian_angles(
self.client.simGetVehiclePose().orientation)
vx = math.cos(yaw) * speed
vy = math.sin(yaw) * speed
self.client.moveByVelocityZAsync(
vx, vy, self.z, duration,
airsim.DrivetrainType.ForwardOnly).join()
start = time.time()
return start, duration
def moveByVolocity(self, velocity, angle):
pitch, roll, self.yaw = airsim.to_eularian_angles(
self.client.simGetVehiclePose().orientation)
self.yaw = (self.yaw + angle)
self.vx = velocity * math.cos(self.yaw)
self.vy = velocity * math.sin(self.yaw)
if (self.vx == 0 and self.vy == 0):
self.vx = velocity * math.cos(self.yaw)
self.vy = velocity * math.sin(self.yaw)
self.client.moveByVelocityZAsync(self.vx, self.vy, -2, 0.5,
airsim.DrivetrainType.ForwardOnly,
airsim.YawMode(False, 0)).join()
def straight(self, speed):
pitch, roll, yaw = airsim.to_eularian_angles(
self.client.simGetVehiclePose().orientation)
dx = math.cos(yaw) * speed
dy = math.sin(yaw) * speed
x = self.client.simGetVehiclePose().position.x_val
y = self.client.simGetVehiclePose().position.y_val
"""client.moveByVelocityAsync() causes z position to dip too much due to lack of PID control"""
self.client.moveToPositionAsync(x + dx, y + dy, self.z, speed,
airsim.DrivetrainType.ForwardOnly)
init_time = time.time()
return init_time
def goalDirection(self, goal, pos):
yaw = airsim.to_eularian_angles(
self.client.simGetVehiclePose().orientation)[2]
yaw = math.degrees(yaw)
pos_angle = math.atan2(goal[1] - pos.y_val, goal[0] - pos.x_val)
pos_angle = math.degrees(pos_angle) % 360
track = math.radians(pos_angle - yaw)
return ((math.degrees(track) - 180) % 360) - 180
def load_airsim(airsim_client):
rpcinfo = airsim_client.getMultirotorState()
gps_location = rpcinfo.gps_location
kinematics_estimated = rpcinfo.kinematics_estimated
pitch, roll, yaw = airsim.to_eularian_angles(
rpcinfo.kinematics_estimated.orientation)
homepoint = airsim_client.getHomeGeoPoint()
global way_point_status
global initialize_height
if (initialize_height is not 0):
height_above_takeoff = - \
(kinematics_estimated.position.z_val) + initialize_height
else:
height_above_takeoff = initialize_height
telemetry = {
"battery_state": {
"percentage": 70.04
},
"distance_from_home": 1561.4,
"gimbal": {
"roll": roll,
"pitch": pitch,
"yaw": yaw
},
"height_above_takeoff": height_above_takeoff,
"gps_health": 5,
"heading": math.degrees(yaw),
"velocity": {
"x": kinematics_estimated.linear_velocity.x_val,
"y": kinematics_estimated.linear_velocity.y_val,
"z": kinematics_estimated.linear_velocity.z_val
},
"gps_position": {
"latitude": gps_location.latitude,
"altitude": gps_location.altitude,
"longitude": gps_location.longitude
},
"last_change_time": rpcinfo.timestamp,
"lastHome": {
"latitude": homepoint.latitude,
"operationalAlt": homepoint.altitude,
"longitude": homepoint.longitude
},
"owner": "droneService",
"state": {
"armed": True
},
"wayPoints": {
"status": way_point_status
},
"keepAlive": rpcinfo.timestamp
}
return telemetry
def get_relloc_vehicle(mode='rot_mat'):
"""
Function to get position of human relative to the drone
Parameters
----------
mode: if 'angle', differece in angles (degrees) is returned
if 'rot_mat' rotation matrix is returned. C.dot(T) = O
Returns
-------
rel_pos: Relative position (x, y, z). Numpy array
rel_orient: if mode == angle: Relative orientation (roll, pitch, yaw). Numpy array
if mode == rot_mat: Reotation matix is returned
"""
# Get human's pose
human_pose = client.simGetObjectPose(HUMAN_ID)
# Get drone's pose
drone_pose = client.simGetVehiclePose()
# Get relative position
rel_pos = (human_pose.position - drone_pose.position).to_numpy_array()
if mode == 'angle':
# Get relative orientation
human_rpy = np.array(airsim.to_eularian_angles(human_pose.orientation)) # quaternion to Eularian
drone_rpy = np.array(airsim.to_eularian_angles(drone_pose.orientation)) # quaternion to Eularian
rel_orient = human_rpy - drone_rpy
if mode == 'rot_mat':
# Get rotation matrix
human_rot = R.from_quat(human_pose.orientation.to_numpy_array()).as_matrix()
drone_rot = R.from_quat(drone_pose.orientation.to_numpy_array()).as_matrix()
# Calculate transformation/rotation matrix
drone_rot_inv = np.linalg.inv(drone_rot)
rel_orient = drone_rot_inv.dot(human_rot)
rel_pos = drone_rot_inv.dot(rel_pos)
return rel_pos, rel_orient
def step(self, action_id):
self.client.enableApiControl(True)
self.client.armDisarm(True)
self.steps += 1
action = self.id2action(action_id)
if self.stepped_simulation:
self.client.simPause(False)
if self.drone:
# transform velocity according to the drone's orientation.... there is probably a way to do this more elegantly
yaw = airsim.to_eularian_angles(
self.client.simGetVehiclePose().orientation)[2]
v_x = (action[DroneActIdx.VX.value] * np.cos(yaw) -
action[DroneActIdx.VY.value] * np.sin(yaw))
v_y = (action[DroneActIdx.VX.value] * np.sin(yaw) +
action[DroneActIdx.VY.value] * np.cos(yaw))
self.client.moveByVelocityAsync(
v_x,
v_y,
action[DroneActIdx.VZ.value],
self.step_duration,
yaw_mode=airsim.YawMode(
is_rate=True,
yaw_or_rate=action[DroneActIdx.Yaw.value])).join()
# self.client.moveByAngleThrottleAsync(action[DroneActIdx.Pitch.value], action[DroneActIdx.Roll.value],
# action[DroneActIdx.Throttle.value], action[DroneActIdx.Yaw.value],
# self.step_duration).join()
else:
self.client.moveByVelocity(action[CarActIdx.VX.value],
action[CarActIdx.VY.value],
action[CarActIdx.VZ.value],
self.step_duration).join()
if self.stepped_simulation:
self.client.simContinueForTime(self.step_duration)
state = self._get_state()
step_reward = self._reward_function(state)
terminal = self._terminal_state(state)
self.acc_reward += step_reward
return state, step_reward, terminal
def Control_cloth(self):
pos_a = self.client.simGetVehiclePose().position.to_numpy_array()[0:2]
orientation = self.client.getCarState(
).kinematics_estimated.orientation
pitch, roll, yaw = airsim.to_eularian_angles(orientation)
cones_right_in, cones_left_in = self.get_cone(self.lidar_R)
cones_left_in = cones_left_in - pos_a
cones_right_in = cones_right_in - pos_a
angle = -yaw
if not self.is_drive:
cones_left_in_rot = np.array([
self.rotate_point(0, 0, angle, point)
for point in cones_left_in
])
cones_right_in_rot = np.array([
self.rotate_point(0, 0, angle, point)
for point in cones_right_in
])
c_l = cones_left_in_rot.copy()
c_r = cones_right_in_rot.copy()
ind_l = np.where(c_l[:, 0] > 0)
ind_r = np.where(c_r[:, 0] > 0)
c_l = c_l[ind_l]
c_r = c_r[ind_r]
c_ = np.concatenate([c_l, c_r], axis=0)
self.c_kB, self.c_kA = self.optimization_cloth(c_l, c_r)
print(f"Оптимальная клотоида найдена!")
# Плавные радиусы поворотов
ss = np.linspace(0, self.c_lenght, 30)
self.opt_radius = self.c_lenght / ((self.c_kB - self.c_kA) * ss)
self.is_drive = True
if self.i > 28:
self.is_drive = False
self.i = 0
r = self.opt_radius[int(self.i)]
self.i = self.i + 1
if self.c_kB > 0:
s = self.steering_func_p(r)
print("+")
else:
s = self.steering_func_n(r)
print("-")
# if r>80:
# s = 0
print(f"Оптимальный радиус поворота {r}")
print(f"Оптимальный угол поворота в AirSim {s}")
return s, self.c_kB, self.c_kA, self.c_lenght
def move(self, distance):
"""This function will move the platform a given distance in m.
:param distance in meters.
"""
yaw = (
airsim.to_eularian_angles(
self.client.getMultirotorState(vehicle_name=self.name).kinematics_estimated.orientation)[2])
position = self.get_position()
offset = list(self.calculate_offset(distance, yaw))
self.move_a_to_b(
airsim.Vector3r(position.x_val + offset[0],
position.y_val + offset[1],
position.z_val))
time.sleep(self.delay)
def _move_by_positions(client: airsim.MultirotorClient,
args: argparse.Namespace) -> None:
for position, orientation in args.viewpoints:
_pitch, _roll, yaw = airsim.to_eularian_angles(
airsim.Quaternionr(*orientation))
client.moveToPositionAsync(
*position,
args.flight_velocity,
drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=yaw),
).join()
client.hoverAsync().join()
time.sleep(2)
def yawrateVzController(self, cent, angle):
"""
- function: 通常使用的线速度加偏航角速度控制方法
- params:
- cent: 目标在图像上的坐标
- q: 飞机在世界坐标系的四元数
- return:
- cmd: [vx, vy, vz, yawrate]，世界坐标系下的线速度与偏航角速度
"""
ex, ey = cent[0] - self.width / 2, cent[1] - self.height / 2
pitch, roll, yaw = airsim.to_eularian_angles(q)
print(pitch, roll, yaw)
return self.velocity*np.cos(yaw), \
self.velocity*np.sin(yaw), \
self.kz*ey, \
self.kx*ex
def get_cone_arc(self):
R = 16
FOV = 180
pos_a = self.client.simGetVehiclePose().position.to_numpy_array()[0:2]
orientation = airsim.to_eularian_angles(
self.client.simGetVehiclePose().orientation)
yaw = orientation[2]
arc_pnts = self.get_arc_pnts(R, np.radians(FOV), yaw, pos_a[0],
pos_a[1])
cones_left_in_circle, cones_right_in_circle = self.get_cone(R)
cones_left_in_arc = self.find_in_contour(cones_left_in_circle,
arc_pnts, R, np.radians(FOV),
pos_a[0], pos_a[1], yaw)
cones_right_in_arc = self.find_in_contour(cones_right_in_circle,
arc_pnts, R, np.radians(FOV),
pos_a[0], pos_a[1], yaw)
return cones_left_in_arc, cones_right_in_arc
def build_camera_to_world(self, position, orientation):
# Camera coordinate frame (Z forward, X right, Y down)
# doesn't correspond to the world frame (X forward, Y right, Z down)
# Thus we should swap axes: XYZ -> YZX
M = np.eye(4, dtype=np.float32)
M[0:3, 3] = np.array(
[position['y_val'], position['z_val'], position['x_val']])
(pitch, roll, yaw) = airsim.to_eularian_angles(
namedtuple('Struct', orientation.keys())(*orientation.values()))
R = self.build_rot_mat(roll=roll, pitch=pitch, yaw=yaw)
# quat = np.array([orientation['x_val'], orientation['y_val'], orientation['z_val'], orientation['w_val']])
# R = self.build_rot_mat_from_quat(quat)
M[0:3, 0:3] = R
return M
def to3D(pixelX, pixelY, camInfo, depthImage, color=[], maxDistView = None):
"""From image pixels (2D) to relative(!) 3D coordinates"""
if type(depthImage) == str:
depth,s = airsim.read_pfm(depthImage)
else:
depthData = depthImage.image_data_float
depthArray = np.array(depthData)
depth = np.reshape(depthArray, (depthImage.height, depthImage.width))
if isinstance(maxDistView, float) or isinstance(maxDistView, int):
depth[depth>maxDistView] = maxDistView
height, width = depth.shape
# print(f"Image size: width:{width} -- height:{height}")
halfWidth = width/2
halfHeight= height/2
camPitch, camRoll,camYaw = airsim.to_eularian_angles(camInfo.pose.orientation)
# to rads (its on degrees now)
hFoV = np.radians(camInfo.fov)
vFoV = (height/width)*hFoV
pointsH = np.array(pixelY, dtype=int)
pointsW = np.array(pixelX, dtype=int)
pixelPitch = ((pointsH-halfHeight)/halfHeight) * (vFoV/2)
pixelYaw = ((pointsW-halfWidth)/halfWidth) * (hFoV/2)
theta = (np.pi/2) - pixelPitch
# turn
phi = pixelYaw
r = depth[ pointsH, pointsW]
idx = np.where(r<100)
x = r*np.sin(theta)*np.cos(phi)
y = r*np.sin(theta)*np.sin(phi)
z = r*np.cos(theta)
if len(color) != 0:
return x[idx],y[idx],z[idx],color[idx]
else:
return x[idx],y[idx],z[idx]
def record(self):
assert self.is_start_recording == True, "The recording didn't start"
time_stamp_s, time_stamp_ns = str(time.time()).split('.')
time_stamp = time_stamp_s + time_stamp_ns + "00"
lidarData = self.client.getLidarData()
if (len(lidarData.point_cloud) < 3):
print("\tNo points received from Lidar data")
else:
# print("\tReading %d: time_stamp: %d number_of_points: %d" % (i, lidarData.time_stamp, len(points)))
# print("\t\tlidar position: %s" % (pprint.pformat(lidarData.pose.position)))
# print("\t\tlidar orientation: %s" % (pprint.pformat(lidarData.pose.orientation)))
points = self.parse_lidarData(lidarData)
x, y, z = self.client.getLidarData().pose.position
pitch, yaw, roll = airsim.to_eularian_angles(
self.client.getImuData().orientation)
self.write_to_file_sync(
f"{x} {y} {z} {pitch} {yaw} {roll} {time_stamp}\n")
self.write_lidarData_to_disk(
points, os.path.join(self.path_data, str(time_stamp)))
# In settings.json first activate computer vision mode:
# https://github.com/Microsoft/AirSim/blob/master/docs/image_apis.md#computer-vision-mode
import setup_path
import airsim
client = airsim.VehicleClient()
client.confirmConnection()
pose = client.simGetVehiclePose()
print("x={}, y={}, z={}".format(pose.position.x_val, pose.position.y_val, pose.position.z_val))
angles = airsim.to_eularian_angles(client.simGetVehiclePose().orientation)
print("pitch={}, roll={}, yaw={}".format(angles[0], angles[1], angles[2]))
pose.position.x_val = pose.position.x_val + 1
client.simSetVehiclePose(pose, True)
