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 from roslib import message 42 import rospy 43 import threading 44 45 import gui_resources 46 import node_manager_fkie as nm 47

48 -class EchoDialog(QtGui.QDialog):

49 50 MESSAGE_LINE_LIMIT = 128 51 MESSAGE_HZ_LIMIT = 10 52 MAX_DISPLAY_MSGS = 25 53 STATISTIC_QUEUE_LEN = 5000 54 55 ''' 56 This dialog shows the output of a topic. 57 ''' 58 59 finished_signal = QtCore.Signal(str) 60 ''' 61 finished_signal has as parameter the name of the topic and is emitted, if this 62 dialog was closed. 63 ''' 64 65 msg_signal = QtCore.Signal(object, bool) 66 ''' 67 msg_signal is a signal, which is emitted, if a new message was received. 68 ''' 69 70 text_hz_signal = QtCore.Signal(str) 71 text_signal = QtCore.Signal(str) 72 ''' 73 text_signal is a signal, which is emitted, if a new text to display was received. 74 ''' 75 76 text_error_signal = QtCore.Signal(str) 77 ''' 78 text_error_signal is a signal, which is emitted, if a new error text to display was received. 79 ''' 80 81 request_pw = QtCore.Signal(object) 82

83 - def __init__(self, topic, msg_type, show_only_rate=False, masteruri=None, use_ssh=False, parent=None):

