00001
00002 import roslib;
00003
00004 roslib.load_manifest('ric_base_station')
00005 import rospy
00006
00007 from geometry_msgs.msg import Twist
00008
00009 import sys, select, termios, tty
00010
00011
00012 msg = """
00013 Reading from the keyboard and Publishing to Twist!
00014 ---------------------------
00015 Moving around:
00016 u i o
00017 j k l
00018 m , .
00019
00020 q/z : increase/decrease max speeds by 10%
00021 w/x : increase/decrease only linear speed by 10%
00022 e/c : increase/decrease only angular speed by 10%
00023 anything else : stop
00024
00025 CTRL-C to quit
00026 """
00027
00028 moveBindings = {
00029 'i': (1, 0),
00030 'o': (1, -1),
00031 'j': (0, 1),
00032 'l': (0, -1),
00033 'u': (1, 1),
00034 ',': (-1, 0),
00035 '.': (-1, 1),
00036 'm': (-1, -1),
00037 }
00038
00039 speedBindings = {
00040 'q': (1.1, 1.1),
00041 'z': (.9, .9),
00042 'w': (1.1, 1),
00043 'x': (.9, 1),
00044 'e': (1, 1.1),
00045 'c': (1, .9),
00046 }
00047
00048
00049 def getKey():
00050 tty.setraw(sys.stdin.fileno())
00051 select.select([sys.stdin], [], [], 0)
00052 key = sys.stdin.read(1)
00053 termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
00054 return key
00055
00056
00057 speed = .5
00058 turn = 1
00059
00060
00061 def vels(speed, turn):
00062 return "currently:\tspeed %s\tturn %s " % (speed, turn)
00063
00064
00065 if __name__ == "__main__":
00066 rospy.wait_for_service('/devsOnline')
00067 settings = termios.tcgetattr(sys.stdin)
00068 if (len(sys.argv) == 1):
00069 topic = "cmd_vel"
00070 else:
00071 topic = str(sys.argv[1])
00072 pub = rospy.Publisher(topic, Twist, queue_size=10)
00073 rospy.init_node('teleop_twist_keyboard')
00074 rospy.loginfo("Publishing Twist messeges to topic: " + topic)
00075 r = rospy.Rate(10)
00076 x = 0
00077 th = 0
00078 status = 0
00079
00080 try:
00081 print msg
00082 rospy.loginfo(vels(speed, turn))
00083 while (1):
00084 key = getKey()
00085 if key in moveBindings.keys():
00086 x = moveBindings[key][0]
00087 th = moveBindings[key][1]
00088 elif key in speedBindings.keys():
00089 speed = speed * speedBindings[key][0]
00090 turn = turn * speedBindings[key][1]
00091
00092 rospy.loginfo(vels(speed, turn))
00093 if (status == 14):
00094 print msg
00095 status = (status + 1) % 15
00096 else:
00097 x = 0
00098 th = 0
00099 if (key == '\x03'):
00100 break
00101 twist = Twist()
00102 twist.linear.x = x * speed;
00103 twist.linear.y = 0;
00104 twist.linear.z = 0
00105 twist.angular.x = 0;
00106 twist.angular.y = 0;
00107 twist.angular.z = th * turn
00108 pub.publish(twist)
00109
00110 except:
00111 rospy.logerr(e)
00112
00113 finally:
00114 twist = Twist()
00115 twist.linear.x = 0;
00116 twist.linear.y = 0;
00117 twist.linear.z = 0
00118 twist.angular.x = 0;
00119 twist.angular.y = 0;
00120 twist.angular.z = 0
00121 pub.publish(twist)
00122 r.sleep()
00123 termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)