diff --git a/.gitattributes b/.gitattributes index 320b0ca457eb8425544a9b00c4de476ece44f402..2e145e4d78a17e2fcf3245830f300ec8c6fa012c 100644 --- a/.gitattributes +++ b/.gitattributes @@ -2,6 +2,7 @@ /CMakeLists.txt -text /package.xml -text /setup.py -text +src/lqsimple.py -text src/pyutil/__init__.py -text src/pyutil/relayutil.py -text src/pyutil/tdwutil.py -text diff --git a/src/lqsimple.py b/src/lqsimple.py new file mode 100755 index 0000000000000000000000000000000000000000..77bff48faafbb2bdde373746fa4f8095be9863b1 --- /dev/null +++ b/src/lqsimple.py @@ -0,0 +1,708 @@ +#!/usr/bin/env python + +# +# Module with a simplified interface to the Link Quad +# + +import math +import sys +import os +import time + +import rospy +from lrs_msgs.msg import * + +from quad.qsim import * +from quad.qcon import * + +# +# Put this in a file call limits.py and make sure it is in the path +# This will override the default values below. All limit variables have to be defined +# in the file. +# + + +max_altitude = 140.5 +min_altitude = 0.1 + +max_speed = 5.0 + +max_forward = 20.0 +min_forward = -20.0 + +max_right = 20.0 +min_right = -20.0 + +max_up = 5.0 +min_up = -5.0 + +# +# +# +if os.path.isfile ("limits.py"): + print "READ FILE: limits.py" + import limits + max_altitude = limits.max_altitude + min_altitude = limits.min_altitude + max_speed = limits.max_speed + max_forward = limits.max_forward + min_forward = limits.min_forward + max_right = limits.max_right + min_right = limits.min_right + 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 "-------------------" + + +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) + + 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) + + + resp = qc.queues_clear_token (1, 1, yaw_flag, 0); + if resp.status != 0: print "Failed to clear the queues"; sys.exit (1) + + resp = qc.request_flyq (); + if resp.status != 0: sys.exit (1) + + flyqtoken = resp.token + + yawqtoken = 0 + if yaw_flag: + resp = qc.request_yawq (); + if resp.status != 0: sys.exit (1) + + yawqtoken = resp.token + + resp = qc.request_climbq (); + if resp.status != 0: sys.exit (1) + + climbqtoken = resp.token + + resp = qc.flyq_reset (flyqtoken) + if resp.status != 0: sys.exit (1) + + if yaw_flag: + resp = qc.yawq_reset (yawqtoken) + if resp.status != 0: sys.exit (1) + + resp = qc.climbq_reset (climbqtoken) + if resp.status != 0: sys.exit (1) + + resp = qc.add_flyto_3d_m (flyqtoken, north_m, east_m, relalt, speed, 0, 1, 1) + if resp.status != 0: sys.exit (1) + + if yaw_flag: + resp = qc.yawq_add_use_fly (yawqtoken) + if resp.status != 0: sys.exit (1) + + resp = qc.climbq_add_use_fly (climbqtoken) + if resp.status != 0: sys.exit (1) + + if options.confirm: + raw_input("---> READY. Press Enter to execute...") + + resp = qc.queues_run (flyqtoken, climbqtoken, yawqtoken*yaw_flag, 0) + if resp.status != 0: sys.exit (1) + + qc.wait_for_flyq () + +def take_off (alt, speed): + ns = rospy.get_namespace () + 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) + + resp = qc.request_flyq (); + print resp + if resp.status != 0: sys.exit (1) + flyqtoken = resp.token + + resp = qc.request_climbq (); + print resp + if resp.status != 0: sys.exit (1) + climbqtoken = resp.token + + resp = qc.flyq_reset (flyqtoken) + print resp + if resp.status != 0: sys.exit (1) + + resp = qc.climbq_reset (climbqtoken) + print resp + if resp.status != 0: sys.exit (1) + + resp = qc.flyq_add_take_off (flyqtoken, alt, speed) + print resp + if resp.status != 0: sys.exit (1) + + resp = qc.climbq_add_use_fly (climbqtoken) + print resp + if resp.status != 0: sys.exit (1) + + resp = qc.flyq_add_barrier (flyqtoken, 1, 1, 1, 0, 0) + print resp + if resp.status != 0: sys.exit (1) + + resp = qc.climbq_add_barrier (climbqtoken, 1, 1, 1, 0, 0) + print resp + if resp.status != 0: sys.exit (1) + + + resp = qc.queues_run (flyqtoken, climbqtoken, 0, 0); + print resp + if resp.status != 0: sys.exit (1) + + qc.wait_for_flyq () + +def land (alt, speed): + ns = rospy.get_namespace () + 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) + + + resp = qc.request_flyq (); + print resp + if resp.status != 0: sys.exit (1) + flyqtoken = resp.token + + resp = qc.request_climbq (); + print resp + if resp.status != 0: sys.exit (1) + climbqtoken = resp.token + + resp = qc.flyq_reset (flyqtoken) + print resp + if resp.status != 0: sys.exit (1) + + resp = qc.climbq_reset (climbqtoken) + print resp + if resp.status != 0: sys.exit (1) + + resp = qc.flyq_add_land (flyqtoken, alt, speed) + print resp + if resp.status != 0: sys.exit (1) + + resp = qc.climbq_add_use_fly (climbqtoken) + print resp + if resp.status != 0: sys.exit (1) + + + resp = qc.flyq_add_barrier (flyqtoken, 1, 1, 1, 0, 0) + print resp + if resp.status != 0: sys.exit (1) + + resp = qc.climbq_add_barrier (climbqtoken, 1, 1, 1, 0, 0) + print resp + if resp.status != 0: sys.exit (1) + + + resp = qc.queues_run (flyqtoken, climbqtoken, 0, 0); + print resp + if resp.status != 0: sys.exit (1) + + qc.wait_for_flyq () + + + +def fly_in_dir_ref (): + ns = rospy.get_namespace () + qc = QCon(ns, 10000) + + 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) + + resp = qc.request_flyq (); + if resp.status != 0: sys.exit (1) + + flyqtoken = resp.token + + resp = qc.request_climbq (); + if resp.status != 0: sys.exit (1) + + climbqtoken = resp.token + + resp = qc.flyq_reset (flyqtoken) + if resp.status != 0: sys.exit (1) + + resp = qc.climbq_reset (climbqtoken) + if resp.status != 0: sys.exit (1) + + resp = qc.add_fly_in_dir_ref (flyqtoken) + if resp.status != 0: sys.exit (1) + + resp = qc.add_climb_in_dir_ref (climbqtoken) + if resp.status != 0: sys.exit (1) + + if options.confirm: + raw_input("---> READY. Press Enter to execute...") + + resp = qc.queues_run (flyqtoken, climbqtoken, 0, 0) + if resp.status != 0: sys.exit (1) + +# qc.wait_for_flyq () + +def fly_external (): + ns = rospy.get_namespace () + qc = QCon(ns, 10000) + + 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) + + resp = qc.request_flyq (); + if resp.status != 0: sys.exit (1) + + flyqtoken = resp.token + + resp = qc.request_climbq (); + if resp.status != 0: sys.exit (1) + climbqtoken = resp.token + + resp = qc.request_yawq (); + if resp.status != 0: sys.exit (1) + yawqtoken = resp.token + + + resp = qc.flyq_reset (flyqtoken) + if resp.status != 0: sys.exit (1) + + resp = qc.climbq_reset (climbqtoken) + if resp.status != 0: sys.exit (1) + + resp = qc.yawq_reset (yawqtoken) + if resp.status != 0: sys.exit (1) + + resp = qc.add_fly_ext (flyqtoken) + if resp.status != 0: sys.exit (1) + + resp = qc.add_climb_ext (climbqtoken) + if resp.status != 0: sys.exit (1) + + resp = qc.add_yaw_ext (yawqtoken) + if resp.status != 0: sys.exit (1) + +# if options.confirm: +# raw_input("---> READY. Press Enter to execute...") + + resp = qc.queues_run (flyqtoken, climbqtoken, yawqtoken, 0) + if resp.status != 0: sys.exit (1) + + +def ptu_external (): + ns = rospy.get_namespace () + qc = QCon(ns, 10000) + + 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) + + resp = qc.request_ptuq (); + if resp.status != 0: sys.exit (1) + + ptuqtoken = resp.token + + resp = qc.ptuq_reset (ptuqtoken) + if resp.status != 0: sys.exit (1) + + resp = qc.add_ptu_ext (ptuqtoken) + if resp.status != 0: sys.exit (1) + + resp = qc.queues_run (0, 0, 0, ptuqtoken) + if resp.status != 0: sys.exit (1) + + +def yaw (heading): + ns = rospy.get_namespace () + qc = QCon(ns, 10000) + + 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) + + resp = qc.request_yawq () + if resp.status != 0: sys.exit (1) + + yawqtoken = resp.token + + resp = qc.yawq_reset (yawqtoken) + if resp.status != 0: sys.exit (1) + + rate_only = 0 + use_rate_sign = 0 + rate = 5 + + resp = qc.add_yaw (yawqtoken, heading, rate, rate_only, use_rate_sign) + if resp.status != 0: sys.exit (1) + + resp = qc.queues_run (0, 0, yawqtoken, 0) + if resp.status != 0: sys.exit (1) + + qc.wait_for_yawq () + + +def yawrel (heading): + ns = rospy.get_namespace () + qc = QCon(ns, 10000) + + print "yawrel: heading: {0}".format (heading) + + resp = qc.get_current_state () + 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) + + resp = qc.request_yawq () + if resp.status != 0: sys.exit (1) + + yawqtoken = resp.token + + resp = qc.yawq_reset (yawqtoken) + if resp.status != 0: sys.exit (1) + + rate_only = 0 + use_rate_sign = 0 + rate = 5 + + resp = qc.add_yaw (yawqtoken, heading, rate, rate_only. use_rate_sign) + if resp.status != 0: sys.exit (1) + + if options.confirm: + raw_input("---> READY. Press Enter to execute...") + + resp = qc.queues_run (0, 0, yawqtoken, 0) + if resp.status != 0: sys.exit (1) + + qc.wait_for_yawq () + + +def climb (alt): + ns = rospy.get_namespace () + qc = QCon(ns, 10000) + + 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) + + resp = qc.request_climbq () + if resp.status != 0: sys.exit (1) + + climbqtoken = resp.token + + resp = qc.climbq_reset (climbqtoken) + if resp.status != 0: sys.exit (1) + + resp = qc.add_climb (climbqtoken, alt) + if resp.status != 0: sys.exit (1) + + resp = qc.queues_run (0, climbqtoken, 0, 0) + if resp.status != 0: sys.exit (1) + + qc.wait_for_climbq () + +def climbrel (alt, speed): + ns = rospy.get_namespace () + qc = QCon(ns, 10000) + + 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) + + resp = qc.get_current_state () + 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) + + fly_straight_line (state_east_m, state_north_m, alt_m, speed, 0) + +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) + + resp = qc.queues_clear_token (1, 0, 0, 0); + if resp.status != 0: print "Failed to clear the climb queue"; sys.exit (1) + + resp = qc.get_current_state () + 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 + + 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) + + fly_straight_line (state_east_m + east_m, state_north_m + north_m, state_alt, speed, 0) + + +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) + + resp = qc.queues_clear_token (1, 1, yaw_flag, 0); + 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 + +# 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 + + resp = qc.get_current_targets () + print resp + +#float64 east_m +#float64 north_m +#float64 altitude +#float64 vel_east +#float64 vel_north +#float64 vel_up +#float64 acc_east +#float64 acc_north +#float64 acc_up +#float64 heading + + + state_north_m = resp.north_m + state_east_m = resp.east_m + 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) + + 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) + + fly_straight_line (state_east_m + east_m, state_north_m + north_m, + state_alt + up, speed, yaw_flag) + +def fly_triangle (speed): + ns = rospy.get_namespace () + qc = QCon(ns, 10000) + + resp = qc.get_current_state () + print resp + + state_north_m = resp.lat_m + state_east_m = resp.lon_m + state_alt = resp.alt + + tokens = qc.fly_triangle_yaw_climb_pause (state_east_m, state_north_m, + state_alt, speed) + print tokens + + if options.confirm: + raw_input("---> READY. Press Enter to execute...") + + resp = qc.queues_run (tokens[0], tokens[1], tokens[2], 0) + if resp.status != 0: sys.exit (1) + + qc.wait_for_flyq () + + +if __name__ == "__main__": + from optparse import OptionParser + parser = OptionParser() + parser.add_option ("", "--log", action="store", type="string", dest="logfile", default="") + parser.add_option ("-u", "--unit", action="store", type="int", dest="unit", default=0) + parser.add_option ("", "--looprate", action="store", type="int", dest="looprate", default=500) + parser.add_option ("", "--confirm", action="store_true", dest="confirm", help="If given some fly actions have a confirm in it") + parser.add_option ("", "--limits", action="store_true", dest="limits", help="Check some pre defined limits before flying") + parser.add_option ("", "--getref", action="store_true", dest="getref", help="Get the reference data") + parser.add_option ("", "--setref", action="store_true", dest="setref", help="Set the reference data") + parser.add_option ("", "--set", action="store_true", dest="set", help="Set values and exit") + parser.add_option ("", "--fly", action="store_true", dest="fly", help="Fly straight line") + parser.add_option ("", "--flytriangle", action="store_true", dest="flytriangle", help="Fly triangle with pauses") + parser.add_option ("", "--flyindirref", action="store_true", dest="flyindirref", help="Fly in direction according to ref data") + parser.add_option ("", "--flyxyrel", action="store_true", dest="flyxyrel", help="Fly straight line from current position to cp+(x,y), y is forward and x to the right of the LQ") + parser.add_option ("", "--flyrel", action="store_true", dest="flyrel", help="Fly straight line from current position to cp+(x,y,z), y is forward and x to the right of the LQ and z is up") + parser.add_option ("", "--land", action="store_true", dest="land", help="Land by flying to alt z with speed") + parser.add_option ("", "--takeoff", action="store_true", dest="takeoff", help="Take off by flying to alt z with speed") + parser.add_option ("", "--useyaw", action="store_true", dest="useyaw", help="Fly with yaw towards a waypoint. Otherwise, keep the current yaw", default=False) + parser.add_option ("", "--yaw", action="store_true", dest="yaw", help="Yaw to heading") + parser.add_option ("", "--yawrel", action="store_true", dest="yawrel", help="Yaw to current_heading+heading") + parser.add_option ("", "--climb", action="store_true", dest="climb", help="Climb to z") + parser.add_option ("", "--climbrel", action="store_true", dest="climbrel", help="Climb to current_alt+z") + parser.add_option ("", "--lqreal", action="store_true", dest="lqreal", help="Fly the real LQ") + parser.add_option ("", "--manual", action="store_true", dest="manual", help="Go to manual in the simulator") + + parser.add_option ("-x", "--xpos", action="store", type="float", dest="x", default=8) + parser.add_option ("-y", "--ypos", action="store", type="float", dest="y", default=2) + parser.add_option ("-z", "--zpos", action="store", type="float", dest="z", default=2) + parser.add_option ("", "--forward", action="store", type="float", dest="y", help="The amount of forward to fly", default=0) + parser.add_option ("", "--right", action="store", type="float", dest="x", help="The amount of right to fly", default=0) + parser.add_option ("", "--up", action="store", type="float", dest="z", help="The amount of up to fly", default=0) + parser.add_option ("-s", "--speed", action="store", type="float", dest="speed", default=2) + parser.add_option ("", "--xyspeed", action="store", type="float", dest="xyspeed", default=0.2) + parser.add_option ("", "--zspeed", action="store", type="float", dest="zspeed", default=0.2) + parser.add_option ("", "--heading", action="store", type="float", dest="heading", default=45, help="In degree between 0-360 (negative angles currently do not work)") + parser.add_option ("", "--state", action="store", type="int", dest="state", default=0, help="0 (default) is sim/real outside, 1 is vicon room") + parser.add_option ("", "--joystick", action="store_true", dest="joystick", help="Set ext config and exit") + parser.add_option ("", "--safenav", action="store_true", dest="safenav", help="Set ext config and exit") + (options, args) = parser.parse_args() + + rospy.init_node('lqsimple', anonymous=True) + + ns = rospy.get_namespace () + qc = QCon(ns, 10000) + sim = QSim(ns) + + if options.joystick: + qc.set_ext_config_joystick() + sys.exit(0) + + if options.safenav: + qc.set_ext_config_safenav() + sys.exit(0) + + if options.set: + sim.set_loop_rate (options.looprate) + sys.exit(0) + + if options.lqreal: + if options.takeoff: + options.z = 1.7 + options.speed = 1.0 +# options.speed = 1.5 +# outdoor gps +# options.z = 10 +# options.speed = 2.0 + if options.land: + options.z = -100 + options.speed = 0.3 +# outdoor gps +# options.z = -100 +# 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.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) + + 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) + + 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) + + if options.manual: + sim.to_manual () + time.sleep (1) + sim.to_auto () + sys.exit(0) + + + if options.logfile != "": + print "ENABLE LOGGING" + sim.start_logging (options.logfile + ".lgdat") + + + # Make sure the motor is on in the simulator + if not options.lqreal: + sim.set_use_sticks (0), + sim.set_motor (1) + sim.to_auto () + +####### VICON +# 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) + + if not options.lqreal: + sim.set_use_sticks (0), + sim.to_auto () # go to auto + + + if options.fly: + fly_straight_line (options.x, options.y, options.z, options.speed, 1) + + if options.land: + land (options.z, options.speed) + + if options.takeoff: + take_off (options.z, options.speed) + + if options.flytriangle: + fly_triangle (options.speed) + + if options.yaw: + yaw (options.heading) + + if options.yawrel: + yawrel (options.heading) + + if options.flyindirref: + fly_in_dir_ref () + + if options.setref: + sim.set_ref_data (options.x, options.y, options.z, options.xyspeed, options.zspeed) + + if options.getref: + rdata = sim.get_ref_data () + print rdata + + +# +# Maybe dangerous, use fly_straight_line instead +# + if options.climb: + climb (options.z) + + if options.climbrel: + climbrel (options.z, options.speed) + + if options.flyxyrel: + fly_xy_rel (options.y, options.x, options.speed) + + if options.flyrel: + fly_rel (options.y, options.x, options.z, options.speed, options.useyaw) + + if options.logfile != "": + time.sleep (10) + sim.stop_logging ()