84 ''' 85 Creates an input dialog. 86 @param topic: the name of the topic 87 @type topic: C{str} 88 @param msg_type: the type of the topic 89 @type msg_type: C{str} 90 @raise Exception: if no topic class was found for the given type 91 ''' 92 QtGui.QDialog.__init__(self, parent=parent) 93 self._masteruri = masteruri 94 masteruri_str = '' if masteruri is None else '[%s]'%masteruri 95 self.setObjectName(' - '.join(['EchoDialog', topic, masteruri_str])) 96 self.setAttribute(QtCore.Qt.WA_DeleteOnClose, True) 97 self.setWindowFlags(QtCore.Qt.Window) 98 self.setWindowTitle('%s %s %s'%('Echo --- ' if not show_only_rate else 'Hz --- ', topic, masteruri_str)) 99 self.resize(728,512) 100 self.verticalLayout = QtGui.QVBoxLayout(self) 101 self.verticalLayout.setObjectName("verticalLayout") 102 self.verticalLayout.setContentsMargins(1, 1, 1, 1) 103 self.mIcon = QtGui.QIcon(":/icons/crystal_clear_prop_run_echo.png") 104 self.setWindowIcon(self.mIcon) 105 106 self.topic = topic 107 self.show_only_rate = show_only_rate 108 self.lock = threading.RLock() 109 self.last_printed_count = 0 110 self.msg_t0 = -1. 111 self.msg_tn = 0 112 self.times =[] 113 114 self.message_count = 0 115 self._rate_message = '' 116 self._scrapped_msgs = 0 117 self._scrapped_msgs_sl = 0 118 119 self._last_received_ts = 0 120 self.receiving_hz = self.MESSAGE_HZ_LIMIT 121 self.line_limit = self.MESSAGE_LINE_LIMIT 122 123 self.field_filter_fn = None 124 125 options = QtGui.QWidget(self) 126 if not show_only_rate: 127 hLayout = QtGui.QHBoxLayout(options) 128 hLayout.setContentsMargins(1, 1, 1, 1) 129 self.no_str_checkbox = no_str_checkbox = QtGui.QCheckBox('Hide strings') 130 no_str_checkbox.toggled.connect(self.on_no_str_checkbox_toggled) 131 hLayout.addWidget(no_str_checkbox) 132 self.no_arr_checkbox = no_arr_checkbox = QtGui.QCheckBox('Hide arrays') 133 no_arr_checkbox.toggled.connect(self.on_no_arr_checkbox_toggled) 134 hLayout.addWidget(no_arr_checkbox) 135 self.combobox_reduce_ch = QtGui.QComboBox(self) 136 self.combobox_reduce_ch.addItems([str(self.MESSAGE_LINE_LIMIT), '0', '80', '256', '1024']) 137 self.combobox_reduce_ch.activated[str].connect(self.combobox_reduce_ch_activated) 138 self.combobox_reduce_ch.setEditable(True) 139 self.combobox_reduce_ch.setToolTip("Set maximum line width. 0 disables the limit.") 140 hLayout.addWidget(self.combobox_reduce_ch) 141 # reduce_ch_label = QtGui.QLabel('ch', self) 142 # hLayout.addWidget(reduce_ch_label) 143 # add spacer 144 spacerItem = QtGui.QSpacerItem(515, 20, QtGui.QSizePolicy.Expanding, QtGui.QSizePolicy.Minimum) 145 hLayout.addItem(spacerItem) 146 # add combobox for displaying frequency of messages 147 self.combobox_displ_hz = QtGui.QComboBox(self) 148 self.combobox_displ_hz.addItems([str(self.MESSAGE_HZ_LIMIT), '0', '0.1', '1', '50', '100', '1000']) 149 self.combobox_displ_hz.activated[str].connect(self.on_combobox_hz_activated) 150 self.combobox_displ_hz.setEditable(True) 151 hLayout.addWidget(self.combobox_displ_hz) 152 displ_hz_label = QtGui.QLabel('Hz', self) 153 hLayout.addWidget(displ_hz_label) 154 # add combobox for count of displayed messages 155 self.combobox_msgs_count = QtGui.QComboBox(self) 156 self.combobox_msgs_count.addItems([str(self.MAX_DISPLAY_MSGS), '0', '50', '100']) 157 self.combobox_msgs_count.activated[str].connect(self.on_combobox_count_activated) 158 self.combobox_msgs_count.setEditable(True) 159 self.combobox_msgs_count.setToolTip("Set maximum displayed message count. 0 disables the limit.") 160 hLayout.addWidget(self.combobox_msgs_count) 161 displ_count_label = QtGui.QLabel('#', self) 162 hLayout.addWidget(displ_count_label) 163 # add topic control button for unsubscribe and subscribe 164 self.topic_control_button = QtGui.QToolButton(self) 165 self.topic_control_button.setText('stop') 166 self.topic_control_button.setIcon(QtGui.QIcon(':/icons/deleket_deviantart_stop.png')) 167 self.topic_control_button.clicked.connect(self.on_topic_control_btn_clicked) 168 hLayout.addWidget(self.topic_control_button) 169 # add clear button 170 clearButton = QtGui.QToolButton(self) 171 clearButton.setText('clear') 172 clearButton.clicked.connect(self.on_clear_btn_clicked) 173 hLayout.addWidget(clearButton) 174 self.verticalLayout.addWidget(options) 175 176 self.display = QtGui.QTextBrowser(self) 177 self.display.setReadOnly(True) 178 self.verticalLayout.addWidget(self.display); 179 self.display.document().setMaximumBlockCount(500) 180 self.max_displayed_msgs = self.MAX_DISPLAY_MSGS 181 self._blocks_in_msg = None 182 self.display.setOpenLinks(False) 183 self.display.anchorClicked.connect(self._on_display_anchorClicked) 184 185 self.status_label = QtGui.QLabel('0 messages', self) 186 self.verticalLayout.addWidget(self.status_label) 187 188 # subscribe to the topic 189 errmsg = '' 190 try: 191 self.__msg_class = message.get_message_class(msg_type) 192 if not self.__msg_class: 193 errmsg = "Cannot load message class for [%s]. Did you build messages?"%msg_type 194 # raise Exception("Cannot load message class for [%s]. Did you build messages?"%msg_type) 195 except Exception as e: 196 self.__msg_class = None 197 errmsg = "Cannot load message class for [%s]. Did you build messagest?\nError: %s"%(msg_type, e) 198 # raise Exception("Cannot load message class for [%s]. Did you build messagest?\nError: %s"%(msg_type, e)) 199 # variables for Subscriber 200 self.msg_signal.connect(self._append_message) 201 self.sub = None 202 203 # vairables for SSH connection 204 self.ssh_output_file = None 205 self.ssh_error_file = None 206 self.ssh_input_file = None 207 self.text_signal.connect(self._append_text) 208 self.text_hz_signal.connect(self._append_text_hz) 209 self._current_msg = '' 210 self._current_errmsg = '' 211 self.text_error_signal.connect(self._append_error_text) 212 213 # decide, which connection to open 214 if use_ssh: 215 self.__msg_class = None 216 self._on_display_anchorClicked(QtCore.QUrl(self._masteruri)) 217 elif self.__msg_class is None: 218 errtxt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">\n%s</pre>'%(errmsg) 219 self.display.setText('<a href="%s">open using SSH</a>'%(masteruri)) 220 self.display.append(errtxt) 221 else: 222 self.sub = rospy.Subscriber(self.topic, self.__msg_class, self._msg_handle) 223 224 self.print_hz_timer = QtCore.QTimer() 225 self.print_hz_timer.timeout.connect(self._on_calc_hz) 226 self.print_hz_timer.start(1000)

