josh_teleop2.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #
00003 #  send tele-operation commands to pilot from an HKS racing controller
00004 #
00005 #   Copyright (C) 2011 Austin Robot Technology
00006 #   License: Modified BSD Software License Agreement
00007 #
00008 # $Id: joy_teleop.py 1321 2011-04-19 20:23:37Z jack.oquin $
00009 
00010 PKG_NAME = 'art_teleop'
00011 
00012 # standard Python packages
00013 import sys
00014 import math
00015 import Queue
00016 
00017 import pilot_cmd                # ART pilot interface
00018 import nav_estop                # ART navigator E-stop package
00019 
00020 # ROS node setup
00021 import roslib;
00022 roslib.load_manifest(PKG_NAME)
00023 import rospy
00024 
00025 # ROS messages
00026 from art_msgs.msg import ArtVehicle
00027 from art_msgs.msg import Gear
00028 
00029 # In ROS Electric, the Joy message gained a header and moved to
00030 # sensor_msgs.  Use that if it's available.
00031 try:
00032     from sensor_msgs.msg import Joy
00033 except ImportError:
00034     from joy.msg import Joy
00035 
00036 # dynamic parameter reconfiguration
00037 from driver_base.msg import SensorLevels
00038 from dynamic_reconfigure.server import Server as ReconfigureServer
00039 import art_teleop.cfg.JoyConfig as JoyConfig
00040 
00041 def clamp(minimum, value, maximum):
00042     "constrain value to the range [minimum .. maximum]"
00043     return max(minimum, min(value, maximum))
00044 
00045 class JoyNode():
00046     "Vehicle joystick tele-operation node."
00047 
00048     def __init__(self):
00049         "JoyNode constructor"
00050 
00051         # estop controls
00052         self.run = 3                    # start autonomous run
00053         self.suspend = 12               # suspend autonomous running
00054         self.estop = 13                 # emergency stop
00055 
00056         # tele-operation controls
00057         self.steer = 0                  # steering axis (wheel)
00058         self.drive = 4                  # shift to Drive (^)
00059         self.reverse = 6                # shift to Reverse (v)
00060         self.park = 7                   # shift to Park
00061         self.throttle = 18              # throttle axis (X)
00062         self.throttle_start = True
00063         self.brake = 19                 # brake axis (square)
00064         self.brake_start = True
00065         self.counter = 0
00066         self.throttleQueue = Queue.Queue()
00067         self.cruiseSwitch = 1           # Press down Joystick
00068         self.cruise = False
00069         
00070         # initialize ROS topics
00071         rospy.init_node('josh_teleop2')
00072         self.pilot = pilot_cmd.PilotCommand()
00073         self.reconf_server = ReconfigureServer(JoyConfig, self.reconfigure)
00074         self.joy = rospy.Subscriber('joy', Joy, self.joyCallback)
00075 
00076 
00077     def joyCallback(self, joy):
00078         "invoked every time a joystick message arrives"
00079         rospy.logdebug('joystick input:\n' + str(joy))
00080 
00081         # handle E-stop buttons
00082         if joy.buttons[self.estop]:
00083             rospy.logwarn('emergency stop')
00084             if self.use_navigator:
00085                 self.nav.pause()        # tell navigator to pause
00086             else:
00087                 self.pilot.halt()       # halt car using pilot
00088         elif joy.buttons[self.suspend] and self.use_navigator:
00089             rospy.logwarn('suspend autonomous operation')
00090             self.nav.suspend()
00091         elif joy.buttons[self.run] and self.use_navigator:
00092             rospy.logwarn('start autonomous operation')
00093             self.nav.run()
00094 
00095         # handle shifter buttons
00096         if joy.buttons[self.drive]:
00097             self.pilot.shift(Gear.Drive)
00098             rospy.loginfo('shifting to drive')
00099         elif joy.buttons[self.reverse]:
00100             self.pilot.shift(Gear.Reverse)
00101             rospy.loginfo('shifting to reverse')
00102         elif joy.buttons[self.park]:
00103             self.pilot.shift(Gear.Park)
00104             rospy.loginfo('shifting to park')
00105 
00106         if joy.buttons[self.cruiseSwitch]:
00107                 if self.cruise:
00108                         self.cruise = False
00109                 else:
00110                         self.cruise = True
00111                         self.pilot.pstate.target.speed = self.pilot.pstate.current.speed
00112 
00113         # set steering angle
00114         self.setAngle(joy.axes[self.steer])
00115 
00116         if self.cruise:
00117                 self.pilot.accelerate(0)
00118 
00119         else:
00120                 # adjust speed -- the brake and throttle controls both
00121                 # return 1.0 when released, -1.0 when completely on
00122                 # Convert the -1 to 1 to 0 to 1 in increments of .01
00123                 br = (-joy.axes[self.brake] + 1) / 2
00124                 th = (-joy.axes[self.throttle] + 1) / 2
00125                 rospy.logdebug('joystick brake, throttle: '
00126                                + str(br) + ', ' + str(th))
00127 
00128                 if self.counter < 1:
00129                         self.counter = 1
00130                         self.throttleQueue.put(th)
00131                 else:
00132                         initial_th = self.throttleQueue.get()
00133                         self.throttleQueue.put(th)
00134 
00135                 # initially the ROS /joy topic sends zeroes for each axis
00136                 # until a button press occurs
00137         
00138         
00139                 if self.brake_start:
00140                     if br == 0.5:
00141                         br = 0
00142                     else:
00143                         self.brake_start = False
00144 
00145                 if self.throttle_start:
00146                     if th == 0.5:
00147                         th = 0
00148                     else:
00149                         self.throttle_start = False
00150 
00151                 # set dv according to throttle or brake's current state.
00152                 if br > 0:
00153                         dv = -br * 3
00154 
00155                 elif th == 0 and self.pilot.pstate.current.speed > 0:
00156                         dv = -.1
00157 
00158                 elif th > 0:
00159                         if initial_th < th:
00160                                 dv = th
00161                         elif initial_th > th:
00162                                 dv = -.1 
00163                         elif initial_th == th and self.pilot.pstate.current.acceleration >= 0:
00164                                 dv = th
00165                         elif initial_th == th and self.pilot.pstate.current.acceleration < 0:
00166                                 dv = -.1
00167                         else:
00168                                 dv = 0
00169                 else:
00170                         dv = 0
00171 
00172                 # set acceleration and speed from brake and throttle controls
00173                 self.pilot.accelerate(dv*10)
00174 
00175         if self.nav.is_suspended(): # OK to issue command?
00176             self.pilot.publish()
00177         else:
00178             rospy.logdebug('car running autonomously (command ignored)')
00179 
00180     def reconfigure(self, config, level):
00181         "Dynamic reconfigure server callback."
00182         rospy.loginfo('Reconfigure callback, level ' + str(level))
00183         rospy.loginfo('New config ' + str(config))
00184 
00185         if level & SensorLevels.RECONFIGURE_CLOSE:
00186             # create new EstopNavigator and PilotCommand instances 
00187             self.use_navigator = config['use_navigator']
00188             rospy.loginfo('use navigator = ' + str(self.use_navigator))
00189 
00190             self.nav = nav_estop.EstopNavigator(self.use_navigator)
00191 
00192         self.pilot.reconfigure(config['limit_forward'],
00193                                config['limit_reverse'])
00194 
00195         self.config = config
00196         return config
00197 
00198     def setAngle(self, turn):
00199         "set wheel angle"
00200 
00201         # Try various functions with domain [-1..1] to improve
00202         # sensitivity while retaining sign. At higher speeds, it may
00203         # make sense to limit the range, avoiding too-sharp turns and
00204         # providing better control sensitivity.
00205         #turn = math.pow(turn, 3) * ArtVehicle.max_steer_radians
00206         #turn = math.tan(turn) * ArtVehicle.max_steer_radians
00207         if self.pilot.pstate.current.speed == 0:
00208                 percentage = 1
00209         # Expirimental steering over speed preference
00210         #if self.pilot.pstate.current.speed <= 5:
00211         #       percentage = 1
00212         
00213         else:
00214                 #percentage = -0.2738*(math.log(math.fabs(self.pilot.pstate.current.speed))) + 0.6937
00215                 percentage = (-math.atan2(math.fabs(self.pilot.pstate.current.speed)-3, 1) / 1.5) + 1
00216         turn = turn * percentage * ArtVehicle.max_steer_radians
00217 
00218         # ensure maximum wheel angle never exceeded
00219         self.pilot.steer(turn)
00220 
00221         def maxFinder(self):
00222                 if self.pilot.pstate.current.speed > self.config['limit_forward']:
00223                         return self.pilot.state.current
00224                 return self.config['limit_forward']
00225 
00226 joynode = None
00227 
00228 
00229 def main():
00230     global joynode
00231     joynode = JoyNode()
00232     rospy.loginfo('joystick vehicle controller starting')
00233     try:
00234         rospy.spin()
00235     except rospy.ROSInterruptException: pass
00236     rospy.loginfo('joystick vehicle controller finished')
00237 
00238 if __name__ == '__main__':
00239     # run main function and exit
00240     sys.exit(main())
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


art_teleop
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:43:08