joy_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: joy_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 
00016 import pilot_cmd                # ART pilot interface
00017 import nav_estop                # ART navigator E-stop package
00018 
00019 # ROS node setup
00020 import roslib;
00021 roslib.load_manifest(PKG_NAME)
00022 import rospy
00023 
00024 # ROS messages
00025 from art_msgs.msg import ArtVehicle
00026 from art_msgs.msg import Gear
00027 
00028 # In ROS Electric, the Joy message gained a header and moved to
00029 # sensor_msgs.  Use that if it's available.
00030 try:
00031     from sensor_msgs.msg import Joy
00032 except ImportError:
00033     from joy.msg import Joy
00034 
00035 # dynamic parameter reconfiguration
00036 from driver_base.msg import SensorLevels
00037 from dynamic_reconfigure.server import Server as ReconfigureServer
00038 import art_teleop.cfg.JoyConfig as JoyConfig
00039 
00040 def clamp(minimum, value, maximum):
00041     "constrain value to the range [minimum .. maximum]"
00042     return max(minimum, min(value, maximum))
00043 
00044 class JoyNode():
00045     "Vehicle joystick tele-operation node."
00046 
00047     def __init__(self):
00048         "JoyNode constructor"
00049 
00050         # estop controls
00051         self.run = 3                    # start autonomous run
00052         self.suspend = 12               # suspend autonomous running
00053         self.estop = 13                 # emergency stop
00054 
00055         # tele-operation controls
00056         self.steer = 0                  # steering axis (wheel)
00057         self.drive = 4                  # shift to Drive (^)
00058         self.reverse = 6                # shift to Reverse (v)
00059         self.park = 7                   # shift to Park
00060         self.throttle = 18              # throttle axis (X)
00061         self.throttle_start = True
00062         self.brake = 19                 # brake axis (square)
00063         self.brake_start = True
00064 
00065         # initialize ROS topics
00066         rospy.init_node('joy_teleop')
00067         self.pilot = pilot_cmd.PilotCommand()
00068         self.reconf_server = ReconfigureServer(JoyConfig, self.reconfigure)
00069         self.joy = rospy.Subscriber('joy', Joy, self.joyCallback)
00070 
00071     def joyCallback(self, joy):
00072         "invoked every time a joystick message arrives"
00073         rospy.logdebug('joystick input:\n' + str(joy))
00074 
00075         # handle E-stop buttons
00076         if joy.buttons[self.estop]:
00077             rospy.logwarn('emergency stop')
00078             if self.use_navigator:
00079                 self.nav.pause()        # tell navigator to pause
00080             else:
00081                 self.pilot.halt()       # halt car using pilot
00082         elif joy.buttons[self.suspend] and self.use_navigator:
00083             rospy.logwarn('suspend autonomous operation')
00084             self.nav.suspend()
00085         elif joy.buttons[self.run] and self.use_navigator:
00086             rospy.logwarn('start autonomous operation')
00087             self.nav.run()
00088 
00089         # handle shifter buttons
00090         if joy.buttons[self.drive]:
00091             self.pilot.shift(Gear.Drive)
00092             rospy.loginfo('shifting to drive')
00093         elif joy.buttons[self.reverse]:
00094             self.pilot.shift(Gear.Reverse)
00095             rospy.loginfo('shifting to reverse')
00096         elif joy.buttons[self.park]:
00097             self.pilot.shift(Gear.Park)
00098             rospy.loginfo('shifting to park')
00099 
00100         # set steering angle
00101         self.setAngle(joy.axes[self.steer])
00102 
00103         # adjust speed -- the brake and throttle controls both
00104         # return 1.0 when released, -1.0 when completely on
00105         br = joy.axes[self.brake]
00106         th = joy.axes[self.throttle]
00107         rospy.logdebug('joystick brake, throttle: '
00108                        + str(br) + ', ' + str(th))
00109 
00110         # initially the ROS /joy topic sends zeroes for each axis
00111         # until a button press occurs
00112         if self.brake_start:
00113             if br == 0.0:
00114                 br = 1.0
00115             else:
00116                 self.brake_start = False
00117 
00118         if self.throttle_start:
00119             if th == 0.0:
00120                 th = 1.0
00121             else:
00122                 self.throttle_start = False
00123 
00124         # set acceleration from brake and throttle controls
00125         dv = 0.0
00126         if br < 1.0:
00127             dv = br - 1.0
00128         elif th < 1.0:
00129             dv = 1.0 - th
00130         self.pilot.accelerate(dv * self.config['max_accel'])
00131 
00132         if self.nav.is_suspended(): # OK to issue command?
00133             self.pilot.publish()
00134         else:
00135             rospy.logdebug('car running autonomously (command ignored)')
00136 
00137     def reconfigure(self, config, level):
00138         "Dynamic reconfigure server callback."
00139         rospy.loginfo('Reconfigure callback, level ' + str(level))
00140         rospy.loginfo('New config ' + str(config))
00141 
00142         if level & SensorLevels.RECONFIGURE_CLOSE:
00143             # create new EstopNavigator and PilotCommand instances 
00144             self.use_navigator = config['use_navigator']
00145             rospy.loginfo('use navigator = ' + str(self.use_navigator))
00146 
00147             self.nav = nav_estop.EstopNavigator(self.use_navigator)
00148 
00149         self.pilot.reconfigure(config['limit_forward'],
00150                                config['limit_reverse'])
00151 
00152         self.config = config
00153         return config
00154 
00155     def setAngle(self, turn):
00156         "set wheel angle"
00157 
00158         # Try various functions with domain [-1..1] to improve
00159         # sensitivity while retaining sign. At higher speeds, it may
00160         # make sense to limit the range, avoiding too-sharp turns and
00161         # providing better control sensitivity.
00162         turn = math.pow(turn, 3) * ArtVehicle.max_steer_radians
00163         #turn = math.tan(turn) * ArtVehicle.max_steer_radians
00164 
00165         # ensure maximum wheel angle never exceeded
00166         self.pilot.steer(turn)
00167 
00168 joynode = None
00169 
00170 def main():
00171     global joynode
00172     joynode = JoyNode()
00173     rospy.loginfo('joystick vehicle controller starting')
00174     try:
00175         rospy.spin()
00176     except rospy.ROSInterruptException: pass
00177     rospy.loginfo('joystick vehicle controller finished')
00178 
00179 if __name__ == '__main__':
00180     # run main function and exit
00181     sys.exit(main())


art_teleop
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:09:07