227 228 # print "======== create", self.objectName() 229 # 230 # def __del__(self): 231 # print "******* destroy", self.objectName() 232 233 # def hideEvent(self, event): 234 # self.close() 235

236 - def closeEvent (self, event):

237 if not self.sub is None: 238 self.sub.unregister() 239 del self.sub 240 try: 241 self.ssh_output_file.close() 242 self.ssh_error_file.close() 243 # send Ctrl+C to remote process 244 self.ssh_input_file.write('%s\n'%chr(3)) 245 self.ssh_input_file.close() 246 except: 247 pass 248 self.finished_signal.emit(self.topic) 249 if self.parent() is None: 250 QtGui.QApplication.quit()

251 # else: 252 # self.setParent(None) 253

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

255 def field_filter(val): 256 try: 257 # fields = val.__slots__ 258 # field_types = val._slot_types 259 for f, t in zip(val.__slots__, val._slot_types): 260 if echo_noarr and '[' in t: 261 continue 262 elif echo_nostr and 'string' in t: 263 continue 264 yield f 265 except: 266 pass

267 return field_filter

268

269 - def on_no_str_checkbox_toggled(self, state):

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

271

272 - def on_no_arr_checkbox_toggled(self, state):

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

274

275 - def combobox_reduce_ch_activated(self, ch_txt):

276 try: 277 self.line_limit = int(ch_txt) 278 except ValueError: 279 try: 280 self.line_limit = float(ch_txt) 281 except ValueError: 282 self.combobox_reduce_ch.setEditText(str(self.line_limit))

283

284 - def on_combobox_hz_activated(self, hz_txt):

285 try: 286 self.receiving_hz = int(hz_txt) 287 except ValueError: 288 try: 289 self.receiving_hz = float(hz_txt) 290 except ValueError: 291 self.combobox_displ_hz.setEditText(str(self.receiving_hz))

292

293 - def on_combobox_count_activated(self, count_txt):

294 try: 295 self.max_displayed_msgs = int(count_txt) 296 self._blocks_in_msg = None 297 except ValueError: 298 self.combobox_msgs_count.setEditText(str(self.max_displayed_msgs))

299

300 - def on_clear_btn_clicked(self):

301 self.display.clear() 302 with self.lock: 303 self.message_count = 0 304 self._scrapped_msgs = 0 305 del self.times[:]

306

307 - def on_topic_control_btn_clicked(self):

308 try: 309 if self.sub is None and self.ssh_output_file is None: 310 if self.__msg_class: 311 self.sub = rospy.Subscriber(self.topic, self.__msg_class, self._msg_handle) 312 else: 313 self._on_display_anchorClicked(QtCore.QUrl(self._masteruri)) 314 self.topic_control_button.setText('stop') 315 self.topic_control_button.setIcon(QtGui.QIcon(':/icons/deleket_deviantart_stop.png')) 316 else: 317 if not self.sub is None: 318 self.sub.unregister() 319 self.sub = None 320 elif not self.ssh_output_file is None: 321 self.ssh_output_file.close() 322 self.ssh_error_file.close() 323 self.ssh_output_file = None 324 self.topic_control_button.setText('play') 325 self.topic_control_button.setIcon(QtGui.QIcon(':/icons/deleket_deviantart_play.png')) 326 self.no_str_checkbox.setEnabled(True) 327 self.no_arr_checkbox.setEnabled(True) 328 except Exception as e: 329 rospy.logwarn('Error while stop/play echo for topic %s: %s'%(self.topic, e))

