00001
00002 '''
00003 Created on Sep 10, 2013
00004 \todo Add license information here
00005 @author: dnad
00006 '''
00007 from __future__ import print_function
00008 import os, sys
00009
00010 import rospy
00011 import labust_gui
00012
00013 from python_qt_binding import loadUi, QtCore, QtGui
00014 from qt_gui.plugin import Plugin
00015
00016 from labust_uvapp.srv import ConfigureVelocityController
00017 from navcon_msgs.msg import DOFIdentificationAction, DOFIdentificationGoal
00018 from navcon_msgs.msg import DOFIdentificationResult, DOFIdentificationFeedback
00019 from navcon_msgs.msg import ModelParamsUpdate
00020
00021 from std_msgs.msg import String
00022 from actionlib import SimpleActionClient, SimpleGoalState
00023
00024 class IdentSetup():
00025 def __init__(self):
00026 self.dof = 0
00027 self.ref = 0.0
00028 self.amp = 0.0
00029 self.hyst = 0.0
00030 self.Ts = 0.1
00031 self.yawCheck = False
00032 self.depthCheck = False
00033 self.positionCheck = False
00034
00035 class IdentificationGui(QtGui.QWidget):
00036
00037 onStartIdent = QtCore.pyqtSignal(bool, object, name='onStartIdent')
00038 onTune = QtCore.pyqtSignal(object, bool, name='onTune')
00039
00040 def __init__(self):
00041
00042 QtGui.QWidget.__init__(self)
00043
00044 def setup(self, name, ros):
00045 self.setObjectName(name)
00046 self.setWindowTitle(name)
00047 self._setup();
00048 self._connect_signals(ros)
00049
00050 def unload(self):
00051 pass
00052
00053 def _connect_signals(self, ros):
00054 self.onStartIdent.connect(ros.onIdentGuiReq)
00055 self.onTune.connect(ros.onModelUpdate)
00056
00057 self.startIdent.clicked.connect(self._startIdent_pressed)
00058 self.stopIdent.clicked.connect(self._stopIdent_pressed)
00059 self.tuneButton.clicked.connect(self._tuneButton_pressed)
00060
00061 @QtCore.pyqtSlot(bool)
00062 def onActionLib(self, data):
00063 self.useActionlib.setChecked(data)
00064 self.feedbackGroup.setEnabled(data)
00065 self.loadButton.setEnabled(not data)
00066
00067 @QtCore.pyqtSlot(object, object)
00068 def onActionResult(self, state, result):
00069 print("Finished with state:", state)
00070 print("Result:",result)
00071 self.resAlpha.setText("{:.3f}".format(result.alpha))
00072 self.resBeta.setText("{:.3f}".format(result.beta))
00073 self.resBetaa.setText("{:.3f}".format(result.betaa))
00074 self.resDelta.setText("{:.3f}".format(result.delta))
00075 self.resWn.setText("{:.3f}".format(result.wn))
00076
00077 self.lastResult = result;
00078 self.tuneButton.setEnabled(True)
00079 self._ident_state(False)
00080
00081 @QtCore.pyqtSlot(object)
00082 def onActionFeed(self, feedback):
00083 print("Feedback:",feedback)
00084 self.fbDof.setText(str(feedback.dof))
00085 self.fbOsc.setText(str(feedback.oscillation_num))
00086 self.fbError.setText("{:.2f}".format(feedback.error * 100)+" %")
00087
00088 @QtCore.pyqtSlot()
00089 def _startIdent_pressed(self):
00090 tmp = IdentSetup()
00091 tmp.dof = self.dofCombo.currentIndex()
00092 tmp.ref = self.identRef.value()
00093 tmp.amp = self.identAmp.value()
00094 tmp.hyst = self.identHyst.value()
00095 tmp.yawCheck = self.yawCheck.isChecked()
00096 tmp.depthCheck = self.depthCheck.isChecked()
00097 tmp.positionCheck = self.positionCheck.isChecked()
00098
00099 self._ident_state(True)
00100 self._clear_results()
00101
00102 self.onStartIdent.emit(True, tmp)
00103
00104 def _ident_state(self, onoff):
00105 self.startIdent.setEnabled(not onoff)
00106 self.stopIdent.setEnabled(onoff)
00107
00108 def _clear_results(self):
00109 self.resAlpha.setText('0.00')
00110 self.resBeta.setText('0.00')
00111 self.resBetaa.setText('0.00')
00112 self.resDelta.setText('0.00')
00113 self.resWn.setText('0.00')
00114
00115 @QtCore.pyqtSlot()
00116 def _stopIdent_pressed(self):
00117 self.onStartIdent.emit(False, IdentSetup())
00118
00119 @QtCore.pyqtSlot()
00120 def _tuneButton_pressed(self):
00121 self.onTune.emit(self.lastResult, self.useLinear.isChecked())
00122
00123 def _setup(self):
00124 self.dofCombo.addItem("X")
00125 self.dofCombo.addItem("Y")
00126 self.dofCombo.addItem("Z")
00127 self.dofCombo.addItem("K")
00128 self.dofCombo.addItem("M")
00129 self.dofCombo.addItem("N")
00130 self.dofCombo.addItem("A")
00131
00132 self.canTune = False
00133 self.tuneButton.setEnabled(False)
00134 self.stopIdent.setEnabled(False)
00135
00136
00137 class IdentificationROS(QtCore.QObject):
00138 onActionLib = QtCore.pyqtSignal(bool, name = "onActionLib")
00139 onActionResult = QtCore.pyqtSignal(object, object, name="onActionResult")
00140 onActionFeed = QtCore.pyqtSignal(object, name="onActionFeed")
00141
00142 def __init__(self):
00143 QtCore.QObject.__init__(self)
00144
00145 self.names=("Surge",
00146 "Sway",
00147 "Heave",
00148 "Roll",
00149 "Pitch",
00150 "Yaw",
00151 "Altitude")
00152
00153 def setup(self, gui):
00154 self._connect_signals(gui)
00155 self._subscribe()
00156 pass
00157
00158 @QtCore.pyqtSlot(bool, object)
00159 def onIdentGuiReq(self, active, setup):
00160 if (setup.dof < DOFIdentificationGoal.Surge or
00161 setup.dof > DOFIdentificationGoal.Altitude):
00162 print("DOF select {} is invalid. Ignoring.".format(setup.dof))
00163 return
00164
00165 if self.useAction:
00166 self._actionIdent(active, setup)
00167 else:
00168 self._velconIdent(active, setup)
00169
00170 @QtCore.pyqtSlot(object, bool)
00171 def onModelUpdate(self, result, use_linear):
00172 update = ModelParamsUpdate()
00173 update.dof = result.dof
00174 update.alpha = result.alpha
00175 update.beta = result.beta
00176 update.betaa = result.betaa
00177 update.delta = result.delta
00178 update.wn = result.wn
00179 update.use_linear = use_linear
00180 self.model_update.publish(update)
00181
00182 def _velconIdent(self, active, setup):
00183 velcon = [0,0,0,0,0,0]
00184 '''Identification is number 3'''
00185 if active:
00186 if setup.dof == DOFIdentificationGoal.Altitude:
00187 velcon[DOFIdentificationGoal.Heave] = 3
00188 else:
00189 velcon[setup.dof] = 3
00190
00191 print(setup.amp)
00192 rospy.set_param(self.velconName + "/" +
00193 self.names[setup.dof] +
00194 "_ident_amplitude",setup.amp);
00195 rospy.set_param(self.velconName +"/" +
00196 self.names[setup.dof] +
00197 "_ident_hysteresis",setup.hyst);
00198 rospy.set_param(self.velconName + "/" +
00199 self.names[setup.dof] +
00200 "_ident_ref",setup.ref);
00201 if setup.depthCheck: velcon[2] = 2;
00202 if setup.yawCheck: velcon[5] = 2;
00203 if setup.positionCheck:
00204 velcon[0] = 2;
00205 velcon[1] = 2;
00206
00207 try:
00208 configurer = rospy.ServiceProxy("ConfigureVelocityController",
00209 ConfigureVelocityController)
00210 configurer(desired_mode=velcon)
00211 except rospy.ServiceException, e:
00212 print("Service call failed: %s"%e)
00213
00214 def _actionIdent(self, active, setup):
00215
00216 self._velconIdent(active,setup)
00217 if not active:
00218 self.ac.cancel_all_goals()
00219 return
00220
00221 goal = DOFIdentificationGoal()
00222 goal.dof = setup.dof
00223 goal.command = setup.amp
00224 goal.reference = setup.ref
00225 goal.hysteresis = setup.hyst
00226 goal.sampling_rate = 1/setup.Ts
00227
00228
00229 self._ac_onActive = lambda: print("Goal reported activation.")
00230 self._ac_onFeed = lambda data: self.onActionFeed.emit(data)
00231 self.ac.send_goal(goal, self._ac_onResult, self._ac_onActive, self._ac_onFeed)
00232
00233 def _ac_onResult(self, state, data):
00234 self._velconIdent(False,None)
00235 self.onActionResult.emit(state,data)
00236
00237 def unload(self):
00238 pass
00239
00240 def _connect_signals(self, gui):
00241 self.onActionLib.connect(gui.onActionLib)
00242 self.onActionFeed.connect(gui.onActionFeed)
00243 self.onActionResult.connect(gui.onActionResult)
00244
00245 def _subscribe(self):
00246 self.useAction = rospy.get_param("~use_action",True)
00247 self.velconName = rospy.get_param("~velcon_name","velcon")
00248 self.model_update = rospy.Publisher("model_update", ModelParamsUpdate)
00249
00250 if self.useAction:
00251 '''Configure action server'''
00252 self.ac = SimpleActionClient("Identification",
00253 DOFIdentificationAction)
00254
00255 if not self.ac.wait_for_server(rospy.Duration(30.0)):
00256 print("Action not supported, using fallback.")
00257 self.useAction = False
00258
00259 self.onActionLib.emit(self.useAction)
00260
00261 def _unsubscribe(self):
00262 pass
00263
00264
00265 class IdentificationRqt(Plugin):
00266 def __init__(self, context):
00267 name = "Identification"
00268 resource = os.path.join(os.path.dirname(
00269 os.path.realpath(__file__)),
00270 "resource/" + name + ".ui")
00271 GuiT = IdentificationGui
00272 RosT = IdentificationROS
00273
00274 super(IdentificationRqt, self).__init__(context)
00275 self.setObjectName(name)
00276
00277 from argparse import ArgumentParser
00278 parser = ArgumentParser()
00279 parser.add_argument("-q", "--quiet", action="store_true",
00280 dest="quiet",
00281 help="Put plugin in silent mode")
00282 args, unknowns = parser.parse_known_args(context.argv())
00283 if not args.quiet:
00284 print("arguments: ", args)
00285 print("unknowns: ", unknowns)
00286
00287
00288 self._gui = GuiT()
00289 self._ros = RosT()
00290
00291 loadUi(resource, self._gui)
00292 self._ros.setup(self._gui)
00293 self._gui.setup(name + "Rqt", self._ros)
00294
00295 if context.serial_number() > 1:
00296 self._widget.setWindowTitle(self._widget.windowTitle() +
00297 (" (%d)" % context.serial_number()))
00298 context.add_widget(self._gui)
00299
00300 if __name__ == '__main__':
00301 from labust_rqt import rqt_plugin_meta
00302 name = "Identification"
00303 resource = rqt_plugin_meta.resource_rpath(name, __file__)
00304 rqt_plugin_meta.launch_standalone(name = name,
00305 GuiT = IdentificationGui,
00306 RosT = IdentificationROS,
00307 resource = resource);