jaime_teleop.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: jaime_teleop.py 1633 2011-08-10 15:32:25Z 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 from art_msgs.msg import SteeringState
00029 
00030 # In ROS Electric, the Joy message gained a header and moved to
00031 # sensor_msgs.  Use that if it's available.
00032 try:
00033     from sensor_msgs.msg import Joy
00034 except ImportError:
00035     from joy.msg import Joy
00036 
00037 # dynamic parameter reconfiguration
00038 from driver_base.msg import SensorLevels
00039 from dynamic_reconfigure.server import Server as ReconfigureServer
00040 import art_teleop.cfg.JoyConfig as JoyConfig
00041 
00042 def clamp(minimum, value, maximum):
00043     "constrain value to the range [minimum .. maximum]"
00044     return max(minimum, min(value, maximum))
00045 
00046 class JoyNode():
00047     "Vehicle joystick tele-operation node."
00048 
00049     def __init__(self):
00050         "JoyNode constructor"
00051 
00052         # estop controls
00053         self.run = 3                    # start autonomous run
00054         self.suspend = 12               # suspend autonomous running
00055         self.estop = 13                 # emergency stop
00056 
00057         # tele-operation controls
00058         self.steer = 0                  # steering axis (wheel)
00059         self.drive = 4                  # shift to Drive (^)
00060         self.reverse = 6                # shift to Reverse (v)
00061         self.park = 7                   # shift to Park
00062         self.throttle = 18              # throttle axis (X)
00063         self.throttle_start = True
00064         self.brake = 19                 # brake axis (square)
00065         self.brake_start = True
00066         self.steeringQueue = Queue.Queue()
00067         self.counter = 0
00068 
00069         # initialize ROS topics
00070         rospy.init_node('jaime_teleop')
00071         self.pilot = pilot_cmd.PilotCommand()
00072         self.steering = SteeringState()
00073         self.reconf_server = ReconfigureServer(JoyConfig, self.reconfigure)
00074         self.joy = rospy.Subscriber('joy', Joy, self.joyCallback)
00075 
00076     def joyCallback(self, joy):
00077         "invoked every time a joystick message arrives"
00078         rospy.logdebug('joystick input:\n' + str(joy))
00079 
00080         # handle E-stop buttons
00081         if joy.buttons[self.estop]:
00082             rospy.logwarn('emergency stop')
00083             if self.use_navigator:
00084                 self.nav.pause()        # tell navigator to pause
00085             else:
00086                 self.pilot.halt()       # halt car using pilot
00087         elif joy.buttons[self.suspend] and self.use_navigator:
00088             rospy.logwarn('suspend autonomous operation')
00089             self.nav.suspend()
00090         elif joy.buttons[self.run] and self.use_navigator:
00091             rospy.logwarn('start autonomous operation')
00092             self.nav.run()
00093 
00094         # handle shifter buttons
00095         if joy.buttons[self.drive]:
00096             self.pilot.shift(Gear.Drive)
00097             rospy.loginfo('shifting to drive')
00098         elif joy.buttons[self.reverse]:
00099             self.pilot.shift(Gear.Reverse)
00100             rospy.loginfo('shifting to reverse')
00101         elif joy.buttons[self.park]:
00102             self.pilot.shift(Gear.Park)
00103             rospy.loginfo('shifting to park')
00104 
00105         # set steering angle
00106         self.setAngle(joy.axes[self.steer])
00107 
00108         # adjust speed -- the brake and throttle controls both
00109         # return 1.0 when released, -1.0 when completely on
00110         br = joy.axes[self.brake]
00111         th = joy.axes[self.throttle]
00112         rospy.logdebug('joystick brake, throttle: '
00113                        + str(br) + ', ' + str(th))
00114 
00115         # initially the ROS /joy topic sends zeroes for each axis
00116         # until a button press occurs
00117         if self.brake_start:
00118             if br == 0.0:
00119                 br = 1.0
00120             else:
00121                 self.brake_start = False
00122 
00123         if self.throttle_start:
00124             if th == 0.0:
00125                 th = 1.0
00126             else:
00127                 self.throttle_start = False
00128 
00129         # set acceleration from brake and throttle controls
00130         dv = 0.0
00131         if br < 1.0:
00132             dv = br - 1.0
00133         elif th < 1.0:
00134             dv = 1.0 - th
00135         self.pilot.accelerate(dv * self.config['max_accel'])
00136 
00137         if self.nav.is_suspended(): # OK to issue command?
00138             self.pilot.publish()
00139         else:
00140             rospy.logdebug('car running autonomously (command ignored)')
00141 
00142     def reconfigure(self, config, level):
00143         "Dynamic reconfigure server callback."
00144         rospy.loginfo('Reconfigure callback, level ' + str(level))
00145         rospy.loginfo('New config ' + str(config))
00146 
00147         if level & SensorLevels.RECONFIGURE_CLOSE:
00148             # create new EstopNavigator and PilotCommand instances 
00149             self.use_navigator = config['use_navigator']
00150             rospy.loginfo('use navigator = ' + str(self.use_navigator))
00151 
00152             self.nav = nav_estop.EstopNavigator(self.use_navigator)
00153 
00154         self.pilot.reconfigure(config['limit_forward'],
00155                                config['limit_reverse'])
00156 
00157         self.config = config
00158         return config
00159 
00160     def setAngle(self, turn):
00161         "set wheel angle"
00162 
00163         # Try various functions with domain [-1..1] to improve
00164         # sensitivity while retaining sign. At higher speeds, it may
00165         # make sense to limit the range, avoiding too-sharp turns and
00166         # providing better control sensitivity.
00167         #turn = math.pow(turn, 3) * ArtVehicle.max_steer_radians
00168         
00169         #enqueues first 5 data points based on the first movements on the wheel
00170         if(self.counter < 5):
00171                 self.counter +=1
00172                 self.steeringQueue.put(self.steering.angle/180)
00173         else:
00174                 #dequeue oldest value
00175                 oldVal = self.steeringQueue.get()
00176                 #get rate of change of angle by subtracting final(turn) - initial(oldVal) and dividing by .25s which is time it takes to get 5 values
00177                 currentRate = (self.steering.angle/180 - oldVal)/.25
00178                 limit = 2.0361 * math.pow(.72898, self.pilot.pstate.current.speed)
00179                 rospy.logwarn(str(currentRate) + ' ' + str(limit))
00180                 #if currentRate is above limit, restrict currentRate to the limit at the target velocity, else if it's at or below limit don't do anything
00181                 #get new value of turn by using equation below:
00182                 #(final angle - initial angle)/change time = limit; angle final = limit * change in time + angle inital
00183                 if(math.fabs(currentRate > limit)):
00184                         rospy.logwarn('Success')
00185                         turn = limit*.25 + oldVal
00186                 #put new steering angle into queue
00187 
00188                 self.steeringQueue.put(turn)
00189 
00190 
00191         # ensure maximum wheel angle never exceeded
00192         self.pilot.steer(turn)
00193 
00194 joynode = None
00195 
00196 def main():
00197     global joynode
00198     joynode = JoyNode()
00199     rospy.loginfo('joystick vehicle controller starting')
00200     try:
00201         rospy.spin()
00202     except rospy.ROSInterruptException: pass
00203     rospy.loginfo('joystick vehicle controller finished')
00204 
00205 if __name__ == '__main__':
00206     # run main function and exit
00207     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