330

331 - def _msg_handle(self, data):

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

333

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

335 ''' 336 Adds a label to the dialog's layout and shows the given text. 337 @param msg: the text to add to the dialog 338 @type msg: message object 339 ''' 340 current_time = time.time() 341 self._count_messages(current_time) 342 # skip messages, if they are received often then MESSAGE_HZ_LIMIT 343 if self._last_received_ts != 0 and self.receiving_hz != 0: 344 if not latched and current_time - self._last_received_ts < 1.0 / self.receiving_hz: 345 self._scrapped_msgs += 1 346 self._scrapped_msgs_sl += 1 347 return 348 self._last_received_ts = current_time 349 if not self.show_only_rate: 350 # convert message to string and reduce line width to current limit 351 msg = message.strify_message(msg, field_filter=self.field_filter_fn) 352 if isinstance(msg, tuple): 353 msg = msg[0] 354 msg = self._trim_width(msg) 355 # create a notification about scrapped messages 356 if self._scrapped_msgs_sl > 0: 357 txt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">scrapped %s message because of Hz-settings</pre>'%self._scrapped_msgs_sl 358 self.display.append(txt) 359 self._scrapped_msgs_sl = 0 360 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) 361 # set the count of the displayed messages on receiving the first message 362 self._update_max_msg_count(txt) 363 self.display.append(txt) 364 self._print_status()

365

366 - def _count_messages(self, ts=time.time()):

367 ''' 368 Counts the received messages. Call this method only on receive message. 369 ''' 370 current_time = ts 371 with self.lock: 372 # time reset 373 if self.msg_t0 < 0 or self.msg_t0 > current_time: 374 self.msg_t0 = current_time 375 self.msg_tn = current_time 376 self.times = [] 377 else: 378 self.times.append(current_time - self.msg_tn) 379 self.msg_tn = current_time 380 # keep only statistics for the last 5000 messages so as not to run out of memory 381 if len(self.times) > self.STATISTIC_QUEUE_LEN: 382 self.times.pop(0) 383 self.message_count += 1

384 385

386 - def _trim_width(self, msg):

387 ''' 388 reduce line width to current limit 389 :param msg: the message 390 :type msg: str 391 :return: trimmed message 392 ''' 393 result = msg 394 if self.line_limit != 0: 395 a = '' 396 for l in msg.splitlines(): 397 a = a + (l if len(l)<=self.line_limit else l[0:self.line_limit-3]+'...') + '\n' 398 result = a 399 return result

400

401 - def _update_max_msg_count(self, txt):

402 ''' 403 set the count of the displayed messages on receiving the first message 404 :param txt: text of the message, which will be added to the document 405 :type txt: str 406 ''' 407 if self._blocks_in_msg is None: 408 td = QtGui.QTextDocument(txt) 409 self._blocks_in_msg = td.blockCount() 410 self.display.document().setMaximumBlockCount(self._blocks_in_msg*self.max_displayed_msgs)

411

412 - def _on_calc_hz(self):

413 if rospy.is_shutdown(): 414 self.close() 415 return 416 if self.message_count == self.last_printed_count: 417 return 418 with self.lock: 419 # the code from ROS rostopic 420 n = len(self.times) 421 if n < 2: 422 return 423 mean = sum(self.times) / n 424 rate = 1./mean if mean > 0. else 0 425 #std dev 426 std_dev = math.sqrt(sum((x - mean)**2 for x in self.times) /n) 427 # min and max 428 max_delta = max(self.times) 429 min_delta = min(self.times) 430 self.last_printed_count = self.message_count 431 self._rate_message = "average rate: %.3f\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s"%(rate, min_delta, max_delta, std_dev, n+1) 432 if self._scrapped_msgs > 0: 433 self._rate_message +=" --- scrapped msgs: %s"%self._scrapped_msgs 434 self._print_status() 435 if self.show_only_rate: 436 self.display.append(self._rate_message)

