LGD/hand_control
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

adds land takeoff reset to keyboard_azerty

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-04-25 13:24:31 +02:00
parent 33e872937f
commit 3c1e015411
1 changed files with 47 additions and 33 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

80
hand_control/src/keyboard_azerty.py
View file

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