00001 from BAL.Devices.KeyboardTeleop import KeyboardTeleop
00002 from BAL.Devices.emergencySwitch import EmergencySwitch
00003 from BAL.Devices.joystickTeleop import JoystickTeleop
00004 from BAL.Devices.launchFile import RosLaunch
00005 from BAL.Devices.rosNode import RosNode
00006 from BAL.Devices.velocitySmoother import VelocitySmoother
00007 from GUI.AboutWindow import About
00008 from GUI.SimulationWindow import SimulationWindow
00009
00010 __author__ = 'tom1231'
00011 import rospkg
00012 import shlex
00013 from xml.etree import ElementTree
00014 from lxml.etree import Element, SubElement
00015 from xml.dom import minidom
00016 from BAL.Devices.CloseLoopTwo import CloseLoopTwo
00017 from BAL.Devices.Battery import Battery
00018 from BAL.Devices.CloseLoop import CloseLoop
00019 from BAL.Devices.DiffClose import DiffClose
00020 from BAL.Devices.DiffCloseFour import DiffCloseFour
00021 from BAL.Devices.DiffOpen import DiffOpen
00022 from BAL.Devices.Gps import Gps
00023 from BAL.Devices.Hokuyo import Hokuyo
00024 from BAL.Devices.Imu import Imu
00025 from BAL.Devices.OpenLoop import OpenLoop
00026 from BAL.Devices.Openni import Opennni
00027 from BAL.Devices.PPMReader import PPMReader
00028 from BAL.Devices.Ppm import Ppm
00029 from BAL.Devices.Relay import Relay
00030 from BAL.Devices.RobotModel import RobotModel
00031 from BAL.Devices.Servo import Servo
00032 from BAL.Devices.Slam import Slam
00033 from BAL.Devices.Switch import Switch
00034 from BAL.Devices.Urf import Urf
00035 from BAL.Devices.UsbCam import UsbCam
00036 from BAL.Interface.DeviceFrame import SERVO, BATTERY, SWITCH, IMU, PPM, GPS, RELAY, URF, CLOSE_LOP_ONE, CLOSE_LOP_TWO, \
00037 OPEN_LOP, DIFF_CLOSE, DIFF_OPEN, EX_DEV, HOKUYO, OPRNNI, USBCAM, DIFF_CLOSE_FOUR, ROBOT_MODEL, SLAM, Keyboard, \
00038 JOYSTICK, SMOOTHER, LAUNCH, NODE, EMERGENCY_SWITCH
00039 from GUI.RemoteLaunch import RemoteLaunch
00040 from GUI.ShowRiCBoard import ShowRiCBoard
00041
00042 from PyQt4.QtGui import *
00043 from Schemes.main import Ui_MainWindow
00044 import webbrowser
00045 import pickle
00046 from os import system, path
00047 import subprocess
00048 import re
00049
00050
00051 def prettify(elem):
00052 """Return a pretty-printed XML string for the Element.
00053 """
00054 rough_string = ElementTree.tostring(elem, 'utf-8')
00055 reparsed = minidom.parseString(rough_string)
00056 return reparsed.toprettyxml(indent=" ")
00057
00058
00059 class MainWindow(QMainWindow, Ui_MainWindow):
00060 def __init__(self, parent=None):
00061 super(MainWindow, self).__init__(parent)
00062 self.setupUi(self)
00063 self.data = []
00064 self.motors = []
00065 self.currentShowDev = None
00066 self.root = Element('launch')
00067
00068 self.actionAbout_RIC.triggered.connect(self.about)
00069 self.actionServo.triggered.connect(self.addServo)
00070 self.actionBattery_Monitor.triggered.connect(self.addBattery)
00071 self.actionSwitch.triggered.connect(self.addSwitch)
00072 self.actionIMU.triggered.connect(self.addImu)
00073 self.actionPPM.triggered.connect(self.addPpm)
00074 self.actionGPS.triggered.connect(self.addGps)
00075 self.actionRelay.triggered.connect(self.addRelay)
00076 self.actionURF.triggered.connect(self.addURF)
00077 self.actionMotor_with_one_encoder.triggered.connect(self.addCloseMotorOne)
00078 self.actionMotor_with_two_encoder.triggered.connect(self.addCloseMotorTwo)
00079 self.actionOpen_Loop.triggered.connect(self.addOpenMotor)
00080 self.actionWith_two_motors.triggered.connect(self.addDiffClose)
00081 self.actionWith_four_motors.triggered.connect(self.addDiffCloseFour)
00082 self.actionOpen_Loop_Drive.triggered.connect(self.addDiffOpen)
00083 self.actionUSB_Camera.triggered.connect(self.addUsbCam)
00084 self.actionOPENNI.triggered.connect(self.addOpenni)
00085 self.actionHakoyo.triggered.connect(self.addOHokuyo)
00086 self.actionSave.triggered.connect(self.save)
00087 self.actionOpen.triggered.connect(self.load)
00088 self.actionNew.triggered.connect(self.new)
00089 self.actionReconfig_RiC_Board.triggered.connect(self.configRiCBoard)
00090 self.actionRobot_Model.triggered.connect(self.addRobotModel)
00091 self.actionAbout_RiC_Board.triggered.connect(self.aboutRiCBoard)
00092 self.actionSLAM.triggered.connect(self.addSLAM)
00093 self.actionRemote_robot_launch.triggered.connect(self.launchRemote)
00094 self.actionPPM_Reader.triggered.connect(self.addPPmReader)
00095 self.actionSet_parameters.triggered.connect(self.paramManager)
00096 self.actionKeyboard.triggered.connect(self.addKeyboard)
00097 self.actionJoystick.triggered.connect(self.addJoystick)
00098 self.actionDifferential_Drive_smoother.triggered.connect(self.addDiffSmooth)
00099 self.actionAbout.triggered.connect(self.showAbout)
00100 self.actionImu_calibration.triggered.connect(self.showImuCalib)
00101
00102 self.actionInclude_ros_launch.triggered.connect(self.addRosLaunch)
00103 self.actionInclude_ros_node.triggered.connect(self.addRosNode)
00104 self.actionEmegency_switch.triggered.connect(self.addEmergencySwitch)
00105
00106 self.fileName.textChanged.connect(self.fileNameEven)
00107 self.nameSpace.textChanged.connect(self.namespaceEven)
00108
00109 self.devList.itemPressed.connect(self.clickListEven)
00110 self.devList.doubleClicked.connect(self.devEdit)
00111
00112 self.servoPorts = QComboBox()
00113 self.servoPorts.addItems([
00114 self.tr('1'),
00115 self.tr('2')
00116 ])
00117
00118 self.switchPorts = QComboBox()
00119 self.switchPorts.addItems([
00120 self.tr('1'),
00121 self.tr('2')
00122 ])
00123
00124 self.relayPorts = QComboBox()
00125 self.relayPorts.addItems([
00126 self.tr('1'),
00127 self.tr('2')
00128 ])
00129
00130 self.urfPorts = QComboBox()
00131 self.urfPorts.addItems([
00132 self.tr('1'),
00133 self.tr('2'),
00134 self.tr('3')
00135 ])
00136
00137 self.encoders = QComboBox()
00138 self.encoders.addItems([
00139 self.tr('1'),
00140 self.tr('2'),
00141 self.tr('3'),
00142 self.tr('4')
00143 ])
00144
00145 self._ns = ''
00146 self._fileName = ''
00147
00148 self.haveBattery = False
00149 self.haveIMU = False
00150 self.havePPM = False
00151 self.haveGPS = False
00152 self.haveCloseLoop = False
00153 self.haveOpenLoop = False
00154 self.haveDiff = False
00155 self.haveReader = False
00156
00157
00158 self.diffEnable = False
00159
00160 self.editMode = False
00161 self.listMode = False
00162 self.newDevMode = False
00163 self.override = True
00164 self.pushButton_2.setEnabled(False)
00165 self.pushButton_2.clicked.connect(self.launch)
00166
00167 allDev = subprocess.check_output(shlex.split("ls /dev"))
00168
00169 conDevs = re.findall('ttyUSB.*', allDev) + re.findall('ttyACM.*', allDev) + re.findall('RiCBoard', allDev)
00170
00171 for dev in conDevs: self.ConPortList.addItem(self.tr(dev))
00172
00173 self.ConPortList.setCurrentIndex(self.ConPortList.count() - 1)
00174
00175 def startSimGUI(self):
00176 pass
00177
00178
00179
00180
00181 def showAbout(self):
00182 dialog = About(self)
00183 dialog.show()
00184 dialog.exec_()
00185
00186 def launchRemote(self):
00187 dialog = RemoteLaunch(self)
00188 dialog.show()
00189 dialog.exec_()
00190
00191 def paramManager(self):
00192 subprocess.Popen(shlex.split('roslaunch ric_board startParamMsg.launch'))
00193
00194 def showImuCalib(self):
00195 subprocess.Popen(shlex.split('roslaunch ric_board startImuCalib.launch'))
00196
00197 def about(self):
00198 webbrowser.open('http://wiki.ros.org/ric_board?distro=indigo')
00199
00200 def aboutRiCBoard(self):
00201 dialog = ShowRiCBoard(self)
00202 dialog.show()
00203 dialog.exec_()
00204
00205 def configRiCBoard(self):
00206 pkg = rospkg.RosPack().get_path('ric_board')
00207 path = QFileDialog.getOpenFileName(self, self.tr("Load File"), "%s/setup" % pkg, self.tr("Hex files (*.hex)"))
00208 exitStatus = system("%s/setup/board_loader --mcu=mk20dx256 -sv %s" % (pkg, path))
00209 if exitStatus > 0:
00210 QMessageBox.critical(self, "Error", "Could not build RiCBoard.")
00211 else:
00212 QMessageBox.information(self, "Done", "Firmware successfully updated.")
00213
00214 def new(self):
00215 self.interruptHandler()
00216 size = self.devList.count()
00217 for i in xrange(size):
00218 self.devList.takeItem(0)
00219 self.data = []
00220 self.motors = []
00221 self.currentShowDev = None
00222 self.root = Element('launch')
00223
00224 self.servoPorts = QComboBox()
00225 self.servoPorts.addItems([
00226 self.tr('1'),
00227 self.tr('2')
00228 ])
00229
00230 self.switchPorts = QComboBox()
00231 self.switchPorts.addItems([
00232 self.tr('1'),
00233 self.tr('2')
00234 ])
00235
00236 self.relayPorts = QComboBox()
00237 self.relayPorts.addItems([
00238 self.tr('1'),
00239 self.tr('2')
00240 ])
00241
00242 self.urfPorts = QComboBox()
00243 self.urfPorts.addItems([
00244 self.tr('1'),
00245 self.tr('2'),
00246 self.tr('3')
00247 ])
00248
00249 self.encoders = QComboBox()
00250 self.encoders.addItems([
00251 self.tr('1'),
00252 self.tr('2'),
00253 self.tr('3'),
00254 self.tr('4')
00255 ])
00256
00257 self._ns = ''
00258 self._fileName = ''
00259
00260 self.haveBattery = False
00261 self.haveIMU = False
00262 self.havePPM = False
00263 self.haveGPS = False
00264 self.haveCloseLoop = False
00265 self.haveOpenLoop = False
00266 self.haveDiff = False
00267 self.haveReader = False
00268
00269
00270 self.diffEnable = False
00271
00272 self.editMode = False
00273 self.listMode = False
00274 self.newDevMode = False
00275 self.override = True
00276 self.pushButton_2.setEnabled(False)
00277
00278 self.fileName.setText(self._fileName)
00279 self.nameSpace.setText(self._ns)
00280
00281 def launch(self):
00282 pkg = rospkg.RosPack().get_path('ric_board')
00283 if path.isfile('%s/DATA/%s.RIC' % (pkg, self._fileName)):
00284 devices = pickle.load(open('%s/DATA/%s.RIC' % (pkg, self._fileName)))[2]
00285 newDevices = []
00286
00287 for dev in self.data:
00288 newDevices.append(dev.toDict())
00289
00290 if devices != newDevices:
00291 ans = QMessageBox.warning(self, "Warning",
00292 "There is some changes in the file, if you don`t save them the 'RiCboard' won`t be able to recognize the changes.\n\nDo you want to save before launch?",
00293 QMessageBox.Yes | QMessageBox.No)
00294 if ans == QMessageBox.Yes:
00295 self.override = True
00296 self.save()
00297 subprocess.Popen(shlex.split("gnome-terminal --command='roslaunch ric_board %s.launch'" % self._fileName))
00298
00299 def load(self):
00300 pkg = rospkg.RosPack().get_path('ric_board')
00301 fileName = QFileDialog.getOpenFileName(self, self.tr("Load File"), "%s/DATA" % pkg, self.tr("RiC File (*.RIC)"))
00302 if fileName != '':
00303 self.new()
00304 self.override = False
00305 load = open(fileName)
00306 data = pickle.load(load)
00307
00308 self._fileName = data[0]
00309 self._ns = data[1]
00310
00311 yaml = open("%s/config/%s.yaml" % (pkg, self._fileName))
00312
00313 conDev = yaml.readline().split(': ')[1][:-1]
00314
00315 allDevs = [str(self.ConPortList.itemText(i)) for i in xrange(self.ConPortList.count())]
00316
00317 for i in xrange(len(allDevs)):
00318 if conDev == allDevs[i]:
00319 self.ConPortList.setCurrentIndex(i)
00320 break
00321
00322 self.nameSpace.setText(self._ns)
00323 self.fileName.setText(self._fileName)
00324
00325 devices = data[2]
00326
00327
00328
00329 for dev in devices:
00330 if dev['type'] == BATTERY:
00331 self.currentShowDev = Battery(self.DevFrame, self.data)
00332 self.currentShowDev.fromDict(dev)
00333 elif dev['type'] == SERVO:
00334 self.currentShowDev = Servo(self.DevFrame, self.data, self.servoPorts)
00335 self.currentShowDev.fromDict(dev)
00336 self.servoPorts.removeItem(self.currentShowDev.findItem())
00337 elif dev['type'] == SWITCH:
00338 self.currentShowDev = Switch(self.DevFrame, self.data, self.switchPorts)
00339 self.currentShowDev.fromDict(dev)
00340 self.switchPorts.removeItem(self.currentShowDev.findItem())
00341 elif dev['type'] == IMU:
00342 self.currentShowDev = Imu(self.DevFrame, self.data)
00343 self.currentShowDev.fromDict(dev)
00344 elif dev['type'] == PPM:
00345 self.currentShowDev = Ppm(self.DevFrame, self.data)
00346 self.currentShowDev.fromDict(dev)
00347 elif dev['type'] == GPS:
00348 self.currentShowDev = Gps(self.DevFrame, self.data)
00349 self.currentShowDev.fromDict(dev)
00350 elif dev['type'] == RELAY:
00351 self.currentShowDev = Relay(self.DevFrame, self.data, self.relayPorts)
00352 self.currentShowDev.fromDict(dev)
00353 self.relayPorts.removeItem(self.currentShowDev.findItem())
00354 elif dev['type'] == URF:
00355 self.currentShowDev = Urf(self.DevFrame, self.data, self.urfPorts)
00356 self.currentShowDev.fromDict(dev)
00357 self.urfPorts.removeItem(self.currentShowDev.findItem())
00358 elif dev['type'] == CLOSE_LOP_ONE:
00359 self.currentShowDev = CloseLoop(self.DevFrame, self.data, self.encoders)
00360 self.currentShowDev.fromDict(dev)
00361 self.encoders.removeItem(self.currentShowDev.findItem())
00362 elif dev['type'] == CLOSE_LOP_TWO:
00363 self.currentShowDev = CloseLoopTwo(self.DevFrame, self.data, self.encoders)
00364 self.currentShowDev.fromDict(dev)
00365 self.encoders.removeItem(self.currentShowDev.findItem())
00366 self.encoders.removeItem(self.currentShowDev.findItem2())
00367 elif dev['type'] == OPEN_LOP:
00368 self.currentShowDev = OpenLoop(self.DevFrame, self.data)
00369 self.currentShowDev.fromDict(dev)
00370 elif dev['type'] == DIFF_CLOSE:
00371 self.currentShowDev = DiffClose(self.DevFrame, self.data, self.motors)
00372 self.currentShowDev.fromDict(dev)
00373 elif dev['type'] == DIFF_CLOSE_FOUR:
00374 self.currentShowDev = DiffCloseFour(self.DevFrame, self.data, self.motors)
00375 self.currentShowDev.fromDict(dev)
00376 elif dev['type'] == DIFF_OPEN:
00377 self.currentShowDev = DiffOpen(self.DevFrame, self.data, self.motors)
00378 self.currentShowDev.fromDict(dev)
00379 elif dev['type'] == HOKUYO:
00380 self.currentShowDev = Hokuyo(self.DevFrame, self.data)
00381 self.currentShowDev.fromDict(dev)
00382 elif dev['type'] == OPRNNI:
00383 self.currentShowDev = Opennni(self.DevFrame, self.data)
00384 self.currentShowDev.fromDict(dev)
00385 elif dev['type'] == USBCAM:
00386 self.currentShowDev = UsbCam(self.DevFrame, self.data)
00387 self.currentShowDev.fromDict(dev)
00388 elif dev['type'] == ROBOT_MODEL:
00389 self.currentShowDev = RobotModel(self.DevFrame, self.data)
00390 self.currentShowDev.fromDict(dev)
00391 elif dev['type'] == SLAM:
00392 self.currentShowDev = Slam(self.DevFrame, self.data)
00393 self.currentShowDev.fromDict(dev)
00394 elif dev['type'] == PPMReader:
00395 self.currentShowDev = PPMReader(self.DevFrame, self.data)
00396 self.currentShowDev.fromDict(dev)
00397 elif dev['type'] == Keyboard:
00398 self.currentShowDev = KeyboardTeleop(self.DevFrame, self.data)
00399 self.currentShowDev.fromDict(dev)
00400 elif dev['type'] == JOYSTICK:
00401 self.currentShowDev = JoystickTeleop(self.DevFrame, self.data)
00402 self.currentShowDev.fromDict(dev)
00403 elif dev['type'] == SMOOTHER:
00404 self.currentShowDev = VelocitySmoother(self.DevFrame, self.data)
00405 self.currentShowDev.fromDict(dev)
00406 elif dev['type'] == LAUNCH:
00407 self.currentShowDev = RosLaunch(self.DevFrame, self.data)
00408 self.currentShowDev.fromDict(dev)
00409 elif dev['type'] == NODE:
00410 self.currentShowDev = RosNode(self.DevFrame, self.data)
00411 self.currentShowDev.fromDict(dev)
00412 elif dev['type'] == EMERGENCY_SWITCH:
00413 self.currentShowDev = EmergencySwitch(self.DevFrame, self.data)
00414 self.currentShowDev.fromDict(dev)
00415
00416 if self.currentShowDev.getDevType() == BATTERY:
00417 self.haveBattery = True
00418 if self.currentShowDev.getDevType() == IMU:
00419 self.haveIMU = True
00420 if self.currentShowDev.getDevType() == PPM:
00421 self.havePPM = True
00422 if self.currentShowDev.getDevType() == GPS:
00423 self.haveGPS = True
00424
00425
00426
00427 if (self.currentShowDev.getDevType() == CLOSE_LOP_ONE) or (
00428 self.currentShowDev.getDevType() == CLOSE_LOP_TWO):
00429 self.haveCloseLoop = True
00430 self.motors.append(self.currentShowDev.getName())
00431 if self.currentShowDev.getDevType() == OPEN_LOP:
00432 self.haveOpenLoop = True
00433 self.motors.append(self.currentShowDev.getName())
00434 if self.currentShowDev.getDevType() == DIFF_CLOSE or self.currentShowDev.getDevType() == DIFF_OPEN or self.currentShowDev.getDevType() == DIFF_CLOSE_FOUR:
00435 self.haveDiff = True
00436 self.diffEnable = True
00437 self.devList.addItem(QListWidgetItem(self.currentShowDev.getName()))
00438 self.data.append(self.currentShowDev)
00439 self.currentShowDev = None
00440 self.pushButton_2.setEnabled(True)
00441
00442 def save(self):
00443 pkg = rospkg.RosPack().get_path('ric_board')
00444 if len(self.data) == 0:
00445 QMessageBox.critical(self, "File error", "Can not save a empty file.")
00446 return
00447 if self._fileName == '':
00448 QMessageBox.critical(self, "File error", "Can not save file without a name.")
00449 return
00450 if not self.override and path.isfile('%s/config/%s.yaml' % (pkg, self._fileName)):
00451 ans = QMessageBox.question(self, "Override", "Do you want to override this file",
00452 QMessageBox.Yes | QMessageBox.No)
00453 if ans == QMessageBox.Yes:
00454 self.override = True
00455 else:
00456 return
00457
00458 parent = self.root
00459 if self._ns != '':
00460 parent = SubElement(self.root, 'group', {'ns': self._ns})
00461 for dev in self.data:
00462 if dev.getDevType() != EX_DEV and dev.isToSave():
00463 at = {
00464 'pkg': 'ric_board',
00465 'type': 'Start.py',
00466 'name': 'RiCTraffic',
00467 'output': 'screen'
00468 }
00469
00470 SubElement(parent, 'node', at)
00471 break
00472 initDiffClose = '0'
00473 initDiffOpen = '0'
00474 initDiffCloseFour = '0'
00475 initIMU = '0'
00476 initGPS = '0'
00477 initPPM = '0'
00478 initBAT = '0'
00479 toSave = open("%s/config/%s.yaml" % (pkg, self._fileName), 'w')
00480 launch = open("%s/launch/%s.launch" % (pkg, self._fileName), 'w')
00481 if str(self.ConPortList.currentText()) != '':
00482 toSave.write("CON_PORT: %s\n" % str(self.ConPortList.currentText()))
00483 else:
00484 toSave.write("CON_PORT: RiCBoard\n")
00485 toSave.write("FILE_NAME: %s\n" % self._fileName)
00486 for dev in self.data:
00487 if dev.isToSave():
00488 if dev.getDevType() == EX_DEV:
00489
00490
00491 dev.saveToFile(parent)
00492 else:
00493 dev.saveToFile(toSave)
00494
00495 if dev.getDevType() == DIFF_OPEN:
00496 initDiffOpen = '1'
00497 elif dev.getDevType() == DIFF_CLOSE:
00498 initDiffClose = '1'
00499 elif dev.getDevType() == DIFF_CLOSE_FOUR:
00500 initDiffCloseFour = '1'
00501 elif dev.getDevType() == IMU:
00502 initIMU = '1'
00503 elif dev.getDevType() == GPS:
00504 initGPS = '1'
00505 elif dev.getDevType() == PPM:
00506 initPPM = '1'
00507 elif dev.getDevType() == BATTERY:
00508 initBAT = '1'
00509
00510 toSave.write('IMU_INIT: ' + initIMU + '\n')
00511 toSave.write('GPS_INIT: ' + initGPS + '\n')
00512 toSave.write('PPM_INIT: ' + initPPM + '\n')
00513 toSave.write('BAT_INIT: ' + initBAT + '\n')
00514 toSave.write('DIFF_INIT: ' + initDiffClose + '\n')
00515 toSave.write('DIFF_INIT_OP: ' + initDiffOpen + '\n')
00516 toSave.write('DIFF_CLOSE_FOUR: ' + initDiffCloseFour + '\n')
00517
00518 toSave.write('closeLoopNum: ' + str(CloseLoop.closeLoop) + '\n')
00519 toSave.write('switchNum: ' + str(Switch.switchCount) + '\n')
00520 toSave.write('servoNum: ' + str(Servo.servoCount) + '\n')
00521 toSave.write('relayNum: ' + str(Relay.relayCount) + '\n')
00522 toSave.write('URFNum: ' + str(Urf.urfCount) + '\n')
00523 toSave.write('openLoopNum: ' + str(OpenLoop.openLoopNum) + '\n')
00524 toSave.write('emergencySwitchNum: ' + str(EmergencySwitch.emergency_switch_count) + '\n')
00525
00526 SubElement(parent, 'rosparam', {
00527 'file': '$(find ric_board)/config/' + self._fileName + '.yaml',
00528 'command': 'load'
00529 })
00530 launch.write(prettify(self.root))
00531
00532 fileData = open('%s/DATA/%s.RIC' % (pkg, self._fileName), 'wb')
00533
00534 ls = []
00535 for dev in self.data:
00536 ls.append(dev.toDict())
00537
00538 pickle.dump([self._fileName, self._ns, ls], fileData)
00539
00540 toSave.close()
00541 launch.close()
00542 self.root = Element('launch')
00543 Servo.servoCount = 0
00544 Relay.relayCount = 0
00545 Urf.urfCount = 0
00546 Switch.switchCount = 0
00547 CloseLoop.closeLoop = 0
00548 OpenLoop.openLoopNum = 0
00549
00550
00551 QMessageBox.information(self, "File Saved", "To launch: $ roslaunch ric_board %s.launch" % self._fileName)
00552 self.pushButton_2.setEnabled(True)
00553
00554 def addEmergencySwitch(self):
00555 self.interruptHandler()
00556 self.newDevMode = True
00557 self.currentShowDev = EmergencySwitch(self.DevFrame, self.data)
00558 self.currentShowDev.showDetails()
00559 self.pushButton.clicked.connect(self.addDevToList)
00560
00561 def addRosNode(self):
00562 self.interruptHandler()
00563 self.newDevMode = True
00564 self.currentShowDev = RosNode(self.DevFrame, self.data)
00565 self.currentShowDev.showDetails()
00566 self.pushButton.clicked.connect(self.addDevToList)
00567
00568 def addRosLaunch(self):
00569 self.interruptHandler()
00570 self.newDevMode = True
00571 self.currentShowDev = RosLaunch(self.DevFrame, self.data)
00572 self.currentShowDev.showDetails()
00573 self.pushButton.clicked.connect(self.addDevToList)
00574
00575 def addDiffSmooth(self):
00576 self.interruptHandler()
00577 self.newDevMode = True
00578 self.currentShowDev = VelocitySmoother(self.DevFrame, self.data)
00579 self.currentShowDev.showDetails()
00580 self.pushButton.clicked.connect(self.addDevToList)
00581
00582 def addJoystick(self):
00583 self.interruptHandler()
00584 self.newDevMode = True
00585 self.currentShowDev = JoystickTeleop(self.DevFrame, self.data)
00586 self.currentShowDev.showDetails()
00587 self.pushButton.clicked.connect(self.addDevToList)
00588
00589 def addKeyboard(self):
00590 self.interruptHandler()
00591 self.newDevMode = True
00592 self.currentShowDev = KeyboardTeleop(self.DevFrame, self.data)
00593 self.currentShowDev.showDetails()
00594 self.pushButton.clicked.connect(self.addDevToList)
00595
00596 def addPPmReader(self):
00597 self.interruptHandler()
00598 self.newDevMode = True
00599 self.currentShowDev = PPMReader(self.DevFrame, self.data)
00600 self.currentShowDev.showDetails()
00601 self.pushButton.clicked.connect(self.addDevToList)
00602
00603 def addSLAM(self):
00604 self.interruptHandler()
00605 self.newDevMode = True
00606 self.currentShowDev = Slam(self.DevFrame, self.data)
00607 self.currentShowDev.showDetails()
00608 self.pushButton.clicked.connect(self.addDevToList)
00609
00610 def addRobotModel(self):
00611 self.interruptHandler()
00612 self.newDevMode = True
00613 self.currentShowDev = RobotModel(self.DevFrame, self.data)
00614 self.currentShowDev.showDetails()
00615 self.pushButton.clicked.connect(self.addDevToList)
00616
00617 def addDiffCloseFour(self):
00618 if not self.haveCloseLoop or len(self.motors) < 4:
00619 QMessageBox.critical(self, "Driver error", "Need to have at less four close loop motors.")
00620 return
00621 if self.haveDiff:
00622 QMessageBox.critical(self, "Driver error", "Can not have more.")
00623 return
00624 self.interruptHandler()
00625 self.newDevMode = True
00626 self.currentShowDev = DiffCloseFour(self.DevFrame, self.data, self.motors)
00627 self.currentShowDev.showDetails()
00628 self.pushButton.clicked.connect(self.addDevToList)
00629
00630 def addOpenni(self):
00631 self.interruptHandler()
00632 self.newDevMode = True
00633 self.currentShowDev = Opennni(self.DevFrame, self.data)
00634 self.currentShowDev.showDetails()
00635 self.pushButton.clicked.connect(self.addDevToList)
00636
00637 def addOHokuyo(self):
00638 self.interruptHandler()
00639 self.newDevMode = True
00640 self.currentShowDev = Hokuyo(self.DevFrame, self.data)
00641 self.currentShowDev.showDetails()
00642 self.pushButton.clicked.connect(self.addDevToList)
00643
00644 def addUsbCam(self):
00645 self.interruptHandler()
00646 self.newDevMode = True
00647 self.currentShowDev = UsbCam(self.DevFrame, self.data)
00648 self.currentShowDev.showDetails()
00649 self.pushButton.clicked.connect(self.addDevToList)
00650
00651 def addDiffOpen(self):
00652 if not self.haveOpenLoop or len(self.motors) < 2:
00653 QMessageBox.critical(self, "Driver error", "Need to have at less two open loop motors.")
00654 return
00655 if self.haveDiff:
00656 QMessageBox.critical(self, "Driver error", "Can not have more.")
00657 return
00658 self.interruptHandler()
00659 self.newDevMode = True
00660 self.currentShowDev = DiffOpen(self.DevFrame, self.data, self.motors)
00661 self.currentShowDev.showDetails()
00662 self.pushButton.clicked.connect(self.addDevToList)
00663
00664 def addDiffClose(self):
00665 if not self.haveCloseLoop or len(self.motors) < 2:
00666 QMessageBox.critical(self, "Driver error", "Need to have at less two close loop motors.")
00667 return
00668 if self.haveDiff:
00669 QMessageBox.critical(self, "Driver error", "Can not have more.")
00670 return
00671 self.interruptHandler()
00672 self.newDevMode = True
00673 self.currentShowDev = DiffClose(self.DevFrame, self.data, self.motors)
00674 self.currentShowDev.showDetails()
00675 self.pushButton.clicked.connect(self.addDevToList)
00676
00677 def addOpenMotor(self):
00678 if self.haveCloseLoop:
00679 QMessageBox.critical(self, "Error", "Open and close motors can not exist in the same configuration.")
00680 return
00681 self.interruptHandler()
00682 self.newDevMode = True
00683 self.currentShowDev = OpenLoop(self.DevFrame, self.data)
00684 self.currentShowDev.showDetails()
00685 self.pushButton.clicked.connect(self.addDevToList)
00686
00687 def addCloseMotorTwo(self):
00688 if self.haveOpenLoop:
00689 QMessageBox.critical(self, "Error", "Open and close motors can not exist in the same configuration.")
00690 return
00691 if self.encoders.count() < 2:
00692 QMessageBox.critical(self, "Close Motor Error", "Need two or more encoder ports to build this motor.")
00693 return
00694 self.interruptHandler()
00695 self.newDevMode = True
00696 self.currentShowDev = CloseLoopTwo(self.DevFrame, self.data, self.encoders)
00697 self.currentShowDev.showDetails()
00698 self.pushButton.clicked.connect(self.addDevToList)
00699
00700 def addCloseMotorOne(self):
00701 if self.haveOpenLoop:
00702 QMessageBox.critical(self, "Error", "Open and close motors can not exist in the same configuration.")
00703 return
00704 if self.encoders.count() == 0:
00705 QMessageBox.critical(self, "Close Motor Error", "Out of encoder ports.")
00706 return
00707 self.interruptHandler()
00708 self.newDevMode = True
00709 self.currentShowDev = CloseLoop(self.DevFrame, self.data, self.encoders)
00710 self.currentShowDev.showDetails()
00711 self.pushButton.clicked.connect(self.addDevToList)
00712
00713 def addURF(self):
00714 if self.urfPorts.count() == 0:
00715 QMessageBox.critical(self, "URF Error", "Out of URF ports.")
00716 return
00717 self.interruptHandler()
00718 self.newDevMode = True
00719 self.currentShowDev = Urf(self.DevFrame, self.data, self.urfPorts)
00720 self.currentShowDev.showDetails()
00721 self.pushButton.clicked.connect(self.addDevToList)
00722
00723 def addBattery(self):
00724 if self.haveBattery:
00725 QMessageBox.critical(self, "Battery Error", "Can't add another battery to ric board.")
00726 return
00727 self.interruptHandler()
00728 self.newDevMode = True
00729 self.currentShowDev = Battery(self.DevFrame, self.data)
00730 self.currentShowDev.showDetails()
00731 self.pushButton.clicked.connect(self.addDevToList)
00732
00733 def addServo(self):
00734 if self.servoPorts.count() == 0:
00735 QMessageBox.critical(self, "Servo Error", "Out of servo ports.")
00736 return
00737 self.interruptHandler()
00738 self.newDevMode = True
00739 self.currentShowDev = Servo(self.DevFrame, self.data, self.servoPorts)
00740 self.currentShowDev.showDetails()
00741 self.pushButton.clicked.connect(self.addDevToList)
00742
00743 def addSwitch(self):
00744 if self.switchPorts.count() == 0:
00745 QMessageBox.critical(self, "Switch Error", "Out of switch ports.")
00746 return
00747 self.interruptHandler()
00748 self.newDevMode = True
00749 self.currentShowDev = Switch(self.DevFrame, self.data, self.switchPorts)
00750 self.currentShowDev.showDetails()
00751 self.pushButton.clicked.connect(self.addDevToList)
00752
00753 def addImu(self):
00754 if self.haveIMU:
00755 QMessageBox.critical(self, "IMU Error", "Can't add another IMU to ric board.")
00756 return
00757 self.interruptHandler()
00758 self.newDevMode = True
00759 self.currentShowDev = Imu(self.DevFrame, self.data)
00760 self.currentShowDev.showDetails()
00761 self.pushButton.clicked.connect(self.addDevToList)
00762
00763 def addPpm(self):
00764 if self.havePPM:
00765 QMessageBox.critical(self, "PPM Error", "Can't add another PPM to ric board.")
00766 return
00767 self.interruptHandler()
00768 self.newDevMode = True
00769 self.currentShowDev = Ppm(self.DevFrame, self.data)
00770 self.currentShowDev.showDetails()
00771 self.pushButton.clicked.connect(self.addDevToList)
00772
00773 def addGps(self):
00774 if self.haveGPS:
00775 QMessageBox.critical(self, "GPS Error", "Can't add another GPS to ric board.")
00776 return
00777 self.interruptHandler()
00778 self.newDevMode = True
00779 self.currentShowDev = Gps(self.DevFrame, self.data)
00780 self.currentShowDev.showDetails()
00781 self.pushButton.clicked.connect(self.addDevToList)
00782
00783 def addRelay(self):
00784 if self.relayPorts.count() == 0:
00785 QMessageBox.critical(self, "Relay Error", "Out of relay ports.")
00786 return
00787 self.interruptHandler()
00788 self.newDevMode = True
00789 self.currentShowDev = Relay(self.DevFrame, self.data, self.relayPorts)
00790 self.currentShowDev.showDetails()
00791 self.pushButton.clicked.connect(self.addDevToList)
00792
00793 def clickListEven(self):
00794 self.interruptHandler()
00795 index = self.devList.currentRow()
00796 self.listMode = True
00797 self.currentShowDev = self.data[index]
00798 self.currentShowDev.printDetails()
00799
00800 self.Edit.clicked.connect(self.devEdit)
00801 self.Delete.clicked.connect(self.devDelete)
00802 self.saveStatus.clicked.connect(self.changeDevSaveStatus)
00803
00804 def changeDevSaveStatus(self):
00805 self.interruptHandler()
00806 index = self.devList.currentRow()
00807 self.currentShowDev = self.data[index]
00808
00809 if self.currentShowDev.getDevType() in [CLOSE_LOP_ONE, CLOSE_LOP_TWO, OPEN_LOP] and self.diffEnable:
00810 QMessageBox.critical(self, "Error", "Can't disable a motor while differential drive is present")
00811 return
00812
00813 motorCount = 0
00814 if self.currentShowDev.getDevType() in [DIFF_CLOSE, DIFF_OPEN, DIFF_CLOSE_FOUR] and not self.diffEnable:
00815 for dev in self.data:
00816 if dev.getDevType() in [CLOSE_LOP_ONE, CLOSE_LOP_TWO, OPEN_LOP] and dev.isToSave(): motorCount += 1
00817
00818 if (motorCount <= 1) or (motorCount < 4 and self.currentShowDev.getDevType() == CLOSE_LOP_ONE):
00819 QMessageBox.critical(self, "Error", "Can't enable differential drive motor while motors are disable")
00820 return
00821
00822 if self.currentShowDev.getDevType() in [DIFF_CLOSE, DIFF_OPEN, DIFF_CLOSE_FOUR]:
00823 self.diffEnable = not self.currentShowDev.isToSave()
00824
00825 if self.currentShowDev.isToSave():
00826 self.devList.item(index).setForeground(QColor(255, 0, 0))
00827 self.currentShowDev.setToSave(False)
00828 else:
00829 self.devList.item(index).setForeground(QColor(0, 0, 0))
00830 self.currentShowDev.setToSave(True)
00831
00832 def namespaceEven(self, text):
00833 self._ns = str(text)
00834
00835 def fileNameEven(self, text):
00836 self._fileName = str(text)
00837 self.pushButton_2.setEnabled(False)
00838
00839 def devDelete(self):
00840 if self.currentShowDev.getDevType() == SERVO:
00841 self.servoPorts.addItem(self.currentShowDev.getPort())
00842 if self.currentShowDev.getDevType() == BATTERY:
00843 self.haveBattery = False
00844 if self.currentShowDev.getDevType() == SWITCH:
00845 self.switchPorts.addItem(self.currentShowDev.getPort())
00846 if self.currentShowDev.getDevType() == IMU:
00847 self.haveIMU = False
00848 if self.currentShowDev.getDevType() == PPM:
00849 self.havePPM = False
00850 if self.currentShowDev.getDevType() == GPS:
00851 self.haveGPS = False
00852 if self.currentShowDev.getDevType() == RELAY:
00853 self.relayPorts.addItem(self.currentShowDev.getPort())
00854
00855 if self.currentShowDev.getDevType() == URF:
00856 self.urfPorts.addItem(self.currentShowDev.getPort())
00857 if self.currentShowDev.getDevType() == CLOSE_LOP_ONE:
00858 if self.haveDiff:
00859 QMessageBox.critical(self, "Error", "Can't delete a motor while differential drive is present")
00860 return
00861 self.motors.remove(self.currentShowDev.getName())
00862 self.encoders.addItem(self.currentShowDev.getEncoder())
00863 if self.currentShowDev.getDevType() == CLOSE_LOP_TWO:
00864 if self.haveDiff:
00865 QMessageBox.critical(self, "Error", "Can't delete a motor while differential drive is present")
00866 return
00867 self.motors.remove(self.currentShowDev.getName())
00868 self.encoders.addItem(self.currentShowDev.getEncoders()[0])
00869 self.encoders.addItem(self.currentShowDev.getEncoders()[1])
00870 if self.currentShowDev.getDevType() == OPEN_LOP:
00871 if self.haveDiff:
00872 QMessageBox.critical(self, "Error", "Can't delete a motor while differential drive is present")
00873 return
00874 self.motors.remove(self.currentShowDev.getName())
00875 if self.currentShowDev.getDevType() == DIFF_CLOSE or self.currentShowDev.getDevType() == DIFF_OPEN or self.currentShowDev.getDevType() == DIFF_CLOSE_FOUR:
00876 self.haveDiff = False
00877 self.diffEnable = False
00878
00879 self.data.remove(self.currentShowDev)
00880 self.devList.takeItem(self.devList.currentRow())
00881 self.removeAllFields()
00882
00883 if len(self.motors) == 0 and self.haveCloseLoop: self.haveCloseLoop = False
00884 if len(self.motors) == 0 and self.haveOpenLoop: self.haveOpenLoop = False
00885
00886 self.Edit.clicked.disconnect(self.devEdit)
00887 self.Delete.clicked.disconnect(self.devDelete)
00888 self.listMode = False
00889
00890 def devEdit(self):
00891 self.interruptHandler()
00892 self.editMode = True
00893 if self.currentShowDev.getDevType() == SERVO:
00894 self.servoPorts.addItem(self.currentShowDev.getPort())
00895 if self.currentShowDev.getDevType() == SWITCH:
00896 self.switchPorts.addItem(self.currentShowDev.getPort())
00897 if self.currentShowDev.getDevType() == RELAY:
00898 self.relayPorts.addItem(self.currentShowDev.getPort())
00899 if self.currentShowDev.getDevType() == URF:
00900 self.urfPorts.addItem(self.currentShowDev.getPort())
00901 if self.currentShowDev.getDevType() == CLOSE_LOP_ONE:
00902 self.encoders.addItem(self.currentShowDev.getEncoder())
00903 if self.currentShowDev.getDevType() == CLOSE_LOP_TWO:
00904 self.encoders.addItem(self.currentShowDev.getEncoders()[0])
00905 self.encoders.addItem(self.currentShowDev.getEncoders()[1])
00906
00907 self.currentShowDev.showDetails()
00908 self.pushButton.clicked.connect(self.editList)
00909
00910 def removeAllFields(self):
00911 for i in xrange(self.DevFrame.layout().count()):
00912 self.DevFrame.layout().itemAt(i).widget().deleteLater()
00913
00914 def interruptHandler(self):
00915 self.removeAllFields()
00916 if self.listMode:
00917 self.Edit.clicked.disconnect(self.devEdit)
00918 self.Delete.clicked.disconnect(self.devDelete)
00919 self.saveStatus.clicked.disconnect(self.changeDevSaveStatus)
00920 self.listMode = False
00921 if self.editMode:
00922 self.pushButton.clicked.disconnect(self.editList)
00923 if self.currentShowDev.getDevType() == SERVO:
00924 self.servoPorts.removeItem(self.currentShowDev.findItem())
00925 if self.currentShowDev.getDevType() == SWITCH:
00926 self.switchPorts.removeItem(self.currentShowDev.findItem())
00927 if self.currentShowDev.getDevType() == RELAY:
00928 self.relayPorts.removeItem(self.currentShowDev.findItem())
00929 if self.currentShowDev.getDevType() == URF:
00930 self.urfPorts.removeItem(self.currentShowDev.findItem())
00931 if self.currentShowDev.getDevType() == CLOSE_LOP_ONE:
00932 self.encoders.removeItem(self.currentShowDev.findItem())
00933 if self.currentShowDev.getDevType() == CLOSE_LOP_TWO:
00934 self.encoders.removeItem(self.currentShowDev.findItem())
00935 self.encoders.removeItem(self.currentShowDev.findItem2())
00936 self.editMode = False
00937 if self.newDevMode:
00938 self.pushButton.clicked.disconnect(self.addDevToList)
00939 self.newDevMode = False
00940
00941 def removeFields(self):
00942 if self.currentShowDev.isValid():
00943 self.removeAllFields()
00944 self.pushButton.clicked.disconnect(self.removeFields)
00945 self.newDevMode = False
00946
00947 def addDevToList(self):
00948 self.currentShowDev.add()
00949
00950 if self.currentShowDev.isValid():
00951 if self.currentShowDev.getDevType() == BATTERY:
00952 self.haveBattery = True
00953 if self.currentShowDev.getDevType() == IMU:
00954 self.haveIMU = True
00955 if self.currentShowDev.getDevType() == PPM:
00956 self.havePPM = True
00957 if self.currentShowDev.getDevType() == GPS:
00958 self.haveGPS = True
00959 if (self.currentShowDev.getDevType() == CLOSE_LOP_ONE) or (
00960 self.currentShowDev.getDevType() == CLOSE_LOP_TWO):
00961 self.haveCloseLoop = True
00962 self.motors.append(self.currentShowDev.getName())
00963 if self.currentShowDev.getDevType() == OPEN_LOP:
00964 self.haveOpenLoop = True
00965 self.motors.append(self.currentShowDev.getName())
00966 if self.currentShowDev.getDevType() == DIFF_CLOSE or self.currentShowDev.getDevType() == DIFF_OPEN or self.currentShowDev.getDevType() == DIFF_CLOSE_FOUR:
00967 self.haveDiff = True
00968 self.diffEnable = True
00969
00970 self.devList.addItem(QListWidgetItem(self.currentShowDev.getName()))
00971 self.data.append(self.currentShowDev)
00972 self.removeAllFields()
00973 self.currentShowDev = None
00974 self.pushButton.clicked.disconnect(self.addDevToList)
00975 self.newDevMode = False
00976
00977 def editList(self):
00978 oldName = self.currentShowDev.getName()
00979 self.currentShowDev.add()
00980 if self.currentShowDev.isValid():
00981 self.devList.currentItem().setText(self.currentShowDev.getName())
00982 self.data[self.devList.currentRow()] = self.currentShowDev
00983 if self.currentShowDev.getDevType() == OPEN_LOP or self.currentShowDev.getDevType() == CLOSE_LOP_ONE or self.currentShowDev.getDevType() == CLOSE_LOP_TWO:
00984 for i in xrange(len(self.motors)):
00985 if self.motors[i] == oldName:
00986 self.motors[i] = self.currentShowDev.getName()
00987 break
00988 self.removeAllFields()
00989 self.currentShowDev = None
00990 self.pushButton.clicked.disconnect(self.editList)
00991 self.editMode = False