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