joystick_node.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 # Copyright (c) 2009, Georgia Tech Research Corporation
00003 # All rights reserved.
00004 #
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #     * Redistributions of source code must retain the above copyright
00008 #       notice, this list of conditions and the following disclaimer.
00009 #     * Redistributions in binary form must reproduce the above copyright
00010 #       notice, this list of conditions and the following disclaimer in the
00011 #       documentation and/or other materials provided with the distribution.
00012 #     * Neither the name of the Georgia Tech Research Corporation nor the
00013 #       names of its contributors may be used to endorse or promote products
00014 #       derived from this software without specific prior written permission.
00015 #
00016 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
00017 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
00020 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00021 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00022 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00023 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00024 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00025 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 #
00027 #  \author Chih-Hung King (Healthcare Robotics Lab, Georgia Tech.)
00028 
00029 import math
00030 import numpy as np
00031 import pygame
00032 import pygame.joystick
00033 import time
00034 
00035 import roslib; roslib.load_manifest('tele_mobile')
00036 import rospy
00037 
00038 from pygame.locals import *
00039 from tele_mobile.msg import direction
00040 
00041 
00042 class JoystickCommand:
00043     def __init__(self, topic='tmobile', name='joystick'):
00044         self.pub = rospy.Publisher('tmobile', direction)
00045         try:
00046             rospy.init_node(name, anonymous=True)
00047         except rospy.ROSException, e:
00048             pass
00049 
00050 
00051     def set_platform(self, x, y,reset, zen, xvel, yvel, avel,zen_reset, lock):
00052         cmd = direction(x, y, reset, zen, xvel, yvel, avel,zen_reset, lock)
00053         self.pub.publish(cmd)
00054         print "publishing:", cmd
00055         time.sleep(0.05)
00056         
00057 
00058 if __name__ == '__main__':
00059 
00060         #init pygame
00061         pygame.init()
00062 
00063         #joystick_status
00064         joystick_count = pygame.joystick.get_count()
00065 
00066         print "Joysticks found: %d\n" % joystick_count
00067 
00068         try:
00069                 js = pygame.joystick.Joystick(0)
00070                 js.init()
00071         except pygame.error:
00072                 print "joystick error"
00073                 js = None
00074 
00075         js_status = js.get_init()
00076         print js_status
00077 
00078         screen = pygame.display.set_mode((320, 80))
00079         pygame.display.set_caption('Snozzjoy')
00080 
00081         background = pygame.Surface(screen.get_size())
00082         background = background.convert()
00083         background.fill((250, 250, 250))
00084 
00085         s = JoystickCommand()
00086 
00087         max_xvel = 0.18 
00088         max_yvel = 0.15
00089         max_speed = 0.18        # don't exceed 0.18 under any condition.
00090         max_avel = 0.18
00091 
00092         x = 0.
00093         y = 0.
00094         reset = 0.
00095         lock = 0.
00096         zen = 0.
00097         seg_x = 0.
00098         seg_y = 0.
00099         seg_a = 0.
00100         xvel = 0.
00101         yvel = 0.
00102         avel = 0.
00103         zen_reset = 0.
00104         connected = False
00105 
00106         while not rospy.is_shutdown():
00107                 for event in pygame.event.get():
00108 #                       print event
00109                         if (event.type == JOYHATMOTION):
00110                                 x = event.value[0]
00111                                 y = event.value[1]
00112                         if (event.type == JOYBUTTONDOWN):
00113                                 if(event.button == 0):
00114                                         reset = 1
00115 #                               if(event.button == 1):
00116 #                                       lock = 1
00117                                 if(event.button == 9):
00118                                         zen_reset = 1
00119                                 if(event.button == 4 or event.button == 5 ):
00120                                         zen = 1
00121                                 if(event.button == 2 or event.button == 3 ):
00122                                         zen = -1
00123                                 if(event.button == 2 or event.button == 3 ):
00124                                         zen = -1                
00125                         if (event.type == JOYBUTTONUP):
00126                                 if(event.button == 0):
00127                                         reset = 0
00128 #                               if(event.button == 1):
00129 #                                       lock = 0
00130                                 if(event.button == 9):
00131                                         zen_reset = 0
00132                                 if(event.button == 2 or event.button == 3 or event.button == 4 or event.button == 5):
00133                                         zen = 0
00134                         if (event.type == JOYAXISMOTION):
00135                                 if event.axis == 0:
00136                                         seg_y = -event.value
00137                                 if event.axis == 1:
00138                                         seg_x = -event.value
00139                                 if event.axis == 2:
00140                                         seg_a = -event.value
00141                                 if event.axis == 3:
00142                                         lock = (1 - event.value) / 2
00143                 if seg_x == 0 and seg_y == 0 and seg_a ==0:
00144                         connected = True
00145 
00146         # detect a joystick disconnect
00147 #        try:
00148 #            js.quit()
00149 #            js.init()
00150 #        except pygame.error:
00151 #            print "joystick error"
00152 #            rospy.signal_shutdown()
00153 
00154                 if connected:
00155                         xvel = seg_x * max_xvel * lock
00156                         yvel = seg_y * max_yvel * lock
00157                         avel = seg_a * max_avel * lock
00158                         s.set_platform(x, y, reset, zen, xvel, yvel, avel, zen_reset, lock) 
00159 
00160         s.set_platform(0, 0, 0, 0, 0, 0, 0, 0, 0)


tele_mobile
Author(s): Chih-Hung King, Advisor: Prof. Charlie Kemp, Lab: Healthcare Robotics Lab at Georgia Tech
autogenerated on Wed Nov 27 2013 11:57:13