def Drone_Center_Track(Dr_Obj, X_Ref):
global x, y, z, vx, vy, vz, wx, wy, wz, roll, pitch, yaw
# Pose Subscriber:
Pose_Topic = "/vicon/anafi_5/odom"
gn_mat = Dr_Obj.gn_mat
thr_vec = Dr_Obj.thr_vec
X_tol = Dr_Obj.X_tol
V_tol = Dr_Obj.V_tol
yw_tol = Dr_Obj.yw_tol
lr = Dr_Obj.loop_rate
looprate = rospy.Rate(lr)
while True:
X_Ref[8] = wrapTo2Pi(atan2(-y, -x))
pose_subscriber = rospy.Subscriber(Pose_Topic, Odometry, poseCallback)
X_St = [x, y, z, vx, vy, vz, roll, pitch, yaw]
Er = np.array(X_Ref) - np.array(X_St)
r_er = vec_mag(Er[0:3])
v_er = vec_mag(Er[3:6])
drone_center(drone, X_St, X_Ref, gn_mat, thr_vec, X_tol, yw_tol)
if r_er <= X_tol and v_er <= V_tol:
print(colored(("Drone Reached!"), 'green'))
break
looprate.sleep()
def gimbal_track_moon(Dr_Obj, X_Ref, X_Tar):
global x, y, z, vx, vy, vz, wx, wy, wz, roll, pitch, yaw
# Pose Subscriber:
Pose_Topic = "/vicon/anafi_2/odom"
lr = Dr_Obj.loop_rate
X_tol = Dr_Obj.X_tol
V_tol = Dr_Obj.V_tol
looprate = rospy.Rate(lr)
print(colored(("Gimbal Action Started !!"), 'cyan'))
while True:
pose_subscriber = rospy.Subscriber(Pose_Topic, Odometry, poseCallback)
X_St = [x, y, z, vx, vy, vz, roll, pitch, yaw]
Er = np.array(X_Ref) - np.array(X_St)
DT = np.array(X_Tar) - np.array(X_St[0:3])
r_tar = vec_mag(DT)
r_er = vec_mag(Er[0:3])
v_er = vec_mag(Er[3:6])
th0 = asin(DT[2] / r_tar) * 180 / pi
# print(colored((th0), "red"))
gimbal_target(drone, th0)
if r_er <= X_tol and v_er <= V_tol:
print(colored(("Gimbal Action Completed!"), 'green'))
break
looprate.sleep()
def Drone_Circle(Dr_Obj, X_Ref):
global x, y, z, vx, vy, vz, wx, wy, wz, roll, pitch, yaw
# Pose Subscriber:
Pose_Topic = "/vicon/anafi_2/odom"
gn_mat = Dr_Obj.gn_mat
thr_vec = Dr_Obj.thr_vec
X_tol = Dr_Obj.X_tol
V_tol = Dr_Obj.V_tol
yw_tol = Dr_Obj.yw_tol
lr = Dr_Obj.loop_rate
looprate = rospy.Rate(lr)
dt = 1 / lr
r_amp = 1.1
Tp = 30.0
fr = 2 * pi / Tp
print(colored(("Going to Start !!"), 'cyan'))
while True:
pose_subscriber = rospy.Subscriber(Pose_Topic, Odometry, poseCallback)
X_St = [x, y, z, vx, vy, vz, roll, pitch, yaw]
Er = np.array(X_Ref) - np.array(X_St)
r_er = vec_mag(Er[0:3])
v_er = vec_mag(Er[3:6])
drone_center(drone, X_St, X_Ref, gn_mat, thr_vec, X_tol, yw_tol)
if r_er <= X_tol and v_er <= V_tol:
print(colored(("Drone Reached!"), 'green'))
break
looprate.sleep()
lr2 = 50
looprate = rospy.Rate(lr2)
dt = 1 / lr2
print(colored(("Starting Circle !!"), 'cyan'))
for t in np.arange(0, 2 * Tp, dt):
pose_subscriber = rospy.Subscriber(Pose_Topic, Odometry, poseCallback)
X_St = [x, y, z, vx, vy, vz, roll, pitch, yaw]
X_Ref[0] = r_amp * cos(fr * t)
X_Ref[1] = r_amp * sin(fr * t)
X_Ref[3] = -r_amp * fr * sin(fr * t)
X_Ref[4] = r_amp * fr * cos(fr * t)
X_Ref[8] = wrapTo2Pi(atan2(-y, -x))
drone_center(drone, X_St, X_Ref, gn_mat, thr_vec, X_tol, yw_tol)
looprate.sleep()
print(colored(("Circle commands sent !!"), 'green'))
def Drone_Hover(Dr_Obj, X_Ref):
global x, y, z, vx, vy, vz, wx, wy, wz, roll, pitch, yaw
# Pose Subscriber:
Pose_Topic = "/vicon/anafi_5/odom"
gn_mat = Dr_Obj.gn_mat
thr_vec = Dr_Obj.thr_vec
V_tol = Dr_Obj.V_tol
X_tol = Dr_Obj.X_tol
yw_tol = Dr_Obj.yw_tol
lr = Dr_Obj.loop_rate
looprate = rospy.Rate(lr)
while True:
pose_subscriber = rospy.Subscriber(Pose_Topic, Odometry, poseCallback)
X_St = [x, y, z, vx, vy, vz, roll, pitch, yaw]
Er = np.array(X_Ref) - np.array(X_St)
v_er = vec_mag(Er[3:6])
X_Ref[0:3] = X_St[0:3]
X_Ref[8] = X_St[8]
drone_center(drone, X_St, X_Ref, gn_mat, thr_vec, X_tol, yw_tol)
if v_er <= V_tol:
print(colored(("Drone Hovering!"), 'green'))
break
looprate.sleep()
def drone_center(drone, X_St, X_Ref, gn_mat, thr_vec, X_tol, yw_tol):
# Drone State
x_dr = X_St[0]
y_dr = X_St[1]
z_dr = X_St[2]
vx_dr = X_St[3]
vy_dr = X_St[4]
vz_dr = X_St[5]
yaw_dr = X_St[8]
# Reference
x_R = X_Ref[0]
y_R = X_Ref[1]
z_R = X_Ref[2]
yaw_R = X_Ref[8]
Er = np.array(X_Ref) - np.array(X_St)
r_er = vec_mag(Er[0:3])
Y_ER = compute_yaw_Error(yaw_R, yaw_dr)
#
UR, UP, UTh, UYr = PCMD_Controller(X_St, X_Ref, gn_mat, thr_vec)
# drone(PCMD(1, UR, UP, UYr, UTh , 1))
drone.start_piloting()
drone.piloting_pcmd(UR, UP, UYr, UTh, 0)
def Drone_Move_Orient(Dr_Obj, X_Ref):
global x, y, z, vx, vy, vz, wx, wy, wz, roll, pitch, yaw, wx, wy, wz
X_P = [x, y, z, vx, vy, vz, roll, pitch, yaw, wx, wy, wz]
n = 0
# Pose Subscriber:
Pose_Topic = "/vicon/anafi_2/odom"
gn_mat = Dr_Obj.gn_mat
thr_vec = Dr_Obj.thr_vec
X_tol = Dr_Obj.X_tol
V_tol = Dr_Obj.V_tol
yw_tol = Dr_Obj.yw_tol
lr = Dr_Obj.loop_rate
looprate = rospy.Rate(lr)
dt = 1.0/lr
eps_r = 0.5
while True:
rospy.Subscriber(Pose_Topic, Odometry, poseCallback)
if n==0:
X_St = [x, y, z, vx, vy, vz, roll, pitch, yaw]
elif n>0:
X_ST = [x, y, z, vx, vy, vz, roll, pitch, yaw, wx, wy, wz]
X_ST = drone_filter(X_ST, X_P, eps_r, dt)
X_St = X_ST[0:9]
Er = np.array(X_Ref) - np.array(X_St)
r_er = vec_mag(Er[0:3])
v_er = vec_mag(Er[3:6])
drone_center(drone, X_St, X_Ref, gn_mat, thr_vec, X_tol, yw_tol)
n=1
X_P = [x, y, z, vx, vy, vz, roll, pitch, yaw, wx, wy, wz]
if r_er<=X_tol and v_er<=V_tol:
print( colored( ("Drone Reached!"), 'green') )
break
looprate.sleep()
def Drone_Map_Opn(Dr_Obj, X_Ref, X_Tar, ran_V):
thr_vec = Dr_Obj.thr_vec
X_tol = Dr_Obj.X_tol
yw_tol = Dr_Obj.yw_tol
Drone_Move_Orient(Dr_Obj, X_Ref)
print(colored(("Moved home, executing map command!"), "green"))
time.sleep(0.5)
X_Ref2 = X_Ref
y0 = X_Ref[1]
global x, y, z, vx, vy, vz, wx, wy, wz, roll, pitch, yaw
vyT = -0.15
Tp = 30
lr = 30
looprate = rospy.Rate(lr)
dt = 1.0 / lr
gn_mat = [5, 15, 4, 4, 12, 1.8] # Controller Gains
X_tol = 0.1
for t in np.arange(0, Tp + dt, dt):
pose_subscriber = rospy.Subscriber(Pose_Topic, Odometry, poseCallback)
X_St = [x, y, z, vx, vy, vz, roll, pitch, yaw]
X_Ref[1] = y0 + vyT * t
X_Ref[4] = vyT
X_Ref[8] = wrapTo2Pi(atan2(-y, -x))
DT = np.array(X_Tar) - np.array(X_St[0:3])
r_tar = vec_mag(DT)
th0 = asin(DT[2] / r_tar) * 180 / pi
# print(colored( ("Commanded Pose: ", X_Ref[0], X_Ref[1], X_Ref[2],X_Ref[8]*(180/pi) ), "blue"))
gimbal_target(drone, th0)
drone_line(drone, X_St, X_Ref, gn_mat, thr_vec, X_tol)
looprate.sleep()
print(colored(("Line command Finished !!"), "blue"))
drone.stop_piloting()
time.sleep(0.5)
X_Ref2[1] = -2.0
Drone_Move_Orient(Dr_Obj, X_Ref2)
print(
colored(("Mapping done !! Holding drone at Final Desitination!"),
"green"))
def record_and_fetch(Dr_Obj, X_Tar, ran_V, rec_Vid):
Pose_Topic = "/vicon/anafi_2/odom"
pose_subscriber = rospy.Subscriber(Pose_Topic, Odometry, poseCallback)
global x, y, z, vx, vy, vz, wx, wy, wz, roll, pitch, yaw
rec_Vid = 0
ANAFI_MEDIA_API_URL = Dr_Obj.ANAFI_MEDIA_API_URL
ANAFI_URL = Dr_Obj.ANAFI_URL
X_St = [x, y, z, vx, vy, vz, roll, pitch, yaw]
DT = np.array(X_Tar) - np.array(X_St[0:3])
r_tar = vec_mag(DT)
if r_tar <= 1.001 * ran_V and rec_Vid == 0:
print(colored(("Recording Started"), "blue"))
drone(start_recording(cam_id=0))
elif r_tar <= 1.001 * ran_V and rec_Vid == 1:
print(colored(("Recording In Progress"), "blue"))
elif r_tar > .001 * ran_V and rec_Vid == 2:
print(colored(("Recording Completed"), "blue"))
drone(stop_recording(cam_id=0))
photo_saved = drone(
recording_progress(result="stopped", _policy="wait"))
photo_saved.wait()
media_id = photo_saved.received_events().last().args["media_id"]
print(media_id)
os.chdir("/home/spacetrex/code/Results")
media_info_response = requests.get(ANAFI_MEDIA_API_URL + media_id)
media_info_response.raise_for_status()
download_dir = tempfile.mkdtemp()
Res_dir = filecreation()
#tempfile.gettempdir()
for resource in media_info_response.json()["resources"]:
image_response = requests.get(ANAFI_URL + resource["url"],
stream=True)
download_path = os.path.join(download_dir, resource["resource_id"])
print(colored(("File Transfer in progress"), "blue"))
image_response.raise_for_status()
with open(download_path, "wb") as image_file:
shutil.copyfileobj(image_response.raw, image_file)
shutil.copy2(download_path, Res_dir)
print(colored(("File Transfer Completed !!!!!!!!!!"), "green"))
rec_Vid = 2
def drone_line(drone, X_St, X_Ref, gn_mat, thr_vec, X_tol):
# Drone State
x_dr = X_St[0]
y_dr = X_St[1]
z_dr = X_St[2]
yaw_dr = X_St[8]
# Reference
x_R = X_Ref[0]
y_R = X_Ref[1]
z_R = X_Ref[2]
yaw_R = X_Ref[8]
Er = np.array(X_Ref) - np.array(X_St)
r_er = vec_mag(Er[0:3])
print(colored(("Position error: ", r_er), "red"))
# UR, UP, UTh, UYr = PCMD_Controller(X_St, X_Ref, gn_mat, thr_vec)
# drone(PCMD(1, UR, UP, UYr, UTh , 0))
# drone.start_piloting()
# drone.piloting_pcmd(UR, UP, UYr, UTh , 0)
if r_er >= X_tol:
UR, UP, UTh, UYr = PCMD_Controller(X_St, X_Ref, gn_mat, thr_vec)
# drone(PCMD(1, UR, UP, UYr, UTh , 0))
drone.start_piloting()
drone.piloting_pcmd(UR, UP, UYr, UTh, 0)
print(
colored(("Drone position: ", x_dr, y_dr, z_dr, yaw_dr * 180 / pi),
"yellow"))
print(
colored(("Target position: ", x_R, y_R, z_R, yaw_R * 180 / pi),
"green"))
print(colored(("Controller sent commands: ", UR, UP, UYr, UTh),
"cyan"))
def drone_filter(X_St, X_Pr, eps_r, dt):
R_P = np.array(X_Pr[0:3])
V_P = np.array(X_Pr[3:6])
Yaw_P = np.array(X_Pr[8])
w_P = np.array(X_Pr[11])
R_St = np.array(X_St[0:3])
Er = R_P - R_St
r_er = vec_mag(Er[0:3])
if r_er >= eps_r:
print(colored(("State Rejected: ", r_er), 'green'))
print(colored(("Threshold: ", eps_r), 'green'))
R_St = R_P + V_P * dt
Yaw = wrapTo2Pi(Yaw_P + w_P * dt)
X_St = X_Pr
X_St[0], X_St[1], X_St[2], X_St[8], = R_St[0], R_St[1], R_St[2], Yaw
return X_St
def Drone_Map_Opn2(Dr_Obj, X_Ref, X_Tar, ran_V, Tp, vyT, X_End):
Mr_IP = '192.168.8.240' # Master IP
drone(stop_recording(cam_id=0))
thr_vec = Dr_Obj.thr_vec
X_tol = Dr_Obj.X_tol
yw_tol = Dr_Obj.yw_tol
ANAFI_MEDIA_API_URL = Dr_Obj.ANAFI_MEDIA_API_URL
ANAFI_URL = Dr_Obj.ANAFI_URL
Drone_Move_Orient(Dr_Obj, X_Ref)
print(colored(("Moved home, executing map command!"), "green"))
time.sleep(0.5)
X_Ref2 = X_Ref
y0 = X_Ref[1]
global x, y, z, vx, vy, vz, wx, wy, wz, roll, pitch, yaw
Pose_Topic = "/vicon/anafi_5/odom"
lr = 20
print(colored(("Loop Rate: ", lr, " Hz"), "magenta"))
print(colored(("Travel Time: ", Tp, " s"), "magenta"))
print(colored(("Velocity: ", vyT, " m/s"), "magenta"))
looprate = rospy.Rate(lr)
dt = 1.0 / lr
X_tol = 0.1
gn_mat = [5.5, 18, 4.3, 2, 12, 1.3] # Controller Gains
rec_Vid = 0
photo_saved = drone(recording_progress(result="stopped", _policy="wait"))
for t in np.arange(0, Tp + dt, dt):
pose_subscriber = rospy.Subscriber(Pose_Topic, Odometry, poseCallback)
X_St = [x, y, z, vx, vy, vz, roll, pitch, yaw]
X_Ref[1] = y0 + vyT * t
X_Ref[4] = vyT
X_Ref[8] = wrapTo2Pi(atan2(-y, -x))
DT = np.array(X_Tar) - np.array(X_St[0:3])
r_tar = vec_mag(DT)
th0 = asin(DT[2] / r_tar) * 180 / pi
gimbal_target(drone, th0)
drone_center(drone, X_St, X_Ref, gn_mat, thr_vec, X_tol, yw_tol)
print(colored(("Distance to target: ", r_tar), "red"))
print(colored(("Record Status: ", rec_Vid), "red"))
print(colored(("Distance threshhold: ", 1.01 * ran_V), "blue"))
# print(colored(("Gimbal angle computed: ", th0),"blue"))
if r_tar <= 1.01 * ran_V and rec_Vid == 0:
rec_Vid = 1
drone(start_recording(cam_id=0))
print(colored(("Recording Started"), "blue"))
elif r_tar > 1.01 * ran_V and rec_Vid == 1:
rec_Vid = 2
print(colored(("Recording Completed"), "blue"))
drone(stop_recording(cam_id=0))
photo_saved.wait()
looprate.sleep()
print(colored(("Current Time: ", t, "sec"), "cyan"))
print(colored(("Targetted End Y: ", X_Ref[1], "m/s"), "cyan"))
print(colored(("Achieved End Y: ", X_St[1], "m/s"), "cyan"))
drone_center(drone, X_St, X_End, gn_mat, thr_vec, X_tol, yw_tol)
drone(Landing()).wait()
if rec_Vid == 2:
print(colored(("Starting transfer to drone computer!"), "green"))
media_id = photo_saved.received_events().last().args["media_id"]
print(colored((media_id), "green"))
os.chdir("/home/spacetrex/code/Results")
media_info_response = requests.get(ANAFI_MEDIA_API_URL + media_id)
media_info_response.raise_for_status()
download_dir = tempfile.mkdtemp()
Res_dir = filecreation()
#tempfile.gettempdir()
for resource in media_info_response.json()["resources"]:
image_response = requests.get(ANAFI_URL + resource["url"],
stream=True)
download_path = os.path.join(download_dir, resource["resource_id"])
print(
colored(("File transfer to drone computer in progress"),
"green"))
image_response.raise_for_status()
with open(download_path, "wb") as image_file:
shutil.copyfileobj(image_response.raw, image_file)
shutil.copy2(download_path, Res_dir)
print(colored(("File transfer to drone computer !!"), "green"))
ssh = paramiko.SSHClient()
ssh.set_missing_host_key_policy(paramiko.AutoAddPolicy())
ssh.connect(hostname=Mr_IP,
username='******',
password='******',
port=22)
sftp_client = ssh.open_sftp()
loc_path = download_path
rem_path = '/home/spacetrex/Results/ANAFI_5.MP4'
print(colored(("Starting transfer to Master Computer"), "cyan"))
sftp_client.put(loc_path, rem_path)
sftp_client.close()
ssh.close()
print(
colored(("File transfer to Master computer completed !!"), "cyan"))
else:
print(colored(("No Recording obtained!!"), "red"))
print(
colored(("Mapping done!! Holding drone at Final Desitination!"),
"green"))
def Drone_Map_Opn2(Dr_Obj, X_Ref, X_Tar, ran_V):
drone(stop_recording(cam_id=0))
thr_vec = Dr_Obj.thr_vec
X_tol = Dr_Obj.X_tol
yw_tol = Dr_Obj.yw_tol
ANAFI_MEDIA_API_URL = Dr_Obj.ANAFI_MEDIA_API_URL
ANAFI_URL = Dr_Obj.ANAFI_URL
Drone_Move_Orient(Dr_Obj, X_Ref)
print(colored(("Moved home, executing map command!"), "green"))
time.sleep(0.5)
X_Ref2 = X_Ref
y0 = X_Ref[1]
global x, y, z, vx, vy, vz, wx, wy, wz, roll, pitch, yaw
# Pose_Topic = "/vicon/anafi_2/odom"
vyT = -0.15
Tp = 30
lr = 35
looprate = rospy.Rate(lr)
dt = 1.0 / lr
X_tol = 0.1
gn_mat = [5, 15, 4, 4, 12, 1.8] # Controller Gains
rec_Vid = 0
photo_saved = drone(recording_progress(result="stopped", _policy="wait"))
for t in np.arange(0, Tp + dt, dt):
pose_subscriber = rospy.Subscriber(Pose_Topic, Odometry, poseCallback)
X_St = [x, y, z, vx, vy, vz, roll, pitch, yaw]
X_Ref[1] = y0 + vyT * t
X_Ref[4] = vyT
X_Ref[8] = wrapTo2Pi(atan2(-y, -x))
Yaw_Ref_D = X_Ref[8] * 180 / pi
DT = np.array(X_Tar) - np.array(X_St[0:3])
r_tar = vec_mag(DT)
Er = np.array(X_Ref[0:3]) - np.array(X_St[0:3])
er = vec_mag(Er)
th0 = asin(DT[2] / r_tar) * 180 / pi
Yaw_D = yaw * 180 / pi
yw_er = abs(Yaw_Ref_D - Yaw_D)
gimbal_target(drone, th0)
drone_center(drone, X_St, X_Ref, gn_mat, thr_vec, X_tol, yw_tol)
print(" ")
print(colored(("Reference tracking error: ", er), "yellow"))
print(colored(("Yaw tracking Error: ", yw_er), "yellow"))
print(colored(("Z desired: ", X_Ref[2]), "green"))
print(colored(("Z achieved: ", z), "green"))
print(colored(("Distance to target: ", r_tar), "red"))
print(colored(("Record Status: ", rec_Vid), "red"))
print(colored(("Distance threshhold: ", 1.01 * ran_V), "blue"))
print(colored(("Gimbal angle computed: ", th0), "blue"))
if r_tar <= 1.01 * ran_V and rec_Vid == 0:
rec_Vid = 1
drone(start_recording(cam_id=0))
print(colored(("Recording Started"), "blue"))
elif r_tar > 1.01 * ran_V and rec_Vid == 1:
rec_Vid = 2
print(colored(("Recording Completed"), "blue"))
drone(stop_recording(cam_id=0))
photo_saved.wait()
looprate.sleep()
time.sleep(0.5)
# X_Ref2[1] = -2.0
# Drone_Move_Orient(Dr_Obj, X_Ref2)
drone(Landing()).wait()
print(colored(("Starting Transfer"), "green"))
# photo_saved = drone(recording_progress(result="stopped", _policy="wait"))
# photo_saved.wait()
media_id = photo_saved.received_events().last().args["media_id"]
print(colored((media_id), "green"))
os.chdir("/home/spacetrex/code/Results")
media_info_response = requests.get(ANAFI_MEDIA_API_URL + media_id)
media_info_response.raise_for_status()
download_dir = tempfile.mkdtemp()
Res_dir = filecreation()
#tempfile.gettempdir()
for resource in media_info_response.json()["resources"]:
image_response = requests.get(ANAFI_URL + resource["url"], stream=True)
download_path = os.path.join(download_dir, resource["resource_id"])
print(colored(("File Transfer in progress"), "blue"))
image_response.raise_for_status()
with open(download_path, "wb") as image_file:
shutil.copyfileobj(image_response.raw, image_file)
shutil.copy2(download_path, Res_dir)
print(colored(("File Transfer Completed !!!!!!!!!!"), "green"))
print(
colored(("Mapping done !! Holding drone at Final Desitination!"),
"green"))
Pose_Topic = "/vicon/anafi_5/odom"
=======
Pose_Topic = "/vicon/anafi_2/odom"
>>>>>>> 1d491c6b6b0ad141dc94e80d05adbb491ba5e36e
gn_mat = Dr_Obj.gn_mat
thr_vec = Dr_Obj.thr_vec
X_tol = Dr_Obj.X_tol
V_tol = Dr_Obj.V_tol
lr = Dr_Obj.loop_rate
looprate = rospy.Rate(lr)
while True:
pose_subscriber = rospy.Subscriber(Pose_Topic, Odometry, poseCallback)
X_St = [x, y, z, vx, vy, vz, roll, pitch, yaw]
Er = np.array(X_Ref) - np.array(X_St)
r_er = vec_mag(Er[0:3])
v_er = vec_mag(Er[3:6])
drone_line(drone, X_St, X_Ref, gn_mat, thr_vec, X_tol)
if r_er<=X_tol and v_er<=V_tol:
print( colored( ("Drone Reached!"), 'green') )
break
looprate.sleep()
def Drone_Move_Orient(Dr_Obj, X_Ref):
global x, y, z, vx, vy, vz, wx, wy, wz, roll, pitch, yaw
# Pose Subscriber:
<<<<<<< HEAD
Pose_Topic = "/vicon/anafi_5/odom"
