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"