art_teleop: teleop.py Source File

00001 #!/usr/bin/python
00002 #
00003 # Qt python script to send tele-operation commands to pilot
00004 #
00005 #   Copyright (C) 2009, 2011 Austin Robot Technology
00006 #   License: Modified BSD Software License Agreement
00007 #
00008 # $Id: teleop.py 1541 2011-05-09 19:07:23Z jack.oquin $
00009 
00010 PKG_NAME = 'art_teleop'
00011 
00012 import sys
00013 import os
00014 import math
00015 import threading
00016 
00017 from PyQt4 import QtGui
00018 from PyQt4 import QtCore
00019 
00020 import roslib;
00021 roslib.load_manifest(PKG_NAME)
00022 import rospy
00023 
00024 from art_msgs.msg import ArtVehicle
00025 from art_msgs.msg import CarDrive
00026 from art_msgs.msg import CarDriveStamped
00027 from art_msgs.msg import Epsilon
00028 from art_msgs.msg import Gear
00029 
00030 g_topic = rospy.Publisher('pilot/drive', CarDriveStamped)
00031 rospy.init_node('teleop')
00032 
00033 # set path name for resolving icons
00034 icons_path = os.path.join(roslib.packages.get_pkg_dir(PKG_NAME),
00035                           os.path.join("icons", "teleop"))
00036 
00037 def find_icon(dir, basename, extlist=['.svg', '.png']):
00038     """Find icon file with basename in dir.
00039 
00040     extlist: a list of possible extensions
00041     returns: full path name if file found, None otherwise
00042 """
00043     for ext in extlist:
00044         pathname = os.path.join(dir, basename + ext)
00045         if os.path.exists(pathname):
00046             return pathname
00047     return None
00048 
00049 def pkg_icon(name):
00050     """Resolve package-relative icon name to path name.
00051     """
00052     pname = find_icon(icons_path, name)
00053     if pname == None:
00054         raise NameError, 'icon file not found'
00055     return pname
00056 
00057 
00058 class MainWindow(QtGui.QMainWindow):
00059     "Create vehicle tele-operation control window."
00060 
00061     def __init__(self, topic):
00062         QtGui.QMainWindow.__init__(self)
00063 
00064         self.topic = topic
00065 
00066         self.resize(350, 250)
00067         self.setIconSize(QtCore.QSize(32,32))
00068         self.setWindowTitle('Vehicle Tele-Operation')
00069 
00070         qexit = QtGui.QAction(QtGui.QIcon(pkg_icon('exit')), 'Exit', self)
00071         qexit.setShortcut('Ctrl+Q')
00072         self.connect(qexit, QtCore.SIGNAL('triggered()'), QtCore.SLOT('close()'))
00073 
00074         center_wheel = QtGui.QAction(QtGui.QIcon(pkg_icon('go-first')),
00075                             'center wheel', self)
00076         center_wheel.setShortcut(QtCore.Qt.Key_Home)
00077         self.connect(center_wheel, QtCore.SIGNAL('triggered()'),
00078                      self.center_wheel)
00079 
00080         go_left = QtGui.QAction(QtGui.QIcon(pkg_icon('go-previous')),
00081                             'steer left', self)
00082         go_left.setShortcut(QtCore.Qt.Key_Left)
00083         self.connect(go_left, QtCore.SIGNAL('triggered()'), self.go_left)
00084 
00085         go_left_more = QtGui.QAction('steer left more', self)
00086         go_left_more.setShortcut(QtGui.QKeySequence.SelectPreviousWord)
00087         self.connect(go_left_more, QtCore.SIGNAL('triggered()'),
00088                      self.go_left_more)
00089 
00090         go_left_less = QtGui.QAction('steer left less', self)
00091         go_left_less.setShortcut(QtGui.QKeySequence.MoveToPreviousWord)
00092         self.connect(go_left_less, QtCore.SIGNAL('triggered()'),
00093                      self.go_left_less)
00094 
00095         slow_down = QtGui.QAction(QtGui.QIcon(pkg_icon('go-down')),
00096                             'slow down', self)
00097         slow_down.setShortcut(QtCore.Qt.Key_Down)
00098         self.connect(slow_down, QtCore.SIGNAL('triggered()'), self.slow_down)
00099 
00100         speed_up = QtGui.QAction(QtGui.QIcon(pkg_icon('go-up')),
00101                               'speed up', self)
00102         speed_up.setShortcut(QtCore.Qt.Key_Up)
00103         self.connect(speed_up, QtCore.SIGNAL('triggered()'), self.speed_up)
00104 
00105         go_right = QtGui.QAction(QtGui.QIcon(pkg_icon('go-next')),
00106                               'steer right', self)
00107         go_right.setShortcut(QtCore.Qt.Key_Right)
00108         self.connect(go_right, QtCore.SIGNAL('triggered()'), self.go_right)
00109 
00110         go_right_less = QtGui.QAction('steer right less', self)
00111         go_right_less.setShortcut(QtGui.QKeySequence.MoveToNextWord)
00112         self.connect(go_right_less, QtCore.SIGNAL('triggered()'),
00113                      self.go_right_less)
00114 
00115         go_right_more = QtGui.QAction('steer right more', self)
00116         go_right_more.setShortcut(QtGui.QKeySequence.SelectNextWord)
00117         self.connect(go_right_more, QtCore.SIGNAL('triggered()'),
00118                      self.go_right_more)
00119 
00120         stop_car = QtGui.QAction(QtGui.QIcon(pkg_icon('go-bottom')),
00121                                  'stop car', self)
00122         stop_car.setShortcut(QtCore.Qt.Key_End)
00123         self.connect(stop_car, QtCore.SIGNAL('triggered()'), self.stop_car)
00124 
00125 
00126         menubar = self.menuBar()
00127         mfile = menubar.addMenu('&File')
00128         mfile.addAction(qexit)
00129 
00130         speed = menubar.addMenu('&Speed')
00131         speed.addAction(slow_down)
00132         speed.addAction(stop_car)
00133         speed.addAction(speed_up)
00134 
00135         wheel = menubar.addMenu('&Wheel')
00136         wheel.addAction(center_wheel)
00137         wheel.addAction(go_left)
00138         wheel.addAction(go_left_less)
00139         wheel.addAction(go_left_more)
00140         wheel.addAction(go_right)
00141         wheel.addAction(go_right_less)
00142         wheel.addAction(go_right_more)
00143 
00144         toolbar = self.addToolBar('Controls')
00145         toolbar.addAction(qexit)
00146         toolbar.addAction(center_wheel)
00147         toolbar.addAction(go_left)
00148         toolbar.addAction(slow_down)
00149         toolbar.addAction(stop_car)
00150         toolbar.addAction(speed_up)
00151         toolbar.addAction(go_right)
00152 
00153         self.car_msg = CarDriveStamped()
00154         self.car_msg.header.stamp = rospy.Time.now()
00155         self.car_ctl = CarDrive()
00156         self.car_msg.control = self.car_ctl
00157         self.topic.publish(self.car_msg)
00158 
00159         self.updateStatusBar()
00160 
00161     def updateStatusBar(self):
00162         self.statusBar().showMessage('speed: '
00163                                      + str(self.car_ctl.speed)
00164                                      + ' m/s,    angle: '
00165                                      + str(math.degrees(self.car_ctl.steering_angle))
00166                                      + ' deg')
00167 
00168     def adjustCarCmd(self, v, a):
00169         """adjust pilot CarDrive command
00170 
00171               @param v change in velocity (m/s)
00172               @param a change in steering angle (radians)
00173         """
00174 
00175         self.car_ctl.speed += v
00176         if abs(self.car_ctl.speed) > Epsilon.speed:
00177             if self.car_ctl.speed < 0.0:
00178                 self.car_ctl.gear.value = Gear.Reverse
00179             else:
00180                 self.car_ctl.gear.value = Gear.Drive
00181 
00182         self.car_ctl.steering_angle += a
00183         if self.car_ctl.steering_angle > ArtVehicle.max_steer_radians:
00184             self.car_ctl.steering_angle = ArtVehicle.max_steer_radians
00185         if self.car_ctl.steering_angle < -ArtVehicle.max_steer_radians:
00186             self.car_ctl.steering_angle = -ArtVehicle.max_steer_radians
00187 
00188         self.car_msg.header.stamp = rospy.Time.now()
00189         self.car_msg.control = self.car_ctl
00190         self.topic.publish(self.car_msg)
00191 
00192         self.updateStatusBar()
00193 
00194     def center_wheel(self):
00195         "center steering wheel"
00196         self.adjustCarCmd(0.0, -self.car_ctl.steering_angle)
00197 
00198     def go_left(self):
00199         "steer left"
00200         self.adjustCarCmd(0.0, math.radians(1.0))
00201 
00202     def go_left_more(self):
00203         "steer more to left"
00204         self.adjustCarCmd(0.0, math.radians(4.0))
00205 
00206     def go_left_less(self):
00207         "steer a little to left"
00208         self.adjustCarCmd(0.0, math.radians(0.25))
00209 
00210     def go_right(self):
00211         "steer right"
00212         self.adjustCarCmd(0.0, math.radians(-1.0))
00213 
00214     def go_right_more(self):
00215         "steer more to right"
00216         self.adjustCarCmd(0.0, math.radians(-4.0))
00217 
00218     def go_right_less(self):
00219         "steer far to right"
00220         self.adjustCarCmd(0.0, math.radians(-0.25))
00221 
00222     def slow_down(self):
00223         "go one m/s slower"
00224         self.adjustCarCmd(-1.0, 0.0)    # one m/s slower
00225 
00226     def speed_up(self):
00227         "go one m/s faster"
00228         self.adjustCarCmd(1.0, 0.0)     # one m/s faster
00229 
00230     def stop_car(self):
00231         "stop car immediately"
00232         self.adjustCarCmd(-self.car_ctl.speed, 0.0)
00233 
00234 
00235 class QtThread(threading.Thread):
00236 
00237     def __init__(self, topic):
00238         self.topic = topic
00239         threading.Thread.__init__(self)
00240 
00241     def run(self):
00242         # run the program
00243         app = QtGui.QApplication(sys.argv)
00244         
00245         teleop = MainWindow(self.topic)
00246         teleop.show()
00247 
00248         # run Qt main loop until window terminates
00249         exit_status = app.exec_()
00250 
00251         rospy.signal_shutdown('Qt exit')
00252 
00253         sys.exit(exit_status)
00254 
00255 
00256 if __name__ == '__main__':
00257 
00258     #rospy.loginfo('starting tele-operation')
00259 
00260     try:
00261         QtThread(g_topic).start()
00262         rospy.spin()
00263     except rospy.ROSInterruptException: pass
00264 
00265     #rospy.loginfo('tele-operation completed')