$search
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())