diff --git a/hand_control/src/keyboard_azerty.py b/hand_control/src/keyboard_azerty.py index d1ca1e5..366b09e 100755 --- a/hand_control/src/keyboard_azerty.py +++ b/hand_control/src/keyboard_azerty.py @@ -4,16 +4,18 @@ import roslib; roslib.load_manifest('teleop_twist_keyboard') import rospy from geometry_msgs.msg import Twist +from std_msgs.msg import Empty import sys, select, termios, tty msg = """ -Reading from the keyboard and Publishing to Twist! ---------------------------- -Moving around: -|⇑ y|↖ u|↑ i|↗ o| -|⇐ h|← j| k|→ l|⇒ m| -|⇓ n|↙ ,|↓ ;|↘ :| + --------------------- +takeoff>| t|⇑ y|↖ u|↑ i|↗ o| + |---|---|---|---|---|---- + reset>| g|⇐ h|← j| k|→ l|⇒ m| + |---|---|---|---|---|---- + land>| b|⇓ n|↙ ,|↓ ;|↘ :| + --------------------- a/w : increase/decrease max speeds by 10% z/x : increase/decrease only linear speed by 10% @@ -63,7 +65,10 @@ def vels(speed,turn): if __name__=="__main__": settings = termios.tcgetattr(sys.stdin) - pub = rospy.Publisher('cmd_vel', Twist, queue_size=1) + pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) + land = rospy.Publisher('/ardrone/land', Empty, queue_size=1) + takeoff = rospy.Publisher('/ardrone/takeoff', Empty, queue_size=1) + reset = rospy.Publisher('/ardrone/reset', Empty, queue_size=1) rospy.init_node('keyboard_azerty') x = 0 th = 0 @@ -75,33 +80,42 @@ if __name__=="__main__": print vels(speed,turn) while(1): key = getKey() - if key in moveBindings.keys(): - x = moveBindings[key][0] - th = moveBindings[key][1] - y = moveBindings[key][2] - z = moveBindings[key][3] - elif key in speedBindings.keys(): - speed = speed * speedBindings[key][0] - turn = turn * speedBindings[key][1] - - print vels(speed,turn) - if (status == 14): - print msg - status = (status + 1) % 15 + if (key == 't'): + takeoff.publish(Empty()) + print "takeoff" + elif (key == 'g'): + reset.publish(Empty()) + print "reset" + elif (key == 'b'): + land.publish(Empty()) + print "land" else: - x = 0 - th = 0 - y = 0 - z = 0 - if (key == '\x03'): - break - twist = Twist() - twist.linear.x = x*speed - twist.linear.y = y*speed - twist.linear.z = z*speed - twist.angular.x = 0; twist.angular.y = 0; - twist.angular.z = th*turn - pub.publish(twist) + if key in moveBindings.keys(): + x = moveBindings[key][0] + th = moveBindings[key][1] + y = moveBindings[key][2] + z = moveBindings[key][3] + elif key in speedBindings.keys(): + speed = speed * speedBindings[key][0] + turn = turn * speedBindings[key][1] + print vels(speed,turn) + if (status == 14): + print msg + status = (status + 1) % 15 + else: + x = 0 + th = 0 + y = 0 + z = 0 + if (key == '\x03'): + break + twist = Twist() + twist.linear.x = x*speed + twist.linear.y = y*speed + twist.linear.z = z*speed + twist.angular.x = 0; twist.angular.y = 0; + twist.angular.z = th*turn + pub.publish(twist) except: print e finally: