l

.gitattributes

@@ -15,4 +15,5 @@ src/quad/lq.py -text
 src/quad/qcon.py -text
 src/quad/qsim.py -text
 src/rotaterescuerpose.py -text
+src/runjoystick.py -text
 src/testpyutil.py -text

src/runjoystick.py

+#!/usr/bin/env python
+
+import pygame
+
+import signal
+import quad.qsim
+
+import rospy
+from lrs_msgs.msg import *
+
+from quad.qsim import *
+from quad.qcon import *
+from lqsimple import *
+
+from quad.joystick import *
+
+from optparse import OptionParser
+
+parser = OptionParser()
+
+def handler(signum, frame):
+#    print "do whatever, like call thread.interrupt_main()"
+    sys.exit(0)
+
+signal.signal(signal.SIGINT, handler)
+parser.add_option ("", "--lqreal", action="store_true", dest="lqreal", help="Use real LinkQuad")
+parser.add_option ("-b", "--bodyrelative", action="store_true", dest="bodyrelative", help="Joystick bodyrelative (default off)")
+parser.add_option ("", "--fly", action="store_true", dest="fly", help="Control the flying")
+parser.add_option ("", "--ptu", action="store_true", dest="ptu", help="Control the ptu")
+parser.add_option ("", "--topic", action="store_true", dest="topic", help="Control using topics")
+(options, args) = parser.parse_args()
+
+def callback(data, controller):
+    controller.notify(data)
+    controller.control()
+    cmd = controller.get_command ()
+    finished = controller.get_ext_finished()
+
+    ns = rospy.get_namespace ()
+    qc = QCon(ns, 10000)
+    if options.fly:
+        if options.topic:
+            ed = ExtData (cmd.lon_m, cmd.lat_m, cmd.relalt, cmd.vel_east, 
+                          cmd.vel_north, cmd.vel_up, cmd.acc_east, cmd.acc_north, 
+                          cmd.acc_up, cmd.heading_deg, 
+                          3.0, False, False, True, True, True,
+                          finished, True)
+            global extpub
+            extpub.publish (ed)
+        else:
+            qc.set_ext_data (cmd.lon_m, cmd.lat_m, cmd.relalt, cmd.vel_east, 
+                             cmd.vel_north, cmd.vel_up, cmd.acc_east, cmd.acc_north, 
+                             cmd.acc_up, cmd.heading_deg, 
+                             3.0, False, False, True, True, True,
+                             finished)
+
+    if options.ptu:
+        if controller.ptu_flag:
+            mode = 0
+            qc.set_ptu_ext_data (mode, controller.pan, controller.tilt, finished)
+
+
+
+
+def listener():
+    joystick = JoyStick()
+    joystick.body_relative = options.bodyrelative
+    ns = rospy.get_namespace ()
+    if options.ptu:
+        joystick.ptu_flag = True
+    rospy.Subscriber("quadstate", QuadState, callback, joystick, 1)
+    while (not joystick.get_ext_finished()):
+        time.sleep(0.3)
+#        rospy.spin()
+    rospy.loginfo ("Control finished, exiting script")
+
+
+if __name__ == '__main__':
+    rospy.init_node('listener', anonymous=True)
+    ns = rospy.get_namespace ()
+    rospy.loginfo ("NAMESPACE: %s", ns)
+    sim = QSim(ns)
+    extpub = rospy.Publisher ("ext_data_from_joystick", ExtData)
+    if not options.lqreal:
+        sim.set_use_sticks (0)
+        sim.to_auto ()
+
+    if options.fly:
+        fly_external ()
+    if options.ptu:
+        ptu_external ()
+    listener()
+