node_manager_fkie.echo

1 # Software License Agreement (BSD License) 2 # 3 # Copyright (c) 2012, Fraunhofer FKIE/US, Alexander Tiderko 4 # All rights reserved. 5 # 6 # Redistribution and use in source and binary forms, with or without 7 # modification, are permitted provided that the following conditions 8 # are met: 9 # 10 # * Redistributions of source code must retain the above copyright 11 # notice, this list of conditions and the following disclaimer. 12 # * Redistributions in binary form must reproduce the above 13 # copyright notice, this list of conditions and the following 14 # disclaimer in the documentation and/or other materials provided 15 # with the distribution. 16 # * Neither the name of Fraunhofer nor the names of its 17 # contributors may be used to endorse or promote products derived 18 # from this software without specific prior written permission. 19 # 20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 # POSSIBILITY OF SUCH DAMAGE. 32 33 34 from python_qt_binding import QtCore, QtGui 35 36 import time 37 import math 38 from datetime import datetime 39 40 import roslib 41 import roslib.message 42 import rospy 43 import threading 44 45 import gui_resources 46

47 -class EchoDialog(QtGui.QDialog):

48 49 MESSAGE_LINE_LIMIT = 128 50 MESSAGE_HZ_LIMIT = 10 51 MAX_DISPLAY_MSGS = 25 52 STATISTIC_QUEUE_LEN = 5000 53 54 ''' 55 This dialog shows the output of a topic. 56 ''' 57 58 finished_signal = QtCore.Signal(str) 59 ''' 60 finished_signal has as parameter the name of the topic and is emitted, if this 61 dialog was closed. 62 ''' 63 64 msg_signal = QtCore.Signal(object, bool) 65 ''' 66 msg_signal is a signal, which is emitted, if a new message was received. 67 ''' 68

69 - def __init__(self, topic, type, show_only_rate=False, masteruri=None, parent=None):

