fmk_ros_bridge: joy_teleop.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
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: # wii
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: #wii
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()