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 QtCore, QtGui
00014 from labust_rqt.rqt_plugin_meta import RqtPluginMeta
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
00131 self.canTune = False
00132 self.tuneButton.setEnabled(False)
00133 self.stopIdent.setEnabled(False)
00134
00135
00136 class IdentificationROS(QtCore.QObject):
00137 onActionLib = QtCore.pyqtSignal(bool, name = "onActionLib")
00138 onActionResult = QtCore.pyqtSignal(object, object, name="onActionResult")
00139 onActionFeed = QtCore.pyqtSignal(object, name="onActionFeed")
00140
00141 def __init__(self):
00142 QtCore.QObject.__init__(self)
00143
00144 self.names=("Surge",
00145 "Sway",
00146 "Heave",
00147 "Roll",
00148 "Pitch",
00149 "Yaw")
00150
00151 def setup(self, gui):
00152 self._connect_signals(gui)
00153 self._subscribe()
00154 pass
00155
00156 @QtCore.pyqtSlot(bool, object)
00157 def onIdentGuiReq(self, active, setup):
00158 if setup.dof < 0 or setup.dof >= 6:
00159 print("DOF select {} is invalid. Ignoring.".format(dof))
00160 return
00161
00162 if self.useAction:
00163 self._actionIdent(active, setup)
00164 else:
00165 self._velconIdent(active, setup)
00166 @QtCore.pyqtSlot(object, bool)
00167 def onModelUpdate(self, result, use_linear):
00168 update = ModelParamsUpdate()
00169 update.dof = result.dof
00170 update.alpha = result.alpha
00171 update.beta = result.beta
00172 update.betaa = result.betaa
00173 update.delta = result.delta
00174 update.wn = result.wn
00175 update.use_linear = use_linear
00176 self.model_update.publish(update)
00177
00178 def _velconIdent(self, active, setup):
00179 velcon = [0,0,0,0,0,0]
00180 '''Identification is number 3'''
00181 if active:
00182 velcon[setup.dof] = 3
00183 print(setup.amp)
00184 rospy.set_param(self.velconName + "/" +
00185 self.names[setup.dof] +
00186 "_ident_amplitude",setup.amp);
00187 rospy.set_param(self.velconName +"/" +
00188 self.names[setup.dof] +
00189 "_ident_hysteresis",setup.hyst);
00190 rospy.set_param(self.velconName + "/" +
00191 self.names[setup.dof] +
00192 "_ident_ref",setup.ref);
00193 if setup.depthCheck: velcon[2] = 2;
00194 if setup.yawCheck: velcon[5] = 2;
00195 if setup.positionCheck:
00196 velcon[0] = 2;
00197 velcon[1] = 2;
00198
00199 try:
00200 configurer = rospy.ServiceProxy("ConfigureVelocityController",
00201 ConfigureVelocityController)
00202 configurer(desired_mode=velcon)
00203 except rospy.ServiceException, e:
00204 print("Service call failed: %s"%e)
00205
00206 def _actionIdent(self, active, setup):
00207
00208 self._velconIdent(active,setup)
00209 if not active:
00210 self.ac.cancel_all_goals()
00211 return
00212
00213 goal = DOFIdentificationGoal()
00214 goal.dof = setup.dof
00215 goal.command = setup.amp
00216 goal.reference = setup.ref
00217 goal.hysteresis = setup.hyst
00218 goal.sampling_rate = 1/setup.Ts
00219
00220
00221 self._ac_onActive = lambda: print("Goal reported activation.")
00222 self._ac_onFeed = lambda data: self.onActionFeed.emit(data)
00223 self.ac.send_goal(goal, self._ac_onResult, self._ac_onActive, self._ac_onFeed)
00224
00225 def _ac_onResult(self, state, data):
00226 self._velconIdent(False,None)
00227 self.onActionResult.emit(state,data)
00228
00229 def unload(self):
00230 pass
00231
00232 def _connect_signals(self, gui):
00233 self.onActionLib.connect(gui.onActionLib)
00234 self.onActionFeed.connect(gui.onActionFeed)
00235 self.onActionResult.connect(gui.onActionResult)
00236
00237 def _subscribe(self):
00238 self.useAction = rospy.get_param("~use_action",True)
00239 self.velconName = rospy.get_param("~velcon_name","velcon")
00240 self.model_update = rospy.Publisher("model_update", ModelParamsUpdate)
00241
00242 if self.useAction:
00243 '''Configure action server'''
00244 self.ac = SimpleActionClient("Identification",
00245 DOFIdentificationAction)
00246
00247 if not self.ac.wait_for_server(rospy.Duration(30.0)):
00248 print("Action not supported, using fallback.")
00249 self.useAction = False
00250
00251 self.onActionLib.emit(self.useAction)
00252
00253 def _unsubscribe(self):
00254 pass
00255
00256 class IdentificationRqt(RqtPluginMeta):
00257 def __init__(self, context):
00258 name = "Identification"
00259 resource = rqt_plugin_meta.resource_rpath(name, __file__)
00260 super(IdentificationRqt, self).__init__(context = context,
00261 name = name,
00262 GuiT = IdentificationGui,
00263 RosT = IdentificationROS,
00264 resource=resource)
00265
00266 if __name__ == '__main__':
00267 from labust_rqt import rqt_plugin_meta
00268 name = "Identification"
00269 resource = rqt_plugin_meta.resource_rpath(name, __file__)
00270 rqt_plugin_meta.launch_standalone(name = name,
00271 GuiT = IdentificationGui,
00272 RosT = IdentificationROS,
00273 resource = resource);