70 ''' 71 Creates an input dialog. 72 @param topic: the name of the topic 73 @type topic: C{str} 74 @param type: the type of the topic 75 @type type: C{str} 76 @raise Exception: if no topic class was found for the given type 77 ''' 78 QtGui.QDialog.__init__(self, parent=parent) 79 masteruri_str = '' if masteruri is None else '[%s]'%masteruri 80 self.setObjectName(' - '.join(['EchoDialog', topic, masteruri_str])) 81 self.setAttribute(QtCore.Qt.WA_DeleteOnClose, True) 82 self.setWindowFlags(QtCore.Qt.Window) 83 self.setWindowTitle('%s %s %s'%('Echo --- ' if not show_only_rate else 'Hz --- ', topic, masteruri_str)) 84 self.resize(728,512) 85 self.verticalLayout = QtGui.QVBoxLayout(self) 86 self.verticalLayout.setObjectName("verticalLayout") 87 self.verticalLayout.setContentsMargins(1, 1, 1, 1) 88 self.mIcon = QtGui.QIcon(":/icons/crystal_clear_prop_run_echo.png") 89 self.setWindowIcon(self.mIcon) 90 91 self.topic = topic 92 self.show_only_rate = show_only_rate 93 self.lock = threading.RLock() 94 self.last_printed_count = 0 95 self.msg_t0 = -1. 96 self.msg_tn = 0 97 self.times =[] 98 99 self.message_count = 0 100 self._rate_message = '' 101 self._scrapped_msgs = 0 102 self._scrapped_msgs_sl = 0 103 104 self._last_received_ts = 0 105 self.receiving_hz = self.MESSAGE_HZ_LIMIT 106 self.line_limit = self.MESSAGE_LINE_LIMIT 107 108 self.field_filter_fn = None 109 110 options = QtGui.QWidget(self) 111 if not show_only_rate: 112 hLayout = QtGui.QHBoxLayout(options) 113 hLayout.setContentsMargins(1, 1, 1, 1) 114 self.no_str_checkbox = no_str_checkbox = QtGui.QCheckBox('Hide strings') 115 no_str_checkbox.toggled.connect(self.on_no_str_checkbox_toggled) 116 hLayout.addWidget(no_str_checkbox) 117 self.no_arr_checkbox = no_arr_checkbox = QtGui.QCheckBox('Hide arrays') 118 no_arr_checkbox.toggled.connect(self.on_no_arr_checkbox_toggled) 119 hLayout.addWidget(no_arr_checkbox) 120 self.combobox_reduce_ch = QtGui.QComboBox(self) 121 self.combobox_reduce_ch.addItems([str(self.MESSAGE_LINE_LIMIT), '0', '80', '256', '1024']) 122 self.combobox_reduce_ch.activated[str].connect(self.combobox_reduce_ch_activated) 123 self.combobox_reduce_ch.setEditable(True) 124 self.combobox_reduce_ch.setToolTip("Set maximum line width. 0 disables the limit.") 125 hLayout.addWidget(self.combobox_reduce_ch) 126 # reduce_ch_label = QtGui.QLabel('ch', self) 127 # hLayout.addWidget(reduce_ch_label) 128 # add spacer 129 spacerItem = QtGui.QSpacerItem(515, 20, QtGui.QSizePolicy.Expanding, QtGui.QSizePolicy.Minimum) 130 hLayout.addItem(spacerItem) 131 # add combobox for displaying frequency of messages 132 self.combobox_displ_hz = QtGui.QComboBox(self) 133 self.combobox_displ_hz.addItems([str(self.MESSAGE_HZ_LIMIT), '0', '0.1', '1', '50', '100', '1000']) 134 self.combobox_displ_hz.activated[str].connect(self.on_combobox_hz_activated) 135 self.combobox_displ_hz.setEditable(True) 136 hLayout.addWidget(self.combobox_displ_hz) 137 displ_hz_label = QtGui.QLabel('Hz', self) 138 hLayout.addWidget(displ_hz_label) 139 # add combobox for count of displayed messages 140 self.combobox_msgs_count = QtGui.QComboBox(self) 141 self.combobox_msgs_count.addItems([str(self.MAX_DISPLAY_MSGS), '0', '50', '100']) 142 self.combobox_msgs_count.activated[str].connect(self.on_combobox_count_activated) 143 self.combobox_msgs_count.setEditable(True) 144 self.combobox_msgs_count.setToolTip("Set maximum displayed message count. 0 disables the limit.") 145 hLayout.addWidget(self.combobox_msgs_count) 146 displ_count_label = QtGui.QLabel('#', self) 147 hLayout.addWidget(displ_count_label) 148 # add topic control button for unsubscribe and subscribe 149 self.topic_control_button = QtGui.QToolButton(self) 150 self.topic_control_button.setText('stop') 151 self.topic_control_button.setIcon(QtGui.QIcon(':/icons/deleket_deviantart_stop.png')) 152 self.topic_control_button.clicked.connect(self.on_topic_control_btn_clicked) 153 hLayout.addWidget(self.topic_control_button) 154 # add clear button 155 clearButton = QtGui.QToolButton(self) 156 clearButton.setText('clear') 157 clearButton.clicked.connect(self.on_clear_btn_clicked) 158 hLayout.addWidget(clearButton) 159 self.verticalLayout.addWidget(options) 160 161 self.display = QtGui.QTextEdit(self) 162 self.display.setReadOnly(True) 163 self.verticalLayout.addWidget(self.display); 164 self.display.document().setMaximumBlockCount(500) 165 self.max_displayed_msgs = self.MAX_DISPLAY_MSGS 166 self._blocks_in_msg = None 167 168 self.status_label = QtGui.QLabel('0 messages', self) 169 self.verticalLayout.addWidget(self.status_label) 170 171 # subscribe to the topic 172 self.__msg_class = roslib.message.get_message_class(type) 173 if not self.__msg_class: 174 raise Exception("Cannot load message class for [%s]. Are your messages built?"%type) 175 176 self.print_hz_timer = QtCore.QTimer() 177 self.print_hz_timer.timeout.connect(self._on_calc_hz) 178 self.print_hz_timer.start(1000) 179 180 self.msg_signal.connect(self._append_message) 181 self.sub = rospy.Subscriber(self.topic, self.__msg_class, self._msg_handle)

182 183 # print "======== create", self.objectName() 184 # 185 # def __del__(self): 186 # print "******* destroy", self.objectName() 187 188 # def hideEvent(self, event): 189 # self.close() 190

191 - def closeEvent (self, event):

192 if not self.sub is None: 193 self.sub.unregister() 194 del self.sub 195 self.finished_signal.emit(self.topic) 196 if self.parent() is None: 197 QtGui.QApplication.quit()

198 # else: 199 # self.setParent(None) 200

201 - def create_field_filter(self, echo_nostr, echo_noarr):

202 def field_filter(val): 203 try: 204 fields = val.__slots__ 205 field_types = val._slot_types 206 for f, t in zip(val.__slots__, val._slot_types): 207 if echo_noarr and '[' in t: 208 continue 209 elif echo_nostr and 'string' in t: 210 continue 211 yield f 212 except: 213 pass

214 return field_filter

215

216 - def on_no_str_checkbox_toggled(self, state):

217 self.field_filter_fn = self.create_field_filter(state, self.no_arr_checkbox.isChecked())

218

219 - def on_no_arr_checkbox_toggled(self, state):

220 self.field_filter_fn = self.create_field_filter(self.no_str_checkbox.isChecked(), state)

221

222 - def combobox_reduce_ch_activated(self, ch_txt):

223 try: 224 self.line_limit = int(ch_txt) 225 except ValueError: 226 try: 227 self.line_limit = float(ch_txt) 228 except ValueError: 229 self.combobox_reduce_ch.setEditText(str(self.line_limit))

230

231 - def on_combobox_hz_activated(self, hz_txt):

