127 lines
3.1 KiB
Python
Executable file
127 lines
3.1 KiB
Python
Executable file
#!/usr/bin/env python
|
|
# -*- coding: utf-8 -*-
|
|
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 = """
|
|
---------------------
|
|
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%
|
|
e/c : increase/decrease only angular speed by 10%
|
|
anything else : stop
|
|
|
|
CTRL-C to quit
|
|
"""
|
|
|
|
moveBindings = {
|
|
# x th y z
|
|
'i':(1,0,0,0),
|
|
'o':(1,-1,0,0),
|
|
'j':(0,1,0,0),
|
|
'l':(0,-1,0,0),
|
|
'u':(1,1,0,0),
|
|
';':(-1,0,0,0),
|
|
':':(-1,1,0,0),
|
|
',':(-1,-1,0,0),
|
|
'h':(0,0,-1,0),
|
|
'm':(0,0,1,0),
|
|
'y':(0,0,0,1),
|
|
'n':(0,0,0,-1),
|
|
}
|
|
|
|
speedBindings={
|
|
'a':(1.1,1.1),
|
|
'w':(.9,.9),
|
|
'z':(1.1,1),
|
|
'x':(.9,1),
|
|
'e':(1,1.1),
|
|
'c':(1,.9),
|
|
}
|
|
|
|
def getKey():
|
|
tty.setraw(sys.stdin.fileno())
|
|
select.select([sys.stdin], [], [], 0)
|
|
key = sys.stdin.read(1)
|
|
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
|
return key
|
|
|
|
speed = 0.1
|
|
turn = 0.5
|
|
|
|
def vels(speed,turn):
|
|
return "currently:\tspeed %s\tturn %s " % (speed,turn)
|
|
|
|
if __name__=="__main__":
|
|
settings = termios.tcgetattr(sys.stdin)
|
|
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
|
|
y = 0
|
|
z = 0
|
|
status = 0
|
|
try:
|
|
print msg
|
|
print vels(speed,turn)
|
|
while(1):
|
|
key = getKey()
|
|
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:
|
|
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:
|
|
twist = Twist()
|
|
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
|
|
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
|
|
pub.publish(twist)
|
|
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
|
|