437

438 - def _print_status(self):

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

440

441 - def _append_text(self, text):

442 ''' 443 Append echo text received through the SSH. 444 ''' 445 with self.lock: 446 self._current_msg += text 447 if self._current_msg.find('---') != -1: 448 messages = self._current_msg.split('---') 449 for m in messages[:-1]: 450 current_time = time.time() 451 self._count_messages(current_time) 452 # limit the displayed text width 453 m = self._trim_width(m) 454 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"), m) 455 # set the count of the displayed messages on receiving the first message 456 self._update_max_msg_count(txt) 457 self.display.append(txt) 458 self._current_msg = messages[-1] 459 self._print_status()

460

461 - def _append_error_text(self, text):

462 ''' 463 Append error text received through the SSH. 464 ''' 465 with self.lock: 466 self._current_errmsg += text 467 if self._current_errmsg.find('\n') != -1: 468 messages = self._current_errmsg.split('\n') 469 for m in messages[:-1]: 470 txt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">%s</pre>'%m 471 self.display.append(txt) 472 self._current_errmsg = messages[-1]

473

474 - def _append_text_hz(self, text):

475 ''' 476 Append text received through the SSH for hz view. 477 ''' 478 with self.lock: 479 self._current_msg += text 480 if self._current_msg.find('\n') != -1: 481 messages = self._current_msg.split('\n') 482 for m in messages[:-1]: 483 txt = '<div style="font-family:Fixedsys,Courier;">%s</div>'%(m) 484 self.display.append(txt) 485 self._current_msg = messages[-1]

486

487 - def _on_display_anchorClicked(self, url, user=None, pw=None):

488 try: 489 ok = False 490 if self.show_only_rate: 491 self.ssh_input_file, self.ssh_output_file, self.ssh_error_file, ok = nm.ssh().ssh_exec(url.host(), ['rostopic hz %s'%(self.topic)], user, pw, auto_pw_request=True, get_pty=True) 492 self.status_label.setText('connected to %s over SSH'%url.host()) 493 else: 494 self.combobox_displ_hz.setEnabled(False) 495 nostr = '--nostr' if self.no_str_checkbox.isChecked() else '' 496 noarr = '--noarr' if self.no_arr_checkbox.isChecked() else '' 497 self.ssh_input_file, self.ssh_output_file, self.ssh_error_file, ok = nm.ssh().ssh_exec(url.host(), ['rostopic echo %s %s %s'%(nostr, noarr, self.topic)], user, pw, auto_pw_request=True, get_pty=True) 498 if ok: 499 self.display.clear() 500 target = self._read_output_hz if self.show_only_rate else self._read_output 501 thread = threading.Thread(target=target, args=((self.ssh_output_file,))) 502 thread.setDaemon(True) 503 thread.start() 504 thread = threading.Thread(target=self._read_error, args=((self.ssh_error_file,))) 505 thread.setDaemon(True) 506 thread.start() 507 elif self.ssh_output_file: 508 self.ssh_output_file.close() 509 self.ssh_error_file.close() 510 except Exception as e: 511 self._append_error_text('%s\n'%e)

512 # import traceback 513 # print traceback.format_exc() 514

515 - def _read_output_hz(self, output_file):

516 try: 517 while not output_file.closed: 518 text = output_file.read(1) 519 if text: 520 self.text_hz_signal.emit(text) 521 except: 522 pass

523 # import traceback 524 # print traceback.format_exc() 525

526 - def _read_output(self, output_file):

527 while not output_file.closed: 528 text = output_file.read(1) 529 if text: 530 self.text_signal.emit(text)

531

532 - def _read_error(self, error_file):

533 try: 534 while not error_file.closed: 535 text = error_file.read(1) 536 if text: 537 self.text_error_signal.emit(text) 538 except: 539 pass

540 # import traceback 541 # print traceback.format_exc() 542

Source Code for Module node_manager_fkie.echo_dialog