00001
00002 '''
00003 Created on Sep 10, 2013
00004 \todo Add license information here
00005 @author: dnad
00006 '''
00007 import os, sys
00008
00009 import rospy
00010 import labust_gui
00011 import math
00012
00013 from python_qt_binding import loadUi,QtCore, QtGui
00014 from qt_gui.plugin import Plugin
00015
00016 from std_msgs.msg import String
00017 from std_msgs.msg import Bool
00018 from auv_msgs.msg import Bool6Axis
00019 from navcon_msgs.srv import EnableControl
00020 from navcon_msgs.srv import ConfigureAxes
00021 from labust_uvapp.srv import ConfigureVelocityController
00022
00023 class SimManGui(QtGui.QWidget):
00024 def __init__(self):
00025
00026 QtGui.QWidget.__init__(self)
00027
00028 self.manualSelection = 0;
00029 self.movebaseSelection = 2;
00030
00031 pass
00032
00033 def setup(self, name, ros):
00034 self.setObjectName(name)
00035 self.setWindowTitle(name)
00036 self._connect_signals(ros)
00037 self._booststrap()
00038 pass
00039
00040 def unload(self):
00041 pass
00042
00043 def onStateHat(self,data):
00044 self.statusNorth.setText("{:.2f}".format(data.position.north))
00045 self.statusEast.setText("{:.2f}".format(data.position.east))
00046 self.statusDepth.setText("{:.2f}".format(data.position.depth))
00047 hdg = math.degrees(data.orientation.yaw)
00048 if hdg < 0: hdg += 360
00049 self.statusHeading.setText("{:.2f}".format(hdg))
00050 self.statusAltitude.setText("{:.2f}".format(data.altitude))
00051 self.statusRoll.setText("{:.2f}".format(math.degrees(data.orientation.roll)))
00052 self.statusPitch.setText("{:.2f}".format(math.degrees(data.orientation.pitch)))
00053
00054 self._update_refs(data)
00055
00056 pass
00057
00058 def _connect_signals(self, ros):
00059 QtCore.QObject.connect(self, QtCore.SIGNAL("onHeadingEnable"), ros.onHeadingEnable)
00060 self.headingEnable.toggled.connect(lambda state:
00061 self.emit(QtCore.SIGNAL("onHeadingEnable"),
00062 state,
00063 self.headingRef.value()))
00064 QtCore.QObject.connect(self, QtCore.SIGNAL("onManualEnable"), ros.onManualEnable)
00065 self.manualEnable.toggled.connect(lambda state:
00066 self.emit(QtCore.SIGNAL("onManualEnable"),
00067 state, self.manualSelection))
00068 QtCore.QObject.connect(self, QtCore.SIGNAL("onDepthEnable"), ros.onDepthEnable)
00069 self.depthEnable.toggled.connect(self._onDepthEnable)
00070 QtCore.QObject.connect(self, QtCore.SIGNAL("onAltitudeEnable"), ros.onAltitudeEnable)
00071 self.altitudeEnable.toggled.connect(self._onAltitudeEnable)
00072 QtCore.QObject.connect(self, QtCore.SIGNAL("onDPEnable"), ros.onDPEnable)
00073 self.dpEnable.toggled.connect(self._onDpEnable)
00074 QtCore.QObject.connect(self, QtCore.SIGNAL("onPitchEnable"), ros.onPitchEnable)
00075 self.pitchEnable.toggled.connect(lambda state:
00076 self.emit(QtCore.SIGNAL("onPitchEnable"),
00077 state,
00078 self.pitchRef.value()))
00079
00080
00081 QtCore.QObject.connect(self, QtCore.SIGNAL("onHeadingUpdate"), ros.onHeadingUpdate)
00082 self.headingRef.editingFinished.connect(lambda:
00083 self.emit(QtCore.SIGNAL("onHeadingUpdate"),
00084 self.headingRef.value()))
00085 QtCore.QObject.connect(self, QtCore.SIGNAL("onDepthUpdate"), ros.onDepthUpdate)
00086 self.depthRef.editingFinished.connect(lambda:
00087 self.emit(QtCore.SIGNAL("onDepthUpdate"),
00088 self.depthRef.value()))
00089 QtCore.QObject.connect(self, QtCore.SIGNAL("onAltitudeUpdate"), ros.onAltitudeUpdate)
00090 self.altitudeRef.editingFinished.connect(lambda:
00091 self.emit(QtCore.SIGNAL("onAltitudeUpdate"),
00092 self.altitudeRef.value()))
00093
00094 QtCore.QObject.connect(self, QtCore.SIGNAL("onMoveBaseRef"), ros.onMoveBaseRef)
00095
00096 self.Stop.clicked.connect(self._onStop)
00097
00098 self.tauRadio.toggled.connect(self._onJoystickRadio)
00099 self.nuRadio.toggled.connect(self._onJoystickRadio)
00100 self.relRadio.toggled.connect(self._onJoystickRadio)
00101
00102 self.absRadio.toggled.connect(self._onMoveBaseRadio)
00103 self.nePosRadio.toggled.connect(self._onMoveBaseRadio)
00104 self.relPosRadio.toggled.connect(self._onMoveBaseRadio)
00105 self.northRef.editingFinished.connect(self._onMoveBaseRadio)
00106 self.eastRef.editingFinished.connect(self._onMoveBaseRadio)
00107
00108 def _onStop(self):
00109 self.dpEnable.setChecked(False)
00110 self.headingEnable.setChecked(False)
00111 self.depthEnable.setChecked(False)
00112 self.manualEnable.setChecked(False)
00113
00114 def _onJoystickRadio(self):
00115 if self.tauRadio.isChecked(): self.manualSelection = 0;
00116 if self.nuRadio.isChecked(): self.manualSelection = 1;
00117 if self.relRadio.isChecked(): self.manualSelection = 2;
00118 self.emit(QtCore.SIGNAL("onManualEnable"),
00119 self.manualEnable.isChecked(),
00120 self.manualSelection)
00121
00122 def _onMoveBaseRadio(self):
00123 if self.absRadio.isChecked(): self.movebaseSelection = 0;
00124 if self.nePosRadio.isChecked(): self.movebaseSelection = 1;
00125 if self.relPosRadio.isChecked(): self.movebaseSelection = 2;
00126 self.emit(QtCore.SIGNAL("onMoveBaseRef"),
00127 self.northRef.value(),
00128 self.eastRef.value(),
00129 self.movebaseSelection)
00130
00131 def _onDpEnable(self, state):
00132 self.emit(QtCore.SIGNAL("onDPEnable"), state)
00133 self.emit(QtCore.SIGNAL("onMoveBaseRef"),
00134 self.northRef.value(),
00135 self.eastRef.value(),
00136 self.movebaseSelection)
00137
00138 def _onAltitudeEnable(self, state):
00139
00140 if self.depthEnable.isChecked():
00141 self.emit(QtCore.SIGNAL("onDepthEnable"), False, 0)
00142
00143 self.emit(QtCore.SIGNAL("onAltitudeEnable"), state, self.altitudeRef.value())
00144
00145 def _onDepthEnable(self, state):
00146
00147 if self.altitudeEnable.isChecked():
00148 self.emit(QtCore.SIGNAL("onAltitudeEnable"), False, 0)
00149
00150 self.emit(QtCore.SIGNAL("onDepthEnable"), state, self.depthRef.value())
00151
00152
00153 def _update_refs(self,data):
00154 if not self.headingEnable.isChecked():
00155 hdg = math.degrees(data.orientation.yaw)
00156 if hdg < 0: hdg += 360
00157 self.headingRef.setValue(hdg)
00158 if not self.depthEnable.isChecked():
00159 self.depthRef.setValue(data.position.depth)
00160 if not self.pitchEnable.isChecked():
00161 self.pitchRef.setValue(math.degrees(data.orientation.pitch))
00162 if not self.altitudeEnable.isChecked():
00163 self.altitudeRef.setValue(data.altitude)
00164
00165 def _booststrap(self):
00166 self.headingEnable.setChecked(False)
00167 self.depthEnable.setChecked(False)
00168
00169
00170 from auv_msgs.msg import NavSts
00171
00172 class HLManager:
00173 def __init__(self):
00174 self.man_nu = Bool6Axis()
00175 self.man_nu.x = False
00176 self.man_nu.y = False
00177 self.man_nu.z = False
00178 self.man_nu.roll = True
00179 self.man_nu.pitch = True
00180 self.man_nu.yaw = False
00181
00182 self.velcon = [0,0,0,0,0,0]
00183 self.manualEnable = 0
00184 self.manualSelection = 0
00185
00186 self.hdgEnable = 0
00187 self.altEnable = 0
00188 self.depthEnable = 0
00189 self.dpEnable = 0
00190 self.pitchEnable = 0
00191
00192 self.enablers={"HDG":"HDG_enable",
00193 "ALT":"ALT_enable",
00194 "DEPTH":"DEPTH_enable",
00195 "DP":"FADP_enable",
00196 "NU":"NU_enable",
00197 "REF":"REF_enable",
00198 "PITCH":"PITCH_enable"};
00199 for v in self.enablers.keys():
00200 self._invoke_enabler(v, False)
00201
00202 self._invoke_manual_nu_cfg()
00203 self._invoke_velcon_cfg()
00204
00205 def manual_control(self, state, selection):
00206 """Turn on manual control for
00207 uncontrolled axes"""
00208 self.manualEnable = state
00209 self.manualSelection = selection
00210
00211 self._invoke_enabler("NU", False)
00212 self._invoke_enabler("REF", False)
00213 if state:
00214 if selection == 0:
00215 if not self.dpEnable:
00216 self.velcon[0] = 1
00217 self.velcon[1] = 1
00218
00219 if not self.altEnable: self.velcon[2] = 1
00220 if not self.hdgEnable: self.velcon[5] = 1
00221 if not self.pitchEnable: self.velcon[4] = 1
00222 elif selection == 1:
00223 for i,e in enumerate(self.velcon):
00224 if (e != 2): self.velcon[i]=2
00225 self._invoke_enabler("NU", True)
00226 elif selection == 2:
00227 self._invoke_enabler("REF", True)
00228 else:
00229 for i,e in enumerate(self.velcon):
00230 if (e == 1): self.velcon[i]=0
00231
00232
00233 self.velcon[3] = 0
00234
00235 self._invoke_manual_nu_cfg()
00236 self._invoke_velcon_cfg()
00237
00238 def heading_control(self, state):
00239 self.hdgEnable = state
00240 self._invoke_enabler("HDG", state)
00241 if state:
00242 self.man_nu.yaw = 1
00243 self.velcon[5] = 2
00244 else:
00245 self.man_nu.yaw = 0
00246 self.velcon[5] = 0
00247 self.manual_control(self.manualEnable, self.manualSelection)
00248
00249 self._invoke_manual_nu_cfg()
00250 self._invoke_velcon_cfg()
00251
00252 def depth_control(self, state):
00253 if state and self.altEnable:
00254
00255 self._invoke_enabler("ALT", False)
00256 self.altEnable = False;
00257
00258 self.depthEnable = state
00259 self._invoke_enabler("DEPTH", state)
00260 if state:
00261 self.man_nu.z = 1
00262 self.velcon[2] = 2
00263 else:
00264 self.man_nu.z = 0
00265 self.velcon[2] = 0
00266 self.manual_control(self.manualEnable, self.manualSelection)
00267
00268 self._invoke_manual_nu_cfg()
00269 self._invoke_velcon_cfg()
00270
00271 def altitude_control(self, state):
00272 if state and self.depthEnable:
00273
00274 self._invoke_enabler("DEPTH", False)
00275 self.depthEnable = False;
00276
00277 self.altEnable = state
00278 self._invoke_enabler("ALT", state)
00279 if state:
00280 self.man_nu.z = 1
00281 self.velcon[2] = 2
00282 else:
00283 self.man_nu.z = 0
00284 self.velcon[2] = 0
00285 self.manual_control(self.manualEnable, self.manualSelection)
00286
00287 self._invoke_manual_nu_cfg()
00288 self._invoke_velcon_cfg()
00289
00290 def pitch_control(self, state):
00291 self.pitchEnable = state
00292 self._invoke_enabler("PITCH", state)
00293 if state:
00294 self.man_nu.pitch = 1
00295 self.velcon[4] = 2
00296 else:
00297 self.man_nu.pitch = 0
00298 self.velcon[4] = 0
00299 self.manual_control(self.manualEnable, self.manualSelection)
00300
00301 self._invoke_manual_nu_cfg()
00302 self._invoke_velcon_cfg()
00303
00304 def dp_control(self, state):
00305 self.dpEnable = state
00306 self._invoke_enabler("DP", state)
00307 if state:
00308 self.man_nu.x = 1
00309 self.man_nu.y = 1
00310 self.velcon[0] = 2
00311 self.velcon[1] = 2
00312 else:
00313 self.man_nu.x = 0
00314 self.man_nu.y = 0
00315 self.velcon[0] = 0
00316 self.velcon[1] = 0
00317 self.manual_control(self.manualEnable, self.manualSelection)
00318
00319 self._invoke_manual_nu_cfg()
00320 self._invoke_velcon_cfg()
00321
00322 def _invoke_enabler(self, name, data):
00323 if self.enablers[name] == "None": return
00324 try:
00325 rospy.wait_for_service(self.enablers[name], timeout=1.0)
00326 enabler = rospy.ServiceProxy(self.enablers[name], EnableControl)
00327 enabler(data)
00328 except (rospy.ROSException, rospy.ServiceException), e:
00329 print "Removing enabler from list."
00330 self.enablers[name] = "None"
00331 print "Service call failed: %s"%e
00332
00333 def _invoke_manual_nu_cfg(self):
00334 try:
00335 rospy.wait_for_service("ConfigureAxes")
00336 configurer = rospy.ServiceProxy("ConfigureAxes", ConfigureAxes)
00337 configurer(self.man_nu)
00338 except (rospy.ROSException, rospy.ServiceException), e:
00339 print "Service call failed: %s"%e
00340
00341 def _invoke_velcon_cfg(self):
00342 try:
00343 rospy.wait_for_service("ConfigureVelocityController")
00344 configurer = rospy.ServiceProxy("ConfigureVelocityController", ConfigureVelocityController)
00345 configurer(desired_mode=self.velcon)
00346 except rospy.ServiceException, e:
00347 print "Service call failed: %s"%e
00348
00349
00350 class SimManROS(QtCore.QObject):
00351 def __init__(self):
00352 QtCore.QObject.__init__(self)
00353 self._subscribers = []
00354 self._stateRef = NavSts()
00355 self._stateHat = NavSts()
00356
00357 self.use_dp = True
00358 self.manager = HLManager()
00359
00360 def setup(self, gui):
00361 self._subscribe()
00362 self._advertise()
00363 self._connect_signals(gui)
00364 pass
00365
00366 def unload(self):
00367 self._unsubscribe()
00368 pass
00369
00370 def onHeadingUpdate(self, heading):
00371 self._stateRef.orientation.yaw = math.radians(heading)
00372 self._update_stateRef()
00373
00374 def onDepthUpdate(self, depth):
00375 self._stateRef.position.depth = depth
00376 self._update_stateRef()
00377
00378 def onPitchUpdate(self, pitch):
00379 self._stateRef.orientation.pitch = math.radians(pitch)
00380 self._update_stateRef()
00381
00382 def onAltitudeUpdate(self, altitude):
00383 self._stateRef.altitude = altitude
00384 self._update_stateRef()
00385
00386 def onMoveBaseRef(self, x, y, selection):
00387 if selection == 0:
00388 self._stateRef.position.north = x
00389 self._stateRef.position.east = y
00390 elif selection == 1:
00391 self._stateRef.position.north = self._stateHat.position.north + x
00392 self._stateRef.position.east = self._stateHat.position.east + y
00393
00394
00395 elif selection == 2:
00396 import numpy
00397 yaw = self._stateHat.orientation.yaw
00398 R = numpy.array([[math.cos(yaw),-math.sin(yaw)],
00399 [math.sin(yaw),math.cos(yaw)]],numpy.float32)
00400 xy = numpy.array([x,y],numpy.float32)
00401 ne = numpy.dot(R,xy)
00402 self._stateRef.position.north = self._stateHat.position.north + ne[0]
00403 self._stateRef.position.east = self._stateHat.position.east + ne[1]
00404
00405
00406
00407 self._update_stateRef()
00408
00409 def onHeadingEnable(self, state, heading):
00410
00411 self.manager.heading_control(state);
00412 self.onHeadingUpdate(heading)
00413
00414 def onDepthEnable(self, state, depth):
00415
00416 self.manager.depth_control(state);
00417 self.onDepthUpdate(depth)
00418
00419 def onAltitudeEnable(self, state, altitude):
00420
00421 self.manager.altitude_control(state);
00422 self.onAltitudeUpdate(altitude)
00423
00424 def onPitchEnable(self, state, depth):
00425
00426 self.manager.pitch_control(state);
00427 self.onPitchUpdate(depth)
00428
00429 def onDPEnable(self, state):
00430
00431 self.manager.dp_control(state);
00432
00433 def onManualEnable(self, state, selection):
00434 self.manager.manual_control(state, selection);
00435
00436 def _update_stateRef(self):
00437 self._stateRefPub.publish(self._stateRef)
00438 if self.use_dp: self._stateRefPub2.publish(self._stateRef)
00439
00440 def _connect_signals(self, gui):
00441 QtCore.QObject.connect(self,
00442 QtCore.SIGNAL("onStateHat"), gui.onStateHat)
00443
00444 def _onStateHat(self, data):
00445 self._stateHat = data;
00446 self.emit(QtCore.SIGNAL("onStateHat"), data)
00447
00448 def _onUseDP(self, data):
00449 self.use_dp = data.data
00450
00451 def _subscribe(self):
00452 self._subscribers.append(rospy.Subscriber("stateHat",
00453 NavSts,
00454 self._onStateHat))
00455 self._subscribers.append(rospy.Subscriber("use_dp",
00456 Bool,
00457 self._onUseDP))
00458
00459 def _advertise(self):
00460 self._stateRefPub = rospy.Publisher("stateRef", NavSts)
00461 self._stateRefPub2 = rospy.Publisher("TrackPoint", NavSts)
00462
00463 def _unsubscribe(self):
00464 for subscriber in self._subscribers:
00465 subscriber.unregister()
00466
00467 self._stateRefPub.unregister()
00468
00469
00470 class SimManRqt(Plugin):
00471 def __init__(self, context):
00472 name = "SimMan"
00473 resource = os.path.join(os.path.dirname(
00474 os.path.realpath(__file__)),
00475 "resource/" + name + ".ui")
00476 GuiT = SimManGui
00477 RosT = SimManROS
00478
00479 super(SimManRqt, self).__init__(context)
00480 self.setObjectName(name)
00481
00482 from argparse import ArgumentParser
00483 parser = ArgumentParser()
00484 parser.add_argument("-q", "--quiet", action="store_true",
00485 dest="quiet",
00486 help="Put plugin in silent mode")
00487 args, unknowns = parser.parse_known_args(context.argv())
00488 if not args.quiet:
00489 print "arguments: ", args
00490 print "unknowns: ", unknowns
00491
00492
00493 self._gui = GuiT()
00494 self._ros = RosT()
00495
00496 loadUi(resource, self._gui)
00497 self._ros.setup(self._gui)
00498 self._gui.setup(name + "Rqt", self._ros)
00499
00500 if context.serial_number() > 1:
00501 self._widget.setWindowTitle(self._widget.windowTitle() +
00502 (" (%d)" % context.serial_number()))
00503 context.add_widget(self._gui)
00504
00505 if __name__ == '__main__':
00506 from labust_rqt import rqt_plugin_meta
00507 name = "SimMan"
00508 resource = rqt_plugin_meta.resource_rpath(name, __file__)
00509 rqt_plugin_meta.launch_standalone(name = name,
00510 GuiT = SimManGui,
00511 RosT = SimManROS,
00512 resource = resource);