Go to the documentation of this file.00001
00002
00003 import roslib
00004 roslib.load_manifest('rospy')
00005 roslib.load_manifest('geometry_msgs')
00006 roslib.load_manifest('joy')
00007 roslib.load_manifest('nav_msgs')
00008 import rospy
00009 import geometry_msgs.msg
00010 from sensor_msgs.msg import Joy
00011 from nav_msgs.msg import Odometry
00012 import math
00013
00014 def joy_cb(msg):
00015 tws = geometry_msgs.msg.Twist()
00016 but = msg.buttons
00017 ax = msg.axes
00018 if but[10] > 0:
00019 if but[11] > 0:
00020 sp = 0.5
00021 else:
00022 sp = 0.25
00023 tws.linear = geometry_msgs.msg.Vector3(ax[3] * sp,0,0)
00024 if tws.linear.x < 0:
00025 tws.angular = geometry_msgs.msg.Vector3(0,0,-1.0 * (ax[0] + ax[2]) * math.pi * 0.9 * sp)
00026 else:
00027 tws.angular = geometry_msgs.msg.Vector3(0,0,(ax[0] + ax[2]) * math.pi * 0.9 * sp)
00028 elif len(but) == 11 and but[3] > 0:
00029 tws.linear = geometry_msgs.msg.Vector3((but[8]-but[9]),0,0)
00030 tws.angular = geometry_msgs.msg.Vector3(0,0,(but[6]-but[7]) * math.pi * 0.4)
00031 elif len(but) == 11:
00032 return
00033 else:
00034 tws.linear = geometry_msgs.msg.Vector3(0,0,0)
00035 tws.angular = geometry_msgs.msg.Vector3(0,0,0)
00036 pub.publish(tws)
00037
00038 def fmk_teleop():
00039 global pub
00040 rospy.init_node('fmk_teleop',anonymous=True)
00041 rospy.Subscriber("/joy",Joy,joy_cb)
00042 pub = rospy.Publisher('/teleop/cmd_vel',geometry_msgs.msg.Twist)
00043 rospy.spin()
00044
00045 if __name__ == '__main__':
00046 fmk_teleop()