232 try: 233 self.receiving_hz = int(hz_txt) 234 except ValueError: 235 try: 236 self.receiving_hz = float(hz_txt) 237 except ValueError: 238 self.combobox_displ_hz.setEditText(str(self.receiving_hz))

239

240 - def on_combobox_count_activated(self, count_txt):

241 try: 242 self.max_displayed_msgs = int(count_txt) 243 self._blocks_in_msg = None 244 except ValueError: 245 self.combobox_msgs_count.setEditText(str(self.max_displayed_msgs))

246

247 - def on_clear_btn_clicked(self):

248 self.display.clear() 249 with self.lock: 250 self.message_count = 0 251 self._scrapped_msgs = 0 252 del self.times[:]

253

254 - def on_topic_control_btn_clicked(self):

255 if self.sub is None: 256 self.sub = rospy.Subscriber(self.topic, self.__msg_class, self._msg_handle) 257 self.topic_control_button.setText('stop') 258 self.topic_control_button.setIcon(QtGui.QIcon(':/icons/deleket_deviantart_stop.png')) 259 else: 260 self.sub.unregister() 261 self.sub = None 262 self.topic_control_button.setText('play') 263 self.topic_control_button.setIcon(QtGui.QIcon(':/icons/deleket_deviantart_play.png'))

264

265 - def _msg_handle(self, data):

266 self.msg_signal.emit(data, (data._connection_header['latching'] != '0'))

267

268 - def _append_message(self, msg, latched):

269 ''' 270 Adds a label to the dialog's layout and shows the given text. 271 @param msg: the text to add to the dialog 272 @type msg: message object 273 ''' 274 current_time = time.time() 275 with self.lock: 276 # time reset 277 if self.msg_t0 < 0 or self.msg_t0 > current_time: 278 self.msg_t0 = current_time 279 self.msg_tn = current_time 280 self.times = [] 281 else: 282 self.times.append(current_time - self.msg_tn) 283 self.msg_tn = current_time 284 # keep only statistics for the last 5000 messages so as not to run out of memory 285 if len(self.times) > self.STATISTIC_QUEUE_LEN: 286 self.times.pop(0) 287 self.message_count += 1 288 # skip messages, if they are received often then MESSAGE_HZ_LIMIT 289 if self._last_received_ts != 0 and self.receiving_hz != 0: 290 if not latched and current_time - self._last_received_ts < 1.0 / self.receiving_hz: 291 self._scrapped_msgs += 1 292 self._scrapped_msgs_sl += 1 293 return 294 self._last_received_ts = current_time 295 if not self.show_only_rate: 296 # convert message to string and reduce line width to current limit 297 msg = roslib.message.strify_message(msg, field_filter=self.field_filter_fn) 298 if isinstance(msg, tuple): 299 msg = msg[0] 300 if self.line_limit != 0: 301 a = '' 302 for l in msg.splitlines(): 303 a = a + (l if len(l)<=self.line_limit else l[0:self.line_limit-3]+'...') + '\n' 304 msg = a 305 # create a notification about scrapped messages 306 if self._scrapped_msgs_sl > 0: 307 txt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">scrapped %s message because of Hz-settings</pre>'%self._scrapped_msgs_sl 308 self.display.append(txt) 309 self._scrapped_msgs_sl = 0 310 txt = '<pre style="background-color:#FFFCCC; font-family:Fixedsys,Courier; padding:10px;">---------- %s --------------------\n%s</pre>'%(datetime.now().strftime("%d.%m.%Y %H:%M:%S.%f"), msg) 311 # set the count of the displayed messages on receiving the first message 312 if self._blocks_in_msg is None: 313 td = QtGui.QTextDocument(txt) 314 self._blocks_in_msg = td.blockCount() 315 self.display.document().setMaximumBlockCount(self._blocks_in_msg*self.max_displayed_msgs) 316 self.display.append(txt) 317 self._print_status()

318

319 - def _on_calc_hz(self):

320 if rospy.is_shutdown(): 321 self.close() 322 return 323 if self.message_count == self.last_printed_count: 324 return 325 with self.lock: 326 # the code from ROS rostopic 327 n = len(self.times) 328 if n < 2: 329 return 330 mean = sum(self.times) / n 331 rate = 1./mean if mean > 0. else 0 332 #std dev 333 std_dev = math.sqrt(sum((x - mean)**2 for x in self.times) /n) 334 # min and max 335 max_delta = max(self.times) 336 min_delta = min(self.times) 337 self.last_printed_count = self.message_count 338 self._rate_message = "average rate: %.3f\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s"%(rate, min_delta, max_delta, std_dev, n+1) 339 if self._scrapped_msgs > 0: 340 self._rate_message +=" --- scrapped msgs: %s"%self._scrapped_msgs 341 self._print_status() 342 if self.show_only_rate: 343 self.display.append(self._rate_message)

344

345 - def _print_status(self):

346 self.status_label.setText('%s messages %s'%(self.message_count, self._rate_message))

347

Source Code for Module node_manager_fkie.echo_dialog