diff --git a/src/lqsimple.py b/src/lqsimple.py index 4f651ac9c178320e1c3253b080c437acf8f9c2e3..bd1d13ad3017b75a553140541cca4f556150804b 100755 --- a/src/lqsimple.py +++ b/src/lqsimple.py @@ -39,18 +39,18 @@ min_up = -5.0 def fly_straight_line (east_m, north_m, relalt, speed, yaw_flag): - print "fly_straight_line: east_m:{0} north_m:{1} alt_m:{2} spd:{3}".format (east_m, north_m, relalt, speed) + print("fly_straight_line: east_m:{0} north_m:{1} alt_m:{2} spd:{3}".format (east_m, north_m, relalt, speed)) ns = rospy.get_namespace () qc = QCon(ns, 10000) if options.limits: - if relalt > max_altitude: print "Target too high... Exiting. "; sys.exit (1), - if relalt < min_altitude: print "Target too low... Exiting. "; sys.exit (1) + if relalt > max_altitude: print("Target too high... Exiting. "); sys.exit (1), + if relalt < min_altitude: print("Target too low... Exiting. "); sys.exit (1) resp = qc.queues_clear_token (1, 1, yaw_flag, 0); - if resp.status != 0: print "Failed to clear the queues"; sys.exit (1) + if resp.status != 0: print("Failed to clear the queues"); sys.exit (1) resp = qc.request_flyq (); if resp.status != 0: sys.exit (1) @@ -102,45 +102,45 @@ def take_off (alt, speed): qc = QCon(ns, 10000) resp = qc.queues_clear_token (1, 1, 1, 0); - if resp.status != 0: print "Failed to clear the queues"; sys.exit (1) + if resp.status != 0: print("Failed to clear the queues"); sys.exit (1) resp = qc.request_flyq (); - print resp + print(resp) if resp.status != 0: sys.exit (1) flyqtoken = resp.token resp = qc.request_climbq (); - print resp + print(resp) if resp.status != 0: sys.exit (1) climbqtoken = resp.token resp = qc.flyq_reset (flyqtoken) - print resp + print(resp) if resp.status != 0: sys.exit (1) resp = qc.climbq_reset (climbqtoken) - print resp + print(resp) if resp.status != 0: sys.exit (1) resp = qc.flyq_add_take_off (flyqtoken, alt, speed) - print resp + print(resp) if resp.status != 0: sys.exit (1) resp = qc.climbq_add_use_fly (climbqtoken) - print resp + print(resp) if resp.status != 0: sys.exit (1) resp = qc.flyq_add_barrier (flyqtoken, 1, 1, 1, 0, 0) - print resp + print(resp) if resp.status != 0: sys.exit (1) resp = qc.climbq_add_barrier (climbqtoken, 1, 1, 1, 0, 0) - print resp + print(resp) if resp.status != 0: sys.exit (1) resp = qc.queues_run (flyqtoken, climbqtoken, 0, 0); - print resp + print(resp) if resp.status != 0: sys.exit (1) qc.wait_for_flyq () @@ -150,47 +150,47 @@ def land (alt, speed): qc = QCon(ns, 10000) resp = qc.queues_clear_token (1, 1, 1, 0); - if resp.status != 0: print "Failed to clear the queues"; sys.exit (1) + if resp.status != 0: print("Failed to clear the queues"); sys.exit (1) resp = qc.request_flyq (); - print resp + print(resp) if resp.status != 0: sys.exit (1) flyqtoken = resp.token resp = qc.request_climbq (); - print resp + print(resp) if resp.status != 0: sys.exit (1) climbqtoken = resp.token resp = qc.flyq_reset (flyqtoken) - print resp + print(resp) if resp.status != 0: sys.exit (1) resp = qc.climbq_reset (climbqtoken) - print resp + print(resp) if resp.status != 0: sys.exit (1) resp = qc.flyq_add_land (flyqtoken, alt, speed) - print resp + print(resp) if resp.status != 0: sys.exit (1) resp = qc.climbq_add_use_fly (climbqtoken) - print resp + print(resp) if resp.status != 0: sys.exit (1) resp = qc.flyq_add_barrier (flyqtoken, 1, 1, 1, 0, 0) - print resp + print(resp) if resp.status != 0: sys.exit (1) resp = qc.climbq_add_barrier (climbqtoken, 1, 1, 1, 0, 0) - print resp + print(resp) if resp.status != 0: sys.exit (1) resp = qc.queues_run (flyqtoken, climbqtoken, 0, 0); - print resp + print(resp) if resp.status != 0: sys.exit (1) qc.wait_for_flyq () @@ -201,10 +201,10 @@ def fly_in_dir_ref (): ns = rospy.get_namespace () qc = QCon(ns, 10000) - print "fly_in_dir_ref: " + print("fly_in_dir_ref: ") resp = qc.queues_clear_token (1, 1, 1, 0); - if resp.status != 0: print "Failed to clear the queues"; sys.exit (1) + if resp.status != 0: print("Failed to clear the queues"); sys.exit (1) resp = qc.request_flyq (); if resp.status != 0: sys.exit (1) @@ -240,10 +240,10 @@ def fly_external (): ns = rospy.get_namespace () qc = QCon(ns, 10000) - print "fly_external: " + print("fly_external: ") resp = qc.queues_clear_token (1, 1, 1, 0); - if resp.status != 0: print "Failed to clear the queues: {}".format(resp.status); sys.exit (1) + if resp.status != 0: print("Failed to clear the queues: {}".format(resp.status)); sys.exit (1) resp = qc.request_flyq (); if resp.status != 0: sys.exit (1) @@ -288,11 +288,11 @@ def ptu_external (): ns = rospy.get_namespace () qc = QCon(ns, 10000) - print "ptu_external: {0}".format (ns) + print("ptu_external: {0}".format (ns)) resp = qc.queues_clear_token (0, 0, 0, 1); - if resp.status != 0: print "Failed to clear the ptu queue: {}".format(resp.status); sys.exit (1) + if resp.status != 0: print("Failed to clear the ptu queue: {}".format(resp.status)); sys.exit (1) resp = qc.request_ptuq (); if resp.status != 0: sys.exit (1) @@ -313,9 +313,9 @@ def yaw (heading): ns = rospy.get_namespace () qc = QCon(ns, 10000) - print "yaw: {0}".format (heading) + print("yaw: {0}".format (heading)) resp = qc.queues_clear_token (0, 0, 1, 0) - if resp.status != 0: print "Failed to clear the yaw queue"; sys.exit (1) + if resp.status != 0: print("Failed to clear the yaw queue"); sys.exit (1) resp = qc.request_yawq () if resp.status != 0: sys.exit (1) @@ -342,15 +342,15 @@ def yawrel (heading): ns = rospy.get_namespace () qc = QCon(ns, 10000) - print "yawrel: heading: {0}".format (heading) + print("yawrel: heading: {0}".format (heading)) resp = qc.get_current_state () - print resp + print(resp) state_yaw = resp.yaw heading = heading + state_yaw resp = qc.queues_clear_token (0, 0, 1, 0) - if resp.status != 0: print "Failed to clear the yaw queue"; sys.exit (1) + if resp.status != 0: print("Failed to clear the yaw queue"); sys.exit (1) resp = qc.request_yawq () if resp.status != 0: sys.exit (1) @@ -380,9 +380,9 @@ def climb (alt): ns = rospy.get_namespace () qc = QCon(ns, 10000) - print "climb: {0} {1}".format (ns, alt) + print("climb: {0} {1}".format (ns, alt)) resp = qc.queues_clear_token (0, 1, 0, 0) - if resp.status != 0: print "Failed to clear the climb queue"; sys.exit (1) + if resp.status != 0: print("Failed to clear the climb queue"); sys.exit (1) resp = qc.request_climbq () if resp.status != 0: sys.exit (1) @@ -404,21 +404,21 @@ def climbrel (alt, speed): ns = rospy.get_namespace () qc = QCon(ns, 10000) - print "climb: {0} {1}".format (ns, alt) + print("climb: {0} {1}".format (ns, alt)) resp = qc.queues_clear_token (0, 1, 0, 0); - if resp.status != 0: print "Failed to clear the climb queue"; sys.exit (1) + if resp.status != 0: print("Failed to clear the climb queue"); sys.exit (1) resp = qc.get_current_state () - print resp + print(resp) state_north_m = resp.lat_m state_east_m = resp.lon_m state_alt = resp.alt alt_m = state_alt+alt - if alt_m > 140.5: print "Target too high... Exiting. "; sys.exit (1), - if alt_m < 0.5: print "Target too low... Exiting. "; sys.exit (1) + if alt_m > 140.5: print("Target too high... Exiting. "); sys.exit (1), + if alt_m < 0.5: print("Target too low... Exiting. "); sys.exit (1) fly_straight_line (state_east_m, state_north_m, alt_m, speed, 0) @@ -426,25 +426,25 @@ def fly_xy_rel (forward, right, speed): ns = rospy.get_namespace () qc = QCon(ns, 10000) - print "Flyto xy rel: unit:{0} forwrd:{1:.3f} right:{2:.3f} speed:{3:.3f}".format(ns, forward, right, speed) + print("Flyto xy rel: unit:{0} forwrd:{1:.3f} right:{2:.3f} speed:{3:.3f}".format(ns, forward, right, speed)) resp = qc.queues_clear_token (1, 0, 0, 0); - if resp.status != 0: print "Failed to clear the climb queue"; sys.exit (1) + if resp.status != 0: print("Failed to clear the climb queue"); sys.exit (1) resp = qc.get_current_state () - print resp + print(resp) state_north_m = resp.lat_m state_east_m = resp.lon_m state_yaw_rad = resp.yaw/180.0 * math.pi state_alt = resp.alt - print "-----> Current state: north: ", state_north_m, "east: ", state_east_m, "yaw: ", state_yaw_rad/math.pi*180.0 + print("-----> Current state: north: ", state_north_m, "east: ", state_east_m, "yaw: ", state_yaw_rad/math.pi*180.0) north_m = forward * math.cos(state_yaw_rad) - right * math.sin(state_yaw_rad) east_m = forward * math.sin(state_yaw_rad) + right * math.cos(state_yaw_rad) - print "-----> Relative current pos: north: {0:.3f} east: {1:.3f}".format(north_m,east_m) + print("-----> Relative current pos: north: {0:.3f} east: {1:.3f}".format(north_m,east_m)) fly_straight_line (state_east_m + east_m, state_north_m + north_m, state_alt, speed, 0) @@ -453,15 +453,15 @@ def fly_rel (forward, right, up, speed, yaw_flag): ns = rospy.get_namespace () qc = QCon(ns, 10000) - print "Flyto relative: unit: {0} forward: {1:.3f} right: {2:.3f} up: {3:.3f} speed: {4:.3f} yaw to waypoint: {5}".format(ns, forward, right, up, speed,yaw_flag) - if forward==0.0 and right==0 and up==0: print "All parameters are 0.0. Exiting...", sys.exit(1) + print("Flyto relative: unit: {0} forward: {1:.3f} right: {2:.3f} up: {3:.3f} speed: {4:.3f} yaw to waypoint: {5}".format(ns, forward, right, up, speed,yaw_flag)) + if forward==0.0 and right==0 and up==0: print("All parameters are 0.0. Exiting..."); sys.exit(1) resp = qc.queues_clear_token (1, 1, yaw_flag, 0); - if resp.status != 0: print "Failed to clear the queues"; sys.exit (1) + if resp.status != 0: print("Failed to clear the queues"); sys.exit (1) # old way of doing this - current state instead of targets # resp = qc.get_current_state () -# print resp +# print(resp) # state_north_m = resp.lat_m # state_east_m = resp.lon_m @@ -469,7 +469,7 @@ def fly_rel (forward, right, up, speed, yaw_flag): # state_alt = resp.alt resp = qc.get_current_targets () - print resp + print(resp) #float64 east_m #float64 north_m @@ -488,12 +488,12 @@ def fly_rel (forward, right, up, speed, yaw_flag): state_yaw_rad = resp.heading/180.0 * math.pi state_alt = resp.altitude - print "-----> Current targets: north: {0}, east: {1}, alt: {2}, yaw: {3}".format (state_north_m, state_east_m, state_alt, state_yaw_rad/math.pi*180.0) + print("-----> Current targets: north: {0}, east: {1}, alt: {2}, yaw: {3}".format (state_north_m, state_east_m, state_alt, state_yaw_rad/math.pi*180.0)) north_m = forward * math.cos(state_yaw_rad) - right * math.sin(state_yaw_rad) east_m = forward * math.sin(state_yaw_rad) + right * math.cos(state_yaw_rad) - print "-----> Relative current pos: north: {0:.3f} east: {1:.3f} up: {2}".format(north_m,east_m, up) + print("-----> Relative current pos: north: {0:.3f} east: {1:.3f} up: {2}".format(north_m,east_m, up)) fly_straight_line (state_east_m + east_m, state_north_m + north_m, state_alt + up, speed, yaw_flag) @@ -503,7 +503,7 @@ def fly_triangle (speed): qc = QCon(ns, 10000) resp = qc.get_current_state () - print resp + print(resp) state_north_m = resp.lat_m state_east_m = resp.lon_m @@ -511,7 +511,7 @@ def fly_triangle (speed): tokens = qc.fly_triangle_yaw_climb_pause (state_east_m, state_north_m, state_alt, speed) - print tokens + print(tokens) if options.confirm: raw_input("---> READY. Press Enter to execute...") @@ -570,7 +570,7 @@ if __name__ == "__main__": sim = QSim(ns) if options.limits: - print "IMPORTING: limits.py" + print("IMPORTING: limits.py") import limits max_altitude = limits.max_altitude min_altitude = limits.min_altitude @@ -582,13 +582,13 @@ if __name__ == "__main__": max_up = limits.max_up min_up = limits.min_up - print "----- LIMITS ------" - print "Altitude limits: <{0},{1}> ".format(min_altitude,max_altitude) - print "Max speed: ", max_speed - print "Forward limits: <{0},{1}> ".format(min_forward,max_forward) - print "Right limits: <{0},{1}> ".format(min_right,max_right) - print "Up limits: <{0},{1}> ".format(min_up,max_up) - print "-------------------" + print("----- LIMITS ------") + print("Altitude limits: <{0},{1}> ".format(min_altitude,max_altitude)) + print("Max speed: ", max_speed) + print("Forward limits: <{0},{1}> ".format(min_forward,max_forward)) + print("Right limits: <{0},{1}> ".format(min_right,max_right)) + print("Up limits: <{0},{1}> ".format(min_up,max_up)) + print("-------------------") @@ -620,15 +620,15 @@ if __name__ == "__main__": # options.speed = 1.0 if options.limits: - if options.speed > max_speed: print "Speed {0} is too high (limit: {1}).".format (options.speed, max_speed);sys.exit (1) + if options.speed > max_speed: print("Speed {0} is too high (limit: {1}).".format (options.speed, max_speed));sys.exit (1) if options.y > max_forward or options.y < min_forward: - print "Forward {0} is outside limits: <{1},{2}>.".format (options.y, min_forward, max_forward);sys.exit (1) + print("Forward {0} is outside limits: <{1},{2}>.".format (options.y, min_forward, max_forward));sys.exit (1) if options.x > max_right or options.x < min_right: - print "Right {0} is outside limits: <{1},{2}>.".format (options.x, min_right, max_right);sys.exit (1) + print("Right {0} is outside limits: <{1},{2}>.".format (options.x, min_right, max_right));sys.exit (1) if options.z > max_up or options.z < min_up: - print "Up {0} is outside limits: <{1},{2}>.".format (options.z, min_up, max_up);sys.exit (1) + print("Up {0} is outside limits: <{1},{2}>.".format (options.z, min_up, max_up));sys.exit (1) if options.manual: sim.to_manual () @@ -638,7 +638,7 @@ if __name__ == "__main__": if options.logfile != "": - print "ENABLE LOGGING" + print("ENABLE LOGGING") sim.start_logging (options.logfile + ".lgdat") @@ -649,10 +649,10 @@ if __name__ == "__main__": sim.to_auto () ####### VICON -# print "Setting UAV state source: {0:1d} ".format(options.state) +# print("Setting UAV state source: {0:1d} ".format(options.state)) # resp = qc.set_uav_states_config (0, options.state) -# print resp -# if resp.status != 0: print "Failed to set the uav states config"; sys.exit (1) +# print(resp) +# if resp.status != 0: print("Failed to set the uav states config"); sys.exit (1) if not options.lqreal: sim.set_use_sticks (0), @@ -685,7 +685,7 @@ if __name__ == "__main__": if options.getref: rdata = sim.get_ref_data () - print rdata + print(rdata) #