Go to the documentation of this file.00001
00002
00003 from std_msgs.msg import Float64MultiArray
00004 import termios, fcntl, sys, os
00005 import rospy
00006
00007
00008 from std_srvs.srv import Empty
00009
00010 if len(sys.argv) != 4:
00011 sys.exit("Usage: "+sys.argv[0]+" <thrusters_topic>")
00012
00013
00014 thrusters_topic=sys.argv[1]
00015
00016 fd = sys.stdin.fileno()
00017 oldterm = termios.tcgetattr(fd)
00018 newattr = termios.tcgetattr(fd)
00019 newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO
00020 termios.tcsetattr(fd, termios.TCSANOW, newattr)
00021
00022 oldflags = fcntl.fcntl(fd, fcntl.F_GETFL)
00023 fcntl.fcntl(fd, fcntl.F_SETFL, oldflags | os.O_NONBLOCK)
00024
00025 pub = rospy.Publisher(thrusters_topic, Float64MultiArray)
00026 rospy.init_node('keyboard')
00027 rospy.wait_for_service('/dynamics/reset')
00028 reset=rospy.ServiceProxy('/dynamics/reset', Empty)
00029 try:
00030 while not rospy.is_shutdown():
00031 thrusters=[0,0,0,0,0]
00032 msg = Float64MultiArray()
00033 try:
00034 c = sys.stdin.read(1)
00035
00036 if c=='w':
00037 thrusters[0]=thrusters[1]=0.4
00038 elif c=='s':
00039 thrusters[0]=thrusters[1]=-0.4
00040 elif c=='a':
00041 thrusters[4]=0.4
00042 elif c=='d':
00043 thrusters[4]=-0.4
00044 elif c==' ':
00045 reset()
00046 elif c=='\x1b':
00047 c2= sys.stdin.read(1)
00048 c2= sys.stdin.read(1)
00049 if c2=='A':
00050 thrusters[2]=thrusters[3]=0.4
00051 elif c2=='B':
00052 thrusters[2]=thrusters[3]=-0.4
00053 elif c2=='C':
00054 thrusters[0]=-0.4
00055 thrusters[1]=0.4
00056 elif c2=='D':
00057 thrusters[0]=0.4
00058 thrusters[1]=-0.4
00059 else:
00060 print 'wrong key pressed'
00061 while c!='':
00062 c = sys.stdin.read(1)
00063 except IOError: pass
00064 msg.data = thrusters
00065 pub.publish(msg)
00066 rospy.sleep(0.1)
00067 finally:
00068 termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm)
00069 fcntl.fcntl(fd, fcntl.F_SETFL, oldflags)