class MicoButtonsWidget(QWidget): def __init__(self, widget): super(MicoButtonsWidget, self).__init__() rospkg_pack = rospkg.RosPack() ui_file = os.path.join(rospkg_pack.get_path('mico_controller'), 'resource', 'MicoButtons.ui') loadUi(ui_file, self) self.start_arm_srv = rospy.ServiceProxy('mico_arm_driver/in/start', MicoStart) self.stop_arm_srv = rospy.ServiceProxy('mico_arm_driver/in/stop', MicoStop) self.home_arm_srv = rospy.ServiceProxy('mico_arm_driver/in/home_arm', MicoHome) self._updateTimer = QTimer(self) self._updateTimer.timeout.connect(self.timeout_callback) def start(self): self._updateTimer.start(1000) # loop rate[ms] def stop(self): self._updateTimer.stop() def timeout_callback(self): pass # rqt override def save_settings(self, plugin_settings, instance_settings): pass # instance_settings.set_value('topic_name', self._topic_name) def restore_settings(self, plugin_settings, instance_settings): pass # topic_name = instance_settings.value('topic_name') # try: # self._topic_name = eval(topic_name) # except Exception: # self._topic_name = self.TOPIC_NAME def shutdown_plugin(self): self.stop() @Slot() def on_qt_start_btn_clicked(self): rospy.loginfo(self.start_arm_srv()) @Slot() def on_qt_stop_btn_clicked(self): rospy.loginfo(self.stop_arm_srv()) @Slot() def on_qt_home_btn_clicked(self): rospy.loginfo(self.home_arm_srv())
class MotionEditorWidget(QWidget): position_renamed = Signal(QListWidgetItem) def __init__(self, motion_publisher, robot_config): super(MotionEditorWidget, self).__init__() self.robot_config = robot_config self._motion_publisher = motion_publisher self._motion_data = MotionData(robot_config) self._filter_pattern = '' self._playback_marker = None self._playback_timer = None ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'motion_editor.ui') loadUi(ui_file, self) self.list_widgets = {} for group_type in robot_config.group_types(): list_widget = QListWidget() list_widget.setSortingEnabled(True) list_widget.setDragDropMode(QAbstractItemView.DragOnly) list_widget.setContextMenuPolicy(Qt.CustomContextMenu) list_widget.customContextMenuRequested.connect( lambda pos, _group_type=group_type: self.positions_list_context_menu(_group_type, pos) ) list_widget.itemChanged.connect(self.on_list_item_changed) self.position_lists_layout.addWidget(list_widget) self.list_widgets[group_type] = list_widget self._timeline_widget = TimelineWidget() for track_name in self.robot_config.sorted_groups(): track_type = self.robot_config.groups[track_name].group_type track = self._timeline_widget.add_track(track_name, track_type) list_widget = self.list_widgets[track_type] palette = list_widget.palette() palette.setColor(QPalette.Base, track._colors['track']) list_widget.setPalette(palette) self.timeline_group.layout().addWidget(self._timeline_widget) for group_type in robot_config.group_types(): label = QLabel(group_type) label.setAlignment(Qt.AlignHCenter | Qt.AlignVCenter) self.group_label_layout.addWidget(label) self.update_motion_name_combo() self.stop_motion_button.pressed.connect(self.on_motion_stop_pressed) def on_list_item_changed(self, item): print 'Position name changed from', item._text, 'to', item.text() self.position_renamed.emit(item) def on_motion_stop_pressed(self): self._clear_playback_marker() self._motion_publisher.stop_motion() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('splitter', self.splitter.saveState()) instance_settings.set_value('filter_pattern', self.filter_pattern_edit.text()) instance_settings.set_value('filter_motions_checked', self.filter_motions_check.isChecked()) instance_settings.set_value('filter_positions_checked', self.filter_positions_check.isChecked()) def restore_settings(self, plugin_settings, instance_settings): if instance_settings.contains('splitter'): self.splitter.restoreState(instance_settings.value('splitter')) else: self.splitter.setSizes([300, 100]) self.filter_pattern_edit.setText(instance_settings.value('filter_pattern', '')) self.filter_motions_check.setChecked(instance_settings.value('filter_motions_checked', False) in (1, True, 'true')) self.filter_positions_check.setChecked(instance_settings.value('filter_positions_checked', False) in (1, True, 'true')) @Slot() def on_clear_motion_button_clicked(self): self._timeline_widget.scene().clear() @Slot() def on_delete_motion_button_clicked(self): motion_name = str(self.motion_name_combo.currentText()) if len(motion_name) == 0: QMessageBox.warning(self, 'Error', 'No motion selected to delete.') return if motion_name not in self._motion_data: QMessageBox.warning(self, 'Error', 'Motion "%s" not in motion list.' % motion_name) return self._motion_data.delete(motion_name) self.update_motion_name_combo() @Slot() def on_save_motion_button_clicked(self): motion_name = str(self.motion_name_combo.currentText()) if len(motion_name) == 0: QMessageBox.warning(self, 'Error', 'No name given to save this motion.') return self._motion_data.save(motion_name, self.get_motion_from_timeline()) self.update_motion_name_combo() @Slot() def on_load_motion_button_clicked(self): motion_name = str(self.motion_name_combo.currentText()) if len(motion_name) == 0: QMessageBox.warning(self, 'Error', 'No motion selected to load.') return if motion_name not in self._motion_data: QMessageBox.warning(self, 'Error', 'Motion "%s" not in motion list.' % motion_name) return self.on_clear_motion_button_clicked() motion = self._motion_data[motion_name] for group_name, poses in motion.items(): for pose in poses: data = self.robot_config.groups[group_name].adapt_to_side(pose['positions']) self._timeline_widget.scene().add_clip(group_name, pose['name'], pose['starttime'], pose['duration'], data) @Slot() def on_null_motion_button_clicked(self): self._clear_playback_marker() for group_name in self.robot_config.groups: target_position = [0] * len(self.robot_config.groups[group_name].joints) self._motion_publisher.move_to_position(group_name, target_position, self.time_factor_spin.value()) @Slot() def on_run_motion_button_clicked(self): motion_name = str(self.motion_name_combo.currentText()) if len(motion_name) == 0: QMessageBox.warning(self, 'Error', 'No motion selected to run.') return if motion_name not in self._motion_data: QMessageBox.warning(self, 'Error', 'Motion "%s" not in motion list.' % motion_name) return self._clear_playback_marker() self._motion_publisher.publish_motion(self._motion_data[motion_name], self.time_factor_spin.value()) print '[Motion Editor] Running motion:', motion_name @Slot(str) def on_filter_pattern_edit_textChanged(self, pattern): self._filter_pattern = pattern self._apply_filter_to_position_lists() self.update_motion_name_combo() def _apply_filter_to_position_lists(self): for group_type in self.robot_config.group_types(): list_widget = self.list_widgets[group_type] for row in range(list_widget.count()): item = list_widget.item(row) hidden = self.filter_positions_check.isChecked() and re.search(self._filter_pattern, item.text()) is None item.setHidden(hidden) @Slot(bool) def on_filter_positions_check_toggled(self, checked): self._apply_filter_to_position_lists() @Slot(bool) def on_filter_motions_check_toggled(self, checked): self.update_motion_name_combo() def update_motion_name_combo(self): combo = self.motion_name_combo # remember selected motion name motion_name = str(combo.currentText()) # update motion names combo.clear() motion_names = self._motion_data.keys() if self.filter_motions_check.isChecked(): motion_names = [name for name in motion_names if re.search(self._filter_pattern, name) is not None] combo.addItems(sorted(motion_names)) # restore previously selected motion name motion_index = combo.findText(motion_name) combo.setCurrentIndex(motion_index) def update_positions_lists(self, positions): for group_type in self.robot_config.group_types(): list_widget = self.list_widgets[group_type] list_widget.clear() for name, position in positions[group_type].items(): item = QListWidgetItem(name) item._data = position item._text = name item._type = group_type item.setFlags(item.flags() | Qt.ItemIsEditable) list_widget.addItem(item) self._apply_filter_to_position_lists() def positions_list_context_menu(self, group_type, pos): list_widget = self.list_widgets[group_type] list_item = list_widget.itemAt(pos) if list_item is None: return menu = QMenu() move_to = {} for group in self.robot_config.group_list(): if list_item._type == group.group_type: move_to[menu.addAction('move "%s"' % group.name)] = [group.name] # move_to[menu.addAction('move all')] = list(move_to.itervalues()) move_to[menu.addAction('move all')] = [group_list[0] for group_list in list(move_to.itervalues())] result = menu.exec_(list_widget.mapToGlobal(pos)) if result in move_to: group_names = move_to[result] for group_name in group_names: target_positions = self.robot_config.groups[group_name].adapt_to_side(list_item._data) self._motion_publisher.move_to_position(group_name, target_positions, self.time_factor_spin.value()) print '[Motion Editor] Moving %s to: %s' % (group_name, zip(self.robot_config.groups[group_name].joints_sorted(), target_positions)) def get_motion_from_timeline(self): motion = {} for group_name, clips in self._timeline_widget.scene().clips().items(): motion[group_name] = [] for clip in clips: pose = { 'name': clip.text(), 'starttime': clip.starttime(), 'duration': clip.duration(), 'positions': self.robot_config.groups[group_name].adapt_to_side(clip.data()), } motion[group_name].append(pose) return motion @Slot() def on_run_timeline_button_clicked(self): print '[Motion Editor] Running timeline.' self._playback_duration = 0.0 for clips in self._timeline_widget.scene().clips().values(): if len(clips) > 0 and self._playback_duration < clips[-1].endtime(): self._playback_duration = clips[-1].endtime() self._playback_time_factor = self.time_factor_spin.value() self._clear_playback_marker() self._playback_marker = self._timeline_widget.scene().add_marker(0.0) self._playback_timer = QTimer() self._playback_timer.setInterval(30) self._playback_timer.timeout.connect(self._playback_update) self._playback_start = rospy.get_time() self._motion_publisher.publish_motion(self.get_motion_from_timeline(), self.time_factor_spin.value()) self._playback_timer.start() def _clear_playback_marker(self): if self._playback_timer is not None: self._playback_timer.stop() if self._playback_marker is not None: self._playback_marker.remove() @Slot() def _playback_update(self): duration = (rospy.get_time() - self._playback_start) / self._playback_time_factor if duration > self._playback_duration: self._clear_playback_marker() else: self._playback_marker.set_time(duration)
class RobotMonitorWidget(QWidget): """ NOTE: RobotMonitorWidget.shutdown function needs to be called when the instance of this class terminates. RobotMonitorWidget itself doesn't store previous diagnostic states. It instead delegates that function to Timeline class. """ _TREE_ALL = 1 _TREE_WARN = 2 _TREE_ERR = 3 message_updated = Signal(DiagnosticArray) def __init__(self, context, topic=None): """ :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, 'PluginContext' :param topic: Diagnostic topic to subscribe to 'str' """ super(RobotMonitorWidget, self).__init__() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_robot_monitor'), 'resource', 'robotmonitor_mainwidget.ui') loadUi(ui_file, self) obj_name = 'Robot Monitor' self.setObjectName(obj_name) self.setWindowTitle(obj_name) self.message_updated.connect(self.message_cb) # if we're given a topic, create a timeline. otherwise, don't # this can be used later when writing an rqt_bag plugin if topic: # create timeline data structure self._timeline = Timeline(topic, DiagnosticArray) self._timeline.message_updated.connect(self.message_updated) # create timeline pane widget self.timeline_pane = TimelinePane(self) self.timeline_pane.set_timeline(self._timeline) self.vlayout_top.addWidget(self.timeline_pane) self.timeline_pane.show() else: self._timeline = None self.timeline_pane = None self._inspectors = {} # keep a copy of the current message for opening new inspectors self._current_msg = None self.tree_all_devices.itemDoubleClicked.connect(self._tree_clicked) self.warn_flattree.itemDoubleClicked.connect(self._tree_clicked) self.err_flattree.itemDoubleClicked.connect(self._tree_clicked) # TODO: resize on itemCollapsed and itemExpanded self._is_stale = False self._timer = QTimer() self._timer.timeout.connect(self._update_message_state) self._timer.start(1000) palette = self.tree_all_devices.palette() self._original_base_color = palette.base().color() self._original_alt_base_color = palette.alternateBase().color() self._tree = StatusItem(self.tree_all_devices.invisibleRootItem()) self._warn_tree = StatusItem(self.warn_flattree.invisibleRootItem()) self._err_tree = StatusItem(self.err_flattree.invisibleRootItem()) @Slot(DiagnosticArray) def message_cb(self, msg): """ DiagnosticArray message callback """ self._current_msg = msg # Walk the status array and update the tree for status in msg.status: # Compute path and walk to appropriate subtree path = status.name.split('/') if path[0] == '': path = path[1:] tmp_tree = self._tree for p in path: tmp_tree = tmp_tree[p] tmp_tree.update(status, util.get_resource_name(status.name)) # Check for warnings if status.level == DiagnosticStatus.WARN: name = status.name self._warn_tree[name].update(status, status.name) # Check for errors if status.level == DiagnosticStatus.ERROR: name = status.name self._err_tree[name].update(status, status.name) # For any items in the tree that were not updated, remove them self._tree.prune() self._warn_tree.prune() self._err_tree.prune() # TODO(ahendrix): implement # Insight: for any item that is not OK, it only provides additional # information if all of it's children are OK # # otherwise, it's just an aggregation of its children # and doesn't provide any additional value when added to # the warning and error flat trees self.tree_all_devices.resizeColumnToContents(0) self.warn_flattree.resizeColumnToContents(0) self.err_flattree.resizeColumnToContents(0) def resizeEvent(self, evt): """Overridden from QWidget""" rospy.logdebug('RobotMonitorWidget resizeEvent') if self.timeline_pane: self.timeline_pane.redraw() @Slot(str) def _inspector_closed(self, name): """ Called when an inspector window is closed """ if name in self._inspectors: del self._inspectors[name] def _tree_clicked(self, item, column): """ Slot to QTreeWidget.itemDoubleClicked :type item: QTreeWidgetItem :type column: int """ rospy.logdebug('RobotMonitorWidget _tree_clicked col=%d', column) if item.name in self._inspectors: self._inspectors[item.name].activateWindow() else: self._inspectors[item.name] = InspectorWindow(self, item.name, self._current_msg, self._timeline) self._inspectors[item.name].closed.connect(self._inspector_closed) self.message_updated.connect(self._inspectors[item.name].message_updated) def _update_message_state(self): """ Update the display if it's stale """ if self._timeline is not None: if self._timeline.has_messages: previous_stale_state = self._is_stale self._is_stale = self._timeline.is_stale time_diff = int(self._timeline.data_age()) msg_template = "Last message received %s %s ago" if time_diff == 1: msg = msg_template % (time_diff, "second") else: msg = msg_template % (time_diff, "seconds") self.timeline_pane._msg_label.setText(msg) if previous_stale_state != self._is_stale: self._update_background_color() else: # no messages received yet self.timeline_pane._msg_label.setText("No messages received") def _update_background_color(self): """ Update the background color based on staleness """ p = self.tree_all_devices.palette() if self._is_stale: p.setColor(QPalette.Base, Qt.darkGray) p.setColor(QPalette.AlternateBase, Qt.lightGray) else: p.setColor(QPalette.Base, self._original_base_color) p.setColor(QPalette.AlternateBase, self._original_alt_base_color) self.tree_all_devices.setPalette(p) self.warn_flattree.setPalette(p) self.err_flattree.setPalette(p) def shutdown(self): """ This needs to be called whenever this class terminates. This closes all the instances on all trees. Also unregisters ROS' subscriber, stops timer. """ rospy.logdebug('RobotMonitorWidget in shutdown') names = self._inspectors.keys() for name in names: self._inspectors[name].close() if self._timeline: self._timeline.shutdown() self._timer.stop() del self._timer def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('splitter', self.splitter.saveState()) # TODO(ahendrix): persist the device paths, positions and sizes of any # inspector windows def restore_settings(self, plugin_settings, instance_settings): if instance_settings.contains('splitter'): self.splitter.restoreState(instance_settings.value('splitter')) else: self.splitter.setSizes([100, 100, 200])
class MainVirtJoy(QWidget): ########################################################################## ########################################################################## ##################################################################### def __init__(self): ##################################################################### super(MainVirtJoy, self).__init__() self.timer_rate = rospy.get_param('~publish_rate', 150) self.pub_twist = rospy.Publisher('twist', Twist) self.x_min = rospy.get_param("~x_min", -0.10) self.x_max = rospy.get_param("~x_max", 0.10) self.r_min = rospy.get_param("~r_min", -1.0) self.r_max = rospy.get_param("~r_max", 1.0) self.timer = QTimer() self.timer.singleShot = False self.timer.timeout.connect(self.timerEvent) ##################################################################### def mousePressEvent(self, event): ##################################################################### rospy.logdebug('-D- virtual_joystick mouse clicked') self.timer.start(self.timer_rate) self.get_position(event) ##################################################################### def mouseReleaseEvent(self, event): ##################################################################### rospy.logdebug('-D- virtual_joystick mouse released') self.x = 0.5 self.y = 0.5 self.pubTwist() self.timer.stop() ##################################################################### def mouseMoveEvent(self, event): ##################################################################### self.get_position(event) ##################################################################### def get_position(self, event): ##################################################################### s = self.size() s_w = s.width() s_h = s.height() pos = event.pos() self.x = 1.0 * pos.x() / s_w self.y = 1.0 * pos.y() / s_h rospy.logdebug('-D- virtual_joystick point (%0.2f, %0.2f)' % (self.x,self.y)) ##################################################################### def timerEvent(self): ##################################################################### rospy.logdebug("-D- virtual_joystick timer event") self.pubTwist() ####################################################### def pubTwist(self): ####################################################### # rospy.loginfo("publishing twist from (%0.3f,%0.3f)" %(self.x,self.y)) self.twist = Twist() self.twist.linear.x = (1-self.y) * (self.x_max - self.x_min) + self.x_min self.twist.linear.y = 0 self.twist.linear.z = 0 self.twist.angular.x = 0 self.twist.angular.y = 0 self.twist.angular.z = (1-self.x) * (self.r_max - self.r_min) + self.r_min if self.twist.linear.x > self.x_max: self.twist.linear.x = self.x_max if self.twist.linear.x < self.x_min: self.twist.linear.x = self.x_min if self.twist.angular.z > self.r_max: self.twist.angular.z = self.r_max if self.twist.angular.z < self.r_min: self.twist.angular.z = self.r_min self.pub_twist.publish( self.twist )
class SysCheck(Plugin): def __init__(self, context): super(SysCheck, self).__init__(context) # Set the name of the object # (Usually should be the same as the class name) self.setObjectName('SysCheck') # Get the thruster parameters self.names = [] try: self.names = rospy.get_param('thrusters/mapping') except KeyError: print "Thruster mapping not loaded into parameter server" # Setup the publisher and message object for sending thruster messages self.pub = rospy.Publisher('thruster', thruster, queue_size=1) self.thrusterMessage = thruster() # Subscribe to the depth and orientation topics self.depth_sub = rospy.Subscriber('depth', Float32Stamped, self.depthSubCallback, queue_size=1) self.imu_sub = rospy.Subscriber('pretty/orientation', Euler, self.imuSubCallback, queue_size=1) # Initialize the timers self.depthTimer = QTimer(self) self.imuTimer = QTimer(self) self.sendTimer = QTimer(self) self.depthTimer.timeout.connect(self.depthMissed) self.imuTimer.timeout.connect(self.imuMissed) self.sendTimer.timeout.connect(self.sendMessage) self.depthTimer.start(1000) self.imuTimer.start(1000) # Only start the param timer if the params aren't loaded if len(self.names) == 0: self.paramTimer = QTimer(self) self.paramTimer.timeout.connect(self.loadParam) self.paramTimer.start(1000) # Create QWidget self._widget = QWidget() # Get path to UI file which should be in the "resource" folder of # this package ui_file = os.path.join(rospkg.RosPack().get_path('robosub'), 'src/rqt/rqt_syscheck/resource', 'SysCheck.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects a name (Usually the class name + 'Ui') self._widget.setObjectName('SysCheckUi') # Add RoboSub Logo to the GUI logo_file = os.path.join(rospkg.RosPack().get_path('robosub'), 'src/rqt/resource', 'robosub_logo.png') self._widget.setStyleSheet(".QWidget {background-image: url(" + logo_file + "); background-repeat: no-repeat;" + "background-position:bottom right}") # Hide the stale labels on init self._widget.imuStale.hide() self._widget.depthStale.hide() # Connect the valueChanged signal to our updateSpeed function self._widget.thrusterSpeed.valueChanged[int].connect(self.updateSpeed) self._widget.thrusterEnable.setCheckable(True) self._widget.thrusterEnable.toggled[bool].connect(self.enable) self._widget.thrusterKill.clicked[bool].connect(self.kill) # Load in the thruster buttons and connect callbacks self.thrusterButtons = [] self.thrusterScales = [] self.thrusterCallbacks = {} self.loadThrusters() # If the context is not the root add the serial number to the window # title if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) # Add widget to the user interface context.add_widget(self._widget) def kill(self): for i in self.thrusterButtons: i.setChecked(False) self._widget.thrusterSpeed.setValue(0) def enable(self, s): if s: self.sendTimer.start(1000) else: self.sendTimer.stop() def loadThrusters(self): # Loop over all of the thruster values found in the params for i in range(0, len(self.names)): # Add a button to a list so we can mess with it later self.thrusterButtons.append(QPushButton(self.names[i]['name'])) # Modify setting of the button self.thrusterButtons[i].setCheckable(True) # Save the callbacks in a list self.thrusterCallbacks[self.names[i]['name']] = \ getattr(self, '_handle_thruster' + str(i)) # Connect the callback to the button's toggle event self.thrusterButtons[i].toggled[bool].connect( self.thrusterCallbacks[self.names[i]['name']]) # Add the button to the Ui self._widget.thrusterButtons.addWidget(self.thrusterButtons[i]) # Get the orientation self.thrusterScales.append(0) for v in self.names[i]['orientation'].values(): self.thrusterScales[i] = self.thrusterScales[i] + v # Append a value to the thruster message for this button self.thrusterMessage.data.append(0.0) print self.thrusterScales def loadParam(self): try: self.names = rospy.get_param('thrusters/mapping') self.loadThrusters() # Stop the timer if the params were successfully loaded self.paramTimer.stop() except KeyError: # Don't throw an error if we hit a KeyError pass def shutdown_plugin(self): # Stop the send timer before unregistering the publisher self.sendTimer.stop() # Unregister the thruster publisher and subscribers self.pub.unregister() self.depth_sub.unregister() self.imu_sub.unregister() # Stop the Other Timers try: self.paramTimer.stop() except AttributeError: pass self.imuTimer.stop() self.depthTimer.stop() def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass def imuMissed(self): # If an Imu message was not received by the time the timer fired # show the stale label and hide the active label if not self._widget.imuStale.isVisible(): self._widget.imuStale.show() if self._widget.imuActive.isVisible(): self._widget.imuActive.hide() def imuSubCallback(self, m): # Stop the imuTimer so it doesn't fire in this function self.imuTimer.stop() # Reset the active label hiding the stale label if self._widget.imuStale.isVisible(): self._widget.imuStale.hide() if not self._widget.imuActive.isVisible(): self._widget.imuActive.show() self._widget.currentRoll.setText(str(m.roll)) self._widget.currentPitch.setText(str(m.pitch)) self._widget.currentYaw.setText(str(m.yaw)) # Restart the timer self.imuTimer.start(1000) def depthMissed(self): # If an Depth message was not received by the time the timer fired # show the stale label and hide the active label if not self._widget.depthStale.isVisible(): self._widget.depthStale.show() if self._widget.depthActive.isVisible(): self._widget.depthActive.hide() def depthSubCallback(self, m): # Stop the imuTimer so it doesn't fire in this function self.depthTimer.stop() # Reset the active label hiding the stale label if self._widget.depthStale.isVisible(): self._widget.depthStale.hide() if not self._widget.depthActive.isVisible(): self._widget.depthActive.show() self._widget.currentDepth.setText("{0:.2f}".format(m.data)) # Restart the timer self.depthTimer.start(1000) def sendMessage(self): # Publish the message that we have constructed self.pub.publish(self.thrusterMessage) # Update the speeds in the thruster message based on the slider def updateSpeed(self, value): # Update the speed label so the user knows the value that is set self._widget.speedLabel.setText("Speed ({:+.2f})".format( float(value)/100)) # Loop over the thruster message and update the value for i in range(0, len(self.thrusterMessage.data)): # Check if the thruster is enabled if self.thrusterButtons[i].isChecked(): self.thrusterMessage.data[i] = self.thrusterScales[i] * \ float(value)/100 ''' The following functions handle updating the thruster message based on the buttons. ''' def _handle_thruster0(self, state): if state: self.thrusterMessage.data[0] = 0.01 * self.thrusterScales[0] * \ self._widget.thrusterSpeed.value() else: self.thrusterMessage.data[0] = 0 def _handle_thruster1(self, state): if state: self.thrusterMessage.data[1] = 0.01 * self.thrusterScales[1] * \ self._widget.thrusterSpeed.value() else: self.thrusterMessage.data[1] = 0 def _handle_thruster2(self, state): if state: self.thrusterMessage.data[2] = 0.01 * self.thrusterScales[2] * \ self._widget.thrusterSpeed.value() else: self.thrusterMessage.data[2] = 0 def _handle_thruster3(self, state): if state: self.thrusterMessage.data[3] = 0.01 * self.thrusterScales[3] * \ self._widget.thrusterSpeed.value() else: self.thrusterMessage.data[3] = 0 def _handle_thruster4(self, state): if state: self.thrusterMessage.data[4] = 0.01 * self.thrusterScales[4] * \ self._widget.thrusterSpeed.value() else: self.thrusterMessage.data[4] = 0 def _handle_thruster5(self, state): if state: self.thrusterMessage.data[5] = 0.01 * self.thrusterScales[5] * \ self._widget.thrusterSpeed.value() else: self.thrusterMessage.data[5] = 0 def _handle_thruster6(self, state): if state: self.thrusterMessage.data[6] = 0.01 * self.thrusterScales[6] * \ self._widget.thrusterSpeed.value() else: self.thrusterMessage.data[6] = 0 def _handle_thruster7(self, state): if state: self.thrusterMessage.data[7] = 0.01 * self.thrusterScales[7] * \ self._widget.thrusterSpeed.value() else: self.thrusterMessage.data[7] = 0
class Top(Plugin): NODE_FIELDS = [ 'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads'] OUT_FIELDS = ['node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads' ] FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s' ] NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads' ] SORT_TYPE = [str, str, float, float, float ] TOOLTIPS = { 0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))), 3: ('memory_info', lambda x: ('Resident: %0.2f MiB, Virtual: %0.2f MiB' % (x[0]/2**20, x[1]/2**20))) } _node_info = NodeInfo() name_filter = re.compile('') def __init__(self, context): super(Top, self).__init__(context) # Give QObjects reasonable names self.setObjectName('Top') # Process standalone plugin command-line arguments from argparse import ArgumentParser parser = ArgumentParser() # Add argument(s) to the parser. parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode") args, unknowns = parser.parse_known_args(context.argv()) # if not args.quiet: # print 'arguments: ', args # print 'unknowns: ', unknowns self._selected_node = '' self._selected_node_lock = RLock() # Setup the toolbar self._toolbar = QToolBar() self._filter_box = QLineEdit() self._regex_box = QCheckBox() self._regex_box.setText('regex') self._toolbar.addWidget(QLabel('Filter')) self._toolbar.addWidget(self._filter_box) self._toolbar.addWidget(self._regex_box) self._filter_box.returnPressed.connect(self.update_filter) self._regex_box.stateChanged.connect(self.update_filter) # Create a container widget and give it a layout self._container = QWidget() self._container.setWindowTitle('Process Monitor') self._layout = QVBoxLayout() self._container.setLayout(self._layout) self._layout.addWidget(self._toolbar) # Create the table widget self._table_widget = QTreeWidget() self._table_widget.setObjectName('TopTable') self._table_widget.setColumnCount(len(self.NODE_LABELS)) self._table_widget.setHeaderLabels(self.NODE_LABELS) self._table_widget.itemClicked.connect(self._tableItemClicked) self._table_widget.setSortingEnabled(True) self._table_widget.setAlternatingRowColors(True) self._layout.addWidget(self._table_widget) context.add_widget(self._container) # Add a button for killing nodes self._kill_button = QPushButton('Kill Node') self._layout.addWidget(self._kill_button) self._kill_button.clicked.connect(self._kill_node) # Update twice since the first cpu% lookup will always return 0 self.update_table() self.update_table() self._table_widget.resizeColumnToContents(0) # Start a timer to trigger updates self._update_timer = QTimer() self._update_timer.setInterval(1000) self._update_timer.timeout.connect(self.update_table) self._update_timer.start() def _tableItemClicked(self, item, column): with self._selected_node_lock: self._selected_node = item.text(0) def update_filter(self, *args): if self._regex_box.isChecked(): expr = self._filter_box.text() else: expr = re.escape(self._filter_box.text()) self.name_filter = re.compile(expr) self.update_table() def _kill_node(self): self._node_info.kill_node(self._selected_node) def update_one_item(self, row, info): twi = TopWidgetItem() for col, field in enumerate(self.OUT_FIELDS): val = info[field] twi.setText(col, self.FORMAT_STRS[col] % val) self._table_widget.insertTopLevelItem(row, twi) for col, (key, func) in self.TOOLTIPS.iteritems(): twi.setToolTip(col, func(info[key])) with self._selected_node_lock: if twi.text(0) == self._selected_node: twi.setSelected(True) self._table_widget.setItemHidden(twi, len(self.name_filter.findall(info['node_name'])) == 0) def update_table(self): self._table_widget.clear() infos = self._node_info.get_all_node_fields(self.NODE_FIELDS) for nx, info in enumerate(infos): self.update_one_item(nx, info) def shutdown_plugin(self): self._update_timer.stop() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('filter_text', self._filter_box.text()) instance_settings.set_value('is_regex', int(self._regex_box.checkState())) def restore_settings(self, plugin_settings, instance_settings): self._filter_box.setText(instance_settings.value('filter_text')) is_regex_int = instance_settings.value('is_regex') if is_regex_int: self._regex_box.setCheckState(Qt.CheckState(is_regex_int)) else: self._regex_box.setCheckState(Qt.CheckState(0)) self.update_filter()
class LifeFrame(QFrame): STATE_STOPPED = "stopped" STATE_RUN = "running" STATE_IDLE = "idle" def __init__(self, parent=None): super(LifeFrame, self).__init__(parent) self._ui = Ui_life_frame() self._motion = Rotate('/mobile_base/commands/velocity') self._motion_thread = None self._timer = QTimer() #self._timer.setInterval(60000) #60s self._timer.setInterval(250) #60s QObject.connect(self._timer, SIGNAL('timeout()'), self, SLOT('update_progress_callback()')) self._state = LifeFrame.STATE_STOPPED self._is_alive = False # Used to indicate whether the frame is alive or not (see hibernate/restore methods) def setupUi(self): self._ui.setupUi(self) self._ui.start_button.setEnabled(True) self._ui.stop_button.setEnabled(False) self._motion.init(self._ui.angular_speed_spinbox.value()) def shutdown(self): ''' Used to terminate the plugin ''' rospy.loginfo("Kobuki TestSuite: life frame shutdown") self._motion.shutdown() ########################################################################## # Widget Management ########################################################################## def hibernate(self): ''' This gets called when the frame goes out of focus (tab switch). Disable everything to avoid running N tabs in parallel when in reality we are only running one. ''' pass def restore(self): ''' Restore the frame after a hibernate. ''' pass ########################################################################## # Motion Callbacks ########################################################################## def start(self): self._state = LifeFrame.STATE_RUN self._ui.run_progress.reset() self._ui.idle_progress.reset() self._motion_thread = WorkerThread(self._motion.execute, None) self._motion_thread.start() def stop(self): self._state = LifeFrame.STATE_STOPPED self._motion.stop() if self._motion_thread: self._motion_thread.wait() ########################################################################## # Qt Callbacks ########################################################################## @Slot() def on_start_button_clicked(self): self._ui.start_button.setEnabled(False) self._ui.stop_button.setEnabled(True) self._timer.start() self.start() @Slot() def on_stop_button_clicked(self): self.stop() self._timer.stop() self._ui.start_button.setEnabled(True) self._ui.stop_button.setEnabled(False) @pyqtSlot(float) def on_angular_speed_spinbox_valueChanged(self, value): self._motion.init(self._ui.angular_speed_spinbox.value()) ########################################################################## # Timer Callbacks ########################################################################## @Slot() def update_progress_callback(self): if self._state == LifeFrame.STATE_RUN: new_value = self._ui.run_progress.value()+1 if new_value == self._ui.run_progress.maximum(): print(" Switching to idle") self._motion.stop() self._state = LifeFrame.STATE_IDLE else: self._ui.run_progress.setValue(new_value) if self._state == LifeFrame.STATE_IDLE: new_value = self._ui.idle_progress.value()+1 if new_value == self._ui.idle_progress.maximum(): print(" Switching to run") self.start() else: self._ui.idle_progress.setValue(new_value)
class ControllerManager(Plugin): """ Graphical frontend for managing ros_control controllers. """ _cm_update_freq = 1 # Hz def __init__(self, context): super(ControllerManager, self).__init__(context) self.setObjectName('ControllerManager') # Create QWidget and extend it with all the attributes and children # from the UI file self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_controller_manager'), 'resource', 'controller_manager.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('ControllerManagerUi') # Pop-up that displays controller information self._popup_widget = QWidget() ui_file = os.path.join(rp.get_path('rqt_controller_manager'), 'resource', 'controller_info.ui') loadUi(ui_file, self._popup_widget) self._popup_widget.setObjectName('ControllerInfoUi') # Show _widget.windowTitle on left-top of each plugin (when # it's set in _widget). This is useful when you open multiple # plugins at once. Also if you open multiple instances of your # plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) # Add widget to the user interface context.add_widget(self._widget) # Initialize members self._cm_ns = [] # Namespace of the selected controller manager self._controllers = [] # State of each controller self._table_model = None self._controller_lister = None # Controller manager service proxies self._load_srv = None self._unload_srv = None self._switch_srv = None # Controller state icons rospack = rospkg.RosPack() path = rospack.get_path('rqt_controller_manager') self._icons = {'running': QIcon(path + '/resource/led_green.png'), 'stopped': QIcon(path + '/resource/led_red.png'), 'uninitialized': QIcon(path + '/resource/led_off.png')} # Controllers display table_view = self._widget.table_view table_view.setContextMenuPolicy(Qt.CustomContextMenu) table_view.customContextMenuRequested.connect(self._on_ctrl_menu) table_view.doubleClicked.connect(self._on_ctrl_info) header = table_view.horizontalHeader() header.setSectionResizeMode(QHeaderView.ResizeToContents) header.setContextMenuPolicy(Qt.CustomContextMenu) header.customContextMenuRequested.connect(self._on_header_menu) # Timer for controller manager updates self._list_cm = ControllerManagerLister() self._update_cm_list_timer = QTimer(self) self._update_cm_list_timer.setInterval(1000.0 / self._cm_update_freq) self._update_cm_list_timer.timeout.connect(self._update_cm_list) self._update_cm_list_timer.start() # Timer for running controller updates self._update_ctrl_list_timer = QTimer(self) self._update_ctrl_list_timer.setInterval(1000.0 / self._cm_update_freq) self._update_ctrl_list_timer.timeout.connect(self._update_controllers) self._update_ctrl_list_timer.start() # Signal connections w = self._widget w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change) def shutdown_plugin(self): self._update_cm_list_timer.stop() self._update_ctrl_list_timer.stop() self._popup_widget.hide() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('cm_ns', self._cm_ns) def restore_settings(self, plugin_settings, instance_settings): # Restore last session's controller_manager, if present self._update_cm_list() cm_ns = instance_settings.value('cm_ns') cm_combo = self._widget.cm_combo cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] try: idx = cm_list.index(cm_ns) cm_combo.setCurrentIndex(idx) except (ValueError): pass # def trigger_configuration(self): # Comment in to signal that the plugin has a way to configure # This will enable a setting button (gear icon) in each dock widget # title bar # Usually used to open a modal configuration dialog def _update_cm_list(self): update_combo(self._widget.cm_combo, self._list_cm()) def _on_cm_change(self, cm_ns): self._cm_ns = cm_ns # Setup services for communicating with the selected controller manager self._set_cm_services(cm_ns) # Controller lister for the selected controller manager if cm_ns: self._controller_lister = ControllerLister(cm_ns) self._update_controllers() else: self._controller_lister = None def _set_cm_services(self, cm_ns): if cm_ns: # NOTE: Persistent services are used for performance reasons. # If the controller manager dies, we detect it and disconnect from # it anyway load_srv_name = _append_ns(cm_ns, 'load_controller') self._load_srv = rospy.ServiceProxy(load_srv_name, LoadController, persistent=True) unload_srv_name = _append_ns(cm_ns, 'unload_controller') self._unload_srv = rospy.ServiceProxy(unload_srv_name, UnloadController, persistent=True) switch_srv_name = _append_ns(cm_ns, 'switch_controller') self._switch_srv = rospy.ServiceProxy(switch_srv_name, SwitchController, persistent=True) else: self._load_srv = None self._unload_srv = None self._switch_srv = None def _update_controllers(self): # Find controllers associated to the selected controller manager controllers = self._list_controllers() # Update controller display, if necessary if self._controllers != controllers: self._controllers = controllers self._show_controllers() # NOTE: Model is recomputed from scratch def _list_controllers(self): """ @return List of controllers associated to a controller manager namespace. Contains both stopped/running controllers, as returned by the C{list_controllers} service, plus uninitialized controllers with configurations loaded in the parameter server. @rtype [str] """ if not self._cm_ns: return [] # Add loaded controllers first controllers = self._controller_lister() # Append potential controller configs found in the parameter server all_ctrls_ns = _resolve_controllers_ns(self._cm_ns) for name in get_rosparam_controller_names(all_ctrls_ns): add_ctrl = not any(name == ctrl.name for ctrl in controllers) if add_ctrl: type_str = _rosparam_controller_type(all_ctrls_ns, name) uninit_ctrl = ControllerState(name=name, type=type_str, state='uninitialized') controllers.append(uninit_ctrl) return controllers def _show_controllers(self): table_view = self._widget.table_view self._table_model = ControllerTable(self._controllers, self._icons) table_view.setModel(self._table_model) def _on_ctrl_menu(self, pos): # Get data of selected controller row = self._widget.table_view.rowAt(pos.y()) if row < 0: return # Cursor is not under a valid item ctrl = self._controllers[row] # Show context menu menu = QMenu(self._widget.table_view) if ctrl.state == 'running': action_stop = menu.addAction(self._icons['stopped'], 'Stop') action_kill = menu.addAction(self._icons['uninitialized'], 'Stop and Unload') elif ctrl.state == 'stopped': action_start = menu.addAction(self._icons['running'], 'Start') action_unload = menu.addAction(self._icons['uninitialized'], 'Unload') elif ctrl.state == 'uninitialized': action_load = menu.addAction(self._icons['stopped'], 'Load') action_spawn = menu.addAction(self._icons['running'], 'Load and Start') action = menu.exec_(self._widget.table_view.mapToGlobal(pos)) # Evaluate user action if ctrl.state == 'running': if action is action_stop: self._stop_controller(ctrl.name) elif action is action_kill: self._stop_controller(ctrl.name) self._unload_controller(ctrl.name) elif ctrl.state == 'stopped': if action is action_start: self._start_controller(ctrl.name) elif action is action_unload: self._unload_controller(ctrl.name) elif ctrl.state == 'uninitialized': if action is action_load: self._load_controller(ctrl.name) if action is action_spawn: self._load_controller(ctrl.name) self._start_controller(ctrl.name) def _on_ctrl_info(self, index): popup = self._popup_widget ctrl = self._controllers[index.row()] popup.ctrl_name.setText(ctrl.name) popup.ctrl_type.setText(ctrl.type) res_model = QStandardItemModel() model_root = QStandardItem('Claimed Resources') res_model.appendRow(model_root) for hw_res in ctrl.claimed_resources: hw_iface_item = QStandardItem(hw_res.hardware_interface) model_root.appendRow(hw_iface_item) for res in hw_res.resources: res_item = QStandardItem(res) hw_iface_item.appendRow(res_item) popup.resource_tree.setModel(res_model) popup.resource_tree.setItemDelegate(FontDelegate(popup.resource_tree)) popup.resource_tree.expandAll() popup.move(QCursor.pos()) popup.show() def _on_header_menu(self, pos): header = self._widget.table_view.horizontalHeader() # Show context menu menu = QMenu(self._widget.table_view) action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize') action = menu.exec_(header.mapToGlobal(pos)) # Evaluate user action if action is action_toggle_auto_resize: if header.resizeMode(0) == QHeaderView.ResizeToContents: header.setSectionResizeMode(QHeaderView.Interactive) else: header.setSectionResizeMode(QHeaderView.ResizeToContents) def _load_controller(self, name): self._load_srv.call(LoadControllerRequest(name=name)) def _unload_controller(self, name): self._unload_srv.call(UnloadControllerRequest(name=name)) def _start_controller(self, name): strict = SwitchControllerRequest.STRICT req = SwitchControllerRequest(start_controllers=[name], stop_controllers=[], strictness=strict) self._switch_srv.call(req) def _stop_controller(self, name): strict = SwitchControllerRequest.STRICT req = SwitchControllerRequest(start_controllers=[], stop_controllers=[name], strictness=strict) self._switch_srv.call(req)
class ROSData(object): """ Subscriber to ROS topic that buffers incoming data """ def __init__(self, topic_code, topic_item, start_time): self.name = topic_code + '/' + topic_item self.start_time = start_time self.error = None self.lock = threading.Lock() self.buff_x = [] self.buff_y = [] self.interval = 100 # in milliseconds, period of regular update self.timer = QTimer() self.timer.setInterval(self.interval) self.code = topic_code self.item = topic_item # go through options and decide what your self.data will be, given the ros subscribers if topic_code == 's': if topic_item == 'chi': self.timer.timeout.connect(self.state_chi_cb) elif topic_item == 'phi': self.timer.timeout.connect(self.state_phi_cb) elif topic_item == 'theta': self.timer.timeout.connect(self.state_theta_cb) elif topic_item == 'Va': self.timer.timeout.connect(self.state_Va_cb) elif topic_code == 'ci': if topic_item == 'phi_c': self.timer.timeout.connect(self.conin_phi_c_cb) elif topic_item == 'theta_c': self.timer.timeout.connect(self.conin_theta_c_cb) elif topic_code == 'cc': if topic_item == 'chi_c': self.timer.timeout.connect(self.concom_chi_c_cb) elif topic_item == 'Va_c': self.timer.timeout.connect(self.concom_Va_c_cb) self.timer.start() def close(self): self.timer.stop() def state_chi_cb(self): self.buff_x.append(rospy.get_time() - self.start_time) self.buff_y.append(StateSub.chi) def state_phi_cb(self): self.buff_x.append(rospy.get_time() - self.start_time) self.buff_y.append(StateSub.phi) def state_theta_cb(self): self.buff_x.append(rospy.get_time() - self.start_time) self.buff_y.append(StateSub.theta) def state_Va_cb(self): self.buff_x.append(rospy.get_time() - self.start_time) self.buff_y.append(StateSub.Va) def conin_phi_c_cb(self): self.buff_x.append(rospy.get_time() - self.start_time) self.buff_y.append(ConInSub.phi_c) def conin_theta_c_cb(self): self.buff_x.append(rospy.get_time() - self.start_time) self.buff_y.append(ConInSub.theta_c) def concom_chi_c_cb(self): self.buff_x.append(rospy.get_time() - self.start_time) self.buff_y.append(ConComSub.chi_c) def concom_Va_c_cb(self): self.buff_x.append(rospy.get_time() - self.start_time) self.buff_y.append(ConComSub.Va_c) def __next__(self): """ Get the next data in the series :returns: [xdata], [ydata] """ if self.error: raise self.error try: self.lock.acquire() buff_x = self.buff_x buff_y = self.buff_y self.buff_x = [] self.buff_y = [] finally: self.lock.release() return buff_x, buff_y
class HistogramPlotWidget(QWidget): _redraw_interval = 40 def __init__(self, topics): super(HistogramPlotWidget, self).__init__() self.setObjectName('HistogramPlotWidget') rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource', 'plot_histogram.ui') loadUi(ui_file, self) self.cv_bridge = CvBridge() self.subscribe_topic_button.setIcon(QIcon.fromTheme('add')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = MatHistogramPlot(self) self.data_plot_layout.addWidget(self.data_plot) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent self._start_time = rospy.get_time() self._rosdata = None if len(topics) != 0: self.subscribe_topic(topics) self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) self._update_plot_timer.start(self._redraw_interval) @Slot('QDropEvent*') def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.subscribe_topic(topic_name) @Slot() def on_topic_edit_returnPressed(self): if self.subscribe_topic_button.isEnabled(): self.subscribe_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.subscribe_topic(str(self.topic_edit.text())) def subscribe_topic(self, topic_name): self.topic_with_field_name = topic_name self.pub_image = rospy.Publisher(topic_name + "/histogram_image", Image) if not self._rosdata: self._rosdata = ROSData(topic_name, self._start_time) else: if self._rosdata != topic_name: self._rosdata.close() self.data_plot.clear() self._rosdata = ROSData(topic_name, self._start_time) else: rospy.logwarn("%s is already subscribed", topic_name) def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop() @Slot() def on_clear_button_clicked(self): self.data_plot.clear() @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) def update_plot(self): if not self._rosdata: return data_x, data_y = self._rosdata.next() if len(data_y) == 0: return axes = self.data_plot._canvas.axes axes.cla() if isinstance(data_y[-1], HistogramWithRange): xs = [y.count for y in data_y[-1].bins] pos = [y.min_value for y in data_y[-1].bins] widths = [y.max_value - y.min_value for y in data_y[-1].bins] axes.set_xlim(xmin=pos[0], xmax=pos[-1] + widths[-1]) elif isinstance(data_y[-1], collections.Sequence): xs = data_y[-1] pos = np.arange(len(xs)) widths = [1] * len(xs) axes.set_xlim(xmin=0, xmax=len(xs)) else: rospy.logerr( "Topic/Field name '%s' has unsupported '%s' type." "List of float values and " "jsk_recognition_msgs/HistogramWithRange are supported." % (self.topic_with_field_name, self._rosdata.sub.data_class)) return #axes.xticks(range(5)) for p, x, w in zip(pos, xs, widths): axes.bar(p, x, color='r', align='center', width=w) axes.legend([self.topic_with_field_name], prop={'size': '8'}) self.data_plot._canvas.draw() buffer = StringIO() self.data_plot._canvas.figure.savefig(buffer, format="png") buffer.seek(0) img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8) img = cv2.imdecode(img_array, cv2.CV_LOAD_IMAGE_COLOR) self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class Top(Plugin): NODE_FIELDS = [ 'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads'] OUT_FIELDS = ['node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads' ] FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s' ] NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads' ] SORT_TYPE = [str, str, float, float, float ] TOOLTIPS = { 0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))), 3: ('memory_info', lambda x: ('Resident: %0.2f MiB, Virtual: %0.2f MiB' % (x[0]/2**20, x[1]/2**20))) } _node_info = NodeInfo() name_filter = re.compile('') def __init__(self, context): super(Top, self).__init__(context) # Give QObjects reasonable names self.setObjectName('Top') # Process standalone plugin command-line arguments from argparse import ArgumentParser parser = ArgumentParser() # Add argument(s) to the parser. parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode") args, unknowns = parser.parse_known_args(context.argv()) # if not args.quiet: # print 'arguments: ', args # print 'unknowns: ', unknowns self._selected_node = '' self._selected_node_lock = RLock() # Setup the toolbar self._toolbar = QToolBar() self._filter_box = QLineEdit() self._regex_box = QCheckBox() self._regex_box.setText('regex') self._toolbar.addWidget(QLabel('Filter')) self._toolbar.addWidget(self._filter_box) self._toolbar.addWidget(self._regex_box) self._filter_box.returnPressed.connect(self.update_filter) self._regex_box.stateChanged.connect(self.update_filter) # Create a container widget and give it a layout self._container = QWidget() self._container.setWindowTitle('Process Monitor') self._layout = QVBoxLayout() self._container.setLayout(self._layout) self._layout.addWidget(self._toolbar) # Create the table widget self._table_widget = QTreeWidget() self._table_widget.setObjectName('TopTable') self._table_widget.setColumnCount(len(self.NODE_LABELS)) self._table_widget.setHeaderLabels(self.NODE_LABELS) self._table_widget.itemClicked.connect(self._tableItemClicked) self._table_widget.setSortingEnabled(True) self._table_widget.setAlternatingRowColors(True) self._layout.addWidget(self._table_widget) context.add_widget(self._container) # Add a button for killing nodes self._kill_button = QPushButton('Kill Node') self._layout.addWidget(self._kill_button) self._kill_button.clicked.connect(self._kill_node) # Update twice since the first cpu% lookup will always return 0 self.update_table() self.update_table() self._table_widget.resizeColumnToContents(0) # Start a timer to trigger updates self._update_timer = QTimer() self._update_timer.setInterval(1000) self._update_timer.timeout.connect(self.update_table) self._update_timer.start() def _tableItemClicked(self, item, column): with self._selected_node_lock: self._selected_node = item.text(0) def update_filter(self, *args): if self._regex_box.isChecked(): expr = self._filter_box.text() else: expr = re.escape(self._filter_box.text()) self.name_filter = re.compile(expr) self.update_table() def _kill_node(self): self._node_info.kill_node(self._selected_node) def update_one_item(self, row, info): twi = TopWidgetItem() for col, field in enumerate(self.OUT_FIELDS): val = info[field] twi.setText(col, self.FORMAT_STRS[col] % val) self._table_widget.insertTopLevelItem(row, twi) for col, (key, func) in self.TOOLTIPS.iteritems(): twi.setToolTip(col, func(info[key])) with self._selected_node_lock: if twi.text(0) == self._selected_node: twi.setSelected(True) #self._table_widget.setItemHidden(twi, len(self.name_filter.findall(info['node_name'])) == 0) twi.setHidden(len(self.name_filter.findall(info['node_name'])) == 0) def update_table(self): self._table_widget.clear() infos = self._node_info.get_all_node_fields(self.NODE_FIELDS) for nx, info in enumerate(infos): self.update_one_item(nx, info) def shutdown_plugin(self): self._update_timer.stop() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('filter_text', self._filter_box.text()) instance_settings.set_value('is_regex', int(self._regex_box.checkState())) def restore_settings(self, plugin_settings, instance_settings): self._filter_box.setText(instance_settings.value('filter_text')) is_regex_int = instance_settings.value('is_regex') if is_regex_int: self._regex_box.setCheckState(Qt.CheckState(is_regex_int)) else: self._regex_box.setCheckState(Qt.CheckState(0)) self.update_filter()
class BagTimeline(QGraphicsScene): """ BagTimeline contains bag files, all information required to display the bag data visualization on the screen Also handles events """ status_bar_changed_signal = Signal() selected_region_changed = Signal(rospy.Time, rospy.Time) def __init__(self, context, publish_clock): """ :param context: plugin context hook to enable adding rqt_bag plugin widgets as ROS_GUI snapin panes, ''PluginContext'' """ super(BagTimeline, self).__init__() self._bags = [] self._bag_lock = threading.RLock() self.background_task = None # Display string self.background_task_cancel = False # Playing / Recording self._playhead_lock = threading.RLock() self._max_play_speed = 1024.0 # fastest X play speed self._min_play_speed = 1.0 / 1024.0 # slowest X play speed self._play_speed = 0.0 self._play_all = False self._playhead_positions_cvs = {} self._playhead_positions = {} # topic -> (bag, position) self._message_loaders = {} self._messages_cvs = {} self._messages = {} # topic -> (bag, msg_data) self._message_listener_threads = {} # listener -> MessageListenerThread self._player = False self._publish_clock = publish_clock self._recorder = None self.last_frame = None self.last_playhead = None self.desired_playhead = None self.wrap = True # should the playhead wrap when it reaches the end? self.stick_to_end = False # should the playhead stick to the end? self._play_timer = QTimer() self._play_timer.timeout.connect(self.on_idle) self._play_timer.setInterval(3) # Plugin popup management self._context = context self.popups = {} self._views = [] self._listeners = {} # Initialize scene # the timeline renderer fixes use of black pens and fills, so ensure we fix white here for contrast. # otherwise a dark qt theme will default it to black and the frame render pen will be unreadable self.setBackgroundBrush(Qt.white) self._timeline_frame = TimelineFrame(self) self._timeline_frame.setPos(0, 0) self.addItem(self._timeline_frame) self.background_progress = 0 self.__closed = False def get_context(self): """ :returns: the ROS_GUI context, 'PluginContext' """ return self._context def handle_close(self): """ Cleans up the timeline, bag and any threads """ if self.__closed: return else: self.__closed = True self._play_timer.stop() for topic in self._get_topics(): self.stop_publishing(topic) self._message_loaders[topic].stop() if self._player: self._player.stop() if self._recorder: self._recorder.stop() if self.background_task is not None: self.background_task_cancel = True self._timeline_frame.handle_close() for bag in self._bags: bag.close() for frame in self._views: if frame.parent(): self._context.remove_widget(frame) # Bag Management and access def add_bag(self, bag): """ creates an indexing thread for each new topic in the bag fixes the boarders and notifies the indexing thread to index the new items bags :param bag: ros bag file, ''rosbag.bag'' """ self._bags.append(bag) bag_topics = bag_helper.get_topics(bag) new_topics = set(bag_topics) - set(self._timeline_frame.topics) for topic in new_topics: self._playhead_positions_cvs[topic] = threading.Condition() self._messages_cvs[topic] = threading.Condition() self._message_loaders[topic] = MessageLoaderThread(self, topic) self._timeline_frame._start_stamp = self._get_start_stamp() self._timeline_frame._end_stamp = self._get_end_stamp() self._timeline_frame.topics = self._get_topics() self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype() # If this is the first bag, reset the timeline if self._timeline_frame._stamp_left is None: self._timeline_frame.reset_timeline() # Invalidate entire index cache for all topics in this bag with self._timeline_frame.index_cache_cv: for topic in bag_topics: self._timeline_frame.invalidated_caches.add(topic) if topic in self._timeline_frame.index_cache: del self._timeline_frame.index_cache[topic] self._timeline_frame.index_cache_cv.notify() def file_size(self): with self._bag_lock: return sum(b.size for b in self._bags) #TODO Rethink API and if these need to be visible def _get_start_stamp(self): """ :return: first stamp in the bags, ''rospy.Time'' """ with self._bag_lock: start_stamp = None for bag in self._bags: bag_start_stamp = bag_helper.get_start_stamp(bag) if bag_start_stamp is not None and (start_stamp is None or bag_start_stamp < start_stamp): start_stamp = bag_start_stamp return start_stamp def _get_end_stamp(self): """ :return: last stamp in the bags, ''rospy.Time'' """ with self._bag_lock: end_stamp = None for bag in self._bags: bag_end_stamp = bag_helper.get_end_stamp(bag) if bag_end_stamp is not None and (end_stamp is None or bag_end_stamp > end_stamp): end_stamp = bag_end_stamp return end_stamp def _get_topics(self): """ :return: sorted list of topic names, ''list(str)'' """ with self._bag_lock: topics = set() for bag in self._bags: for topic in bag_helper.get_topics(bag): topics.add(topic) return sorted(topics) def _get_topics_by_datatype(self): """ :return: dict of list of topics for each datatype, ''dict(datatype:list(topic))'' """ with self._bag_lock: topics_by_datatype = {} for bag in self._bags: for datatype, topics in bag_helper.get_topics_by_datatype(bag).items(): topics_by_datatype.setdefault(datatype, []).extend(topics) return topics_by_datatype def get_datatype(self, topic): """ :return: datatype associated with a topic, ''str'' :raises: if there are multiple datatypes assigned to a single topic, ''Exception'' """ with self._bag_lock: datatype = None for bag in self._bags: bag_datatype = bag_helper.get_datatype(bag, topic) if datatype and bag_datatype and (bag_datatype != datatype): raise Exception('topic %s has multiple datatypes: %s and %s' % (topic, datatype, bag_datatype)) if bag_datatype: datatype = bag_datatype return datatype def get_entries(self, topics, start_stamp, end_stamp): """ generator function for bag entries :param topics: list of topics to query, ''list(str)'' :param start_stamp: stamp to start at, ''rospy.Time'' :param end_stamp: stamp to end at, ''rospy,Time'' :returns: entries the bag file, ''msg'' """ with self._bag_lock: from rosbag import bag # for _mergesort bag_entries = [] for b in self._bags: bag_start_time = bag_helper.get_start_stamp(b) if bag_start_time is not None and bag_start_time > end_stamp: continue bag_end_time = bag_helper.get_end_stamp(b) if bag_end_time is not None and bag_end_time < start_stamp: continue connections = list(b._get_connections(topics)) bag_entries.append(b._get_entries(connections, start_stamp, end_stamp)) for entry, _ in bag._mergesort(bag_entries, key=lambda entry: entry.time): yield entry def get_entries_with_bags(self, topic, start_stamp, end_stamp): """ generator function for bag entries :param topics: list of topics to query, ''list(str)'' :param start_stamp: stamp to start at, ''rospy.Time'' :param end_stamp: stamp to end at, ''rospy,Time'' :returns: tuple of (bag, entry) for the entries in the bag file, ''(rosbag.bag, msg)'' """ with self._bag_lock: from rosbag import bag # for _mergesort bag_entries = [] bag_by_iter = {} for b in self._bags: bag_start_time = bag_helper.get_start_stamp(b) if bag_start_time is not None and bag_start_time > end_stamp: continue bag_end_time = bag_helper.get_end_stamp(b) if bag_end_time is not None and bag_end_time < start_stamp: continue connections = list(b._get_connections(topic)) it = iter(b._get_entries(connections, start_stamp, end_stamp)) bag_by_iter[it] = b bag_entries.append(it) for entry, it in bag._mergesort(bag_entries, key=lambda entry: entry.time): yield bag_by_iter[it], entry def get_entry(self, t, topic): """ Access a bag entry :param t: time, ''rospy.Time'' :param topic: the topic to be accessed, ''str'' :return: tuple of (bag, entry) corisponding to time t and topic, ''(rosbag.bag, msg)'' """ with self._bag_lock: entry_bag, entry = None, None for bag in self._bags: bag_entry = bag._get_entry(t, bag._get_connections(topic)) if bag_entry and (not entry or bag_entry.time > entry.time): entry_bag, entry = bag, bag_entry return entry_bag, entry def get_entry_before(self, t): """ Access a bag entry :param t: time, ''rospy.Time'' :return: tuple of (bag, entry) corresponding to time t, ''(rosbag.bag, msg)'' """ with self._bag_lock: entry_bag, entry = None, None for bag in self._bags: bag_entry = bag._get_entry(t-rospy.Duration(0,1), bag._get_connections()) if bag_entry and (not entry or bag_entry.time < entry.time): entry_bag, entry = bag, bag_entry return entry_bag, entry def get_entry_after(self, t): """ Access a bag entry :param t: time, ''rospy.Time'' :return: tuple of (bag, entry) corisponding to time t, ''(rosbag.bag, msg)'' """ with self._bag_lock: entry_bag, entry = None, None for bag in self._bags: bag_entry = bag._get_entry_after(t, bag._get_connections()) if bag_entry and (not entry or bag_entry.time < entry.time): entry_bag, entry = bag, bag_entry return entry_bag, entry def get_next_message_time(self): """ :return: time of the next message after the current playhead position,''rospy.Time'' """ if self._timeline_frame.playhead is None: return None _, entry = self.get_entry_after(self._timeline_frame.playhead) if entry is None: return self._timeline_frame._start_stamp return entry.time def get_previous_message_time(self): """ :return: time of the next message before the current playhead position,''rospy.Time'' """ if self._timeline_frame.playhead is None: return None _, entry = self.get_entry_before(self._timeline_frame.playhead) if entry is None: return self._timeline_frame._end_stamp return entry.time def resume(self): if (self._player): self._player.resume() ### Copy messages to... def start_background_task(self, background_task): """ Verify that a background task is not currently running before starting a new one :param background_task: name of the background task, ''str'' """ if self.background_task is not None: QMessageBox(QMessageBox.Warning, 'Exclamation', 'Background operation already running:\n\n%s' % self.background_task, QMessageBox.Ok).exec_() return False self.background_task = background_task self.background_task_cancel = False return True def stop_background_task(self): self.background_task = None def copy_region_to_bag(self, filename): if len(self._bags) > 0: self._export_region(filename, self._timeline_frame.topics, self._timeline_frame.play_region[0], self._timeline_frame.play_region[1]) def _export_region(self, path, topics, start_stamp, end_stamp): """ Starts a thread to save the current selection to a new bag file :param path: filesystem path to write to, ''str'' :param topics: topics to write to the file, ''list(str)'' :param start_stamp: start of area to save, ''rospy.Time'' :param end_stamp: end of area to save, ''rospy.Time'' """ if not self.start_background_task('Copying messages to "%s"' % path): return # TODO implement a status bar area with information on the current save status bag_entries = list(self.get_entries_with_bags(topics, start_stamp, end_stamp)) if self.background_task_cancel: return # Get the total number of messages to copy total_messages = len(bag_entries) # If no messages, prompt the user and return if total_messages == 0: QMessageBox(QMessageBox.Warning, 'rqt_bag', 'No messages found', QMessageBox.Ok).exec_() self.stop_background_task() return # Open the path for writing try: export_bag = rosbag.Bag(path, 'w') except Exception: QMessageBox(QMessageBox.Warning, 'rqt_bag', 'Error opening bag file [%s] for writing' % path, QMessageBox.Ok).exec_() self.stop_background_task() return # Run copying in a background thread self._export_thread = threading.Thread(target=self._run_export_region, args=(export_bag, topics, start_stamp, end_stamp, bag_entries)) self._export_thread.start() def _run_export_region(self, export_bag, topics, start_stamp, end_stamp, bag_entries): """ Threaded function that saves the current selection to a new bag file :param export_bag: bagfile to write to, ''rosbag.bag'' :param topics: topics to write to the file, ''list(str)'' :param start_stamp: start of area to save, ''rospy.Time'' :param end_stamp: end of area to save, ''rospy.Time'' """ total_messages = len(bag_entries) update_step = max(1, total_messages / 100) message_num = 1 progress = 0 # Write out the messages for bag, entry in bag_entries: if self.background_task_cancel: break try: topic, msg, t = self.read_message(bag, entry.position) export_bag.write(topic, msg, t) except Exception as ex: qWarning('Error exporting message at position %s: %s' % (str(entry.position), str(ex))) export_bag.close() self.stop_background_task() return if message_num % update_step == 0 or message_num == total_messages: new_progress = int(100.0 * (float(message_num) / total_messages)) if new_progress != progress: progress = new_progress if not self.background_task_cancel: self.background_progress = progress self.status_bar_changed_signal.emit() message_num += 1 # Close the bag try: self.background_progress = 0 self.status_bar_changed_signal.emit() export_bag.close() except Exception as ex: QMessageBox(QMessageBox.Warning, 'rqt_bag', 'Error closing bag file [%s]: %s' % (export_bag.filename, str(ex)), QMessageBox.Ok).exec_() self.stop_background_task() def read_message(self, bag, position): with self._bag_lock: return bag._read_message(position) ### Mouse events def on_mouse_down(self, event): if event.buttons() == Qt.LeftButton: self._timeline_frame.on_left_down(event) elif event.buttons() == Qt.MidButton: self._timeline_frame.on_middle_down(event) elif event.buttons() == Qt.RightButton: topic = self._timeline_frame.map_y_to_topic(self.views()[0].mapToScene(event.pos()).y()) TimelinePopupMenu(self, event, topic) def on_mouse_up(self, event): self._timeline_frame.on_mouse_up(event) def on_mouse_move(self, event): self._timeline_frame.on_mouse_move(event) def on_mousewheel(self, event): self._timeline_frame.on_mousewheel(event) # Zooming def zoom_in(self): self._timeline_frame.zoom_in() def zoom_out(self): self._timeline_frame.zoom_out() def reset_zoom(self): self._timeline_frame.reset_zoom() def translate_timeline_left(self): self._timeline_frame.translate_timeline_left() def translate_timeline_right(self): self._timeline_frame.translate_timeline_right() ### Publishing def is_publishing(self, topic): return self._player and self._player.is_publishing(topic) def start_publishing(self, topic): if not self._player and not self._create_player(): return False self._player.start_publishing(topic) return True def stop_publishing(self, topic): if not self._player: return False self._player.stop_publishing(topic) return True def _create_player(self): if not self._player: try: self._player = Player(self) if self._publish_clock: self._player.start_clock_publishing() except Exception as ex: qWarning('Error starting player; aborting publish: %s' % str(ex)) return False return True def set_publishing_state(self, start_publishing): if start_publishing: for topic in self._timeline_frame.topics: if not self.start_publishing(topic): break else: for topic in self._timeline_frame.topics: self.stop_publishing(topic) # property: play_all def _get_play_all(self): return self._play_all def _set_play_all(self, play_all): if play_all == self._play_all: return self._play_all = not self._play_all self.last_frame = None self.last_playhead = None self.desired_playhead = None play_all = property(_get_play_all, _set_play_all) def toggle_play_all(self): self.play_all = not self.play_all ### Playing def on_idle(self): self._step_playhead() def _step_playhead(self): """ moves the playhead to the next position based on the desired position """ # Reset when the playing mode switchs if self._timeline_frame.playhead != self.last_playhead: self.last_frame = None self.last_playhead = None self.desired_playhead = None if self._play_all: self.step_next_message() else: self.step_fixed() def step_fixed(self): """ Moves the playhead a fixed distance into the future based on the current play speed """ if self.play_speed == 0.0 or not self._timeline_frame.playhead: self.last_frame = None self.last_playhead = None return now = rospy.Time.from_sec(time.time()) if self.last_frame: # Get new playhead if self.stick_to_end: new_playhead = self.end_stamp else: new_playhead = self._timeline_frame.playhead + rospy.Duration.from_sec((now - self.last_frame).to_sec() * self.play_speed) start_stamp, end_stamp = self._timeline_frame.play_region if new_playhead > end_stamp: if self.wrap: if self.play_speed > 0.0: new_playhead = start_stamp else: new_playhead = end_stamp else: new_playhead = end_stamp if self.play_speed > 0.0: self.stick_to_end = True elif new_playhead < start_stamp: if self.wrap: if self.play_speed < 0.0: new_playhead = end_stamp else: new_playhead = start_stamp else: new_playhead = start_stamp # Update the playhead self._timeline_frame.playhead = new_playhead self.last_frame = now self.last_playhead = self._timeline_frame.playhead def step_next_message(self): """ Move the playhead to the next message """ if self.play_speed <= 0.0 or not self._timeline_frame.playhead: self.last_frame = None self.last_playhead = None return if self.last_frame: if not self.desired_playhead: self.desired_playhead = self._timeline_frame.playhead else: delta = rospy.Time.from_sec(time.time()) - self.last_frame if delta > rospy.Duration.from_sec(0.1): delta = rospy.Duration.from_sec(0.1) self.desired_playhead += delta # Get the occurrence of the next message next_message_time = self.get_next_message_time() if next_message_time < self.desired_playhead: self._timeline_frame.playhead = next_message_time else: self._timeline_frame.playhead = self.desired_playhead self.last_frame = rospy.Time.from_sec(time.time()) self.last_playhead = self._timeline_frame.playhead ### Recording def record_bag(self, filename, all=True, topics=[], regex=False, limit=0): try: self._recorder = Recorder(filename, bag_lock=self._bag_lock, all=all, topics=topics, regex=regex, limit=limit) except Exception as ex: qWarning('Error opening bag for recording [%s]: %s' % (filename, str(ex))) return self._recorder.add_listener(self._message_recorded) self.add_bag(self._recorder.bag) self._recorder.start() self.wrap = False self._timeline_frame._index_cache_thread.period = 0.1 self.update() def toggle_recording(self): if self._recorder: self._recorder.toggle_paused() self.update() def _message_recorded(self, topic, msg, t): if self._timeline_frame._start_stamp is None: self._timeline_frame._start_stamp = t self._timeline_frame._end_stamp = t self._timeline_frame._playhead = t elif self._timeline_frame._end_stamp is None or t > self._timeline_frame._end_stamp: self._timeline_frame._end_stamp = t if not self._timeline_frame.topics or topic not in self._timeline_frame.topics: self._timeline_frame.topics = self._get_topics() self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype() self._playhead_positions_cvs[topic] = threading.Condition() self._messages_cvs[topic] = threading.Condition() self._message_loaders[topic] = MessageLoaderThread(self, topic) if self._timeline_frame._stamp_left is None: self.reset_zoom() # Notify the index caching thread that it has work to do with self._timeline_frame.index_cache_cv: self._timeline_frame.invalidated_caches.add(topic) self._timeline_frame.index_cache_cv.notify() if topic in self._listeners: for listener in self._listeners[topic]: try: listener.timeline_changed() except Exception as ex: qWarning('Error calling timeline_changed on %s: %s' % (type(listener), str(ex))) ### Views / listeners def add_view(self, topic, frame): self._views.append(frame) def has_listeners(self, topic): return topic in self._listeners def add_listener(self, topic, listener): self._listeners.setdefault(topic, []).append(listener) self._message_listener_threads[(topic, listener)] = MessageListenerThread(self, topic, listener) # Notify the message listeners self._message_loaders[topic].reset() with self._playhead_positions_cvs[topic]: self._playhead_positions_cvs[topic].notify_all() self.update() def remove_listener(self, topic, listener): topic_listeners = self._listeners.get(topic) if topic_listeners is not None and listener in topic_listeners: topic_listeners.remove(listener) if len(topic_listeners) == 0: del self._listeners[topic] # Stop the message listener thread if (topic, listener) in self._message_listener_threads: self._message_listener_threads[(topic, listener)].stop() del self._message_listener_threads[(topic, listener)] self.update() ### Playhead # property: play_speed def _get_play_speed(self): if self._timeline_frame._paused: return 0.0 return self._play_speed def _set_play_speed(self, play_speed): if play_speed == self._play_speed: return if play_speed > 0.0: self._play_speed = min(self._max_play_speed, max(self._min_play_speed, play_speed)) elif play_speed < 0.0: self._play_speed = max(-self._max_play_speed, min(-self._min_play_speed, play_speed)) else: self._play_speed = play_speed if self._play_speed < 1.0: self.stick_to_end = False self.update() play_speed = property(_get_play_speed, _set_play_speed) def toggle_play(self): if self._play_speed != 0.0: self.play_speed = 0.0 else: self.play_speed = 1.0 def navigate_play(self): self.play_speed = 1.0 self.last_frame = rospy.Time.from_sec(time.time()) self.last_playhead = self._timeline_frame.playhead self._play_timer.start() def navigate_stop(self): self.play_speed = 0.0 self._play_timer.stop() def navigate_previous(self): self.navigate_stop() self._timeline_frame.playhead = self.get_previous_message_time() self.last_playhead = self._timeline_frame.playhead def navigate_next(self): self.navigate_stop() self._timeline_frame.playhead = self.get_next_message_time() self.last_playhead = self._timeline_frame.playhead def navigate_rewind(self): if self._play_speed < 0.0: new_play_speed = self._play_speed * 2.0 elif self._play_speed == 0.0: new_play_speed = -1.0 else: new_play_speed = self._play_speed * 0.5 self.play_speed = new_play_speed def navigate_fastforward(self): if self._play_speed > 0.0: new_play_speed = self._play_speed * 2.0 elif self._play_speed == 0.0: new_play_speed = 2.0 else: new_play_speed = self._play_speed * 0.5 self.play_speed = new_play_speed def navigate_start(self): self._timeline_frame.playhead = self._timeline_frame.play_region[0] def navigate_end(self): self._timeline_frame.playhead = self._timeline_frame.play_region[1]
class ControllerManager(Plugin): """ Graphical frontend for managing ros_control controllers. """ _cm_update_freq = 1 # Hz def __init__(self, context): super(ControllerManager, self).__init__(context) self.setObjectName('ControllerManager') # Create QWidget and extend it with all the attributes and children # from the UI file self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_controller_manager'), 'resource', 'controller_manager.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('ControllerManagerUi') # Pop-up that displays controller information self._popup_widget = QWidget() ui_file = os.path.join(rp.get_path('rqt_controller_manager'), 'resource', 'controller_info.ui') loadUi(ui_file, self._popup_widget) self._popup_widget.setObjectName('ControllerInfoUi') # Show _widget.windowTitle on left-top of each plugin (when # it's set in _widget). This is useful when you open multiple # plugins at once. Also if you open multiple instances of your # plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) # Add widget to the user interface context.add_widget(self._widget) # Initialize members self._cm_ns = [] # Namespace of the selected controller manager self._controllers = [] # State of each controller self._table_model = None self._controller_lister = None # Controller manager service proxies self._load_srv = None self._unload_srv = None self._switch_srv = None # Controller state icons rospack = rospkg.RosPack() path = rospack.get_path('rqt_controller_manager') self._icons = {'running': QIcon(path + '/resource/led_green.png'), 'stopped': QIcon(path + '/resource/led_red.png'), 'uninitialized': QIcon(path + '/resource/led_off.png'), 'initialized': QIcon(path + '/resource/led_red.png')} # Controllers display table_view = self._widget.table_view table_view.setContextMenuPolicy(Qt.CustomContextMenu) table_view.customContextMenuRequested.connect(self._on_ctrl_menu) table_view.doubleClicked.connect(self._on_ctrl_info) header = table_view.horizontalHeader() header.setSectionResizeMode(QHeaderView.ResizeToContents) header.setContextMenuPolicy(Qt.CustomContextMenu) header.customContextMenuRequested.connect(self._on_header_menu) # Timer for controller manager updates self._list_cm = ControllerManagerLister() self._update_cm_list_timer = QTimer(self) self._update_cm_list_timer.setInterval(1000.0 / self._cm_update_freq) self._update_cm_list_timer.timeout.connect(self._update_cm_list) self._update_cm_list_timer.start() # Timer for running controller updates self._update_ctrl_list_timer = QTimer(self) self._update_ctrl_list_timer.setInterval(1000.0 / self._cm_update_freq) self._update_ctrl_list_timer.timeout.connect(self._update_controllers) self._update_ctrl_list_timer.start() # Signal connections w = self._widget w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change) def shutdown_plugin(self): self._update_cm_list_timer.stop() self._update_ctrl_list_timer.stop() self._popup_widget.hide() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('cm_ns', self._cm_ns) def restore_settings(self, plugin_settings, instance_settings): # Restore last session's controller_manager, if present self._update_cm_list() cm_ns = instance_settings.value('cm_ns') cm_combo = self._widget.cm_combo cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] try: idx = cm_list.index(cm_ns) cm_combo.setCurrentIndex(idx) except (ValueError): pass # def trigger_configuration(self): # Comment in to signal that the plugin has a way to configure # This will enable a setting button (gear icon) in each dock widget # title bar # Usually used to open a modal configuration dialog def _update_cm_list(self): update_combo(self._widget.cm_combo, self._list_cm()) def _on_cm_change(self, cm_ns): self._cm_ns = cm_ns # Setup services for communicating with the selected controller manager self._set_cm_services(cm_ns) # Controller lister for the selected controller manager if cm_ns: self._controller_lister = ControllerLister(cm_ns) self._update_controllers() else: self._controller_lister = None def _set_cm_services(self, cm_ns): if cm_ns: # NOTE: Persistent services are used for performance reasons. # If the controller manager dies, we detect it and disconnect from # it anyway load_srv_name = _append_ns(cm_ns, 'load_controller') self._load_srv = rospy.ServiceProxy(load_srv_name, LoadController, persistent=True) unload_srv_name = _append_ns(cm_ns, 'unload_controller') self._unload_srv = rospy.ServiceProxy(unload_srv_name, UnloadController, persistent=True) switch_srv_name = _append_ns(cm_ns, 'switch_controller') self._switch_srv = rospy.ServiceProxy(switch_srv_name, SwitchController, persistent=True) else: self._load_srv = None self._unload_srv = None self._switch_srv = None def _update_controllers(self): # Find controllers associated to the selected controller manager controllers = self._list_controllers() # Update controller display, if necessary if self._controllers != controllers: self._controllers = controllers self._show_controllers() # NOTE: Model is recomputed from scratch def _list_controllers(self): """ @return List of controllers associated to a controller manager namespace. Contains both stopped/running controllers, as returned by the C{list_controllers} service, plus uninitialized controllers with configurations loaded in the parameter server. @rtype [str] """ if not self._cm_ns: return [] # Add loaded controllers first controllers = self._controller_lister() # Append potential controller configs found in the parameter server all_ctrls_ns = _resolve_controllers_ns(self._cm_ns) for name in get_rosparam_controller_names(all_ctrls_ns): add_ctrl = not any(name == ctrl.name for ctrl in controllers) if add_ctrl: type_str = _rosparam_controller_type(all_ctrls_ns, name) uninit_ctrl = ControllerState(name=name, type=type_str, state='uninitialized') controllers.append(uninit_ctrl) return controllers def _show_controllers(self): table_view = self._widget.table_view self._table_model = ControllerTable(self._controllers, self._icons) table_view.setModel(self._table_model) def _on_ctrl_menu(self, pos): # Get data of selected controller row = self._widget.table_view.rowAt(pos.y()) if row < 0: return # Cursor is not under a valid item ctrl = self._controllers[row] # Show context menu menu = QMenu(self._widget.table_view) if ctrl.state == 'running': action_stop = menu.addAction(self._icons['stopped'], 'Stop') action_kill = menu.addAction(self._icons['uninitialized'], 'Stop and Unload') elif ctrl.state == 'stopped': action_start = menu.addAction(self._icons['running'], 'Start again') action_unload = menu.addAction(self._icons['uninitialized'], 'Unload') elif ctrl.state == 'initialized': action_start = menu.addAction(self._icons['running'], 'Start') action_unload = menu.addAction(self._icons['uninitialized'], 'Unload') elif ctrl.state == 'uninitialized': action_load = menu.addAction(self._icons['stopped'], 'Load') action_spawn = menu.addAction(self._icons['running'], 'Load and Start') action = menu.exec_(self._widget.table_view.mapToGlobal(pos)) # Evaluate user action if ctrl.state == 'running': if action is action_stop: self._stop_controller(ctrl.name) elif action is action_kill: self._stop_controller(ctrl.name) self._unload_controller(ctrl.name) elif ctrl.state == 'stopped' or ctrl.state == 'initialized': if action is action_start: self._start_controller(ctrl.name) elif action is action_unload: self._unload_controller(ctrl.name) elif ctrl.state == 'uninitialized': if action is action_load: self._load_controller(ctrl.name) if action is action_spawn: self._load_controller(ctrl.name) self._start_controller(ctrl.name) def _on_ctrl_info(self, index): popup = self._popup_widget ctrl = self._controllers[index.row()] popup.ctrl_name.setText(ctrl.name) popup.ctrl_type.setText(ctrl.type) res_model = QStandardItemModel() model_root = QStandardItem('Claimed Resources') res_model.appendRow(model_root) for hw_res in ctrl.claimed_resources: hw_iface_item = QStandardItem(hw_res.hardware_interface) model_root.appendRow(hw_iface_item) for res in hw_res.resources: res_item = QStandardItem(res) hw_iface_item.appendRow(res_item) popup.resource_tree.setModel(res_model) popup.resource_tree.setItemDelegate(FontDelegate(popup.resource_tree)) popup.resource_tree.expandAll() popup.move(QCursor.pos()) popup.show() def _on_header_menu(self, pos): header = self._widget.table_view.horizontalHeader() # Show context menu menu = QMenu(self._widget.table_view) action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize') action = menu.exec_(header.mapToGlobal(pos)) # Evaluate user action if action is action_toggle_auto_resize: if header.resizeMode(0) == QHeaderView.ResizeToContents: header.setSectionResizeMode(QHeaderView.Interactive) else: header.setSectionResizeMode(QHeaderView.ResizeToContents) def _load_controller(self, name): self._load_srv.call(LoadControllerRequest(name=name)) def _unload_controller(self, name): self._unload_srv.call(UnloadControllerRequest(name=name)) def _start_controller(self, name): strict = SwitchControllerRequest.STRICT req = SwitchControllerRequest(start_controllers=[name], stop_controllers=[], strictness=strict) self._switch_srv.call(req) def _stop_controller(self, name): strict = SwitchControllerRequest.STRICT req = SwitchControllerRequest(start_controllers=[], stop_controllers=[name], strictness=strict) self._switch_srv.call(req)
class RobotMonitorWidget(QWidget): """ NOTE: RobotMonitorWidget.shutdown function needs to be called when the instance of this class terminates. RobotMonitorWidget itself doesn't store previous diagnostic states. It instead delegates that function to Timeline class. """ _TREE_ALL = 1 _TREE_WARN = 2 _TREE_ERR = 3 message_updated = Signal(Gui) def __init__(self, context, topic=None): """ :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, 'PluginContext' :param topic: Diagnostic topic to subscribe to 'str' """ super(RobotMonitorWidget, self).__init__() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_robot_monitor'), 'resource', 'robotmonitor_mainwidget.ui') loadUi(ui_file, self) obj_name = 'ROS Monitor' self.setObjectName(obj_name) self.setWindowTitle(obj_name) self.message_updated.connect(self.monitoring_cb) # if we're given a topic, create a timeline. otherwise, don't # this can be used later when writing an rqt_bag plugin if topic: # create timeline data structure self._timeline = Timeline(topic, Gui) self._timeline.message_updated.connect(self.message_updated) # create timeline pane widget self.timeline_pane = TimelinePane(self) self.timeline_pane.set_timeline(self._timeline) self.vlayout_top.addWidget(self.timeline_pane) self.timeline_pane.show() else: self._timeline = None self.timeline_pane = None self._inspectors = {} # keep a copy of the current message for opening new inspectors self._current_msg = None self.tree_all_devices.itemDoubleClicked.connect(self._tree_clicked) self.warn_flattree.itemDoubleClicked.connect(self._tree_clicked) self.err_flattree.itemDoubleClicked.connect(self._tree_clicked) # TODO: resize on itemCollapsed and itemExpanded self._is_stale = False self._timer = QTimer() self._timer.timeout.connect(self._update_message_state) self._timer.start(1000) palette = self.tree_all_devices.palette() self._original_base_color = palette.base().color() self._original_alt_base_color = palette.alternateBase().color() self._tree = StatusItem(self.tree_all_devices.invisibleRootItem()) self._warn_tree = StatusItem(self.warn_flattree.invisibleRootItem()) self._err_tree = StatusItem(self.err_flattree.invisibleRootItem()) @Slot(Gui) def monitoring_cb(self, msg): """ Gui message callback """ self._current_msg = msg # Walk the status array and update the tree for info in msg.infos: # Compute path and walk to appropriate subtree path = info.name.split('/') if path[0] == '': path = path[1:] tmp_tree = self._tree for p in path: if p == '': continue tmp_tree = tmp_tree[p] infoP = GuiInfo() infoP.name = p infoP.errorlevel = info.errorlevel tmp_tree.update(infoP, p) tmp_tree.update(info, path[-1]) # Check for warnings if int(info.errorlevel * 2.9) == DiagnosticStatus.WARN: name = info.name self._warn_tree[name].update(info, info.name) # Check for errors if int(info.errorlevel * 2.9) == DiagnosticStatus.ERROR: name = info.name self._err_tree[name].update(info, info.name) # For any items in the tree that were not updated, remove them self._tree.prune() self._warn_tree.prune() self._err_tree.prune() # TODO(ahendrix): implement # Insight: for any item that is not OK, it only provides additional # information if all of it's children are OK # # otherwise, it's just an aggregation of its children # and doesn't provide any additional value when added to # the warning and error flat trees self.tree_all_devices.resizeColumnToContents(0) self.warn_flattree.resizeColumnToContents(0) self.err_flattree.resizeColumnToContents(0) def resizeEvent(self, evt): """Overridden from QWidget""" rospy.logdebug('RobotMonitorWidget resizeEvent') if self.timeline_pane: self.timeline_pane.redraw() @Slot(str) def _inspector_closed(self, name): """ Called when an inspector window is closed """ if name in self._inspectors: del self._inspectors[name] def _tree_clicked(self, item, column): """ Slot to QTreeWidget.itemDoubleClicked :type item: QTreeWidgetItem :type column: int """ rospy.logdebug('RobotMonitorWidget _tree_clicked col=%d', column) if item.name in self._inspectors: self._inspectors[item.name].activateWindow() else: self._inspectors[item.name] = InspectorWindow( self, item.name, self._current_msg, self._timeline) self._inspectors[item.name].closed.connect(self._inspector_closed) self.message_updated.connect( self._inspectors[item.name].message_updated) def _update_message_state(self): """ Update the display if it's stale """ if self._timeline is not None: if self._timeline.has_messages: previous_stale_state = self._is_stale self._is_stale = self._timeline.is_stale time_diff = int(self._timeline.data_age()) msg_template = "Last message received %s %s ago" if time_diff == 1: msg = msg_template % (time_diff, "second") else: msg = msg_template % (time_diff, "seconds") self.timeline_pane._msg_label.setText(msg) if previous_stale_state != self._is_stale: self._update_background_color() else: # no messages received yet self.timeline_pane._msg_label.setText("No messages received") def _update_background_color(self): """ Update the background color based on staleness """ p = self.tree_all_devices.palette() if self._is_stale: p.setColor(QPalette.Base, Qt.darkGray) p.setColor(QPalette.AlternateBase, Qt.lightGray) else: p.setColor(QPalette.Base, self._original_base_color) p.setColor(QPalette.AlternateBase, self._original_alt_base_color) self.tree_all_devices.setPalette(p) self.warn_flattree.setPalette(p) self.err_flattree.setPalette(p) def shutdown(self): """ This needs to be called whenever this class terminates. This closes all the instances on all trees. Also unregisters ROS' subscriber, stops timer. """ rospy.logdebug('RobotMonitorWidget in shutdown') names = self._inspectors.keys() for name in names: self._inspectors[name].close() if self._timeline: self._timeline.shutdown() self._timer.stop() del self._timer def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('splitter', self.splitter.saveState()) # TODO(ahendrix): persist the device paths, positions and sizes of any # inspector windows def restore_settings(self, plugin_settings, instance_settings): if instance_settings.contains('splitter'): self.splitter.restoreState(instance_settings.value('splitter')) else: self.splitter.setSizes([100, 100, 200])
class TopicWidget(QWidget): SELECT_BY_NAME = 0 SELECT_BY_MSGTYPE = 1 _column_names = ['topic', 'type', 'bandwidth', 'rate', 'value'] def __init__(self, plugin=None, selected_topics=None, select_topic_type=SELECT_BY_NAME): """ @type selected_topics: list of tuples. @param selected_topics: [($NAME_TOPIC$, $TYPE_TOPIC$), ...] @type select_topic_type: int @param select_topic_type: Can specify either the name of topics or by the type of topic, to filter the topics to show. If 'select_topic_type' argument is None, this arg shouldn't be meaningful. """ super(TopicWidget, self).__init__() self._select_topic_type = select_topic_type rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_topic'), 'resource', 'TopicWidget.ui') loadUi(ui_file, self) self._plugin = plugin self.topics_tree_widget.sortByColumn(0, Qt.AscendingOrder) header = self.topics_tree_widget.header() header.setResizeMode(QHeaderView.ResizeToContents) header.customContextMenuRequested.connect( self.handle_header_view_customContextMenuRequested) header.setContextMenuPolicy(Qt.CustomContextMenu) # Whether to get all topics or only the topics that are set in advance. # Can be also set by the setter method "set_selected_topics". self._selected_topics = selected_topics self._current_topic_list = [] self._topics = {} self._tree_items = {} self._column_index = {} for column_name in self._column_names: self._column_index[column_name] = len(self._column_index) # self.refresh_topics() # init and start update timer self._timer_refresh_topics = QTimer(self) self._timer_refresh_topics.timeout.connect(self.refresh_topics) def set_topic_specifier(self, specifier): self._select_topic_type = specifier def start(self): """ This method needs to be called to start updating topic pane. """ self._timer_refresh_topics.start(1000) @Slot() def refresh_topics(self): """ refresh tree view items """ if self._selected_topics is None: topic_list = rospy.get_published_topics() if topic_list is None: rospy.logerr( 'Not even a single published topic found. Check network configuration' ) return else: # Topics to show are specified. topic_list = self._selected_topics topic_specifiers_server_all = None topic_specifiers_required = None rospy.logdebug('refresh_topics) self._selected_topics=%s' % (topic_list, )) if self._select_topic_type == self.SELECT_BY_NAME: topic_specifiers_server_all = [ name for name, type in rospy.get_published_topics() ] topic_specifiers_required = [name for name, type in topic_list] elif self._select_topic_type == self.SELECT_BY_MSGTYPE: # The topics that are required (by whoever uses this class). topic_specifiers_required = [type for name, type in topic_list] # The required topics that match with published topics. topics_match = [(name, type) for name, type in rospy.get_published_topics() if type in topic_specifiers_required] topic_list = topics_match rospy.logdebug('selected & published topic types=%s' % (topic_list, )) rospy.logdebug('server_all=%s\nrequired=%s\ntlist=%s' % (topic_specifiers_server_all, topic_specifiers_required, topic_list)) if len(topic_list) == 0: rospy.logerr( 'None of the following required topics are found.\n(NAME, TYPE): %s' % (self._selected_topics, )) return if self._current_topic_list != topic_list: self._current_topic_list = topic_list # start new topic dict new_topics = {} for topic_name, topic_type in topic_list: # if topic is new or has changed its type if topic_name not in self._topics or \ self._topics[topic_name]['type'] != topic_type: # create new TopicInfo topic_info = TopicInfo(topic_name, topic_type) message_instance = None if topic_info.message_class is not None: message_instance = topic_info.message_class() # add it to the dict and tree view topic_item = self._recursive_create_widget_items( self.topics_tree_widget, topic_name, topic_type, message_instance) new_topics[topic_name] = { 'item': topic_item, 'info': topic_info, 'type': topic_type, } else: # if topic has been seen before, copy it to new dict and # remove it from the old one new_topics[topic_name] = self._topics[topic_name] del self._topics[topic_name] # clean up old topics for topic_name in self._topics.keys(): self._topics[topic_name]['info'].stop_monitoring() index = self.topics_tree_widget.indexOfTopLevelItem( self._topics[topic_name]['item']) self.topics_tree_widget.takeTopLevelItem(index) del self._topics[topic_name] # switch to new topic dict self._topics = new_topics self._update_topics_data() def _update_topics_data(self): for topic in self._topics.values(): topic_info = topic['info'] if topic_info.monitoring: # update rate rate, _, _, _ = topic_info.get_hz() rate_text = '%1.2f' % rate if rate != None else 'unknown' # update bandwidth bytes_per_s, _, _, _ = topic_info.get_bw() if bytes_per_s is None: bandwidth_text = 'unknown' elif bytes_per_s < 1000: bandwidth_text = '%.2fB/s' % bytes_per_s elif bytes_per_s < 1000000: bandwidth_text = '%.2fKB/s' % (bytes_per_s / 1000.) else: bandwidth_text = '%.2fMB/s' % (bytes_per_s / 1000000.) # update values value_text = '' self.update_value(topic_info._topic_name, topic_info.last_message) else: rate_text = '' bandwidth_text = '' value_text = 'not monitored' if topic_info.error is None else topic_info.error self._tree_items[topic_info._topic_name].setText( self._column_index['rate'], rate_text) self._tree_items[topic_info._topic_name].setText( self._column_index['bandwidth'], bandwidth_text) self._tree_items[topic_info._topic_name].setText( self._column_index['value'], value_text) def update_value(self, topic_name, message): if hasattr(message, '__slots__') and hasattr(message, '_slot_types'): for slot_name in message.__slots__: self.update_value(topic_name + '/' + slot_name, getattr(message, slot_name)) elif type(message) in (list, tuple) and (len(message) > 0) and hasattr( message[0], '__slots__'): for index, slot in enumerate(message): if topic_name + '[%d]' % index in self._tree_items: self.update_value(topic_name + '[%d]' % index, slot) else: base_type_str, _ = self._extract_array_info( self._tree_items[topic_name].text( self._column_index['type'])) self._recursive_create_widget_items( self._tree_items[topic_name], topic_name + '[%d]' % index, base_type_str, slot) # remove obsolete children if len(message) < self._tree_items[topic_name].childCount(): for i in range(len(message), self._tree_items[topic_name].childCount()): item_topic_name = topic_name + '[%d]' % i self._recursive_delete_widget_items( self._tree_items[item_topic_name]) else: if topic_name in self._tree_items: self._tree_items[topic_name].setText( self._column_index['value'], repr(message)) def _extract_array_info(self, type_str): array_size = None if '[' in type_str and type_str[-1] == ']': type_str, array_size_str = type_str.split('[', 1) array_size_str = array_size_str[:-1] if len(array_size_str) > 0: array_size = int(array_size_str) else: array_size = 0 return type_str, array_size def _recursive_create_widget_items(self, parent, topic_name, type_name, message): if parent is self.topics_tree_widget: # show full topic name with preceding namespace on toplevel item topic_text = topic_name item = TreeWidgetItem(self._toggle_monitoring, topic_name, parent) else: topic_text = topic_name.split('/')[-1] if '[' in topic_text: topic_text = topic_text[topic_text.index('['):] item = QTreeWidgetItem(parent) item.setText(self._column_index['topic'], topic_text) item.setText(self._column_index['type'], type_name) item.setData(0, Qt.UserRole, topic_name) self._tree_items[topic_name] = item if hasattr(message, '__slots__') and hasattr(message, '_slot_types'): for slot_name, type_name in zip(message.__slots__, message._slot_types): self._recursive_create_widget_items( item, topic_name + '/' + slot_name, type_name, getattr(message, slot_name)) else: base_type_str, array_size = self._extract_array_info(type_name) try: base_instance = roslib.message.get_message_class( base_type_str)() except (ValueError, TypeError): base_instance = None if array_size is not None and hasattr(base_instance, '__slots__'): for index in range(array_size): self._recursive_create_widget_items( item, topic_name + '[%d]' % index, base_type_str, base_instance) return item def _toggle_monitoring(self, topic_name): item = self._tree_items[topic_name] if item.checkState(0): self._topics[topic_name]['info'].start_monitoring() else: self._topics[topic_name]['info'].stop_monitoring() def _recursive_delete_widget_items(self, item): def _recursive_remove_items_from_tree(item): for index in reversed(range(item.childCount())): _recursive_remove_items_from_tree(item.child(index)) topic_name = item.data(0, Qt.UserRole) del self._tree_items[topic_name] _recursive_remove_items_from_tree(item) item.parent().removeChild(item) @Slot('QPoint') def handle_header_view_customContextMenuRequested(self, pos): header = self.topics_tree_widget.header() # show context menu menu = QMenu(self) action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize') action = menu.exec_(header.mapToGlobal(pos)) # evaluate user action if action is action_toggle_auto_resize: if header.resizeMode(0) == QHeaderView.ResizeToContents: header.setResizeMode(QHeaderView.Interactive) else: header.setResizeMode(QHeaderView.ResizeToContents) @Slot('QPoint') def on_topics_tree_widget_customContextMenuRequested(self, pos): item = self.topics_tree_widget.itemAt(pos) if item is None: return # show context menu menu = QMenu(self) action_item_expand = menu.addAction(QIcon.fromTheme('zoom-in'), 'Expand All Children') action_item_collapse = menu.addAction(QIcon.fromTheme('zoom-out'), 'Collapse All Children') action = menu.exec_(self.topics_tree_widget.mapToGlobal(pos)) # evaluate user action if action in (action_item_expand, action_item_collapse): expanded = (action is action_item_expand) def recursive_set_expanded(item): item.setExpanded(expanded) for index in range(item.childCount()): recursive_set_expanded(item.child(index)) recursive_set_expanded(item) def shutdown_plugin(self): for topic in self._topics.values(): topic['info'].stop_monitoring() self._timer_refresh_topics.stop() def set_selected_topics(self, selected_topics): """ @param selected_topics: list of tuple. [(topic_name, topic_type)] @type selected_topics: [] """ rospy.logdebug('set_selected_topics topics={}'.format( len(selected_topics))) self._selected_topics = selected_topics
class ControllerManagerWidget(QWidget): _column_names = ['name', 'state', 'type', 'hw_iface', 'resources'] _column_names_pretty = ['Controller Name', 'State', 'Type', 'HW Interface', 'Claimed Resources'] sig_sysmsg = Signal(str) def __init__(self, plugin): super(ControllerManagerWidget, self).__init__() self._plugin = plugin self.setWindowTitle('Controller Manager') # create layouts vlayout_outer = QtGui.QVBoxLayout(self) vlayout_outer.setObjectName('vert_layout_outer') hlayout_top = QtGui.QHBoxLayout(self) hlayout_top.setObjectName('hori_layout_top') vlayout_outer.addLayout(hlayout_top) # create top bar # controller manager namespace combo box & label cm_ns_label = QtGui.QLabel(self) cm_ns_label.setObjectName('cm_ns_label') cm_ns_label.setText('CM Namespace:') fixed_policy = QtGui.QSizePolicy(QtGui.QSizePolicy.Fixed) cm_ns_label.setSizePolicy(fixed_policy) hlayout_top.addWidget(cm_ns_label) cm_namespace_combo = QtGui.QComboBox(self) cm_namespace_combo.setObjectName('cm_namespace_combo') hlayout_top.addWidget(cm_namespace_combo) self.cm_namespace_combo = cm_namespace_combo # load controller combo box & label load_ctrl_label = QtGui.QLabel(self) load_ctrl_label.setObjectName('load_ctrl_label') load_ctrl_label.setText('Load Controller:') fixed_policy = QtGui.QSizePolicy(QtGui.QSizePolicy.Fixed) load_ctrl_label.setSizePolicy(fixed_policy) hlayout_top.addWidget(load_ctrl_label) load_ctrl_combo = QtGui.QComboBox(self) load_ctrl_combo.setObjectName('load_ctrl_combo') load_ctrl_size_policy = QtGui.QSizePolicy(QtGui.QSizePolicy.Expanding, QtGui.QSizePolicy.Fixed) load_ctrl_combo.setSizePolicy(load_ctrl_size_policy) hlayout_top.addWidget(load_ctrl_combo) self.load_ctrl_combo = load_ctrl_combo # load control button load_ctrl_button = QtGui.QPushButton(self) load_ctrl_button.setObjectName('load_ctrl_button') button_size_policy = QtGui.QSizePolicy(QtGui.QSizePolicy.Fixed, QtGui.QSizePolicy.Fixed) button_size_policy.setHorizontalStretch(0) button_size_policy.setVerticalStretch(0) button_size_policy.setHeightForWidth(load_ctrl_button.sizePolicy().hasHeightForWidth()) load_ctrl_button.setSizePolicy(button_size_policy) load_ctrl_button.setBaseSize(QtCore.QSize(30, 30)) load_ctrl_button.setIcon(QIcon.fromTheme('list-add')) load_ctrl_button.setIconSize(QtCore.QSize(20,20)) load_ctrl_button.clicked.connect(self.load_cur_ctrl) hlayout_top.addWidget(load_ctrl_button) # start control button start_ctrl_button = QtGui.QPushButton(self) start_ctrl_button.setObjectName('start_ctrl_button') button_size_policy = QtGui.QSizePolicy(QtGui.QSizePolicy.Fixed, QtGui.QSizePolicy.Fixed) button_size_policy.setHorizontalStretch(0) button_size_policy.setVerticalStretch(0) button_size_policy.setHeightForWidth(start_ctrl_button.sizePolicy().hasHeightForWidth()) start_ctrl_button.setSizePolicy(button_size_policy) start_ctrl_button.setBaseSize(QtCore.QSize(30, 30)) start_ctrl_button.setIcon(QIcon.fromTheme('media-playback-start')) start_ctrl_button.setIconSize(QtCore.QSize(20,20)) start_ctrl_button.clicked.connect(self.start_cur_ctrl) hlayout_top.addWidget(start_ctrl_button) # create tree/list widget ctrl_list_tree_widget = QtGui.QTreeWidget(self) ctrl_list_tree_widget.setObjectName('ctrl_list_tree_widget') self.ctrl_list_tree_widget = ctrl_list_tree_widget ctrl_list_tree_widget.setColumnCount(len(self._column_names)) ctrl_list_tree_widget.setHeaderLabels(self._column_names_pretty) ctrl_list_tree_widget.sortByColumn(0, Qt.AscendingOrder) ctrl_list_tree_widget.setContextMenuPolicy(Qt.CustomContextMenu) ctrl_list_tree_widget.customContextMenuRequested.connect( self.on_ctrl_list_tree_widget_customContextMenuRequested) vlayout_outer.addWidget(ctrl_list_tree_widget) header = self.ctrl_list_tree_widget.header() header.setResizeMode(QHeaderView.ResizeToContents) header.customContextMenuRequested.connect( self.handle_header_view_customContextMenuRequested) header.setContextMenuPolicy(Qt.CustomContextMenu) self._ctrlers = {} self._column_index = {} for column_name in self._column_names: self._column_index[column_name] = len(self._column_index) # controller manager services self.list_types = {} self.list_ctrlers = {} self.load_ctrler = {} self.unload_ctrler = {} self.switch_ctrlers = {} self.ctrlman_ns_cur = '' self.loadable_params = {} # init and start update timer self._timer_refresh_ctrlers = QTimer(self) self._timer_refresh_ctrlers.timeout.connect(self._refresh_ctrlers_cb) def controller_manager_connect(self, ctrlman_ns): self.list_types[ctrlman_ns] = rospy.ServiceProxy( ctrlman_ns + '/controller_manager/list_controller_types', ListControllerTypes) self.list_ctrlers[ctrlman_ns] = rospy.ServiceProxy( ctrlman_ns + '/controller_manager/list_controllers', ListControllers) self.load_ctrler[ctrlman_ns] = rospy.ServiceProxy( ctrlman_ns + '/controller_manager/load_controller', LoadController) self.unload_ctrler[ctrlman_ns] = rospy.ServiceProxy( ctrlman_ns + '/controller_manager/unload_controller', UnloadController) self.switch_ctrlers[ctrlman_ns] = rospy.ServiceProxy( ctrlman_ns + '/controller_manager/switch_controller', SwitchController) self.cm_namespace_combo.addItem(ctrlman_ns) def controller_manager_disconnect(self, ctrlman_ns): self.list_types[ctrlman_ns].close() del self.list_types[ctrlman_ns] self.list_ctrlers[ctrlman_ns].close() del self.list_ctrlers[ctrlman_ns] self.load_ctrler[ctrlman_ns].close() del self.load_ctrler[ctrlman_ns] self.unload_ctrler[ctrlman_ns].close() del self.unload_ctrler[ctrlman_ns] self.switch_ctrlers[ctrlman_ns].close() del self.switch_ctrlers[ctrlman_ns] combo_ind = self.cm_namespace_combo.findText(ctrlman_ns) self.cm_namespace_combo.removeItem(combo_ind) def _refresh_ctrlers_cb(self): try: # refresh the list of controller managers we can find srv_list = rosservice.get_service_list() ctrlman_ns_list_cur = [] for srv_name in srv_list: if 'controller_manager/list_controllers' in srv_name: srv_type = rosservice.get_service_type(srv_name) if srv_type == 'controller_manager_msgs/ListControllers': ctrlman_ns = srv_name.split('/controller_manager/list_controllers')[0] if ctrlman_ns == '': ctrlman_ns = '/' # ctrlman_ns is a Controller Manager namespace if ctrlman_ns not in self.list_ctrlers: # we haven't connected to it yet, create the service proxies self.controller_manager_connect(ctrlman_ns) ctrlman_ns_list_cur.append(ctrlman_ns) # remove every controller manager which isn't up anymore for ctrlman_ns_old in self.list_ctrlers.keys(): if ctrlman_ns_old not in ctrlman_ns_list_cur: self.controller_manager_disconnect(ctrlman_ns_old) # refresh the controller list for the current controller manager self.refresh_loadable_ctrlers() self.refresh_ctrlers() except Exception as e: self.sig_sysmsg.emit(e.message) def remove_ctrler_from_list(self, ctrler_name): item = self._ctrlers[ctrler_name]['item'] index = self.ctrl_list_tree_widget.indexOfTopLevelItem(item) self.ctrl_list_tree_widget.takeTopLevelItem(index) del self._ctrlers[ctrler_name] def remove_loadable_from_list(self, load_text): load_ctrl_ind = self.load_ctrl_combo.findText(load_text) self.load_ctrl_combo.removeItem(load_ctrl_ind) del self.loadable_params[load_text] def refresh_loadable_ctrlers(self): if self.cm_namespace_combo.count() == 0: # no controller managers found so there are no loadable controllers # remove old loadables for old_loadable_text in self.loadable_params.keys(): self.remove_loadable_from_list(old_loadable_text) return ctrlman_ns = self.cm_namespace_combo.currentText() if self.ctrlman_ns_cur != ctrlman_ns: # new controller manager selected # remove old loadables from list from last CM for old_loadable_text in self.loadable_params.keys(): self.remove_loadable_from_list(old_loadable_text) rospy.wait_for_service(ctrlman_ns + '/controller_manager/list_controller_types', 0.2) try: resp = self.list_types[ctrlman_ns].call(ListControllerTypesRequest()) except rospy.ServiceException as e: # TODO: display warning somehow return ctrler_types = resp.types loadable_params_cur = [] all_params = rosparam.list_params('/') # for every parameter for pname in all_params: # remove the controller manager namespace if ctrlman_ns == '/': pname_sub = pname else: pname_sub = pname[len(ctrlman_ns):] psplit = pname_sub.split('/') if len(psplit) > 2 and psplit[2] == 'type': loadable_type = rosparam.get_param(pname) if loadable_type in ctrler_types: load_text = pname[:-5] + ' - ' + loadable_type loadable_params_cur.append(load_text) if load_text not in self.loadable_params: self.loadable_params[load_text] = psplit[1] self.load_ctrl_combo.addItem(load_text) # remove loadable parameters no longer in the parameter server for load_text_old in self.loadable_params.keys(): if load_text_old not in loadable_params_cur: self.remove_loadable_from_list(load_text_old) @Slot() def refresh_ctrlers(self): if self.cm_namespace_combo.count() == 0: # no controller managers found so there are no controllers to update # remove old controllers for old_ctrler_name in self._ctrlers.keys(): self.remove_ctrler_from_list(old_ctrler_name) return ctrlman_ns = self.cm_namespace_combo.currentText() if self.ctrlman_ns_cur != ctrlman_ns: # new controller manager selected # remove old controllers from list from last CM for old_ctrler_name in self._ctrlers.keys(): self.remove_ctrler_from_list(old_ctrler_name) self.ctrlman_ns_cur = ctrlman_ns rospy.wait_for_service(ctrlman_ns + '/controller_manager/list_controllers', 0.2) try: resp = self.list_ctrlers[ctrlman_ns].call(ListControllersRequest()) except rospy.ServiceException as e: # TODO: display warning somehow return controller_list = resp.controller new_ctrlers = {} for c in controller_list: if c.name not in self._ctrlers: # new controller item = QTreeWidgetItem(self.ctrl_list_tree_widget) item.setData(0, Qt.UserRole, c.name) ctrler = {'item' : item, 'state' : c.state, 'type' : c.type, 'hw_iface' : c.hardware_interface, 'resources' : "[" + ", ".join(c.resources) + "]"} ctrler['item'].setText(self._column_index['name'], c.name) update_type = True update_state = True else: # controller already in list ctrler = self._ctrlers[c.name] update_type = False update_state = False if ctrler['type'] != c.type or ctrler['hw_iface'] != c.hardware_interface: # total controller change ctrler['state'] = c.state ctrler['type'] = c.type ctrler['hw_iface'] = c.hardware_interface ctrler['resources'] = "[" + ", ".join(c.resources) + "]" update_type = True if ctrler['state'] != c.state: # state change ctrler['state'] = c.state update_state = True # update entries if needed if update_type: ctrler['item'].setText(self._column_index['type'], ctrler['type']) ctrler['item'].setText(self._column_index['hw_iface'], ctrler['hw_iface']) ctrler['item'].setText(self._column_index['resources'], ctrler['resources']) if update_state or update_type: ctrler['item'].setText(self._column_index['state'], ctrler['state']) new_ctrlers[c.name] = ctrler # remove missing controllers for old_ctrler_name in self._ctrlers.keys(): if old_ctrler_name not in new_ctrlers: self.remove_ctrler_from_list(old_ctrler_name) # update current controllers self._ctrlers = new_ctrlers def start(self): self._timer_refresh_ctrlers.start(1000) @Slot('QPoint') def handle_header_view_customContextMenuRequested(self, pos): header = self.ctrl_list_tree_widget.header() # show context menu menu = QMenu(self) action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize') action = menu.exec_(header.mapToGlobal(pos)) # evaluate user action if action is action_toggle_auto_resize: if header.resizeMode(0) == QHeaderView.ResizeToContents: header.setResizeMode(QHeaderView.Interactive) else: header.setResizeMode(QHeaderView.ResizeToContents) def load_cur_ctrl(self): ctrlman_ns = self.cm_namespace_combo.currentText() load_text = self.load_ctrl_combo.currentText() load_param = self.loadable_params[load_text] self.load_controller(ctrlman_ns, load_param) def start_cur_ctrl(self): ctrlman_ns = self.cm_namespace_combo.currentText() load_text = self.load_ctrl_combo.currentText() load_param = self.loadable_params[load_text] self.load_controller(ctrlman_ns, load_param) self.start_stop_controller(ctrlman_ns, load_param, True) @Slot('QPoint') def on_ctrl_list_tree_widget_customContextMenuRequested(self, pos): ctrlman_ns = self.cm_namespace_combo.currentText() item = self.ctrl_list_tree_widget.itemAt(pos) if item is None: return ctrl_name = item.data(0, Qt.UserRole) ctrl_state = self._ctrlers[ctrl_name]['state'] # show context menu menu = QMenu(self) if ctrl_state == 'running': action_stop = menu.addAction( QIcon.fromTheme('media-playback-stop'), 'Stop Controller') action_kill = menu.addAction( QIcon.fromTheme('list-remove'), 'Stop and Unload Controller') elif ctrl_state == 'stopped': action_start = menu.addAction( QIcon.fromTheme('media-playback-start'), 'Start Controller') action_unload = menu.addAction( QIcon.fromTheme('list-remove'), 'Unload Controller') action = menu.exec_(self.ctrl_list_tree_widget.mapToGlobal(pos)) # evaluate user action if ctrl_state == 'running': if action is action_stop: self.start_stop_controller(ctrlman_ns, ctrl_name, False) elif action is action_kill: self.start_stop_controller(ctrlman_ns, ctrl_name, False) self.unload_controller(ctrlman_ns, ctrl_name) elif ctrl_state == 'stopped': if action is action_start: self.start_stop_controller(ctrlman_ns, ctrl_name, True) elif action is action_unload: self.unload_controller(ctrlman_ns, ctrl_name) def load_controller(self, ctrlman_ns, name): rospy.wait_for_service(ctrlman_ns + '/controller_manager/load_controller', 0.2) try: resp = self.load_ctrler[ctrlman_ns].call(LoadControllerRequest(name)) if resp.ok == 1: # successful return 0 else: # failure return 1 except rospy.ServiceException as e: # exception return 2 def unload_controller(self, ctrlman_ns, name): rospy.wait_for_service(ctrlman_ns + '/controller_manager/unload_controller', 0.2) try: resp = self.unload_ctrler[ctrlman_ns].call(UnloadControllerRequest(name)) if resp.ok == 1: # successful return 0 else: # failure return 1 except rospy.ServiceException as e: # exception return 2 def start_stop_controller(self, ctrlman_ns, name, is_start): rospy.wait_for_service(ctrlman_ns + '/controller_manager/switch_controller', 0.2) start = [] stop = [] strictness = SwitchControllerRequest.STRICT if is_start: start = [name] else: stop = [name] try: resp = self.switch_ctrlers[ctrlman_ns].call( SwitchControllerRequest(start, stop, strictness)) if resp.ok == 1: # successful return 0 else: # failure return 1 except rospy.ServiceException as e: # exception return 2 def shutdown_plugin(self): for ctrlman_ns in self.list_ctrlers.keys(): self.controller_manager_disconnect(ctrlman_ns) self._timer_refresh_ctrlers.stop()
class CopterPlugin(Plugin): plot_start_time = -1 status_time = 0 state_time = 0 pause = 0 # Observed parameters voltage = 10 cpu_load = 0 flight_mode_ll = 'flight_mode' state_estimation = 'state_estimate' position_control = 'position_control' motor_status = 'motor_status' flight_time = 0 gps_status = 'gps_status' gps_num_satellites = 0 def __init__(self, context): super(CopterPlugin, self).__init__(context) self.setObjectName('CopterPlugin') self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_copter'), 'resource', 'CopterPlugin.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('CopterPluginUi') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._widget) # Add icons to the buttons self._widget.pause_resume_plot_button.setIcon(QIcon.fromTheme('media-playback-pause')) self._widget.start_reset_plot_button.setIcon(QIcon.fromTheme('view-refresh')) self._widget.srv_refresh_button.setIcon(QIcon.fromTheme('view-refresh')) self._widget.battery_alert.setIcon(QIcon.fromTheme('dialog-warning')) self._widget.battery_alert.setVisible(0) # Initialize the timer self._start_time = rospy.get_time() self._timer = QTimer(self) self._timer.timeout.connect(self._timer_update) self._timer.start(50) # Initialize all plots and subscribers self._state_subscriber = None self._status_subscriber = None self._client = None self._create_plots() self._get_init_services() # Add event functions self._widget.start_reset_plot_button.clicked.connect(self._reset_plots) self._widget.pause_resume_plot_button.clicked.connect(self._pause_resume_plots) self._widget.copter_namespace_textbox.returnPressed.connect(self._reset_subscriber) self._widget.copter_namespace_button.clicked.connect(self._reset_subscriber) self._widget.srv_refresh_button.clicked.connect(self._refresh_init_services) self._widget.copter_namespace_textbox.setFocus() # Update the displayed data in the widget def _timer_update(self): # Check if Voltage low if self.voltage < 10: self._widget.battery_alert.setVisible(1) else: self._widget.battery_alert.setVisible(0) # Only update the current tab tab = self._widget.tab_widget.currentIndex() # This is the status tab update if tab is 0: self.plot_battery_voltage.rescale_axis_y() self._widget.cpu_load_bar.setValue(self.cpu_load) self._widget.flight_mode_ll_textbox.setText(self.flight_mode_ll) self._widget.state_estimation_textbox.setText(self.state_estimation) self._widget.position_control_textbox.setText(self.position_control) self._widget.motor_status_textbox.setText(self.motor_status) self._widget.flight_time_box.setValue(self.flight_time) self._widget.gps_status_textbox.setText(self.gps_status) self._widget.gps_num_satellites_box.setValue(self.gps_num_satellites) self._widget.battery_voltage_display.setValue(self.voltage) # This is the state-plot tab if tab is 1 and not self.pause: self.plot_position.rescale_axis_y() self.plot_velocity.rescale_axis_y() self.plot_acceleration_bias.rescale_axis_y() self.plot_scale.rescale_axis_y() def _pause_resume_plots(self): if self.pause is 0: self.pause = 1 self._widget.pause_resume_plot_button.setIcon(QIcon.fromTheme('media-playback-start')) self._widget.pause_resume_plot_button.setText("Resume") else: self.pause = 0 self._widget.pause_resume_plot_button.setIcon(QIcon.fromTheme('media-playback-pause')) self._widget.pause_resume_plot_button.setText("Pause") # Get all available msf-initialisation services def _get_init_services(self): self._init_services = [] service_names = rosservice.get_service_list() for service_name in service_names: if "initialize_msf" in service_name: self._init_services.append(SrvWidget(self._widget.srv_container_layout, service_name)) def _refresh_init_services(self): for init_service in self._init_services: self._widget.srv_container_layout.removeWidget(init_service._widget) init_service._widget.close() self._get_init_services() # Subscribe to rostopics according to the namespace def _reset_subscriber(self): namespace = self._widget.copter_namespace_textbox.text() state_topic = namespace + '/fcu/ekf_state_out' status_topic = namespace + '/fcu/status' print "Subscribing to", state_topic print "Subscribing to", status_topic if self._state_subscriber is not None: self._state_subscriber.unregister() if self._status_subscriber is not None: self._status_subscriber.unregister() self._state_subscriber = rospy.Subscriber(state_topic, ExtEkf, self._state_subscriber_callback) self._status_subscriber = rospy.Subscriber(status_topic, mav_status, self._status_subscriber_callback) def _state_subscriber_callback(self, input): if self.plot_start_time is -1: self.plot_start_time = input.header.stamp.to_sec() self.state_time = input.header.stamp.to_sec() - self.plot_start_time #if self.plot_position is not None: self.plot_position.update_value('x', self.state_time, input.state[0]) self.plot_position.update_value('y', self.state_time, input.state[1]) self.plot_position.update_value('z', self.state_time, input.state[2]) #if self.plot_velocity is not None: self.plot_velocity.update_value('x', self.state_time, input.state[3]) self.plot_velocity.update_value('y', self.state_time, input.state[4]) self.plot_velocity.update_value('z', self.state_time, input.state[5]) #if self.plot_acceleration_bias is not None: self.plot_acceleration_bias.update_value('x', self.state_time, input.state[6]) self.plot_acceleration_bias.update_value('y', self.state_time, input.state[7]) self.plot_acceleration_bias.update_value('z', self.state_time, input.state[8]) #if self.plot_scale is not None: self.plot_scale.update_value('scale', self.state_time, input.state[9]) def _status_subscriber_callback(self, input): self.status_time = input.header.stamp.to_sec() - self.plot_start_time self.voltage = input.battery_voltage self.cpu_load = input.cpu_load self.flight_mode_ll = input.flight_mode_ll self.state_estimation = input.state_estimation self.position_control = input.position_control self.motor_status = input.motor_status self.flight_time = input.flight_time self.gps_status = input.gps_status self.gps_num_satellites = input.gps_num_satellites #if self.plot_battery_voltage is not None: self.plot_battery_voltage.update_value('voltage', self.status_time, input.battery_voltage) def _create_plots(self): self.plot_start_time = -1 self.plot_battery_voltage = QwtDataPlot(self._widget) self._widget.plot_battery_voltage_layout.addWidget(self.plot_battery_voltage) self.plot_battery_voltage.add_curve('voltage', 'Voltage', [0], [0]) self.plot_position = QwtDataPlot(self._widget) self._widget.plot_position_layout.addWidget(self.plot_position) self.plot_position.add_curve('x', 'x-position', [0], [0]) self.plot_position.add_curve('y', 'y-position', [0], [0]) self.plot_position.add_curve('z', 'z-position', [0], [0]) self.plot_velocity = QwtDataPlot(self._widget) self._widget.plot_velocity_layout.addWidget(self.plot_velocity) self.plot_velocity.add_curve('x', 'x-velocity', [0], [0]) self.plot_velocity.add_curve('y', 'y-velocity', [0], [0]) self.plot_velocity.add_curve('z', 'z-velocity', [0], [0]) self.plot_acceleration_bias = QwtDataPlot(self._widget) self._widget.plot_acceleration_bias_layout.addWidget(self.plot_acceleration_bias) self.plot_acceleration_bias.add_curve('x', 'x-acc-bias', [0], [0]) self.plot_acceleration_bias.add_curve('y', 'y-acc-bias', [0], [0]) self.plot_acceleration_bias.add_curve('z', 'z-acc-bias', [0], [0]) self.plot_scale = QwtDataPlot(self._widget) self._widget.plot_scale_layout.addWidget(self.plot_scale) self.plot_scale.add_curve('scale', 'visual scale', [0], [0]) def _reset_plots(self): self._widget.plot_battery_voltage_layout.removeWidget(self.plot_battery_voltage) self.plot_battery_voltage.close() self._widget.plot_position_layout.removeWidget(self.plot_position) self.plot_position.close() self._widget.plot_velocity_layout.removeWidget(self.plot_velocity) self.plot_velocity.close() self._widget.plot_acceleration_bias_layout.removeWidget(self.plot_acceleration_bias) self.plot_acceleration_bias.close() self._widget.plot_scale_layout.removeWidget(self.plot_scale) self.plot_scale.close() self._create_plots() def shutdown_plugin(self): self._timer.stop() if self._state_subscriber is not None: self._state_subscriber.unregister() if self._status_subscriber is not None: self._status_subscriber.unregister() def save_settings(self, plugin_settings, instance_settings): pass def restore_settings(self, plugin_settings, instance_settings): pass
class TFBroadcasterWidget(QWidget): def __init__(self, widget): super(TFBroadcasterWidget, self).__init__() rospkg_pack = rospkg.RosPack() ui_file = os.path.join(rospkg_pack.get_path('gui_nodes'), 'resource', 'TFBroadcaster.ui') loadUi(ui_file, self) self._frame_id, self._child_frame_id = FRAME_ID, CHILD_FRAME_ID self._tf_manager = TFBroadcasterImpl(self._frame_id, self._child_frame_id) self.setup_ui() self._updateTimer = QTimer(self) self._updateTimer.timeout.connect(self.timeout_callback) def setup_ui(self): self._status_widget = StatusInputWidgetSet(self.qt_stop_radiobtn, self.qt_static_tf_radiobtn, self.qt_tf_radiobtn, self._tf_manager.set_stop_status, self._tf_manager.set_static_tf_status, self._tf_manager.set_tf_status) self._frame_id_widget = FrameIDInputWidgetSet(self.qt_frame_id_lineedit, self.qt_frame_id_update_btn, self._frame_id, self._tf_manager.set_frame_id) self._child_frame_id_widget = FrameIDInputWidgetSet(self.qt_child_frame_id_lineedit, self.qt_child_frame_id_update_btn, self._child_frame_id, self._tf_manager.set_child_frame_id) self._x_widget = SliderSpinboxInputWidgetSet(self.qt_x_slider, self.qt_x_spinbox, self.qt_x_btn, 0.0, POS_MAX_VALUE, POS_MIN_VALUE) self._y_widget = SliderSpinboxInputWidgetSet(self.qt_y_slider, self.qt_y_spinbox, self.qt_y_btn, 0.0, POS_MAX_VALUE, POS_MIN_VALUE) self._z_widget = SliderSpinboxInputWidgetSet(self.qt_z_slider, self.qt_z_spinbox, self.qt_z_btn, 0.0, POS_MAX_VALUE, POS_MIN_VALUE) self._roll_widget = SliderSpinboxInputWidgetSet(self.qt_roll_slider, self.qt_roll_spinbox, self.qt_roll_btn, 0.0, ORI_MAX_VALUE, ORI_MIN_VALUE) self._pitch_widget = SliderSpinboxInputWidgetSet(self.qt_pitch_slider, self.qt_pitch_spinbox, self.qt_pitch_btn, 0.0, ORI_MAX_VALUE, ORI_MIN_VALUE) self._yaw_widget = SliderSpinboxInputWidgetSet(self.qt_yaw_slider, self.qt_yaw_spinbox, self.qt_yaw_btn, 0.0, ORI_MAX_VALUE, ORI_MIN_VALUE) def start(self): self._tf_manager.set_frames(self._frame_id, self._child_frame_id) self._updateTimer.start(10) # loop rate[ms] def stop(self): self._updateTimer.stop() def timeout_callback(self): self._tf_manager.set_position(self._x_widget.value, self._y_widget.value, self._z_widget.value) self._tf_manager.set_orientation(self._roll_widget.value, self._pitch_widget.value, self._yaw_widget.value, is_rad=False) self._tf_manager.broadcast_tf() # override def save_settings(self, plugin_settings, instance_settings): self._frame_id = self._frame_id_widget.str self._child_frame_id = self._child_frame_id_widget.str instance_settings.set_value('frame_ids', (self._frame_id, self._child_frame_id)) # override def restore_settings(self, plugin_settings, instance_settings): frames = instance_settings.value('frame_ids') try: self._frame_id, self._child_frame_id = frames self._frame_id_widget.update(self._frame_id) self._child_frame_id_widget.update(self._child_frame_id) except Exception: self._frame_id, self._child_frame_id = FRAME_ID, CHILD_FRAME_ID # override def shutdown_plugin(self): self.stop()
class Plot3DWidget(QWidget): _redraw_interval = 40 def __init__(self, initial_topics=None, start_paused=False, buffer_length=100, use_poly=True, no_legend=False): super(Plot3DWidget, self).__init__() self.setObjectName('Plot3DWidget') self._buffer_length = buffer_length self._initial_topics = initial_topics rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource', 'plot3d.ui') loadUi(ui_file, self) self.subscribe_topic_button.setIcon(QIcon.fromTheme('add')) self.remove_topic_button.setIcon(QIcon.fromTheme('remove')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = MatDataPlot3D(self, self._buffer_length, use_poly, no_legend) self.data_plot_layout.addWidget(self.data_plot) self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked()) self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent self.subscribe_topic_button.setEnabled(False) if start_paused: self.pause_button.setChecked(True) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self._start_time = rospy.get_time() self._rosdata = {} self._remove_topic_menu = QMenu() # init and start update timer for plot self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) if self._initial_topics: for topic_name in self._initial_topics: self.add_topic(topic_name) self._initial_topics = None @Slot('QDragEnterEvent*') def dragEnterEvent(self, event): # get topic name if not event.mimeData().hasText(): if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0: qWarning('Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0') return item = event.source().selectedItems()[0] topic_name = item.data(0, Qt.UserRole) if topic_name == None: qWarning('Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)') return else: topic_name = str(event.mimeData().text()) # check for numeric field type is_numeric, is_array, message = is_slot_numeric(topic_name) if is_numeric and not is_array: event.acceptProposedAction() else: qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message)) @Slot('QDropEvent*') def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.add_topic(topic_name) @Slot(str) def on_topic_edit_textChanged(self, topic_name): # on empty topic name, update topics if topic_name in ('', '/'): self._topic_completer.update_topics() is_numeric, is_array, message = is_slot_numeric(topic_name) self.subscribe_topic_button.setEnabled(is_numeric and not is_array) self.subscribe_topic_button.setToolTip(message) @Slot() def on_topic_edit_returnPressed(self): if self.subscribe_topic_button.isEnabled(): self.add_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.add_topic(str(self.topic_edit.text())) @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) @Slot(bool) def on_autoscroll_checkbox_clicked(self, checked): self.data_plot.autoscroll(checked) @Slot() def on_clear_button_clicked(self): self.clean_up_subscribers() def update_plot(self): if self.data_plot is not None: needs_redraw = False for topic_name, rosdata in self._rosdata.items(): try: data_x, data_y = rosdata.next() if data_x or data_y: self.data_plot.update_values(topic_name, data_x, data_y) needs_redraw = True except RosPlotException as e: qWarning('PlotWidget.update_plot(): error in rosplot: %s' % e) if needs_redraw: self.data_plot.redraw() def _subscribed_topics_changed(self): self._update_remove_topic_menu() if not self.pause_button.isChecked(): # if pause button is not pressed, enable timer based on subscribed topics self.enable_timer(self._rosdata) def _update_remove_topic_menu(self): def make_remove_topic_function(x): return lambda: self.remove_topic(x) self._remove_topic_menu.clear() for topic_name in sorted(self._rosdata.keys()): action = QAction(topic_name, self._remove_topic_menu) action.triggered.connect(make_remove_topic_function(topic_name)) self._remove_topic_menu.addAction(action) self.remove_topic_button.setMenu(self._remove_topic_menu) def add_topic(self, topic_name): if topic_name in self._rosdata: qWarning('PlotWidget.add_topic(): topic already subscribed: %s' % topic_name) return self._rosdata[topic_name] = ROSData(topic_name, self._start_time) if self._rosdata[topic_name].error is not None: qWarning(str(self._rosdata[topic_name].error)) del self._rosdata[topic_name] else: data_x, data_y = self._rosdata[topic_name].next() self.data_plot.add_curve(topic_name, topic_name, data_x, data_y) self._subscribed_topics_changed() def remove_topic(self, topic_name): self._rosdata[topic_name].close() del self._rosdata[topic_name] self.data_plot.remove_curve(topic_name) self._subscribed_topics_changed() def clean_up_subscribers(self): for topic_name, rosdata in self._rosdata.items(): rosdata.close() self.data_plot.remove_curve(topic_name) self._rosdata = {} self._subscribed_topics_changed() def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop()
class JointTrajectoryController(Plugin): """ Graphical frontend for a C{JointTrajectoryController}. There are two modes for interacting with a controller: 1. B{Monitor mode} Joint displays are updated with the state reported by the controller. This is a read-only mode and does I{not} send control commands. Every time a new controller is selected, it starts in monitor mode for safety reasons. 2. B{Control mode} Joint displays update the control command that is sent to the controller. Commands are sent periodically evan if the displays are not being updated by the user. To control the aggressiveness of the motions, the maximum speed of the sent commands can be scaled down using the C{Speed scaling control} This plugin is able to detect and keep track of all active controller managers, as well as the JointTrajectoryControllers that are I{running} in each controller manager. For a controller to be compatible with this plugin, it must comply with the following requisites: - The controller type contains the C{JointTrajectoryController} substring, e.g., C{position_controllers/JointTrajectoryController} - The controller exposes the C{command} and C{state} topics in its ROS interface. Additionally, there must be a URDF loaded with a valid joint limit specification, namely position (when applicable) and velocity limits. A reference implementation of the C{JointTrajectoryController} is available in the C{joint_trajectory_controller} ROS package. """ _cmd_pub_freq = 10.0 # Hz _widget_update_freq = 30.0 # Hz _ctrlrs_update_freq = 1 # Hz _min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration jointStateChanged = Signal([dict]) def __init__(self, context): super(JointTrajectoryController, self).__init__(context) self.setObjectName('JointTrajectoryController') # Create QWidget and extend it with all the attributes and children # from the UI file self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_joint_trajectory_controller'), 'resource', 'joint_trajectory_controller.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('JointTrajectoryControllerUi') # Setup speed scaler speed_scaling = DoubleEditor(1.0, 100.0) speed_scaling.spin_box.setSuffix('%') speed_scaling.spin_box.setValue(50.0) speed_scaling.spin_box.setDecimals(0) speed_scaling.setEnabled(False) self._widget.speed_scaling_layout.addWidget(speed_scaling) self._speed_scaling_widget = speed_scaling speed_scaling.valueChanged.connect(self._on_speed_scaling_change) self._on_speed_scaling_change(speed_scaling.value()) # Show _widget.windowTitle on left-top of each plugin (when # it's set in _widget). This is useful when you open multiple # plugins at once. Also if you open multiple instances of your # plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) # Add widget to the user interface context.add_widget(self._widget) # Initialize members self._jtc_name = [] # Name of selected joint trajectory controller self._cm_ns = [] # Namespace of the selected controller manager self._joint_pos = {} # name->pos map for joints of selected controller self._joint_names = [] # Ordered list of selected controller joints self._robot_joint_limits = {} # Lazily evaluated on first use # Timer for sending commands to active controller self._update_cmd_timer = QTimer(self) self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq) self._update_cmd_timer.timeout.connect(self._update_cmd_cb) # Timer for updating the joint widgets from the controller state self._update_act_pos_timer = QTimer(self) self._update_act_pos_timer.setInterval(1000.0 / self._widget_update_freq) self._update_act_pos_timer.timeout.connect(self._update_joint_widgets) # Timer for controller manager updates self._list_cm = ControllerManagerLister() self._update_cm_list_timer = QTimer(self) self._update_cm_list_timer.setInterval(1000.0 / self._ctrlrs_update_freq) self._update_cm_list_timer.timeout.connect(self._update_cm_list) self._update_cm_list_timer.start() # Timer for running controller updates self._update_jtc_list_timer = QTimer(self) self._update_jtc_list_timer.setInterval(1000.0 / self._ctrlrs_update_freq) self._update_jtc_list_timer.timeout.connect(self._update_jtc_list) self._update_jtc_list_timer.start() # Signal connections w = self._widget w.enable_button.toggled.connect(self._on_jtc_enabled) w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change) w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change) self._cmd_pub = None # Controller command publisher self._state_sub = None # Controller state subscriber self._list_controllers = None def shutdown_plugin(self): self._update_cmd_timer.stop() self._update_act_pos_timer.stop() self._update_cm_list_timer.stop() self._update_jtc_list_timer.stop() self._unregister_state_sub() self._unregister_cmd_pub() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('cm_ns', self._cm_ns) instance_settings.set_value('jtc_name', self._jtc_name) def restore_settings(self, plugin_settings, instance_settings): # Restore last session's controller_manager, if present self._update_cm_list() cm_ns = instance_settings.value('cm_ns') cm_combo = self._widget.cm_combo cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] try: idx = cm_list.index(cm_ns) cm_combo.setCurrentIndex(idx) # Resore last session's controller, if running self._update_jtc_list() jtc_name = instance_settings.value('jtc_name') jtc_combo = self._widget.jtc_combo jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())] try: idx = jtc_list.index(jtc_name) jtc_combo.setCurrentIndex(idx) except (ValueError): pass except (ValueError): pass # def trigger_configuration(self): # Comment in to signal that the plugin has a way to configure # This will enable a setting button (gear icon) in each dock widget # title bar # Usually used to open a modal configuration dialog def _update_cm_list(self): update_combo(self._widget.cm_combo, self._list_cm()) def _update_jtc_list(self): # Clear controller list if no controller information is available if not self._list_controllers: self._widget.jtc_combo.clear() return # List of running controllers with a valid joint limits specification # for _all_ their joints running_jtc = self._running_jtc_info() if running_jtc and not self._robot_joint_limits: self._robot_joint_limits = get_joint_limits() # Lazy evaluation valid_jtc = [] for jtc_info in running_jtc: has_limits = all(name in self._robot_joint_limits for name in _jtc_joint_names(jtc_info)) if has_limits: valid_jtc.append(jtc_info); valid_jtc_names = [data.name for data in valid_jtc] # Update widget update_combo(self._widget.jtc_combo, sorted(valid_jtc_names)) def _on_speed_scaling_change(self, val): self._speed_scale = val / self._speed_scaling_widget.slider.maximum() def _on_joint_state_change(self, actual_pos): assert(len(actual_pos) == len(self._joint_pos)) for name in actual_pos.keys(): self._joint_pos[name]['position'] = actual_pos[name] def _on_cm_change(self, cm_ns): self._cm_ns = cm_ns if cm_ns: self._list_controllers = ControllerLister(cm_ns) # NOTE: Clear below is important, as different controller managers # might have controllers with the same name but different # configurations. Clearing forces controller re-discovery self._widget.jtc_combo.clear() self._update_jtc_list() else: self._list_controllers = None def _on_jtc_change(self, jtc_name): self._unload_jtc() self._jtc_name = jtc_name if self._jtc_name: self._load_jtc() def _on_jtc_enabled(self, val): # Don't allow enabling if there are no controllers selected if not self._jtc_name: self._widget.enable_button.setChecked(False) return # Enable/disable joint displays for joint_widget in self._joint_widgets(): joint_widget.setEnabled(val) # Enable/disable speed scaling self._speed_scaling_widget.setEnabled(val) if val: # Widgets send desired position commands to controller self._update_act_pos_timer.stop() self._update_cmd_timer.start() else: # Controller updates widgets with actual position self._update_cmd_timer.stop() self._update_act_pos_timer.start() def _load_jtc(self): # Initialize joint data corresponding to selected controller running_jtc = self._running_jtc_info() self._joint_names = next(_jtc_joint_names(x) for x in running_jtc if x.name == self._jtc_name) for name in self._joint_names: self._joint_pos[name] = {} # Update joint display try: layout = self._widget.joint_group.layout() for name in self._joint_names: limits = self._robot_joint_limits[name] joint_widget = DoubleEditor(limits['min_position'], limits['max_position']) layout.addRow(name, joint_widget) # NOTE: Using partial instead of a lambda because lambdas # "will not evaluate/look up the argument values before it is # effectively called, breaking situations like using a loop # variable inside it" from functools import partial par = partial(self._update_single_cmd_cb, name=name) joint_widget.valueChanged.connect(par) except: # TODO: Can we do better than swallow the exception? from sys import exc_info print 'Unexpected error:', exc_info()[0] # Enter monitor mode (sending commands disabled) self._on_jtc_enabled(False) # Setup ROS interfaces jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name) state_topic = jtc_ns + '/state' cmd_topic = jtc_ns + '/command' self._state_sub = rospy.Subscriber(state_topic, JointTrajectoryControllerState, self._state_cb, queue_size=1) self._cmd_pub = rospy.Publisher(cmd_topic, JointTrajectory, queue_size=1) # Start updating the joint positions self.jointStateChanged.connect(self._on_joint_state_change) def _unload_jtc(self): # Stop updating the joint positions try: self.jointStateChanged.disconnect(self._on_joint_state_change) except: pass # Reset ROS interfaces self._unregister_state_sub() self._unregister_cmd_pub() # Clear joint widgets # NOTE: Implementation is a workaround for: # https://bugreports.qt-project.org/browse/QTBUG-15990 :( layout = self._widget.joint_group.layout() if layout is not None: while layout.count(): layout.takeAt(0).widget().deleteLater() # Delete existing layout by reparenting to temporary QWidget().setLayout(layout) self._widget.joint_group.setLayout(QFormLayout()) # Reset joint data self._joint_names = [] self._joint_pos = {} # Enforce monitor mode (sending commands disabled) self._widget.enable_button.setChecked(False) def _running_jtc_info(self): from controller_manager_msgs.utils\ import filter_by_type, filter_by_state controller_list = self._list_controllers() jtc_list = filter_by_type(controller_list, 'JointTrajectoryController', match_substring=True) running_jtc_list = filter_by_state(jtc_list, 'running') return running_jtc_list def _unregister_cmd_pub(self): if self._cmd_pub is not None: self._cmd_pub.unregister() self._cmd_pub = None def _unregister_state_sub(self): if self._state_sub is not None: self._state_sub.unregister() self._state_sub = None def _state_cb(self, msg): actual_pos = {} for i in range(len(msg.joint_names)): joint_name = msg.joint_names[i] joint_pos = msg.actual.positions[i] actual_pos[joint_name] = joint_pos self.jointStateChanged.emit(actual_pos) def _update_single_cmd_cb(self, val, name): self._joint_pos[name]['command'] = val def _update_cmd_cb(self): dur = [] traj = JointTrajectory() traj.joint_names = self._joint_names point = JointTrajectoryPoint() for name in traj.joint_names: pos = self._joint_pos[name]['position'] cmd = pos try: cmd = self._joint_pos[name]['command'] except (KeyError): pass max_vel = self._robot_joint_limits[name]['max_velocity'] dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur)) point.positions.append(cmd) point.time_from_start = rospy.Duration(max(dur) / self._speed_scale) traj.points.append(point) self._cmd_pub.publish(traj) def _update_joint_widgets(self): joint_widgets = self._joint_widgets() for id in range(len(joint_widgets)): joint_name = self._joint_names[id] try: joint_pos = self._joint_pos[joint_name]['position'] joint_widgets[id].setValue(joint_pos) except (KeyError): pass # Can happen when first connected to controller def _joint_widgets(self): # TODO: Cache instead of compute every time? widgets = [] layout = self._widget.joint_group.layout() for row_id in range(layout.rowCount()): widgets.append(layout.itemAt(row_id, QFormLayout.FieldRole).widget()) return widgets
class Plot2DWidget(QWidget): _redraw_interval = 40 def __init__(self, topics): super(Plot2DWidget, self).__init__() self.setObjectName("Plot2DWidget") rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path("jsk_rqt_plugins"), "resource", "plot_histogram.ui") loadUi(ui_file, self) self.cv_bridge = CvBridge() self.subscribe_topic_button.setIcon(QIcon.fromTheme("add")) self.pause_button.setIcon(QIcon.fromTheme("media-playback-pause")) self.clear_button.setIcon(QIcon.fromTheme("edit-clear")) self.data_plot = MatPlot2D(self) self.data_plot_layout.addWidget(self.data_plot) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent self._start_time = rospy.get_time() self._rosdata = None if len(topics) != 0: self.subscribe_topic(topics) self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) self._update_plot_timer.start(self._redraw_interval) @Slot("QDropEvent*") def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.subscribe_topic(topic_name) @Slot() def on_topic_edit_returnPressed(self): if self.subscribe_topic_button.isEnabled(): self.subscribe_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.subscribe_topic(str(self.topic_edit.text())) def subscribe_topic(self, topic_name): self.topic_with_field_name = topic_name self.pub_image = rospy.Publisher(topic_name + "/histogram_image", Image) if not self._rosdata: self._rosdata = ROSData(topic_name, self._start_time) else: if self._rosdata != topic_name: self._rosdata.close() self.data_plot.clear() self._rosdata = ROSData(topic_name, self._start_time) else: rospy.logwarn("%s is already subscribed", topic_name) def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop() @Slot() def on_clear_button_clicked(self): self.data_plot.clear() @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) def update_plot(self): if not self._rosdata: return data_x, data_y = self._rosdata.next() if len(data_y) == 0: return axes = self.data_plot._canvas.axes axes.cla() # matplotlib # concatenate x and y in order to sort concatenated_data = zip(data_y[-1].xs, data_y[-1].ys) if self.sort_x: concatenated_data.sort(key=lambda x: x[0]) xs = [d[0] for d in concatenated_data] ys = [d[1] for d in concatenated_data] if self.is_line: axes.plot(xs, ys, label=self.topic_with_field_name) else: axes.scatter(xs, ys) # set limit axes.set_xlim(min(xs), max(xs)) axes.set_ylim(min(ys), max(ys)) # line fitting if self.fit_line: X = np.array(data_y[-1].xs) Y = np.array(data_y[-1].ys) A = np.array([X, np.ones(len(X))]) A = A.T a, b = np.linalg.lstsq(A, Y)[0] axes.plot(X, (a * X + b), "g--", label="{0} x + {1}".format(a, b)) if self.fit_line_ransac: model_ransac = linear_model.RANSACRegressor( linear_model.LinearRegression(), min_samples=2, residual_threshold=self.fit_line_ransac_outlier ) X = np.array(data_y[-1].xs).reshape((len(data_y[-1].xs), 1)) Y = np.array(data_y[-1].ys) model_ransac.fit(X, Y) line_X = X line_y_ransac = model_ransac.predict(line_X) axes.plot( line_X, line_y_ransac, "r--", label="{0} x + {1}".format(model_ransac.estimator_.coef_[0][0], model_ransac.estimator_.intercept_[0]), ) if not self.no_legend: axes.legend(prop={"size": "8"}) axes.grid() if self.xtitle: axes.set_xlabel(self.xtitle) if self.ytitle: axes.set_ylabel(self.ytitle) self.data_plot._canvas.draw() buffer = StringIO() self.data_plot._canvas.figure.savefig(buffer, format="png") buffer.seek(0) img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8) img = cv2.imdecode(img_array, cv2.CV_LOAD_IMAGE_COLOR) self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class ConsoleDashWidget(IconToolButton): """ A widget which brings up the ROS console. :param context: The plugin context to create the monitor in. :type context: qt_gui.plugin_context.PluginContext """ def __init__(self, context, icon_paths=None, minimal=True): ok_icon = ['bg-green.svg', 'ic-console.svg'] warn_icon = ['bg-yellow.svg', 'ic-console.svg', 'ol-warn-badge.svg'] err_icon = ['bg-red.svg', 'ic-console.svg', 'ol-err-badge.svg'] stale_icon = ['bg-grey.svg', 'ic-console.svg', 'ol-stale-badge.svg'] icons = [ok_icon, warn_icon, err_icon, stale_icon] super(ConsoleDashWidget, self).__init__('Console Widget', icons, icon_paths=icon_paths) self.minimal = minimal self.setFixedSize(self._icons[0].actualSize(QSize(50, 30))) self._datamodel = MessageDataModel() self._proxymodel = MessageProxyModel() self._proxymodel.setSourceModel(self._datamodel) self._message_queue = [] self._mutex = QMutex() self._subscriber = rospy.Subscriber('/rosout_agg', Log, self._message_cb) self._console = None self.context = context self.clicked.connect(self._show_console) self.update_state(0) self._timer = QTimer() self._timer.timeout.connect(self._insert_messages) self._timer.start(100) self._rospack = rospkg.RosPack() if self._console is None: self._console = ConsoleWidget(self._proxymodel, self._rospack, minimal=self.minimal) self._console.destroyed.connect(self._console_destroyed) self._console_shown = False self.setToolTip("Rosout") def _show_console(self): if self._console is None: self._console = ConsoleWidget(self._proxymodel, self._rospack, minimal=self.minimal) self._console.destroyed.connect(self._console_destroyed) try: if self._console_shown: self.context.remove_widget(self._console) self._console_shown = not self._console_shown else: self.context.add_widget(self._console) self._console_shown = not self._console_shown except Exception: self._console_shown = not self._console_shown self._show_console() def _insert_messages(self): with QMutexLocker(self._mutex): msgs = self._message_queue self._message_queue = [] if msgs: self._datamodel.insert_rows(msgs) # The console may not yet be initialized or may have been closed # So fail silently try: self.update_rosout() except: pass def _message_cb(self, log_msg): if not self._console._paused: msg = Console.convert_rosgraph_log_message(log_msg) with QMutexLocker(self._mutex): self._message_queue.append(msg) def update_rosout(self): summary_dur = 30.0 if (rospy.get_time() < 30.0): summary_dur = rospy.get_time() - 1.0 if (summary_dur < 0): summary_dur = 0.0 summary = self._console.get_message_summary(summary_dur) if (summary.fatal or summary.error): self.update_state(2) elif (summary.warn): self.update_state(1) else: self.update_state(0) tooltip = "" if (summary.fatal): tooltip += "\nFatal: %s" % (summary.fatal) if (summary.error): tooltip += "\nError: %s" % (summary.error) if (summary.warn): tooltip += "\nWarn: %s" % (summary.warn) if (summary.info): tooltip += "\nInfo: %s" % (summary.info) if (summary.debug): tooltip += "\nDebug: %s" % (summary.debug) if (len(tooltip) == 0): tooltip = "Rosout: no recent activity" else: tooltip = "Rosout: recent activity:" + tooltip if tooltip != self.toolTip(): self.setToolTip(tooltip) def _console_destroyed(self): if self._console: self._console.cleanup_browsers_on_close() self._console = None def shutdown_widget(self): if self._console: self._console.cleanup_browsers_on_close() if self._subscriber: self._subscriber.unregister() self._timer.stop() def save_settings(self, plugin_settings, instance_settings): self._console.save_settings(plugin_settings, instance_settings) def restore_settings(self, plugin_settings, instance_settings): self._console.restore_settings(plugin_settings, instance_settings)
class RobotMonitorWidget(AbstractStatusWidget): """ NOTE: RobotMonitorWidget.shutdown function needs to be called when the instance of this class terminates. RobotMonitorWidget itself doesn't store previous diagnostic states. It instead delegates that function to TimelinePane class. """ _sig_tree_nodes_updated = Signal(int) _sig_new_diagnostic = Signal(DiagnosticArray) _TREE_ALL = 1 _TREE_WARN = 2 _TREE_ERR = 3 def __init__(self, context, topic): """ :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, 'PluginContext' :param topic: Diagnostic topic to subscribe to 'str' """ super(RobotMonitorWidget, self).__init__() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_robot_monitor'), 'resource', 'robotmonitor_mainwidget.ui') loadUi(ui_file, self, {'TimelinePane': TimelinePane}) #loadUi(ui_file, self) obj_name = 'Robot Monitor' self.setObjectName(obj_name) self.setWindowTitle(obj_name) self._toplevel_statitems = [] # StatusItem self._warn_statusitems = [] # StatusItem. Contains ALL DEGREES # (device top level, device' _sub) in parallel self._err_statusitems = [] # StatusItem # TODO: Declaring timeline pane. # Needs to be stashed away into .ui file but so far failed. self.timeline_pane.set_timeline_data(Util.SECONDS_TIMELINE, self.get_color_for_value, self.on_pause) self.tree_all_devices.itemDoubleClicked.connect(self._tree_clicked) self.warn_flattree.itemDoubleClicked.connect(self._tree_clicked) self.err_flattree.itemDoubleClicked.connect(self._tree_clicked) self.tree_all_devices.resizeColumnToContents(0) self._sig_tree_nodes_updated.connect(self._tree_nodes_updated) self.vlayout_top.addWidget(self.timeline_pane) self.timeline_pane.show() self._paused = False self._is_stale = None self._last_message_time = 0.0 self._timer = QTimer() # self._timer.timerEvent.connect(self._update_message_state) self._timer.timeout.connect(self._update_message_state) self._timer.start(1000) self._sub = rospy.Subscriber( topic, # name of the topic DiagnosticArray, # type of the topic self._cb) self._sig_new_diagnostic.connect(self.new_diagnostic) self._original_base_color = self.tree_all_devices.palette().base().color() self._original_alt_base_color = self.tree_all_devices.palette().alternateBase().color() def _cb(self, msg): """ Intended to be called from non-Qt thread, ie. ROS Subscriber in particular. :type msg: DiagnosticArray """ # Directly calling callback function 'new_diagnostic' here results in # segfaults. self._sig_new_diagnostic.emit(msg) def new_diagnostic(self, msg, is_forced=False): """ Overridden from AbstractStatusWidget. When monitoring not paused, this public method updates all the treewidgets contained in this class, and also notifies the StatusItem instances that are stored in the all-device-tree, which eventually updates the InspectorWindows in them. :type msg: DiagnosticArray :param is_forced: Intended for non-incoming-msg trigger (in particular, from child object like TimelinePane). @author: Isaac Saito """ if not self._paused and not is_forced: self.timeline_pane.new_diagnostic(msg) self._update_devices_tree(msg) self._update_warns_errors(msg) self._on_new_message_received(msg) self._notify_statitems(msg) rospy.logdebug(' RobotMonitorWidget _cb stamp=%s', msg.header.stamp) elif is_forced: self._update_devices_tree(msg) self._update_warns_errors(msg) def _notify_statitems(self, diag_arr): """ Notify new message arrival to all existing InespectorWindow instances that are encapsulated in StatusItem instances contained in self._toplevel_statitems. """ for statitem_new in diag_arr.status: corresp = Util.get_correspondent(statitem_new.name, self._toplevel_statitems) statitem_prev = corresp[Util._DICTKEY_STATITEM] if statitem_prev and statitem_prev.inspector: rospy.logdebug(' RobotMonitorWidget _notify_statitems ' + 'name=%s len toplv items=%d', statitem_new.name, len(self._toplevel_statitems)) return def resizeEvent(self, evt): """Overridden from QWidget""" rospy.logdebug('RobotMonitorWidget resizeEvent') self.timeline_pane.redraw() def _tree_clicked(self, item, column): """ Slot to QTreeWidget.itemDoubleClicked :type item: QTreeWidgetItem :type column: int """ rospy.logdebug('RobotMonitorWidget _tree_clicked col=%d', column) item.on_click() def _update_devices_tree(self, diag_array): """ Update the tree from the bottom :type diag_array: DiagnosticArray """ # TODO: 11/5/2012 Currently, in case some devices disappear # while running this program, there's no way to remove # those from the device-tree. statusnames_curr_toplevel = [Util.get_grn_resource_name(status.name) for status in self._toplevel_statitems] # Only the status variable that pops up at the end is # processed by Util.get_grn_resource_name. for status_new in self._get_toplevel_diagnosticstat(diag_array): name = Util.get_grn_resource_name(status_new.name) rospy.logdebug('_update_devices_tree 0 name @ toplevel %s', name) dict_status = 0 if name in statusnames_curr_toplevel: # No change of names # in toplevel since last time. statusitem = self._toplevel_statitems[ statusnames_curr_toplevel.index(name)] dict_status = statusitem.update_children(status_new, diag_array) times_errors = dict_status[Util._DICTKEY_TIMES_ERROR] times_warnings = dict_status[Util._DICTKEY_TIMES_WARN] Util.update_status_images(status_new, statusitem) # TODO: Update status text on each node using dict_status. base_text = Util.gen_headline_status_green(statusitem.status) rospy.logdebug('_update_devices_tree warn_id= %s\n\t\t\t' + 'diagnostic_status.name = %s\n\t\t\t\t' + 'Normal status diag_array = %s', statusitem.warning_id, status_new.name, base_text) if (times_errors > 0 or times_warnings > 0): base_text = "(Err: %s, Wrn: %s) %s %s" % ( times_errors, times_warnings, Util.get_grn_resource_name(status_new.name), status_new.message) rospy.logdebug('_update_dev_tree 1 text to show=%s', base_text) statusitem.setText(0, base_text) statusitem.setText(1, status_new.message) else: rospy.logdebug('_update_dev_tree 2 text to show=%s', base_text) statusitem.setText(0, base_text) statusitem.setText(1, 'OK') else: new_status_item = StatusItem(status_new) new_status_item.update_children(status_new, diag_array) # Figure out if a statusitem and its subtree contains errors. # new_status_item.setIcon(0, self._error_icon) # This shows NG icon at the beginning of each statusitem. Util.update_status_images(status_new, new_status_item) self._toplevel_statitems.append(new_status_item) rospy.logdebug(' _update_devices_tree 2 ' + 'status_new.name %s', new_status_item.name) self.tree_all_devices.addTopLevelItem(new_status_item) self._sig_tree_nodes_updated.emit(self._TREE_ALL) def _tree_nodes_updated(self, tree_type): tree_obj = None if self._TREE_ALL == tree_type: tree_obj = self.tree_all_devices elif self._TREE_WARN == tree_type: tree_obj = self.warn_flattree if self._TREE_ERR == tree_type: tree_obj = self.err_flattree tree_obj.resizeColumnToContents(0) def _get_toplevel_diagnosticstat(self, diag_array): """ Return an array that contains DiagnosticStatus only at the top level of the given msg. :type msg: DiagnosticArray :rtype: DiagnosticStatus[] """ ret = [] for diagnostic_status in diag_array.status: if len(diagnostic_status.name.split('/')) == 2: rospy.logdebug(" _get_toplevel_diagnosticstat " + "TOP lev %s ", diagnostic_status.name) ret.append(diagnostic_status) else: rospy.logdebug(" _get_toplevel_diagnosticstat " + "Not top lev %s ", diagnostic_status.name) return ret def _update_warns_errors(self, diag_array): """ Update the warning and error trees. Unlike _update_devices_tree function where all DiagnosticStatus need to be inspected constantly, this function is used in a trial to reduce unnecessary inspection of the status level for all DiagnosticStatus contained in the incoming DiagnosticArray msg. :type msg: DiagnosticArray """ self._update_flat_tree(diag_array) def pause(self, msg): """ Do nothing if already being _paused. :type msg: DiagnosticArray """ if not self._paused: self._paused = True self.new_diagnostic(msg) def unpause(self, msg=None): """ :type msg: DiagnosticArray """ self._paused = False def on_pause(self, paused, diagnostic_arr): """ Check if InspectorWindows are set. If they are, pause them. Pay attention not to confuse with RobotMonitorWidget.pause. :type paused: bool :type diagnostic_arr: DiagnosticArray """ if paused: self.pause(diagnostic_arr) elif (len(self._toplevel_statitems) > 0): diag_array_queue = self.timeline_pane._get_diagnosticarray() statitems = [] for diag_arr in diag_array_queue: state_instant = InstantaneousState() state_instant.update(diag_arr) statitems.append(state_instant) for statitem_toplv in self._toplevel_statitems: if (paused): statitem_toplv.disable() else: statitem_toplv.enable() for state_instant in statitems: items = state_instant.get_items() if statitem_toplv.get_name() in items: statitem_toplv.update( items[statitem_toplv.get_name()].status) def _update_flat_tree(self, diag_arr): """ Update the given flat tree (ie. tree that doesn't show children nodes - all of its elements will be shown on top level) with all the DiagnosticStatus instances contained in the given DiagnosticArray, regardless of the level of the device in a device category. For both warn / error trees, StatusItem instances are newly generated. :type diag_arr: DiagnosticArray """ for diag_stat_new in diag_arr.status: # Num of loops here should be equal to the num of the top # DiagnosticStatus item. Ex. in PR2, 9 or so. stat_lv_new = diag_stat_new.level dev_name = diag_stat_new.name correspondent_warn_curr = Util.get_correspondent( Util.get_grn_resource_name(dev_name), self._warn_statusitems) dev_index_warn_curr = correspondent_warn_curr[Util._DICTKEY_INDEX] rospy.logdebug(' dev_index_warn_curr=%s dev_name=%s', dev_index_warn_curr, dev_name) correspondent_err_curr = Util.get_correspondent( Util.get_grn_resource_name(dev_name), self._err_statusitems) dev_index_err_curr = correspondent_err_curr[Util._DICTKEY_INDEX] headline = "%s" % diag_stat_new.name if DiagnosticStatus.OK == stat_lv_new: if 0 <= dev_index_warn_curr: rospy.logdebug('dev_index_warn_curr=%s name=%s, ' + 'stat_lv_new=%d', dev_index_warn_curr, dev_name, stat_lv_new) statitem_curr = self._get_statitem(dev_index_warn_curr, self._warn_statusitems, self.warn_flattree, 1) statitem_curr.warning_id = None elif 0 <= dev_index_err_curr: statitem_curr = self._get_statitem(dev_index_err_curr, self._err_statusitems, self.err_flattree, 1) statitem_curr.error_id = None elif DiagnosticStatus.WARN == stat_lv_new: statitem = None if 0 <= dev_index_err_curr: # If the corresponding statusitem is in error tree, # move it to warn tree. statitem = self._get_statitem(dev_index_err_curr, self._err_statusitems, self.err_flattree) self._add_statitem(statitem, self._warn_statusitems, self.warn_flattree, headline, diag_stat_new.message, stat_lv_new) elif (dev_index_warn_curr < 0 and dev_index_err_curr < 0): # If the corresponding statusitem isn't found, # create new obj. statitem = StatusItem(diag_stat_new) self._add_statitem(statitem, self._warn_statusitems, self.warn_flattree, headline, diag_stat_new.message, stat_lv_new) self._warn_statusitems.append(statitem) elif (0 < dev_index_warn_curr): # If the corresponding statusitem is already in warn tree, # obtain the instance. statitem = self._get_statitem(dev_index_warn_curr, self._warn_statusitems) if statitem: # If not None # Updating statusitem will keep popup window also update. statitem.update_children(diag_stat_new, diag_arr) elif ((DiagnosticStatus.ERROR == stat_lv_new) or (DiagnosticStatus.STALE == stat_lv_new)): statitem = None if 0 <= dev_index_warn_curr: # If the corresponding statusitem is in warn tree, # move it to err tree. statitem = self._get_statitem(dev_index_warn_curr, self._warn_statusitems, self.warn_flattree) self._add_statitem(statitem, self._err_statusitems, self.err_flattree, headline, diag_stat_new.message, stat_lv_new) elif (0 <= dev_index_err_curr): # If the corresponding statusitem is already in err tree, # obtain the instance. statitem = self._get_statitem(dev_index_err_curr, self._err_statusitems) elif (dev_index_warn_curr < 0 and dev_index_err_curr < 0): # If the corresponding statusitem isn't found, # create new obj. statitem = StatusItem(diag_stat_new) self._add_statitem(statitem, self._err_statusitems, self.err_flattree, headline, diag_stat_new.message, stat_lv_new) if statitem: # If not None # Updating statusitem will keep popup window also update. statitem.update_children(diag_stat_new, diag_arr) self._sig_tree_nodes_updated.emit(self._TREE_WARN) self._sig_tree_nodes_updated.emit(self._TREE_ERR) def _add_statitem(self, statusitem, statitem_list, tree, headline, statusmsg, statlevel): if 'Warning' == statusmsg or 'Error' == statusmsg: return statusitem.setText(0, headline) statusitem.setText(1, statusmsg) statusitem.setIcon(0, Util.IMG_DICT[statlevel]) statitem_list.append(statusitem) tree.addTopLevelItem(statusitem) rospy.logdebug(' _add_statitem statitem_list length=%d', len(statitem_list)) def _get_statitem(self, item_index, item_list, tree=None, mode=2): """ :param mode: 1 = remove from given list, 2 = w/o removing. """ statitem_existing = item_list[item_index] if 1 == mode: tree.takeTopLevelItem(tree.indexOfTopLevelItem(statitem_existing)) item_list.pop(item_index) return statitem_existing def _on_new_message_received(self, msg): """ Called whenever a new message is received by the timeline. Different from new_message in that it is called even if the timeline is _paused, and only when a new message is received, not when the timeline is scrubbed """ self._last_message_time = rospy.get_time() def _update_message_state(self): current_time = rospy.get_time() time_diff = current_time - self._last_message_time rospy.logdebug('_update_message_state time_diff= %s ' + 'self._last_message_time=%s', time_diff, self._last_message_time) previous_stale_state = self._is_stale if (time_diff > 10.0): self.timeline_pane._msg_label.setText("Last message received " + "%s seconds ago" % (int(time_diff))) self._is_stale = True else: seconds_string = "seconds" if (int(time_diff) == 1): seconds_string = "second" self.timeline_pane._msg_label.setText( "Last message received %s %s ago" % (int(time_diff), seconds_string)) self._is_stale = False if previous_stale_state != self._is_stale: self._update_background_color() def _update_background_color(self): p = self.tree_all_devices.palette() if self._is_stale: p.setColor(QPalette.Base, Qt.darkGray) p.setColor(QPalette.AlternateBase, Qt.lightGray) else: p.setColor(QPalette.Base, self._original_base_color) p.setColor(QPalette.AlternateBase, self._original_alt_base_color) self.tree_all_devices.setPalette(p) self.warn_flattree.setPalette(p) self.err_flattree.setPalette(p) def shutdown(self): """ This needs to be called whenever this class terminates. This closes all the instances on all trees. Also unregisters ROS' subscriber, stops timer. """ rospy.logdebug('RobotMonitorWidget in shutdown') # Close all StatusItem (and each associated InspectWidget) # self.tree_all_devices.clear() # Doesn't work for the purpose # (inspector windows don't get closed) for item in self._err_statusitems: item.close() for item in self._warn_statusitems: item.close() for item in self._toplevel_statitems: item.close() self._sub.unregister() self._timer.stop() del self._timer def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('splitter', self.splitter.saveState()) def restore_settings(self, plugin_settings, instance_settings): if instance_settings.contains('splitter'): self.splitter.restoreState(instance_settings.value('splitter')) else: self.splitter.setSizes([100, 100, 200]) def _clear(self): rospy.logdebug(' RobotMonitorWidget _clear called ') self.err_flattree.clear() self.warn_flattree.clear() def get_color_for_value(self, queue_diagnostic, color_index): """ Overridden from AbstractStatusWidget. :type color_index: int """ len_q = len(queue_diagnostic) rospy.logdebug(' get_color_for_value color_index=%d len_q=%d', color_index, len_q) if (color_index == 1 and len_q == 0): # TODO: Needs to be reverted back return QColor('grey') return Util._get_color_for_message(queue_diagnostic[color_index - 1])
class PluginLogManager(Plugin): """ rqt_console plugin's main class. Handles communication with ros_gui and contains callbacks to handle incoming message """ def __init__(self, context): Plugin.__init__(self, context) def onCreate(self, param): layout = QGridLayout(self) layout.setContentsMargins(2, 2, 2, 2) self._rospack = rospkg.RosPack() self._model = MessageDataModel() self._proxy_model = MessageProxyModel() self._proxy_model.setSourceModel(self._model) self.__widget = ConsoleWidget(self._proxy_model, self._rospack) layout.addWidget(self.__widget, 0, 0, 0, 0) # queue to store incoming data which get flushed periodically to the model # required since QSortProxyModel can not handle a high insert rate self._message_queue = [] self._mutex = QMutex() self._timer = QTimer() self._timer.timeout.connect(self.insert_messages) self._timer.start(100) self._subscriber = None self._topic = '/rosout_agg' self._subscribe(self._topic) def queue_message(self, log_msg): """ Callback for adding an incomming message to the queue. """ if not self.__widget._paused: msg = PluginLogManager.convert_rosgraph_log_message(log_msg) with QMutexLocker(self._mutex): self._message_queue.append(msg) @staticmethod def convert_rosgraph_log_message(log_msg): msg = Message() msg.set_stamp_format('hh:mm:ss.ZZZ (yyyy-MM-dd)') msg.message = log_msg.msg msg.severity = log_msg.level msg.node = log_msg.name msg.stamp = (log_msg.header.stamp.secs, log_msg.header.stamp.nsecs) msg.topics = sorted(log_msg.topics) msg.location = log_msg.file + ':' + log_msg.function + ':' + str( log_msg.line) return msg def insert_messages(self): """ Callback for flushing incoming Log messages from the queue to the model. """ with QMutexLocker(self._mutex): msgs = self._message_queue self._message_queue = [] if msgs: self._model.insert_rows(msgs) def shutdown_plugin(self): self._subscriber.unregister() self._timer.stop() self.__widget.cleanup_browsers_on_close() def save_settings(self, plugin_settings, instance_settings): self.__widget.save_settings(plugin_settings, instance_settings) def restore_settings(self, plugin_settings, instance_settings): self.__widget.restore_settings(plugin_settings, instance_settings) def trigger_configuration(self): topics = [ t for t in rospy.get_published_topics() if t[1] == 'rosgraph_msgs/Log' ] topics.sort(key=lambda tup: tup[0]) dialog = ConsoleSettingsDialog(topics, self._rospack) (topic, message_limit) = dialog.query(self._topic, self._model.get_message_limit()) if topic != self._topic: self._subscribe(topic) if message_limit != self._model.get_message_limit(): self._model.set_message_limit(message_limit) def _subscribe(self, topic): if self._subscriber: self._subscriber.unregister() self._subscriber = rospy.Subscriber(topic, Log, self.queue_message) self._currenttopic = topic def onStart(self): pass def onPause(self): pass def onResume(self): pass def onControlModeChanged(self, mode): if mode == ControlMode.AUTOMATIC: self.setEnabled(False) else: self.setEnabled(True) def onUserChanged(self, user_info): pass def onTranslate(self, lng): pass def onEmergencyStop(self, state): pass def onDestroy(self): self.shutdown_plugin()
class Console(Plugin): """ rqt_console plugin's main class. Handles communication with ros_gui and contains callbacks to handle incoming message """ def __init__(self, context): """ :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext'' """ super(Console, self).__init__(context) self.setObjectName('Console') self._rospack = rospkg.RosPack() self._model = MessageDataModel() self._proxy_model = MessageProxyModel() self._proxy_model.setSourceModel(self._model) self._widget = ConsoleWidget(self._proxy_model, self._rospack) if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._widget) # queue to store incoming data which get flushed periodically to the model # required since QSortProxyModel can not handle a high insert rate self._message_queue = [] self._mutex = QMutex() self._timer = QTimer() self._timer.timeout.connect(self.insert_messages) self._timer.start(100) self._subscriber = None self._topic = '/rosout_agg' self._subscribe(self._topic) def queue_message(self, log_msg): """ Callback for adding an incomming message to the queue. """ if not self._widget._paused: msg = Console.convert_rosgraph_log_message(log_msg) with QMutexLocker(self._mutex): self._message_queue.append(msg) @staticmethod def convert_rosgraph_log_message(log_msg): msg = Message() msg.set_stamp_format('hh:mm:ss.ZZZ (yyyy-MM-dd)') msg.message = log_msg.msg msg.severity = log_msg.level msg.node = log_msg.name msg.stamp = (log_msg.header.stamp.secs, log_msg.header.stamp.nsecs) msg.topics = sorted(log_msg.topics) msg.location = log_msg.file + ':' + log_msg.function + ':' + str(log_msg.line) return msg def insert_messages(self): """ Callback for flushing incoming Log messages from the queue to the model. """ with QMutexLocker(self._mutex): msgs = self._message_queue self._message_queue = [] if msgs: self._model.insert_rows(msgs) def shutdown_plugin(self): self._subscriber.unregister() self._timer.stop() self._widget.cleanup_browsers_on_close() def save_settings(self, plugin_settings, instance_settings): self._widget.save_settings(plugin_settings, instance_settings) def restore_settings(self, plugin_settings, instance_settings): self._widget.restore_settings(plugin_settings, instance_settings) def trigger_configuration(self): topics = [t for t in rospy.get_published_topics() if t[1] == 'rosgraph_msgs/Log'] topics.sort(key=lambda tup: tup[0]) dialog = ConsoleSettingsDialog(topics, self._rospack) (topic, message_limit) = dialog.query(self._topic, self._model.get_message_limit()) if topic != self._topic: self._subscribe(topic) if message_limit != self._model.get_message_limit(): self._model.set_message_limit(message_limit) def _subscribe(self, topic): if self._subscriber: self._subscriber.unregister() self._subscriber = rospy.Subscriber(topic, Log, self.queue_message) self._currenttopic = topic
class ParamWidget(QWidget): """ main class inherits from the ui window class. You can specify the topics that the topic pane. ParamWidget.start must be called in order to update topic pane. """ SELECT_BY_NAME = 0 SELECT_BY_MSGTYPE = 1 # _column_names = ['topic', 'type', 'bandwidth', 'rate', 'value', 'checkbox'] _column_names = ['topic', 'type', 'min', 'value', 'max', 'checkbox'] selectionChanged = pyqtSignal(dict, name='selectionChanged') def keyPressEvent(self, event): event.ignore() def keyReleaseEvent(self, event): event.ignore() def __init__(self, plugin=None, selected_topics=None, select_topic_type=SELECT_BY_NAME): """ @type selected_topics: list of tuples. @param selected_topics: [($NAME_TOPIC$, $TYPE_TOPIC$), ...] @type select_topic_type: int @param select_topic_type: Can specify either the name of topics or by the type of topic, to filter the topics to show. If 'select_topic_type' argument is None, this arg shouldn't be meaningful. """ super(ParamWidget, self).__init__() self._select_topic_type = select_topic_type self.setFocusPolicy(Qt.StrongFocus) rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_dyn_tune'), 'resource', 'ParamWidget.ui') loadUi(ui_file, self) self._plugin = plugin self.topics_tree_widget.sortByColumn(0, Qt.AscendingOrder) header = self.topics_tree_widget.header() try: setSectionResizeMode = header.setSectionResizeMode # Qt5 except AttributeError: setSectionResizeMode = header.setResizeMode # Qt4 setSectionResizeMode(QHeaderView.ResizeToContents) header.customContextMenuRequested.connect( self.handle_header_view_customContextMenuRequested) header.setContextMenuPolicy(Qt.CustomContextMenu) # for i in range(len(self._column_names)): # setSectionResizeMode(i, QHeaderView.Stretch) header.setStretchLastSection(False) setSectionResizeMode(0, QHeaderView.Stretch) setSectionResizeMode(self._column_names.index("value"), QHeaderView.Stretch) setSectionResizeMode(self._column_names.index("checkbox"), QHeaderView.Fixed) # Whether to get all topics or only the topics that are set in advance. # Can be also set by the setter method "set_selected_topics". self._selected_topics = selected_topics self._selected_items = [] self._current_topic_list = [] self._topics = {} self._tree_items = {} self._column_index = {} for column_name in self._column_names: self._column_index[column_name] = len(self._column_index) self.topics_tree_widget.itemExpanded.connect(self.expanded) self.topics_tree_widget.itemCollapsed.connect(self.collapsed) self.topics_tree_widget.itemChanged.connect(self.itemChanged) self.topics_tree_widget.setAlternatingRowColors(True) delegate = EditorDelegate() self.topics_tree_widget.setItemDelegate(delegate) self.topics_tree_widget.setEditTriggers( QAbstractItemView.AnyKeyPressed | QAbstractItemView.DoubleClicked) hitem = self.topics_tree_widget.headerItem() hitem.setTextAlignment(self._column_names.index("min"), Qt.AlignRight | Qt.AlignVCenter) hitem.setTextAlignment(self._column_names.index("max"), Qt.AlignLeft | Qt.AlignVCenter) hitem.setTextAlignment(self._column_names.index("value"), Qt.AlignHCenter | Qt.AlignVCenter) hitem.setTextAlignment(self._column_names.index("type"), Qt.AlignHCenter | Qt.AlignVCenter) # init and start update timer self._timer_refresh_topics = QTimer(self) self._timer_refresh_topics.timeout.connect(self.refresh_topics) @Slot(dict) def selection_changed(data): print "#\n#\n#\nthe selcted items are:", data, "\n\n" self.selectionChanged.connect(selection_changed) def set_topic_specifier(self, specifier): self._select_topic_type = specifier def start(self): """ This method needs to be called to start updating topic pane. """ self._timer_refresh_topics.start(1000) @Slot('QTreeWidgetItem', int) def itemChanged(self, item, column): # print "<<<<<<<<<<<<<<<<<<<<<<<<<< item changed >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>" selected = self.get_selected() if item._topic_name in selected and self._column_names[column] in [ "min", "max", "value" ]: self.selectionChanged.emit(selected) @Slot('QTreeWidgetItem') def expanded(self, item): name = item.data(0, Qt.UserRole) if not isinstance(item, TreeWidgetItem) or not item._is_topic: return print "expanded", name self._topics[item._topic_name]['info'].start_monitoring() @Slot('QTreeWidgetItem') def collapsed(self, item): name = item.data(0, Qt.UserRole) if not isinstance(item, TreeWidgetItem) or not item._is_topic: return print "collapsed", name self._topics[item._topic_name]['info'].stop_monitoring() _items_param = {} def get_desc(self, item): desc = {} if item._topic_name in self._current_params: desc = self._current_params[item._topic_name] vmin = item.data(self._column_names.index("min"), Qt.EditRole) vmax = item.data(self._column_names.index("max"), Qt.EditRole) val = item.data(self._column_names.index("value"), Qt.EditRole) def tryFloat(s): try: return float(s) except: return None vmin = tryFloat(vmin) vmax = tryFloat(vmax) val = tryFloat(val) desc["min"] = vmin desc["max"] = vmax desc["default"] = val return desc def get_selected(self): # param_name:desc return { param: self.get_desc(self._tree_items[param]) for param in self._selected_items if param in self._current_params } pass def insert_param(self, param_name, param_desc, parent=None): if parent == None: parent = self.topics_tree_widget pnames = param_name.split("/") ns = "" item = parent for name in pnames: if name == "": continue _name = "/" + name ns = ns + _name if ns not in self._tree_items: _item = TreeWidgetItem(self._toggle_monitoring, ns, item) _item.setText(self._column_index['topic'], _name) _item.setData(0, Qt.UserRole, "name_space") _item.setFlags(Qt.ItemIsEnabled | Qt.ItemIsUserCheckable | Qt.ItemIsTristate) self._tree_items[ns] = _item item = self._tree_items[ns] item.setFlags(Qt.ItemIsEditable | Qt.ItemIsEnabled | Qt.ItemIsSelectable | Qt.ItemIsUserCheckable) item.setData(self._column_index['min'], Qt.EditRole, str(param_desc["min"])) item.setData(self._column_index['max'], Qt.EditRole, str(param_desc["max"])) item.setData(self._column_index['value'], Qt.EditRole, str(param_desc["default"])) item.setData(self._column_index['type'], Qt.EditRole, str(param_desc["type"])) self._items_param[item] = param_name print param_name, " added" pass _current_params = {} # TODO: implement the delete mechanism def delete_param(self, param_name, parent=None): pass @Slot() def refresh_topics(self): """ refresh tree view items """ nodes = dynamic_reconfigure.find_reconfigure_services() # dparams = [] dparams = {} for node in nodes: client = dynamic_reconfigure.client.Client(node, timeout=3) gdesc = client.get_group_descriptions() for pdesc in gdesc["parameters"]: param = node + "/" + pdesc["name"] dparams[param] = pdesc for param, desc in self._current_params.items(): if param not in dparams: del self._current_params[param] # TODO: delete the tree widget item for param, desc in dparams.items(): if param not in self._current_params: self._current_params[param] = desc self.insert_param(param, desc) def _update_topics_data(self): selected_dict = { self._selected_items[index]: index for index in range(len(self._selected_items)) } print selected_dict for topic in self._topics.values(): topic_info = topic['info'] if topic_info.monitoring: # update rate rate, _, _, _ = topic_info.get_hz() rate_text = '%1.2f' % rate if rate != None else 'unknown' # update bandwidth bytes_per_s, _, _, _ = topic_info.get_bw() if bytes_per_s is None: bandwidth_text = 'unknown' elif bytes_per_s < 1000: bandwidth_text = '%.2fB/s' % bytes_per_s elif bytes_per_s < 1000000: bandwidth_text = '%.2fKB/s' % (bytes_per_s / 1000.) else: bandwidth_text = '%.2fMB/s' % (bytes_per_s / 1000000.) # update values value_text = '' self.update_value(topic_info._topic_name, topic_info.last_message) else: rate_text = '' bandwidth_text = '' value_text = 'not monitored' if topic_info.error is None else topic_info.error self._tree_items[topic_info._topic_name].setText( self._column_index['value'], value_text) def update_value(self, topic_name, message): if hasattr(message, '__slots__') and hasattr(message, '_slot_types'): for slot_name in message.__slots__: self.update_value(topic_name + '/' + slot_name, getattr(message, slot_name)) elif type(message) in (list, tuple) and (len(message) > 0) and hasattr( message[0], '__slots__'): for index, slot in enumerate(message): if topic_name + '[%d]' % index in self._tree_items: self.update_value(topic_name + '[%d]' % index, slot) else: base_type_str, _ = self._extract_array_info( self._tree_items[topic_name].text( self._column_index['type'])) self._recursive_create_widget_items( self._tree_items[topic_name], topic_name + '[%d]' % index, base_type_str, slot) # remove obsolete children if len(message) < self._tree_items[topic_name].childCount(): for i in range(len(message), self._tree_items[topic_name].childCount()): item_topic_name = topic_name + '[%d]' % i self._recursive_delete_widget_items( self._tree_items[item_topic_name]) else: if topic_name in self._tree_items: self._tree_items[topic_name].setText( self._column_index['value'], repr(message)) def _extract_array_info(self, type_str): array_size = None if '[' in type_str and type_str[-1] == ']': type_str, array_size_str = type_str.split('[', 1) array_size_str = array_size_str[:-1] if len(array_size_str) > 0: array_size = int(array_size_str) else: array_size = 0 return type_str, array_size def _recursive_create_widget_items(self, parent, topic_name, type_name, message): if parent is self.topics_tree_widget: # show full topic name with preceding namespace on toplevel item topic_text = topic_name _item = parent topic_names = topic_name.split('/') name_space = "" for name in topic_names: if name == "": continue _name = "/" + name name_space = name_space + _name if name_space not in self._tree_items: is_topic = False if name_space == topic_name: is_topic = True _item = TreeWidgetItem(self._toggle_monitoring, name_space, _item, is_topic=is_topic) _item.setText(self._column_index['topic'], _name) _item.setText(self._column_index['type'], type_name) _item.setData(0, Qt.UserRole, name_space) self._tree_items[name_space] = _item _item = self._tree_items[name_space] item = _item else: topic_text = topic_name.split('/')[-1] if '[' in topic_text: topic_text = topic_text[topic_text.index('['):] item = TreeWidgetItem(self._toggle_monitoring, topic_name, parent) item.setText(self._column_index['topic'], topic_text) item.setText(self._column_index['type'], type_name) item.setData(0, Qt.UserRole, topic_name) self._tree_items[topic_name] = item if hasattr(message, '__slots__') and hasattr(message, '_slot_types'): for slot_name, type_name in zip(message.__slots__, message._slot_types): self._recursive_create_widget_items( item, topic_name + '/' + slot_name, type_name, getattr(message, slot_name)) else: base_type_str, array_size = self._extract_array_info(type_name) try: base_instance = roslib.message.get_message_class( base_type_str)() except (ValueError, TypeError): base_instance = None if array_size is not None and hasattr(base_instance, '__slots__'): for index in range(array_size): self._recursive_create_widget_items( item, topic_name + '[%d]' % index, base_type_str, base_instance) return item def _toggle_monitoring(self, topic_name): item = self._tree_items[topic_name] if item.checkState(CHECK_COLUMN): print "start %s" % topic_name if topic_name not in self._selected_items and topic_name in self._current_params: self._selected_items.append(topic_name) else: print "stop %s" % topic_name if topic_name in self._selected_items: self._selected_items.remove(topic_name) item.setText(CHECK_COLUMN, '') self.selectionChanged.emit(self.get_selected()) def _recursive_delete_widget_items(self, item): def _recursive_remove_items_from_tree(item): for index in reversed(range(item.childCount())): _recursive_remove_items_from_tree(item.child(index)) topic_name = item.data(0, Qt.UserRole) del self._tree_items[topic_name] _recursive_remove_items_from_tree(item) item.parent().removeChild(item) @Slot('QPoint') def handle_header_view_customContextMenuRequested(self, pos): header = self.topics_tree_widget.header() # show context menu menu = QMenu(self) action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize') action = menu.exec_(header.mapToGlobal(pos)) # evaluate user action if action is action_toggle_auto_resize: if header.resizeMode(0) == QHeaderView.ResizeToContents: header.setResizeMode(QHeaderView.Interactive) else: header.setResizeMode(QHeaderView.ResizeToContents) @Slot('QPoint') def on_topics_tree_widget_customContextMenuRequested(self, pos): item = self.topics_tree_widget.itemAt(pos) if item is None: return # show context menu menu = QMenu(self) action_item_expand = menu.addAction(QIcon.fromTheme('zoom-in'), 'Expand All Children') action_item_collapse = menu.addAction(QIcon.fromTheme('zoom-out'), 'Collapse All Children') action = menu.exec_(self.topics_tree_widget.mapToGlobal(pos)) # evaluate user action if action in (action_item_expand, action_item_collapse): expanded = (action is action_item_expand) def recursive_set_expanded(item): item.setExpanded(expanded) for index in range(item.childCount()): recursive_set_expanded(item.child(index)) recursive_set_expanded(item) def shutdown_plugin(self): for topic in self._topics.values(): topic['info'].stop_monitoring() self._timer_refresh_topics.stop() def set_selected_topics(self, selected_topics): """ @param selected_topics: list of tuple. [(topic_name, topic_type)] @type selected_topics: [] """ rospy.logdebug('set_selected_topics topics={}'.format( len(selected_topics))) self._selected_topics = selected_topics # TODO(Enhancement) Save/Restore tree expansion state def save_settings(self, plugin_settings, instance_settings): header_state = self.topics_tree_widget.header().saveState() instance_settings.set_value('tree_widget_header_state', header_state) def restore_settings(self, pluggin_settings, instance_settings): if instance_settings.contains('tree_widget_header_state'): header_state = instance_settings.value('tree_widget_header_state') if not self.topics_tree_widget.header().restoreState(header_state): rospy.logwarn("rqt_dyn_tune: Failed to restore header state.")
class HistogramPlotWidget(QWidget): _redraw_interval = 40 def __init__(self, topics): super(HistogramPlotWidget, self).__init__() self.setObjectName('HistogramPlotWidget') rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource', 'plot_histogram.ui') loadUi(ui_file, self) self.subscribe_topic_button.setIcon(QIcon.fromTheme('add')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = MatHistogramPlot(self) self.data_plot_layout.addWidget(self.data_plot) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent self._start_time = rospy.get_time() self._rosdata = None if len(topics) != 0: self.subscribe_topic(topics) self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) self._update_plot_timer.start(self._redraw_interval) @Slot('QDropEvent*') def dropEvent(self, event): print "hello" if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.subscribe_topic(topic_name) @Slot() def on_topic_edit_returnPressed(self): if self.subscribe_topic_button.isEnabled(): self.subscribe_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.subscribe_topic(str(self.topic_edit.text())) def subscribe_topic(self, topic_name): if not self._rosdata: self._rosdata = ROSData(topic_name, self._start_time) else: if self._rosdata != topic_name: self._rosdata.close() self.data_plot.clear() self._rosdata = ROSData(topic_name, self._start_time) else: rospy.logwarn("%s is already subscribed", topic_name) def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop() @Slot() def on_clear_button_clicked(self): self.data_plot.clear() @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) def update_plot(self): if not self._rosdata: return data_x, data_y = self._rosdata.next() if len(data_y) == 0: return xs = data_y[-1] axes = self.data_plot._canvas.axes axes.cla() axes.set_xlim(xmin=0, xmax=len(xs)) pos = np.arange(len(xs)) #axes.xticks(range(5)) for p, x in zip(pos, xs): axes.bar(p, x, color='r', align='center') self.data_plot._canvas.draw()
class MobrobDashboardWidget(QWidget): column_names = ['service', 'type', 'expression'] def __init__(self): super(MobrobDashboardWidget, self).__init__() self.setObjectName('MobrobDashboardWidget') # create context for the expression eval statement self._eval_locals = {} for module in (math, random, time): self._eval_locals.update(module.__dict__) self._eval_locals['genpy'] = genpy del self._eval_locals['__name__'] del self._eval_locals['__doc__'] rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('mobrob_robin_rqt'), 'resource', 'MobrobDashboard.ui') loadUi(ui_file, self, {'ExtendedComboBox': ExtendedComboBox}) self._column_index = {} for column_name in self.column_names: self._column_index[column_name] = len(self._column_index) self._twist = Twist() self.base_activated = False self._pub = rospy.Publisher('/mobrob_robin/base/drives/control/cmd_vel', Twist, queue_size = 1) self._timer_refresh_state = QTimer(self) self._timer_refresh_state.timeout.connect(self._timer_update) self.start() def start(self): self._timer_refresh_state.start(10) #def save_settings(self, plugin_settings, instance_settings): def shutdown_plugin(self): self._timer_refresh_state.stop() self._sub_lwa_state.unregister() self._sub_gripper_state.unregister() self._sub_pan_tilt_state.unregister() #def restore_settings(self, plugin_settings, instance_settings): #def trigger_configuration(self): def _timer_update(self): self._publish_twist() self._update_goals() def _publish_twist(self): if self.base_activated: self._pub.publish(self._twist) @Slot() def refresh_state(self): self.test_label.setText('test') def call_service( self, string ): service_name = string service_class = rosservice.get_service_class_by_name( service_name ) service = rospy.ServiceProxy(service_name, service_class) request = service_class._request_class() try: response = service() except rospy.ServiceException as e: qWarning('service_caller: request:\n%r' % (request)) qWarning('service_caller: error calling service "%s":\n%s' % (self._service_info['service_name'], e)) #else: #print(response) ##### Base def _base_move(self, x, y, yaw): twist = Twist() twist.linear.x = x; twist.linear.y = y; twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = yaw; self._twist = twist def _base_stop_motion(self): self.base_activated = False self._base_move(0.0, 0.0, 0.0) self._pub.publish(self._twist) @Slot() def on_base_reference_clicked(self): self.call_service( '/mobrob_robin/base/drives/control/reference' ) @Slot() def on_base_start_clicked(self): self.call_service( '/mobrob_robin/base/drives/control/start' ) @Slot() def on_base_stop_clicked(self): self.call_service( '/mobrob_robin/base/drives/control/stop' ) @Slot() def on_base_forward_pressed(self): self._base_move(0.3, 0.0, 0.0) self.base_activated = True @Slot() def on_base_forward_released(self): self._base_stop_motion() @Slot() def on_base_back_pressed(self): self._base_move(-0.3, 0.0, 0.0) self.base_activated = True @Slot() def on_base_back_released(self): self._base_stop_motion() @Slot() def on_base_left_pressed(self): self._base_move(0.0, 0.3, 0.0) self.base_activated = True @Slot() def on_base_left_released(self): self._base_stop_motion() @Slot() def on_base_right_pressed(self): self._base_move(0.0, -0.3, 0.0) self.base_activated = True @Slot() def on_base_right_released(self): self._base_stop_motion() @Slot() def on_base_turn_left_pressed(self): self._base_move(0.0, 0.0, 0.5) self.base_activated = True @Slot() def on_base_turn_left_released(self): self._base_stop_motion() @Slot() def on_base_turn_right_pressed(self): self._base_move(0.0, 0.0, -0.5) self.base_activated = True @Slot() def on_base_turn_right_released(self): self._base_stop_motion() #diagonal @Slot() def on_base_forward_left_pressed(self): self._base_move(0.2, 0.2, 0.0) self.base_activated = True @Slot() def on_base_forward_left_released(self): self._base_stop_motion() @Slot() def on_base_forward_right_pressed(self): self._base_move(0.2, -0.2, 0.0) self.base_activated = True @Slot() def on_base_forward_right_released(self): self._base_stop_motion() @Slot() def on_base_back_left_pressed(self): self._base_move(-0.2, 0.2, 0.0) self.base_activated = True @Slot() def on_base_back_left_released(self): self._base_stop_motion() @Slot() def on_base_back_right_pressed(self): self._base_move(-0.2, -0.2, 0.0) self.base_activated = True @Slot() def on_base_back_right_released(self): self._base_stop_motion()
class RobotSteering(Plugin): slider_factor = 1000.0 def __init__(self, context): super(RobotSteering, self).__init__(context) self.setObjectName('RobotSteering') self._publisher = None self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('RobotSteeringUi') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._widget) self._widget.topic_line_edit.textChanged.connect(self._on_topic_changed) self._widget.stop_push_button.pressed.connect(self._on_stop_pressed) self._widget.x_linear_slider.valueChanged.connect(self._on_x_linear_slider_changed) self._widget.z_angular_slider.valueChanged.connect(self._on_z_angular_slider_changed) self._widget.increase_x_linear_push_button.pressed.connect(self._on_strong_increase_x_linear_pressed) self._widget.reset_x_linear_push_button.pressed.connect(self._on_reset_x_linear_pressed) self._widget.decrease_x_linear_push_button.pressed.connect(self._on_strong_decrease_x_linear_pressed) self._widget.increase_z_angular_push_button.pressed.connect(self._on_strong_increase_z_angular_pressed) self._widget.reset_z_angular_push_button.pressed.connect(self._on_reset_z_angular_pressed) self._widget.decrease_z_angular_push_button.pressed.connect(self._on_strong_decrease_z_angular_pressed) self._widget.max_x_linear_double_spin_box.valueChanged.connect(self._on_max_x_linear_changed) self._widget.min_x_linear_double_spin_box.valueChanged.connect(self._on_min_x_linear_changed) self._widget.max_z_angular_double_spin_box.valueChanged.connect(self._on_max_z_angular_changed) self._widget.min_z_angular_double_spin_box.valueChanged.connect(self._on_min_z_angular_changed) self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget) self.shortcut_w.setContext(Qt.ApplicationShortcut) self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed) self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget) self.shortcut_x.setContext(Qt.ApplicationShortcut) self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed) self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget) self.shortcut_s.setContext(Qt.ApplicationShortcut) self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed) self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget) self.shortcut_a.setContext(Qt.ApplicationShortcut) self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed) self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget) self.shortcut_z.setContext(Qt.ApplicationShortcut) self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed) self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget) self.shortcut_d.setContext(Qt.ApplicationShortcut) self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed) self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget) self.shortcut_shift_w.setContext(Qt.ApplicationShortcut) self.shortcut_shift_w.activated.connect(self._on_strong_increase_x_linear_pressed) self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget) self.shortcut_shift_x.setContext(Qt.ApplicationShortcut) self.shortcut_shift_x.activated.connect(self._on_reset_x_linear_pressed) self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget) self.shortcut_shift_s.setContext(Qt.ApplicationShortcut) self.shortcut_shift_s.activated.connect(self._on_strong_decrease_x_linear_pressed) self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget) self.shortcut_shift_a.setContext(Qt.ApplicationShortcut) self.shortcut_shift_a.activated.connect(self._on_strong_increase_z_angular_pressed) self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget) self.shortcut_shift_z.setContext(Qt.ApplicationShortcut) self.shortcut_shift_z.activated.connect(self._on_reset_z_angular_pressed) self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget) self.shortcut_shift_d.setContext(Qt.ApplicationShortcut) self.shortcut_shift_d.activated.connect(self._on_strong_decrease_z_angular_pressed) self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget) self.shortcut_space.setContext(Qt.ApplicationShortcut) self.shortcut_space.activated.connect(self._on_stop_pressed) self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget) self.shortcut_space.setContext(Qt.ApplicationShortcut) self.shortcut_space.activated.connect(self._on_stop_pressed) self._widget.stop_push_button.setToolTip(self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)')) self._widget.increase_x_linear_push_button.setToolTip(self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)')) self._widget.reset_x_linear_push_button.setToolTip(self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)')) self._widget.decrease_x_linear_push_button.setToolTip(self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)')) self._widget.increase_z_angular_push_button.setToolTip(self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)')) self._widget.reset_z_angular_push_button.setToolTip(self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)')) self._widget.decrease_z_angular_push_button.setToolTip(self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)')) # timer to consecutively send twist messages self._update_parameter_timer = QTimer(self) self._update_parameter_timer.timeout.connect(self._on_parameter_changed) self._update_parameter_timer.start(100) self.zero_cmd_sent = False @Slot(str) def _on_topic_changed(self, topic): topic = str(topic) self._unregister_publisher() self._publisher = rospy.Publisher(topic, Twist) self._publisher_heat = rospy.Publisher('tempeture', Int16) self._sub = rospy.Subscriber("tempeture", Int16, self.heat_callback) def heat_callback(self, data): self._widget.current_z_angular_label.setText('%0.2f rad/s' % data.data) self._on_parameter_changed() def _on_stop_pressed(self): self._widget.x_linear_slider.setValue(0) self._widget.z_angular_slider.setValue(0) def _on_x_linear_slider_changed(self): self._widget.current_x_linear_label.setText('%0.2f m/s' % (self._widget.x_linear_slider.value() / RobotSteering.slider_factor)) self._on_parameter_changed() def _on_z_angular_slider_changed(self): self._widget.current_z_angular_label.setText('%0.2f rad/s' % (self._widget.z_angular_slider.value() / RobotSteering.slider_factor)) self._on_parameter_changed() def _on_increase_x_linear_pressed(self): self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.singleStep()) def _on_reset_x_linear_pressed(self): self._widget.x_linear_slider.setValue(0) def _on_decrease_x_linear_pressed(self): self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.singleStep()) def _on_increase_z_angular_pressed(self): self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.singleStep()) def _on_reset_z_angular_pressed(self): self._widget.z_angular_slider.setValue(0) def _on_decrease_z_angular_pressed(self): self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.singleStep()) def _on_max_x_linear_changed(self, value): self._widget.x_linear_slider.setMaximum(value * RobotSteering.slider_factor) def _on_min_x_linear_changed(self, value): self._widget.x_linear_slider.setMinimum(value * RobotSteering.slider_factor) def _on_max_z_angular_changed(self, value): self._widget.z_angular_slider.setMaximum(value * RobotSteering.slider_factor) def _on_min_z_angular_changed(self, value): self._widget.z_angular_slider.setMinimum(value * RobotSteering.slider_factor) def _on_strong_increase_x_linear_pressed(self): self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.pageStep()) def _on_strong_decrease_x_linear_pressed(self): self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.pageStep()) def _on_strong_increase_z_angular_pressed(self): self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.pageStep()) def _on_strong_decrease_z_angular_pressed(self): self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.pageStep()) def _on_parameter_changed(self): self._send_twist(self._widget.x_linear_slider.value() / RobotSteering.slider_factor, self._widget.z_angular_slider.value() / RobotSteering.slider_factor) def _send_twist(self, x_linear, z_angular): if self._publisher is None: return twist = Twist() twist.linear.x = x_linear twist.linear.y = 0 twist.linear.z = 0 twist.angular.x = 0 twist.angular.y = 0 twist.angular.z = z_angular heat = Int16() heat.data = x_linear # Only send the zero command once so other devices can take control if x_linear == 0 and z_angular == 0: if not self.zero_cmd_sent: self.zero_cmd_sent = True self._publisher.publish(twist) self._publisher_heat.publish(heat) else: self.zero_cmd_sent = False self._publisher.publish(twist) self._publisher_heat.publish(heat) def _unregister_publisher(self): if self._publisher is not None: self._publisher.unregister() self._publisher = None def shutdown_plugin(self): self._update_parameter_timer.stop() self._unregister_publisher() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('topic' , self._widget.topic_line_edit.text()) instance_settings.set_value('vx_max', self._widget.max_x_linear_double_spin_box.value()) instance_settings.set_value('vx_min', self._widget.min_x_linear_double_spin_box.value()) instance_settings.set_value('vw_max', self._widget.max_z_angular_double_spin_box.value()) instance_settings.set_value('vw_min', self._widget.min_z_angular_double_spin_box.value()) def restore_settings(self, plugin_settings, instance_settings): value = instance_settings.value('topic', "/cmd_vel") value = rospy.get_param("~default_topic", value) self._widget.topic_line_edit.setText(value) value = self._widget.max_x_linear_double_spin_box.value() value = instance_settings.value( 'vx_max', value) value = rospy.get_param("~default_vx_max", value) self._widget.max_x_linear_double_spin_box.setValue(float(value)) value = self._widget.min_x_linear_double_spin_box.value() value = instance_settings.value( 'vx_min', value) value = rospy.get_param("~default_vx_min", value) self._widget.min_x_linear_double_spin_box.setValue(float(value)) value = self._widget.max_z_angular_double_spin_box.value() value = instance_settings.value( 'vw_max', value) value = rospy.get_param("~default_vw_max", value) self._widget.max_z_angular_double_spin_box.setValue(float(value)) value = self._widget.min_z_angular_double_spin_box.value() value = instance_settings.value( 'vw_min', value) value = rospy.get_param("~default_vw_min", value) self._widget.min_z_angular_double_spin_box.setValue(float(value))
class EchoDialog(QDialog): MESSAGE_CHARS_LIMIT = 1000 MESSAGE_LINE_LIMIT = 80 MESSAGE_HZ_LIMIT = 10 MAX_DISPLAY_MSGS = 25 STATISTIC_QUEUE_LEN = 1000 SHOW_BYTES = True SHOW_JITTER = True SHOW_STD_DEV = False SHOW_WINDOW_SIZE = False ''' This dialog shows the output of a topic. ''' finished_signal = Signal(str) ''' finished_signal has as parameter the name of the topic and is emitted, if this dialog was closed. ''' msg_signal = Signal(object, bool) ''' msg_signal is a signal, which is emitted, if a new message was received. ''' text_hz_signal = Signal(str) text_signal = Signal(str) ''' text_signal is a signal, which is emitted, if a new text to display was received. ''' text_error_signal = Signal(str) ''' text_error_signal is a signal, which is emitted, if a new error text to display was received. ''' request_pw = Signal(object) def __init__(self, topic, msg_type, show_only_rate=False, masteruri=None, use_ssh=False, parent=None): ''' Creates an input dialog. @param topic: the name of the topic @type topic: C{str} @param msg_type: the type of the topic @type msg_type: C{str} @raise Exception: if no topic class was found for the given type ''' QDialog.__init__(self, parent=parent) self._masteruri = masteruri masteruri_str = '' if masteruri is None else '[%s]' % masteruri echo_dialog_file = os.path.join( os.path.dirname(os.path.realpath(__file__)), 'EchoDialog.ui') loadUi(echo_dialog_file, self) self.setObjectName(' - '.join(['EchoDialog', topic, masteruri_str])) self.setAttribute(Qt.WA_DeleteOnClose, True) self.setWindowFlags(Qt.Window) self.setWindowTitle('%s %s %s' % ('Echo --- ' if not show_only_rate else 'Hz --- ', topic, masteruri_str)) self.resize(900, 512) self.topic = topic self.show_only_rate = show_only_rate self.lock = threading.RLock() self.last_printed_count = 0 self.msg_t0 = -1. self.msg_tn = 0 self.times = [] self.bytes = [] self.message_count = 0 self._state_message = '' self._state_size_message = '' self._scrapped_msgs = 0 self._scrapped_msgs_sl = 0 self._last_received_ts = 0 self.chars_limit = self.MESSAGE_CHARS_LIMIT self.receiving_hz = self.MESSAGE_HZ_LIMIT self.line_limit = self.MESSAGE_LINE_LIMIT self.max_displayed_msgs = self.MAX_DISPLAY_MSGS self.digits_after_in_array = 2 self.enabled_message_filter = True self.field_filter_fn = None self._latched = False self._msgs = [] self.filterFrame.setVisible(False) self.topicControlButton.clicked.connect( self.on_topic_control_btn_clicked) self.clearButton.clicked.connect(self.on_clear_btn_clicked) if show_only_rate: self.filterButton.setVisible(False) else: self.filterButton.clicked.connect(self.on_filter_clicked) self.showStringsCheckBox.toggled.connect( self.on_no_str_checkbox_toggled) self.maxLenStringComboBox.activated[str].connect( self.combobox_reduce_ch_activated) self.showArraysCheckBox.toggled.connect( self.on_no_arr_checkbox_toggled) self.maxDigitsComboBox.activated[str].connect( self.combobox_reduce_digits_activated) self.enableMsgFilterCheckBox.toggled.connect( self.on_enable_msg_filter_checkbox_toggled) self.maxLenComboBox.activated[str].connect( self.on_combobox_chars_activated) self.maxHzComboBox.activated[str].connect( self.on_combobox_hz_activated) self.displayCountComboBox.activated[str].connect( self.on_combobox_count_activated) self.combobox_reduce_ch_activated(self.MESSAGE_LINE_LIMIT) self.on_combobox_chars_activated(self.MESSAGE_CHARS_LIMIT) self.on_combobox_hz_activated(self.MESSAGE_HZ_LIMIT) self.on_combobox_count_activated(self.MAX_DISPLAY_MSGS) self.filterButton.setFocus() self.display.setReadOnly(True) self.display.document().setMaximumBlockCount(500) self._blocks_in_msg = None self.display.setOpenLinks(False) self.display.anchorClicked.connect(self._on_display_anchorClicked) # subscribe to the topic errmsg = '' try: self.__msg_class = message.get_message_class(msg_type) if not self.__msg_class: errmsg = "Cannot load message class for [%s]. Did you build messages?" % msg_type except Exception as e: self.__msg_class = None errmsg = "Cannot load message class for [%s]. Did you build messagest?\nError: %s" % ( msg_type, utf8(e)) # variables for Subscriber self.msg_signal.connect(self._append_message) self.sub = None # vairables for SSH connection self.ssh_output_file = None self.ssh_error_file = None self.ssh_input_file = None self.text_signal.connect(self._append_text) self.text_hz_signal.connect(self._append_text_hz) self._current_msg = '' self._current_errmsg = '' self.text_error_signal.connect(self._append_error_text) # decide, which connection to open if use_ssh: self.__msg_class = None self._on_display_anchorClicked(QUrl(self._masteruri)) elif self.__msg_class is None: errtxt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">\n%s</pre>' % ( errmsg) self.display.setText('<a href="%s">open using SSH</a>' % (masteruri)) self.display.append(errtxt) else: self.sub = rospy.Subscriber(self.topic, self.__msg_class, self._msg_handle) self.print_hz_timer = QTimer() self.print_hz_timer.timeout.connect(self._on_calc_hz) self.print_hz_timer.start(1000) self._start_time = time.time() # print "======== create", self.objectName() # # def __del__(self): # print "******* destroy", self.objectName() # def hideEvent(self, event): # self.close() def closeEvent(self, event): self.print_hz_timer.stop() if self.sub is not None: self.sub.unregister() del self.sub try: self.ssh_output_file.close() self.ssh_error_file.close() # send Ctrl+C to remote process self.ssh_input_file.write('%s\n' % chr(3)) self.ssh_input_file.close() except Exception: pass self.finished_signal.emit(self.topic) if self.parent() is None: QApplication.quit() def create_field_filter(self, echo_nostr, echo_noarr): def field_filter(val): try: # fields = val.__slots__ # field_types = val._slot_types for f, t in zip(val.__slots__, val._slot_types): if echo_noarr and '[' in t: continue elif echo_nostr and 'string' in t: continue yield f except Exception: pass return field_filter def on_filter_clicked(self, checked): self.filterFrame.setVisible(checked) def on_no_str_checkbox_toggled(self, state): self.maxLenStringComboBox.setEnabled(state) self.field_filter_fn = self.create_field_filter( not state, not self.showArraysCheckBox.isChecked()) def on_no_arr_checkbox_toggled(self, state): self.maxDigitsComboBox.setEnabled(state) self.field_filter_fn = self.create_field_filter( not self.showStringsCheckBox.isChecked(), not state) def combobox_reduce_ch_activated(self, ch_txt): try: self.line_limit = int(ch_txt) except ValueError: try: self.line_limit = float(ch_txt) except ValueError: self.maxLenStringComboBox.setEditText(str(self.line_limit)) self.display.clear() with self.lock: for msg, current_time in self._msgs: self._append_message(msg, self._latched, current_time, False) def combobox_reduce_digits_activated(self, ch_txt): try: self.digits_after_in_array = int(ch_txt) except ValueError: self.digits_after_in_array = None self.maxDigitsComboBox.setEditText('') self.display.clear() with self.lock: for msg, current_time in self._msgs: self._append_message(msg, self._latched, current_time, False) def on_enable_msg_filter_checkbox_toggled(self, state): self.enabled_message_filter = state self.maxLenComboBox.setEnabled(state) self.maxHzComboBox.setEnabled(state) if self.enabled_message_filter: self.on_combobox_chars_activated(self.maxLenComboBox.currentText(), False) self.on_combobox_hz_activated(self.maxHzComboBox.currentText(), False) else: self.chars_limit = 0 self.receiving_hz = 0 self.display.clear() with self.lock: for msg, current_time in self._msgs: self._append_message(msg, self._latched, current_time, False) def on_combobox_chars_activated(self, chars_txt, update_display=True): if not self.enabled_message_filter: return try: self.chars_limit = int(chars_txt) except ValueError: try: self.chars_limit = float(chars_txt) except ValueError: self.maxLenComboBox.setEditText(str(self.chars_limit)) if update_display: self.display.clear() with self.lock: for msg, current_time in self._msgs: self._append_message(msg, self._latched, current_time, False) def on_combobox_hz_activated(self, hz_txt, update_display=True): if not self.enabled_message_filter: return try: self.receiving_hz = int(hz_txt) except ValueError: try: self.receiving_hz = float(hz_txt) except ValueError: self.maxHzComboBox.setEditText(str(self.receiving_hz)) if update_display: self.display.clear() with self.lock: for msg, current_time in self._msgs: self._append_message(msg, self._latched, current_time, False) def on_combobox_count_activated(self, count_txt): try: self.max_displayed_msgs = int(count_txt) self._blocks_in_msg = None except ValueError: self.displayCountComboBox.setEditText(str(self.max_displayed_msgs)) def on_clear_btn_clicked(self): self.display.clear() with self.lock: del self._msgs[:] self.message_count = 0 self._scrapped_msgs = 0 del self.times[:] del self.bytes[:] def on_topic_control_btn_clicked(self): try: if self.sub is None and self.ssh_output_file is None: if self.__msg_class: self.sub = rospy.Subscriber(self.topic, self.__msg_class, self._msg_handle) self._start_time = time.time() else: self._on_display_anchorClicked(QUrl(self._masteruri)) self.topicControlButton.setIcon( QIcon(':/icons/sekkyumu_stop.png')) else: if self.sub is not None: self.sub.unregister() self.sub = None elif self.ssh_output_file is not None: self.ssh_output_file.close() self.ssh_error_file.close() self.ssh_output_file = None self.topicControlButton.setIcon( QIcon(':/icons/sekkyumu_play.png')) except Exception as e: rospy.logwarn('Error while stop/play echo for topic %s: %s' % (self.topic, utf8(e))) def _msg_handle(self, data): self.msg_signal.emit(data, (data._connection_header['latching'] != '0')) def _append_message(self, msg, latched, current_time=None, store=True): ''' Adds a label to the dialog's layout and shows the given text. @param msg: the text to add to the dialog @type msg: message object ''' if current_time is None: current_time = time.time() self._latched = latched if store: with self.lock: self._msgs.append((msg, current_time)) if len(self._msgs) > 25: self._msgs.pop() msg_len = -1 if (self.SHOW_BYTES or self.show_only_rate): buff = None try: from cStringIO import StringIO # Python 2.x buff = StringIO() import os msg.serialize(buff) buff.seek(0, os.SEEK_END) msg_len = buff.tell() except ImportError: from io import BytesIO # Python 3.x buff = BytesIO() msg.serialize(buff) msg_len = buff.getbuffer().nbytes self._count_messages(current_time, msg_len) # skip messages, if they are received often then MESSAGE_HZ_LIMIT if self._last_received_ts != 0 and self.receiving_hz != 0: if current_time - self._last_received_ts < 1.0 / self.receiving_hz: if (not latched or (latched and current_time - self._start_time > 3.0)): self._scrapped_msgs += 1 self._scrapped_msgs_sl += 1 return self._last_received_ts = current_time if not self.show_only_rate: # convert message to string and reduce line width to current limit msg = self.strify_message( msg, field_filter=self.field_filter_fn, fixed_numeric_width=self.digits_after_in_array) if isinstance(msg, tuple): msg = msg[0] msg = self._trim_width(msg) msg = msg.replace('<', '<').replace('>', '>') msg_cated = False if self.chars_limit != 0 and len(msg) > self.chars_limit: msg = msg[0:self.chars_limit] msg_cated = True # create a notification about scrapped messages if self._scrapped_msgs_sl > 0: txt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">scrapped %s message because of Hz-settings</pre>' % self._scrapped_msgs_sl self.display.append(txt) self._scrapped_msgs_sl = 0 txt = '<pre style="background-color:#FFFCCC; color:#000000;font-family:Fixedsys,Courier; padding:10px;">---------- %s --------------------\n%s</pre>' % ( datetime.now().strftime("%d.%m.%Y %H:%M:%S.%f"), msg) # set the count of the displayed messages on receiving the first message self._update_max_msg_count(txt) self.display.append(txt) if msg_cated: txt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">message has been cut off</pre>' self.display.append(txt) if store: self._print_status() def _count_messages(self, ts=time.time(), msg_len=-1): ''' Counts the received messages. Call this method only on receive message. ''' current_time = ts with self.lock: # time reset if self.msg_t0 < 0 or self.msg_t0 > current_time: self.msg_t0 = current_time self.msg_tn = current_time self.times = [] self.bytes = [] else: self.times.append(current_time - self.msg_tn) if msg_len > -1: self.bytes.append(msg_len) self.msg_tn = current_time # keep only statistics for the last 5000 messages so as not to run out of memory if len(self.times) > self.STATISTIC_QUEUE_LEN: self.times.pop(0) if len(self.bytes) > self.STATISTIC_QUEUE_LEN: self.bytes.pop(0) self.message_count += 1 def _trim_width(self, msg): ''' reduce line width to current limit :param msg: the message :type msg: str :return: trimmed message ''' result = msg if self.line_limit != 0: a = '' for l in msg.splitlines(): a = a + (l if len(l) <= self.line_limit else l[0:self.line_limit - 3] + '...') + '\n' result = a return result def _update_max_msg_count(self, txt): ''' set the count of the displayed messages on receiving the first message :param txt: text of the message, which will be added to the document :type txt: str ''' if self._blocks_in_msg is None: td = QTextDocument(txt) self._blocks_in_msg = td.blockCount() self.display.document().setMaximumBlockCount( self._blocks_in_msg * self.max_displayed_msgs) def _on_calc_hz(self): if rospy.is_shutdown(): self.close() return if not self.show_only_rate and time.time() - self._last_received_ts > 1: # create a notification about scrapped messages if self._scrapped_msgs_sl > 0: txt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">scrapped %s message because of Hz-settings</pre>' % self._scrapped_msgs_sl self._scrapped_msgs_sl = 0 self.display.append(txt) if self.message_count == self.last_printed_count: return with self.lock: message_rate = '' message_bytes = '' message_jitter = '' message_window = '' message_std_dev = '' message_scrapped = '' sum_times = sum(self.times) if sum_times == 0: sum_times = 1 if (self.SHOW_BYTES or self.show_only_rate) and self.bytes: sum_bytes = sum(self.bytes) avg = sum_bytes / len(self.bytes) last = self.bytes[-1] if avg != last: message_bytes = "size[ last: %s, avg: %s ]" % ( self._normilize_size_print(last), self._normilize_size_print(avg)) else: message_bytes = "size: %s" % ( self._normilize_size_print(last)) byte_rate = float(sum_bytes) / float(sum_times) message_bytes += " bw: %s/s" % ( self._normilize_size_print(byte_rate)) # the code from ROS rostopic n = len(self.times) if n < 2: return mean = sum_times / n rate = 1. / mean if mean > 0. else 0 message_rate = "average rate: %.3f" % rate # min and max if self.SHOW_JITTER or self.show_only_rate: max_delta = max(self.times) min_delta = min(self.times) message_jitter = "jitter[ min: %.3fs max: %.3fs ]" % ( min_delta, max_delta) # std dev self.last_printed_count = self.message_count if self.SHOW_STD_DEV or self.show_only_rate: std_dev = math.sqrt(sum((x - mean)**2 for x in self.times) / n) message_std_dev = "std dev: %.5fs" % (std_dev) if self.SHOW_WINDOW_SIZE or self.show_only_rate: message_window = "window: %s" % (n + 1) if self._scrapped_msgs > 0: message_scrapped += "scrapped msgs: %s" % self._scrapped_msgs self._state_message = '' self._state_size_message = message_bytes for msg in [ message_rate, message_jitter, message_std_dev, message_window, message_scrapped ]: if msg: if self._state_message: self._state_message += ' ' self._state_message += msg self._print_status() if self.show_only_rate: self.display.append("%s %s" % (self._state_message, message_bytes)) def _normilize_size_print(self, size): if size > 999999: return "%.2fMiB" % (size / 1048576.0) if size > 999: return "%.2fKiB" % (size / 1024.0) return "%dB" % size def _print_status(self): text = '%s messages %s' % (self.message_count, self._state_message) if self._latched: text = "[latched] %s" % text self.statusLabel.setText(text) self.statusSizeLabel.setText(self._state_size_message) def _append_text(self, text): ''' Append echo text received through the SSH. ''' with self.lock: self._current_msg += text if self._current_msg.find('---') != -1: messages = self._current_msg.split('---') for m in messages[:-1]: current_time = time.time() self._count_messages(current_time) # limit the displayed text width m = self._trim_width(m) txt = '<pre style="background-color:#FFFCCC; color:#000000;font-family:Fixedsys,Courier; padding:10px;">---------- %s --------------------\n%s</pre>' % ( datetime.now().strftime("%d.%m.%Y %H:%M:%S.%f"), m) # set the count of the displayed messages on receiving the first message self._update_max_msg_count(txt) self.display.append(txt) self._current_msg = messages[-1] self._print_status() def _append_error_text(self, text): ''' Append error text received through the SSH. ''' with self.lock: self._current_errmsg += text if self._current_errmsg.find('\n') != -1: messages = self._current_errmsg.split('\n') for m in messages[:-1]: txt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">%s</pre>' % m self.display.append(txt) self._current_errmsg = messages[-1] def _append_text_hz(self, text): ''' Append text received through the SSH for hz view. ''' with self.lock: self._current_msg += text if self._current_msg.find('\n') != -1: messages = self._current_msg.split('\n') for m in messages[:-1]: txt = '<div style="font-family:Fixedsys,Courier;">%s</div>' % ( m) self.display.append(txt) self._current_msg = messages[-1] def _on_display_anchorClicked(self, url, user=None, pw=None): try: ok = False if self.show_only_rate: 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) self.statusLabel.setText('connected to %s over SSH' % url.host()) else: nostr = '--nostr' if not self.showStringsCheckBox.isChecked( ) else '' noarr = '--noarr' if not self.showArraysCheckBox.isChecked( ) else '' 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) if ok: self.display.clear() target = self._read_output_hz if self.show_only_rate else self._read_output thread = threading.Thread(target=target, args=((self.ssh_output_file, ))) thread.setDaemon(True) thread.start() thread = threading.Thread(target=self._read_error, args=((self.ssh_error_file, ))) thread.setDaemon(True) thread.start() elif self.ssh_output_file: self.ssh_output_file.close() self.ssh_error_file.close() except Exception as e: self._append_error_text('%s\n' % e) def _read_output_hz(self, output_file): try: while not output_file.closed: text = output_file.read(1) if text: self.text_hz_signal.emit(text) except Exception: pass def _read_output(self, output_file): while not output_file.closed: text = output_file.read(1) if text: self.text_signal.emit(text) def _read_error(self, error_file): try: while not error_file.closed: text = error_file.read(1) if text: self.text_error_signal.emit(text) except Exception: pass # ############################################################################# # PARTS OF genpy/messages.py # ############################################################################# @classmethod def strify_message(cls, val, indent='', time_offset=None, current_time=None, field_filter=None, fixed_numeric_width=None, digits_after_in_array=None): """ Convert value to string representation :param val: to convert to string representation. Most likely a Message. ``Value`` :param indent: indentation. If indent is set, then the return value will have a leading \n, ``str`` :param time_offset: if not None, time fields will be displayed as deltas from time_offset, ``Time`` :param current_time: currently not used. Only provided for API compatibility. current_time passes in the current time with respect to the message, ``Time`` :param field_filter: filter the fields that are strified for Messages, ``fn(Message)->iter(str)`` :returns: string (YAML) representation of message, ``str`` """ type_ = type(val) if type_ in (int, long, float) and fixed_numeric_width is not None: if type_ is float: return ('%.' + str(fixed_numeric_width) + 'f') % val else: return ('%d') % val elif type_ in (int, long, float, bool): return utf8(val) elif isstring(val): # TODO: need to escape strings correctly if not val: return "''" return val elif isinstance(val, TVal): if time_offset is not None and isinstance(val, Time): val = val - time_offset if fixed_numeric_width is not None: format_str = '%d' sec_str = '\n%ssecs: ' % indent + (format_str % val.secs) nsec_str = '\n%snsecs: ' % indent + (format_str % val.nsecs) return sec_str + nsec_str else: return '\n%ssecs: %s\n%snsecs: %9d' % (indent, val.secs, indent, val.nsecs) elif type_ in (list, tuple): if len(val) == 0: return "[]" val0 = val[0] if type(val0) in (int, float) and digits_after_in_array is not None: list_str = '[' + ''.join( cls.strify_message(v, indent, time_offset, current_time, field_filter, digits_after_in_array) + ', ' for v in val).rstrip(', ') + ']' return list_str elif type(val0) in (int, float, str, bool): # TODO: escape strings properly return utf8(list(val)) else: pref = indent + '- ' indent = indent + ' ' return '\n' + '\n'.join([ pref + cls.strify_message(v, indent, time_offset, current_time, field_filter, digits_after_in_array) for v in val ]) elif isinstance(val, message.Message): # allow caller to select which fields of message are strified if field_filter is not None: fields = list(field_filter(val)) else: fields = val.__slots__ p = '%s%%s: %%s' % (indent) ni = ' ' + indent python_zip = None if sys.hexversion > 0x03000000: # Python3 python_zip = zip else: # Python2 python_zip = itertools.izip slots = [] for f, t in python_zip(val.__slots__, val._slot_types): if f in fields: cval = _convert_getattr(val, f, t) slot_name = f if isinstance(cval, (list, tuple)): slot_name = "%s[%d]" % (f, len(cval)) slots.append(p % (utf8(slot_name), cls.strify_message( cval, ni, time_offset, current_time, field_filter, fixed_numeric_width))) vals = '\n'.join(slots) if indent: return '\n' + vals else: return vals else: return utf8(val) # punt
class Plot2DWidget(QWidget): _redraw_interval = 10 def __init__(self, topics): super(Plot2DWidget, self).__init__() self.setObjectName('Plot2DWidget') rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource', 'plot_histogram.ui') loadUi(ui_file, self) self.cv_bridge = CvBridge() self.subscribe_topic_button.setIcon(QIcon.fromTheme('add')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = MatPlot2D(self) self.data_plot_layout.addWidget(self.data_plot) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent self._start_time = rospy.get_time() self._rosdata = None if len(topics) != 0: self.subscribe_topic(topics) self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) self._update_plot_timer.start(self._redraw_interval) @Slot('QDropEvent*') def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.subscribe_topic(topic_name) @Slot() def on_topic_edit_returnPressed(self): "callback function when form is entered" if self.subscribe_topic_button.isEnabled(): self.subscribe_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.subscribe_topic(str(self.topic_edit.text())) def subscribe_topic(self, topic_name): rospy.loginfo("subscribe topic") self.topic_with_field_name = topic_name self.pub_image = rospy.Publisher(topic_name + "/plot_image", Image) if not self._rosdata: self._rosdata = ROSData(topic_name, self._start_time) else: if self._rosdata != topic_name: self._rosdata.close() self.data_plot.clear() self._rosdata = ROSData(topic_name, self._start_time) else: rospy.logwarn("%s is already subscribed", topic_name) def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop() @Slot() def on_clear_button_clicked(self): self.data_plot.clear() @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) def plot_one(self, msg, axes): concatenated_data = zip(msg.xs, msg.ys) if self.sort_x: concatenated_data.sort(key=lambda x: x[0]) xs = [d[0] for d in concatenated_data] ys = [d[1] for d in concatenated_data] if self.is_line or msg.type is PlotData.LINE: axes.plot(xs, ys, label=msg.label or self.topic_with_field_name) else: axes.scatter(xs, ys, label=msg.label or self.topic_with_field_name) if msg.fit_line or self.fit_line: X = np.array(msg.xs) Y = np.array(msg.ys) A = np.array([X,np.ones(len(X))]) A = A.T a,b = np.linalg.lstsq(A,Y)[0] axes.plot(X,(a*X+b),"g--", label="{0} x + {1}".format(a, b)) if msg.fit_line_ransac or self.fit_line_ransac: model_ransac = linear_model.RANSACRegressor( linear_model.LinearRegression(), min_samples=2, residual_threshold=self.fit_line_ransac_outlier) X = np.array(msg.xs).reshape((len(msg.xs), 1)) Y = np.array(msg.ys) model_ransac.fit(X, Y) line_X = X line_y_ransac = model_ransac.predict(line_X) axes.plot(line_X, line_y_ransac, "r--", label="{0} x + {1}".format(model_ransac.estimator_.coef_[0][0], model_ransac.estimator_.intercept_[0])) def update_plot(self): if not self._rosdata: return try: data_x, data_y = self._rosdata.next() except RosPlotException, e: rospy.logerr("Exception in subscribing topic") rospy.logerr(e.message) return if len(data_y) == 0: return axes = self.data_plot._canvas.axes axes.cla() # matplotlib # concatenate x and y in order to sort latest_msg = data_y[-1] min_x = None max_x = None min_y = None max_y = None if isinstance(latest_msg, PlotData): data = [latest_msg] legend_size = 8 no_legend = False elif isinstance(latest_msg, PlotDataArray): data = latest_msg.data legend_size = latest_msg.legend_font_size or 8 no_legend = latest_msg.no_legend if latest_msg.min_x != latest_msg.max_x: min_x = latest_msg.min_x max_x = latest_msg.max_x if latest_msg.min_y != latest_msg.max_y: min_y = latest_msg.min_y max_y = latest_msg.max_y else: rospy.logerr("Topic should be jsk_recognition_msgs/PlotData or jsk_recognition_msgs/PlotDataArray") for d in data: self.plot_one(d, axes) xs = add_list([d.xs for d in data]) ys = add_list([d.ys for d in data]) if min_x is None: min_x = min(xs) if max_x is None: max_x = max(xs) if min_y is None: min_y = min(ys) if max_y is None: max_y = max(ys) axes.set_xlim(min_x, max_x) axes.set_ylim(min_y, max_y) # line fitting if not no_legend and not self.no_legend: axes.legend(prop={'size': legend_size}) axes.grid() if self.xtitle: axes.set_xlabel(self.xtitle) if self.ytitle: axes.set_ylabel(self.ytitle) self.data_plot._canvas.draw() buffer = StringIO() self.data_plot._canvas.figure.savefig(buffer, format="png") buffer.seek(0) img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8) img = cv2.imdecode(img_array, cv2.CV_LOAD_IMAGE_COLOR) self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class CommandWidget(QWidget): def __init__(self, node): super(CommandWidget, self).__init__() self.setObjectName('CommandWidget') self.node = node self.REDRAW_INTERVAL = 30 self.PUBLISH_INTERVAL = 100 self.CMD_VEL_X_FACTOR = 1000.0 self.CMD_VEL_YAW_FACTOR = -10.0 pkg_name = 'oroca_rqt_command' ui_filename = 'rqt_command.ui' topic_name = 'cmd_vel' service_name = 'led_control' _, package_path = get_resource('packages', pkg_name) ui_file = os.path.join(package_path, 'share', pkg_name, 'resource', ui_filename) loadUi(ui_file, self) self.pub_velocity = Twist() self.pub_velocity.linear.x = 0.0 self.pub_velocity.angular.z = 0.0 self.sub_velocity = Twist() self.sub_velocity.linear.x = 0.0 self.sub_velocity.angular.z = 0.0 self.slider_x.setValue(0) self.lcd_number_x.display(0.0) self.lcd_number_yaw.display(0.0) qos = QoSProfile(depth=10) self.publisher = self.node.create_publisher(Twist, topic_name, qos) self.subscriber = self.node.create_subscription( Twist, topic_name, self.get_velocity, qos) self.service_server = self.node.create_service(SetBool, service_name, self.set_led_status) self.service_client = self.node.create_client(SetBool, service_name) self.publish_timer = QTimer(self) self.publish_timer.timeout.connect(self.send_velocity) self.publish_timer.start(self.PUBLISH_INTERVAL) self.update_timer = QTimer(self) self.update_timer.timeout.connect(self.update_indicators) self.update_timer.start(self.REDRAW_INTERVAL) self.push_button_w.pressed.connect(self.increase_linear_x) self.push_button_x.pressed.connect(self.decrease_linear_x) self.push_button_a.pressed.connect(self.increase_angular_z) self.push_button_d.pressed.connect(self.decrease_angular_z) self.push_button_s.pressed.connect(self.set_stop) self.push_button_w.setShortcut('w') self.push_button_x.setShortcut('x') self.push_button_a.setShortcut('a') self.push_button_d.setShortcut('d') self.push_button_s.setShortcut('s') self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self) self.shortcut_space.setContext(Qt.ApplicationShortcut) self.shortcut_space.activated.connect(self.push_button_s.pressed) self.radio_button_led_on.clicked.connect(self.call_led_service) self.radio_button_led_off.clicked.connect(self.call_led_service) self.radio_button_led_on.setShortcut('o') self.radio_button_led_off.setShortcut('f') def get_velocity(self, msg): self.sub_velocity = msg def set_led_status(self, request, response): if request.data: self.push_button_led_status.setText('ON') self.push_button_led_status.setStyleSheet( 'color: rgb(255, 170, 0);') response.success = True response.message = 'LED ON' elif not request.data: self.push_button_led_status.setText('OFF') self.push_button_led_status.setStyleSheet('') response.success = True response.message = 'LED OFF' else: response.success = False return response def increase_linear_x(self): self.pub_velocity.linear.x += 0.1 def decrease_linear_x(self): self.pub_velocity.linear.x -= 0.1 def increase_angular_z(self): self.pub_velocity.angular.z += 0.1 def decrease_angular_z(self): self.pub_velocity.angular.z -= 0.1 def set_stop(self): self.pub_velocity.linear.x = 0.0 self.pub_velocity.angular.z = 0.0 def call_led_service(self): request = SetBool.Request() if self.radio_button_led_on.isChecked(): request.data = True elif self.radio_button_led_off.isChecked(): request.data = False wait_count = 1 while not self.service_client.wait_for_service(timeout_sec=0.5): if wait_count > 5: return self.node.get_logger().error( 'Service not available #{0}'.format(wait_count)) wait_count += 1 future = self.service_client.call_async(request) while rclpy.ok(): if future.done(): if future.result() is not None: response = future.result() self.node.get_logger().info( 'Result of service call: {0}'.format(response.message)) else: self.node.get_logger().error('Error calling service') break def send_velocity(self): twist = Twist() twist.linear.x = self.pub_velocity.linear.x twist.linear.y = 0.0 twist.linear.z = 0.0 twist.angular.x = 0.0 twist.angular.y = 0.0 twist.angular.z = self.pub_velocity.angular.z self.publisher.publish(twist) def update_indicators(self): self.slider_x.setValue(self.sub_velocity.linear.x * self.CMD_VEL_X_FACTOR) self.dial_yaw.setValue(self.sub_velocity.angular.z * self.CMD_VEL_YAW_FACTOR) self.lcd_number_x.display(self.sub_velocity.linear.x) self.lcd_number_yaw.display(self.sub_velocity.angular.z) def shutdown_widget(self): self.update_timer.stop() self.publish_timer.stop() self.node.destroy_client(self.service_client) self.node.destroy_service(self.service_server) self.node.destroy_subscription(self.subscriber) self.node.destroy_publisher(self.publisher)
class TopicDataWidget(QWidget): def __init__(self, widget): super(TopicDataWidget, self).__init__() rospkgPack = rospkg.RosPack() uiFile = os.path.join(rospkgPack.get_path('my_topic_viewer'), 'resource', 'TopicViewer.ui') loadUi(uiFile, self) self._updateTimer = QTimer(self) self._updateTimer.timeout.connect(self.timeoutCallback) self._widget = widget self._topicName = TOPIC_NAME self._subscriber = None self.subscribeTopic(self._topicName) def start(self): self._updateTimer.start(1000) def stop(self): self._updateTimer.stop() def timeoutCallback(self): pass # print 'time out' # rqt override def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('topic_name', self._topicName) def restore_settings(self, plugin_settings, instance_settings): topicName = instance_settings.value('topic_name') try: self._topicName = eval(topicName) except Exception: topicName = None def shutdown_plugin(self): self.unregisterTopic() self.stop() # subscribe topic def subscribeTopic(self, topicName): # msgClass, self._topicName, _ = get_topic_class(topicName) self._subscriber = rospy.Subscriber(TOPIC_NAME, MSG_CLASS, self.messageCallback) def messageCallback(self, msg): self.setDisplayedData(msg.data, TOPIC_NAME) def unregisterTopic(self): if self._subscriber: self._subscriber.unregister() def setDisplayedData(self, data, name): if isinstance(data, dict): self.displayData(data) else: dictData = {} if name is None: dictData['unknown'] = data elif isinstance(data, list): dictData = dict(zip(name, data)) else: dictData[name] = data self.displayData(dictData) def displayData(self, dictData): for key in dictData.keys(): self.topicLabel.setText(key+': ') self.topicDataLine.setText(str(dictData[key]))
class BagTimeline(QGraphicsScene): """ BagTimeline contains bag files, all information required to display the bag data visualization on the screen Also handles events """ status_bar_changed_signal = Signal() selected_region_changed = Signal(rospy.Time, rospy.Time) def __init__(self, context, publish_clock): """ :param context: plugin context hook to enable adding rqt_bag plugin widgets as ROS_GUI snapin panes, ''PluginContext'' """ super(BagTimeline, self).__init__() self._bags = [] self._bag_lock = threading.RLock() self.background_task = None # Display string self.background_task_cancel = False # Playing / Recording self._playhead_lock = threading.RLock() self._max_play_speed = 1024.0 # fastest X play speed self._min_play_speed = 1.0 / 1024.0 # slowest X play speed self._play_speed = 0.0 self._play_all = False self._playhead_positions_cvs = {} self._playhead_positions = {} # topic -> (bag, position) self._message_loaders = {} self._messages_cvs = {} self._messages = {} # topic -> (bag, msg_data) self._message_listener_threads = { } # listener -> MessageListenerThread self._player = False self._publish_clock = publish_clock self.last_frame = None self.last_playhead = None self.desired_playhead = None self.wrap = True # should the playhead wrap when it reaches the end? self.stick_to_end = False # should the playhead stick to the end? self._play_timer = QTimer() self._play_timer.timeout.connect(self.on_idle) self._play_timer.setInterval(3) # Plugin popup management self._context = context self.popups = {} self._views = [] self._listeners = {} # Initialize scene # the timeline renderer fixes use of black pens and fills, so ensure we fix white here for contrast. # otherwise a dark qt theme will default it to black and the frame render pen will be unreadable self.setBackgroundBrush(Qt.white) self._timeline_frame = TimelineFrame(self) self._timeline_frame.setPos(0, 0) self.addItem(self._timeline_frame) self.background_progress = 0 self.__closed = False def get_context(self): """ :returns: the ROS_GUI context, 'PluginContext' """ return self._context def handle_close(self): """ Cleans up the timeline, bag and any threads """ if self.__closed: return else: self.__closed = True self._play_timer.stop() for topic in self._get_topics(): self.stop_publishing(topic) self._message_loaders[topic].stop() if self._player: self._player.stop() if self.background_task is not None: self.background_task_cancel = True self._timeline_frame.handle_close() for bag in self._bags: bag.close() for frame in self._views: if frame.parent(): self._context.remove_widget(frame) # Bag Management and access def add_bag(self, bag): """ creates an indexing thread for each new topic in the bag fixes the boarders and notifies the indexing thread to index the new items bags :param bag: ros bag file, ''rosbag.bag'' """ self._bags = [ bag ] # actually, only *one* bag can be used at a time (contrary to original rqt_bag) bag_topics = bag_helper.get_topics(bag) new_topics = set(bag_topics) - set(self._timeline_frame.topics) for topic in new_topics: qWarning("Adding topic %s" % topic) self._playhead_positions_cvs[topic] = threading.Condition() self._messages_cvs[topic] = threading.Condition() self._message_loaders[topic] = MessageLoaderThread(self, topic) self._timeline_frame._start_stamp = self._get_start_stamp() self._timeline_frame._end_stamp = self._get_end_stamp() self._timeline_frame.topics = self._get_topics() self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype( ) self._timeline_frame.set_renderers_active( True) # enable the thumbnails for image streams # If this is the first bag, reset the timeline if self._timeline_frame._stamp_left is None: self._timeline_frame.reset_timeline() # Invalidate entire index cache for all topics in this bag with self._timeline_frame.index_cache_cv: for topic in bag_topics: self._timeline_frame.invalidated_caches.add(topic) if topic in self._timeline_frame.index_cache: del self._timeline_frame.index_cache[topic] self._timeline_frame.index_cache_cv.notify() def file_size(self): with self._bag_lock: return sum(b.size for b in self._bags) #TODO Rethink API and if these need to be visible def _get_start_stamp(self): """ :return: first stamp in the bags, ''rospy.Time'' """ with self._bag_lock: start_stamp = None for bag in self._bags: bag_start_stamp = bag_helper.get_start_stamp(bag) if bag_start_stamp is not None and ( start_stamp is None or bag_start_stamp < start_stamp): start_stamp = bag_start_stamp return start_stamp def _get_end_stamp(self): """ :return: last stamp in the bags, ''rospy.Time'' """ with self._bag_lock: end_stamp = None for bag in self._bags: bag_end_stamp = bag_helper.get_end_stamp(bag) if bag_end_stamp is not None and (end_stamp is None or bag_end_stamp > end_stamp): end_stamp = bag_end_stamp return end_stamp def _get_topics(self): """ :return: sorted list of topic names, ''list(str)'' """ with self._bag_lock: topics = set() for bag in self._bags: for topic in bag_helper.get_topics(bag): topics.add(topic) return sorted(topics) def _get_topics_by_datatype(self): """ :return: dict of list of topics for each datatype, ''dict(datatype:list(topic))'' """ with self._bag_lock: topics_by_datatype = {} for bag in self._bags: for datatype, topics in bag_helper.get_topics_by_datatype( bag).items(): topics_by_datatype.setdefault(datatype, []).extend(topics) return topics_by_datatype def get_datatype(self, topic): """ :return: datatype associated with a topic, ''str'' :raises: if there are multiple datatypes assigned to a single topic, ''Exception'' """ with self._bag_lock: datatype = None for bag in self._bags: bag_datatype = bag_helper.get_datatype(bag, topic) if datatype and bag_datatype and (bag_datatype != datatype): raise Exception( 'topic %s has multiple datatypes: %s and %s' % (topic, datatype, bag_datatype)) if bag_datatype: datatype = bag_datatype return datatype def get_entries(self, topics, start_stamp, end_stamp): """ generator function for bag entries :param topics: list of topics to query, ''list(str)'' :param start_stamp: stamp to start at, ''rospy.Time'' :param end_stamp: stamp to end at, ''rospy,Time'' :returns: entries the bag file, ''msg'' """ with self._bag_lock: from rosbag import bag # for _mergesort bag_entries = [] for b in self._bags: bag_start_time = bag_helper.get_start_stamp(b) if bag_start_time is not None and bag_start_time > end_stamp: continue bag_end_time = bag_helper.get_end_stamp(b) if bag_end_time is not None and bag_end_time < start_stamp: continue connections = list(b._get_connections(topics)) bag_entries.append( b._get_entries(connections, start_stamp, end_stamp)) for entry, _ in bag._mergesort(bag_entries, key=lambda entry: entry.time): yield entry def get_entries_with_bags(self, topic, start_stamp, end_stamp): """ generator function for bag entries :param topics: list of topics to query, ''list(str)'' :param start_stamp: stamp to start at, ''rospy.Time'' :param end_stamp: stamp to end at, ''rospy,Time'' :returns: tuple of (bag, entry) for the entries in the bag file, ''(rosbag.bag, msg)'' """ with self._bag_lock: from rosbag import bag # for _mergesort bag_entries = [] bag_by_iter = {} for b in self._bags: bag_start_time = bag_helper.get_start_stamp(b) if bag_start_time is not None and bag_start_time > end_stamp: continue bag_end_time = bag_helper.get_end_stamp(b) if bag_end_time is not None and bag_end_time < start_stamp: continue connections = list(b._get_connections(topic)) it = iter(b._get_entries(connections, start_stamp, end_stamp)) bag_by_iter[it] = b bag_entries.append(it) for entry, it in bag._mergesort(bag_entries, key=lambda entry: entry.time): yield bag_by_iter[it], entry def get_entry(self, t, topic): """ Access a bag entry :param t: time, ''rospy.Time'' :param topic: the topic to be accessed, ''str'' :return: tuple of (bag, entry) corisponding to time t and topic, ''(rosbag.bag, msg)'' """ with self._bag_lock: entry_bag, entry = None, None for bag in self._bags: bag_entry = bag._get_entry(t, bag._get_connections(topic)) if bag_entry and (not entry or bag_entry.time > entry.time): entry_bag, entry = bag, bag_entry return entry_bag, entry def get_entry_before(self, t): """ Access a bag entry :param t: time, ''rospy.Time'' :return: tuple of (bag, entry) corresponding to time t, ''(rosbag.bag, msg)'' """ with self._bag_lock: entry_bag, entry = None, None for bag in self._bags: bag_entry = bag._get_entry(t - rospy.Duration(0, 1), bag._get_connections()) if bag_entry and (not entry or bag_entry.time < entry.time): entry_bag, entry = bag, bag_entry return entry_bag, entry def get_entry_after(self, t): """ Access a bag entry :param t: time, ''rospy.Time'' :return: tuple of (bag, entry) corisponding to time t, ''(rosbag.bag, msg)'' """ with self._bag_lock: entry_bag, entry = None, None for bag in self._bags: bag_entry = bag._get_entry_after(t, bag._get_connections()) if bag_entry and (not entry or bag_entry.time < entry.time): entry_bag, entry = bag, bag_entry return entry_bag, entry def get_next_message_time(self): """ :return: time of the next message after the current playhead position,''rospy.Time'' """ if self._timeline_frame.playhead is None: return None _, entry = self.get_entry_after(self._timeline_frame.playhead) if entry is None: return self._timeline_frame._start_stamp return entry.time def get_previous_message_time(self): """ :return: time of the next message before the current playhead position,''rospy.Time'' """ if self._timeline_frame.playhead is None: return None _, entry = self.get_entry_before(self._timeline_frame.playhead) if entry is None: return self._timeline_frame._end_stamp return entry.time def resume(self): if (self._player): self._player.resume() ### Copy messages to... def start_background_task(self, background_task): """ Verify that a background task is not currently running before starting a new one :param background_task: name of the background task, ''str'' """ if self.background_task is not None: QMessageBox( QMessageBox.Warning, 'Exclamation', 'Background operation already running:\n\n%s' % self.background_task, QMessageBox.Ok).exec_() return False self.background_task = background_task self.background_task_cancel = False return True def stop_background_task(self): self.background_task = None def copy_region_to_bag(self, filename): if len(self._bags) > 0: self._export_region(filename, self._timeline_frame.topics, self._timeline_frame.play_region[0], self._timeline_frame.play_region[1]) def _export_region(self, path, topics, start_stamp, end_stamp): """ Starts a thread to save the current selection to a new bag file :param path: filesystem path to write to, ''str'' :param topics: topics to write to the file, ''list(str)'' :param start_stamp: start of area to save, ''rospy.Time'' :param end_stamp: end of area to save, ''rospy.Time'' """ if not self.start_background_task('Copying messages to "%s"' % path): return # TODO implement a status bar area with information on the current save status bag_entries = list( self.get_entries_with_bags(topics, start_stamp, end_stamp)) if self.background_task_cancel: return # Get the total number of messages to copy total_messages = len(bag_entries) # If no messages, prompt the user and return if total_messages == 0: QMessageBox(QMessageBox.Warning, 'rqt_bag', 'No messages found', QMessageBox.Ok).exec_() self.stop_background_task() return # Open the path for writing try: export_bag = rosbag.Bag(path, 'w') except Exception: QMessageBox(QMessageBox.Warning, 'rqt_bag', 'Error opening bag file [%s] for writing' % path, QMessageBox.Ok).exec_() self.stop_background_task() return # Run copying in a background thread self._export_thread = threading.Thread(target=self._run_export_region, args=(export_bag, topics, start_stamp, end_stamp, bag_entries)) self._export_thread.start() def _run_export_region(self, export_bag, topics, start_stamp, end_stamp, bag_entries): """ Threaded function that saves the current selection to a new bag file :param export_bag: bagfile to write to, ''rosbag.bag'' :param topics: topics to write to the file, ''list(str)'' :param start_stamp: start of area to save, ''rospy.Time'' :param end_stamp: end of area to save, ''rospy.Time'' """ total_messages = len(bag_entries) update_step = max(1, total_messages / 100) message_num = 1 progress = 0 # Write out the messages for bag, entry in bag_entries: if self.background_task_cancel: break try: topic, msg, t = self.read_message(bag, entry.position) export_bag.write(topic, msg, t) except Exception as ex: qWarning('Error exporting message at position %s: %s' % (str(entry.position), str(ex))) export_bag.close() self.stop_background_task() return if message_num % update_step == 0 or message_num == total_messages: new_progress = int(100.0 * (float(message_num) / total_messages)) if new_progress != progress: progress = new_progress if not self.background_task_cancel: self.background_progress = progress self.status_bar_changed_signal.emit() message_num += 1 # Close the bag try: self.background_progress = 0 self.status_bar_changed_signal.emit() export_bag.close() except Exception as ex: QMessageBox( QMessageBox.Warning, 'rqt_bag', 'Error closing bag file [%s]: %s' % (export_bag.filename, str(ex)), QMessageBox.Ok).exec_() self.stop_background_task() def read_message(self, bag, position): with self._bag_lock: return bag._read_message(position) ### Mouse events def on_mouse_down(self, event): if event.buttons() == Qt.LeftButton: self._timeline_frame.on_left_down(event) elif event.buttons() == Qt.MidButton: self._timeline_frame.on_middle_down(event) elif event.buttons() == Qt.RightButton: topic = self._timeline_frame.map_y_to_topic( self.views()[0].mapToScene(event.pos()).y()) TimelinePopupMenu(self, event, topic) def on_mouse_up(self, event): self._timeline_frame.on_mouse_up(event) def on_mouse_move(self, event): self._timeline_frame.on_mouse_move(event) def on_mousewheel(self, event): self._timeline_frame.on_mousewheel(event) # Zooming def zoom_in(self): self._timeline_frame.zoom_in() def zoom_out(self): self._timeline_frame.zoom_out() def reset_zoom(self): self._timeline_frame.reset_zoom() def translate_timeline_left(self): self._timeline_frame.translate_timeline_left() def translate_timeline_right(self): self._timeline_frame.translate_timeline_right() ### Publishing def is_publishing(self, topic): return self._player and self._player.is_publishing(topic) def start_publishing(self, topic): if not self._player and not self._create_player(): return False self._player.start_publishing(topic) return True def stop_publishing(self, topic): if not self._player: return False self._player.stop_publishing(topic) return True def _create_player(self): if not self._player: try: self._player = Player(self) if self._publish_clock: self._player.start_clock_publishing() except Exception as ex: qWarning('Error starting player; aborting publish: %s' % str(ex)) return False return True def set_publishing_state(self, start_publishing): if start_publishing: for topic in self._timeline_frame.topics: if not self.start_publishing(topic): break else: for topic in self._timeline_frame.topics: self.stop_publishing(topic) # property: play_all def _get_play_all(self): return self._play_all def _set_play_all(self, play_all): if play_all == self._play_all: return self._play_all = not self._play_all self.last_frame = None self.last_playhead = None self.desired_playhead = None play_all = property(_get_play_all, _set_play_all) def toggle_play_all(self): self.play_all = not self.play_all ### Playing def on_idle(self): self._step_playhead() def _step_playhead(self): """ moves the playhead to the next position based on the desired position """ # Reset when the playing mode switchs if self._timeline_frame.playhead != self.last_playhead: self.last_frame = None self.last_playhead = None self.desired_playhead = None if self._play_all: self.step_next_message() else: self.step_fixed() def step_fixed(self): """ Moves the playhead a fixed distance into the future based on the current play speed """ if self.play_speed == 0.0 or not self._timeline_frame.playhead: self.last_frame = None self.last_playhead = None return now = rospy.Time.from_sec(time.time()) if self.last_frame: # Get new playhead if self.stick_to_end: new_playhead = self.end_stamp else: new_playhead = self._timeline_frame.playhead + rospy.Duration.from_sec( (now - self.last_frame).to_sec() * self.play_speed) start_stamp, end_stamp = self._timeline_frame.play_region if new_playhead > end_stamp: if self.wrap: if self.play_speed > 0.0: new_playhead = start_stamp else: new_playhead = end_stamp else: new_playhead = end_stamp if self.play_speed > 0.0: self.stick_to_end = True elif new_playhead < start_stamp: if self.wrap: if self.play_speed < 0.0: new_playhead = end_stamp else: new_playhead = start_stamp else: new_playhead = start_stamp # Update the playhead self._timeline_frame.playhead = new_playhead self.last_frame = now self.last_playhead = self._timeline_frame.playhead def step_next_message(self): """ Move the playhead to the next message """ if self.play_speed <= 0.0 or not self._timeline_frame.playhead: self.last_frame = None self.last_playhead = None return if self.last_frame: if not self.desired_playhead: self.desired_playhead = self._timeline_frame.playhead else: delta = rospy.Time.from_sec(time.time()) - self.last_frame if delta > rospy.Duration.from_sec(0.1): delta = rospy.Duration.from_sec(0.1) self.desired_playhead += delta # Get the occurrence of the next message next_message_time = self.get_next_message_time() if next_message_time < self.desired_playhead: self._timeline_frame.playhead = next_message_time else: self._timeline_frame.playhead = self.desired_playhead self.last_frame = rospy.Time.from_sec(time.time()) self.last_playhead = self._timeline_frame.playhead ### Views / listeners def add_view(self, topic, frame): self._views.append(frame) def has_listeners(self, topic): return topic in self._listeners def add_listener(self, topic, listener): self._listeners.setdefault(topic, []).append(listener) self._message_listener_threads[(topic, listener)] = MessageListenerThread( self, topic, listener) # Notify the message listeners self._message_loaders[topic].reset() with self._playhead_positions_cvs[topic]: self._playhead_positions_cvs[topic].notify_all() self.update() def remove_listener(self, topic, listener): topic_listeners = self._listeners.get(topic) if topic_listeners is not None and listener in topic_listeners: topic_listeners.remove(listener) if len(topic_listeners) == 0: del self._listeners[topic] # Stop the message listener thread if (topic, listener) in self._message_listener_threads: self._message_listener_threads[(topic, listener)].stop() del self._message_listener_threads[(topic, listener)] self.update() ### Playhead # property: play_speed def _get_play_speed(self): if self._timeline_frame._paused: return 0.0 return self._play_speed def _set_play_speed(self, play_speed): if play_speed == self._play_speed: return if play_speed > 0.0: self._play_speed = min(self._max_play_speed, max(self._min_play_speed, play_speed)) elif play_speed < 0.0: self._play_speed = max(-self._max_play_speed, min(-self._min_play_speed, play_speed)) else: self._play_speed = play_speed if self._play_speed < 1.0: self.stick_to_end = False self.update() play_speed = property(_get_play_speed, _set_play_speed) def toggle_play(self): if self._play_speed != 0.0: self.play_speed = 0.0 else: self.play_speed = 1.0 def navigate_play(self): self.play_speed = 1.0 self.last_frame = rospy.Time.from_sec(time.time()) self.last_playhead = self._timeline_frame.playhead self._play_timer.start() def navigate_stop(self): self.play_speed = 0.0 self._play_timer.stop() def navigate_previous(self): self.navigate_stop() self._timeline_frame.playhead = self.get_previous_message_time() self.last_playhead = self._timeline_frame.playhead def navigate_next(self): self.navigate_stop() self._timeline_frame.playhead = self.get_next_message_time() self.last_playhead = self._timeline_frame.playhead def navigate_rewind(self): if self._play_speed < 0.0: new_play_speed = self._play_speed * 2.0 elif self._play_speed == 0.0: new_play_speed = -1.0 else: new_play_speed = self._play_speed * 0.5 self.play_speed = new_play_speed def navigate_fastforward(self): if self._play_speed > 0.0: new_play_speed = self._play_speed * 2.0 elif self._play_speed == 0.0: new_play_speed = 2.0 else: new_play_speed = self._play_speed * 0.5 self.play_speed = new_play_speed def navigate_start(self): self._timeline_frame.playhead = self._timeline_frame.play_region[0] def navigate_end(self): self._timeline_frame.playhead = self._timeline_frame.play_region[1]
class HistogramPlotWidget(QWidget): _redraw_interval = 40 def __init__(self, topics): super(HistogramPlotWidget, self).__init__() self.setObjectName('HistogramPlotWidget') rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource', 'plot_histogram.ui') loadUi(ui_file, self) self.cv_bridge = CvBridge() self.subscribe_topic_button.setIcon(QIcon.fromTheme('add')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = MatHistogramPlot(self) self.data_plot_layout.addWidget(self.data_plot) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent self._start_time = rospy.get_time() self._rosdata = None if len(topics) != 0: self.subscribe_topic(topics) self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) self._update_plot_timer.start(self._redraw_interval) @Slot('QDropEvent*') def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.subscribe_topic(topic_name) @Slot() def on_topic_edit_returnPressed(self): if self.subscribe_topic_button.isEnabled(): self.subscribe_topic(str(self.topic_edit.text())) @Slot() def on_subscribe_topic_button_clicked(self): self.subscribe_topic(str(self.topic_edit.text())) def subscribe_topic(self, topic_name): self.topic_with_field_name = topic_name self.pub_image = rospy.Publisher(topic_name + "/histogram_image", Image) if not self._rosdata: self._rosdata = ROSData(topic_name, self._start_time) else: if self._rosdata != topic_name: self._rosdata.close() self.data_plot.clear() self._rosdata = ROSData(topic_name, self._start_time) else: rospy.logwarn("%s is already subscribed", topic_name) def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop() @Slot() def on_clear_button_clicked(self): self.data_plot.clear() @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) def update_plot(self): if not self._rosdata: return data_x, data_y = self._rosdata.next() if len(data_y) == 0: return axes = self.data_plot._canvas.axes axes.cla() if self._rosdata.sub.data_class is HistogramWithRange: xs = [y.count for y in data_y[-1].bins] pos = [y.min_value for y in data_y[-1].bins] widths = [y.max_value - y.min_value for y in data_y[-1].bins] axes.set_xlim(xmin=pos[0], xmax=pos[-1] + widths[-1]) else: xs = data_y[-1] pos = np.arange(len(xs)) widths = [1] * len(xs) axes.set_xlim(xmin=0, xmax=len(xs)) #axes.xticks(range(5)) for p, x, w in zip(pos, xs, widths): axes.bar(p, x, color='r', align='center', width=w) axes.legend([self.topic_with_field_name], prop={'size': '8'}) self.data_plot._canvas.draw() buffer = StringIO() self.data_plot._canvas.figure.savefig(buffer, format="png") buffer.seek(0) img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8) img = cv2.imdecode(img_array, cv2.CV_LOAD_IMAGE_COLOR) self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class MotionEditorWidget(QWidget): position_renamed = Signal(QListWidgetItem) def __init__(self, motion_publisher, robot_config): super(MotionEditorWidget, self).__init__() self.robot_config = robot_config self._motion_publisher = motion_publisher self._motion_data = MotionData(robot_config) self._filter_pattern = '' self._playback_marker = None self._playback_timer = None ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'motion_editor.ui') loadUi(ui_file, self) self.list_widgets = {} for group_type in robot_config.group_types(): list_widget = QListWidget() list_widget.setSortingEnabled(True) list_widget.setDragDropMode(QAbstractItemView.DragOnly) list_widget.setContextMenuPolicy(Qt.CustomContextMenu) list_widget.customContextMenuRequested.connect( lambda pos, _group_type=group_type: self. positions_list_context_menu(_group_type, pos)) list_widget.itemChanged.connect(self.on_list_item_changed) self.position_lists_layout.addWidget(list_widget) self.list_widgets[group_type] = list_widget self._timeline_widget = TimelineWidget() for track_name in self.robot_config.sorted_groups(): track_type = self.robot_config.groups[track_name].group_type track = self._timeline_widget.add_track(track_name, track_type) list_widget = self.list_widgets[track_type] palette = list_widget.palette() palette.setColor(QPalette.Base, track._colors['track']) list_widget.setPalette(palette) self.timeline_group.layout().addWidget(self._timeline_widget) for group_type in robot_config.group_types(): label = QLabel(group_type) label.setAlignment(Qt.AlignHCenter | Qt.AlignVCenter) self.group_label_layout.addWidget(label) self.update_motion_name_combo() self.stop_motion_button.pressed.connect(self.on_motion_stop_pressed) def on_list_item_changed(self, item): print 'Position name changed from', item._text, 'to', item.text() self.position_renamed.emit(item) def on_motion_stop_pressed(self): self._clear_playback_marker() self._motion_publisher.stop_motion() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('splitter', self.splitter.saveState()) instance_settings.set_value('filter_pattern', self.filter_pattern_edit.text()) instance_settings.set_value('filter_motions_checked', self.filter_motions_check.isChecked()) instance_settings.set_value('filter_positions_checked', self.filter_positions_check.isChecked()) def restore_settings(self, plugin_settings, instance_settings): if instance_settings.contains('splitter'): self.splitter.restoreState(instance_settings.value('splitter')) else: self.splitter.setSizes([300, 100]) self.filter_pattern_edit.setText( instance_settings.value('filter_pattern', '')) self.filter_motions_check.setChecked( instance_settings.value('filter_motions_checked', False) in ( 1, True, 'true')) self.filter_positions_check.setChecked( instance_settings.value('filter_positions_checked', False) in ( 1, True, 'true')) @Slot() def on_clear_motion_button_clicked(self): self._timeline_widget.scene().clear() @Slot() def on_delete_motion_button_clicked(self): motion_name = str(self.motion_name_combo.currentText()) if len(motion_name) == 0: QMessageBox.warning(self, 'Error', 'No motion selected to delete.') return if motion_name not in self._motion_data: QMessageBox.warning( self, 'Error', 'Motion "%s" not in motion list.' % motion_name) return self._motion_data.delete(motion_name) self.update_motion_name_combo() @Slot() def on_save_motion_button_clicked(self): motion_name = str(self.motion_name_combo.currentText()) if len(motion_name) == 0: QMessageBox.warning(self, 'Error', 'No name given to save this motion.') return self._motion_data.save(motion_name, self.get_motion_from_timeline()) self.update_motion_name_combo() @Slot() def on_load_motion_button_clicked(self): motion_name = str(self.motion_name_combo.currentText()) if len(motion_name) == 0: QMessageBox.warning(self, 'Error', 'No motion selected to load.') return if motion_name not in self._motion_data: QMessageBox.warning( self, 'Error', 'Motion "%s" not in motion list.' % motion_name) return self.on_clear_motion_button_clicked() motion = self._motion_data[motion_name] for group_name, poses in motion.items(): for pose in poses: data = self.robot_config.groups[group_name].adapt_to_side( pose['positions']) self._timeline_widget.scene().add_clip(group_name, pose['name'], pose['starttime'], pose['duration'], data) @Slot() def on_null_motion_button_clicked(self): self._clear_playback_marker() for group_name in self.robot_config.groups: target_position = [0] * len( self.robot_config.groups[group_name].joints) self._motion_publisher.move_to_position( group_name, target_position, self.time_factor_spin.value()) @Slot() def on_run_motion_button_clicked(self): motion_name = str(self.motion_name_combo.currentText()) if len(motion_name) == 0: QMessageBox.warning(self, 'Error', 'No motion selected to run.') return if motion_name not in self._motion_data: QMessageBox.warning( self, 'Error', 'Motion "%s" not in motion list.' % motion_name) return self._clear_playback_marker() self._motion_publisher.publish_motion(self._motion_data[motion_name], self.time_factor_spin.value()) print '[Motion Editor] Running motion:', motion_name @Slot(str) def on_filter_pattern_edit_textChanged(self, pattern): self._filter_pattern = pattern self._apply_filter_to_position_lists() self.update_motion_name_combo() def _apply_filter_to_position_lists(self): for group_type in self.robot_config.group_types(): list_widget = self.list_widgets[group_type] for row in range(list_widget.count()): item = list_widget.item(row) hidden = self.filter_positions_check.isChecked() and re.search( self._filter_pattern, item.text()) is None item.setHidden(hidden) @Slot(bool) def on_filter_positions_check_toggled(self, checked): self._apply_filter_to_position_lists() @Slot(bool) def on_filter_motions_check_toggled(self, checked): self.update_motion_name_combo() def update_motion_name_combo(self): combo = self.motion_name_combo # remember selected motion name motion_name = str(combo.currentText()) # update motion names combo.clear() motion_names = self._motion_data.keys() if self.filter_motions_check.isChecked(): motion_names = [ name for name in motion_names if re.search(self._filter_pattern, name) is not None ] combo.addItems(sorted(motion_names)) # restore previously selected motion name motion_index = combo.findText(motion_name) combo.setCurrentIndex(motion_index) def update_positions_lists(self, positions): for group_type in self.robot_config.group_types(): list_widget = self.list_widgets[group_type] list_widget.clear() for name, position in positions[group_type].items(): item = QListWidgetItem(name) item._data = position item._text = name item._type = group_type item.setFlags(item.flags() | Qt.ItemIsEditable) list_widget.addItem(item) self._apply_filter_to_position_lists() def positions_list_context_menu(self, group_type, pos): list_widget = self.list_widgets[group_type] list_item = list_widget.itemAt(pos) if list_item is None: return menu = QMenu() move_to = {} for group in self.robot_config.group_list(): if list_item._type == group.group_type: move_to[menu.addAction('move "%s"' % group.name)] = [group.name] # move_to[menu.addAction('move all')] = list(move_to.itervalues()) move_to[menu.addAction('move all')] = [ group_list[0] for group_list in list(move_to.itervalues()) ] result = menu.exec_(list_widget.mapToGlobal(pos)) if result in move_to: group_names = move_to[result] for group_name in group_names: target_positions = self.robot_config.groups[ group_name].adapt_to_side(list_item._data) self._motion_publisher.move_to_position( group_name, target_positions, self.time_factor_spin.value()) print '[Motion Editor] Moving %s to: %s' % ( group_name, zip(self.robot_config.groups[group_name].joints_sorted(), target_positions)) def get_motion_from_timeline(self): motion = {} for group_name, clips in self._timeline_widget.scene().clips().items(): motion[group_name] = [] for clip in clips: pose = { 'name': clip.text(), 'starttime': clip.starttime(), 'duration': clip.duration(), 'positions': self.robot_config.groups[group_name].adapt_to_side( clip.data()), } motion[group_name].append(pose) return motion @Slot() def on_run_timeline_button_clicked(self): print '[Motion Editor] Running timeline.' self._playback_duration = 0.0 for clips in self._timeline_widget.scene().clips().values(): if len(clips) > 0 and self._playback_duration < clips[-1].endtime( ): self._playback_duration = clips[-1].endtime() self._playback_time_factor = self.time_factor_spin.value() self._clear_playback_marker() self._playback_marker = self._timeline_widget.scene().add_marker(0.0) self._playback_timer = QTimer() self._playback_timer.setInterval(30) self._playback_timer.timeout.connect(self._playback_update) self._playback_start = rospy.get_time() self._motion_publisher.publish_motion(self.get_motion_from_timeline(), self.time_factor_spin.value()) self._playback_timer.start() def _clear_playback_marker(self): if self._playback_timer is not None: self._playback_timer.stop() if self._playback_marker is not None: self._playback_marker.remove() @Slot() def _playback_update(self): duration = (rospy.get_time() - self._playback_start) / self._playback_time_factor if duration > self._playback_duration: self._clear_playback_marker() else: self._playback_marker.set_time(duration)
class View(QWidget): def __init__(self, parent=None): super(View, self).__init__() self.unit = 'deg' self.timer = QTimer(self) self.timer.timeout.connect(lambda: self.publishRequested.emit()) pkg_path = rospkg.RosPack().get_path('rqt_tf_rot') ui_file_path = os.path.join(pkg_path, 'res', 'PluginUI.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file_path, self) self.initUI() publishRequested = Signal() def initUI(self): btnPublish = self.findChild(QAbstractButton, 'btnPublish') btnPublish.clicked.connect(lambda: self.publishRequested.emit()) self.txtChild = self.findChild(QLineEdit, 'txtChild') self.cboParent = self.findChild(QComboBox, 'cboParent') self.spnRollRad = self.findChild(QDoubleSpinBox, 'spnRollRad') self.spnPitchRad = self.findChild(QDoubleSpinBox, 'spnPitchRad') self.spnYawRad = self.findChild(QDoubleSpinBox, 'spnYawRad') spnTimeout = self.findChild(QSpinBox, 'spnTimeout') spnTimeout.valueChanged.connect( lambda timeout: self.timer.setInterval(timeout)) chkTimer = self.findChild(QCheckBox, 'chkTimer') chkTimer.stateChanged.connect(self.onChkTimerChanged) for suffix in ['Yaw', 'Pitch', 'Roll']: degSpin = self.findChild(QDoubleSpinBox, 'spn'+suffix+'Deg') radSpin = self.findChild(QDoubleSpinBox, 'spn'+suffix+'Rad') dial = self.findChild(QDial, 'dial'+suffix) # May the Python gods forgive me # The lambda default parameters are there only to simulate # what would happen if lambdas remembered what objects # they were trying to capture instead of just the names of # the variables in the closure dial.valueChanged.connect( lambda value, degSpin=degSpin, radSpin=radSpin: self.onDialChanged(value, deg_spinbox=degSpin, rad_spinbox=radSpin)) degSpin.valueChanged.connect( lambda value, dial=dial, radSpin=radSpin: self.onDegSpinChanged( value, slider=dial, rad_spinbox=radSpin)) radSpin.valueChanged.connect( lambda value, dial=dial, degSpin=degSpin: self.onRadSpinChanged(value, slider=dial, deg_spinbox=degSpin)) # --- Slots (not all strictly Qt slots) def onDialChanged(self, value, deg_spinbox=None, rad_spinbox=None): setValueWithoutSignals(deg_spinbox, float(value)) setValueWithoutSignals(rad_spinbox, convertUnit(float(value), 'deg', 'rad')) def onDegSpinChanged(self, value, slider=None, rad_spinbox=None): setValueWithoutSignals(slider, int(value)) setValueWithoutSignals(rad_spinbox, convertUnit(float(value), 'deg', 'rad')) def onRadSpinChanged(self, value, slider=None, deg_spinbox=None): setValueWithoutSignals(slider, int(value)) setValueWithoutSignals(deg_spinbox, convertUnit(float(value), 'rad', 'deg')) def onChkTimerChanged(self, state): if state == Qt.Checked: self.timer.start() else: self.timer.stop() @Slot(str) def addLinkToList(self, child_id): if self.cboParent != None: self.cboParent.addItem(child_id) # --- Accessors def childName(self): if self.txtChild != None: return self.txtChild.text() else: return '' def parentName(self): if self.cboParent != None: return self.cboParent.currentText() else: return '' def roll(self): return self.spnRollRad.value() def pitch(self): return self.spnPitchRad.value() def yaw(self): return self.spnYawRad.value()
class MonitorDashWidget(IconToolButton): """ A widget which brings up the rqt_robot_monitor. Times out after certain period of time (set as 5 sec as of Apr 2013) without receiving diagnostics msg ('/diagnostics_toplevel_state' of DiagnosticStatus type), status becomes as 'stale'. :param context: The plugin context to create the monitor in. :type context: qt_gui.plugin_context.PluginContext """ _msg_trigger = Signal() def __init__(self, context, icon_paths=[]): self._graveyard = [] ok_icon = ['bg-green.svg', 'ic-diagnostics.svg'] warn_icon = ['bg-yellow.svg', 'ic-diagnostics.svg', 'ol-warn-badge.svg'] err_icon = ['bg-red.svg', 'ic-diagnostics.svg', 'ol-err-badge.svg'] stale_icon = ['bg-grey.svg', 'ic-diagnostics.svg', 'ol-stale-badge.svg'] icons = [ok_icon, warn_icon, err_icon, stale_icon] super(MonitorDashWidget, self).__init__('MonitorWidget', icons, icon_paths=icon_paths) self.setFixedSize(self._icons[0].actualSize(QSize(50, 30))) self._monitor = None self._close_mutex = QMutex() self._show_mutex = QMutex() self._last_update = rospy.Time.now() self.context = context self.clicked.connect(self._show_monitor) self._monitor_shown = False self.setToolTip('Diagnostics') self._diagnostics_toplevel_state_sub = rospy.Subscriber( 'diagnostics_toplevel_state', DiagnosticStatus, self.toplevel_state_callback) self._top_level_state = -1 self._stall_timer = QTimer() self._stall_timer.timeout.connect(self._stalled) self._stalled() self._plugin_settings = None self._instance_settings = None self._msg_trigger.connect(self._handle_msg_trigger) def toplevel_state_callback(self, msg): self._is_stale = False self._msg_trigger.emit() if self._top_level_state != msg.level: if (msg.level >= 2): self.update_state(2) self.setToolTip("Diagnostics: Error") elif (msg.level == 1): self.update_state(1) self.setToolTip("Diagnostics: Warning") else: self.update_state(0) self.setToolTip("Diagnostics: OK") self._top_level_state = msg.level def _handle_msg_trigger(self): self._stall_timer.start(5000) def _stalled(self): self._stall_timer.stop() self._is_stale = True self.update_state(3) self._top_level_state = 3 self.setToolTip("Diagnostics: Stale\nNo message received on " "/diagnostics_agg in the last 5 seconds") def _show_monitor(self): with QMutexLocker(self._show_mutex): try: if self._monitor_shown: self.context.remove_widget(self._monitor) self._monitor_close() self._monitor_shown = False else: self._monitor = RobotMonitorWidget(self.context, '/diagnostics_agg') if self._plugin_settings: self._monitor.restore_settings(self._plugin_settings, self._instance_settings) self.context.add_widget(self._monitor) self._monitor_shown = True except Exception: if self._monitor_shown == False: raise #TODO: when closeEvents is available fix this hack # (It ensures the button will toggle correctly) self._monitor_shown = False self._show_monitor() def _monitor_close(self): if self._monitor_shown: with QMutexLocker(self._close_mutex): if self._plugin_settings: self._monitor.save_settings(self._plugin_settings, self._instance_settings) self._monitor.shutdown() self._monitor.close() self._graveyard.append(self._monitor) self._monitor = None def shutdown_widget(self): self._stall_timer.stop() if self._monitor: self._monitor.shutdown() self._diagnostics_toplevel_state_sub.unregister() def save_settings(self, plugin_settings, instance_settings): if self._monitor_shown: self._monitor.save_settings(self._plugin_settings, self._instance_settings) def restore_settings(self, plugin_settings, instance_settings): self._plugin_settings = plugin_settings self._instance_settings = instance_settings
class RuntimeMonitorWidget(QWidget): def __init__(self, topic="diagnostics"): super(RuntimeMonitorWidget, self).__init__() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_runtime_monitor'), 'resource', 'runtime_monitor_widget.ui') loadUi(ui_file, self) self.setObjectName('RuntimeMonitorWidget') self._mutex = threading.Lock() self._error_icon = QIcon.fromTheme('dialog-error') self._warning_icon = QIcon.fromTheme('dialog-warning') self._ok_icon = QIcon.fromTheme('dialog-information') self._stale_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Stale (0)']) self._stale_node.setIcon(0, self._error_icon) self.tree_widget.addTopLevelItem(self._stale_node) self._error_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Errors (0)']) self._error_node.setIcon(0, self._error_icon) self.tree_widget.addTopLevelItem(self._error_node) self._warning_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Warnings (0)']) self._warning_node.setIcon(0, self._warning_icon) self.tree_widget.addTopLevelItem(self._warning_node) self._ok_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Ok (0)']) self._ok_node.setIcon(0, self._ok_icon) self.tree_widget.addTopLevelItem(self._ok_node) self.tree_widget.itemSelectionChanged.connect(self._refresh_selection) self.keyPressEvent = self._on_key_press self._name_to_item = {} self._new_errors_callback = None self._subscriber = rospy.Subscriber(topic, DiagnosticArray, self._diagnostics_callback) self._previous_ros_time = rospy.Time.now() self._timer = QTimer() self._timer.timeout.connect(self._on_timer) self._timer.start(1000) self._msg_timer = QTimer() self._msg_timer.timeout.connect(self._update_messages) self._msg_timer.start(100) self._messages = [] self._used_items = 0 def __del__(self): self.shutdown() def shutdown(self): """ Unregisters subscriber and stops timers """ self._msg_timer.stop() self._timer.stop() if rospy.is_shutdown(): return if self._subscriber: self._subscriber.unregister() self._subscriber = None def change_diagnostic_topic(self, topic): """ Changes diagnostics topic name. Must be of type diagnostic_msgs/DiagnosticArray """ if not topic: self.reset_monitor() return if self._subscriber: self._subscriber.unregister() self._subscriber = rospy.Subscriber(str(topic), DiagnosticArray, self._diagnostics_callback) self.reset_monitor() def reset_monitor(self): """ Removes all values from monitor display, resets buffers """ self._name_to_item = {} # Reset all stale topics self._messages = [] self._clear_tree() def _clear_tree(self): for index in range(self._stale_node.childCount()): self._stale_node.removeChild(self._stale_node.child(index)) for index in range(self._error_node.childCount()): self._error_node.removeChild(self._error_node.child(index)) for index in range(self._warning_node.childCount()): self._warning_node.removeChild(self._warning_node.child(index)) for index in range(self._ok_node.childCount()): self._ok_node.removeChild(self._ok_node.child(index)) self._update_root_labels() # Diagnostics callbacks (subscriber thread) def _diagnostics_callback(self, message): with self._mutex: self._messages.append(message) # Update display of messages from main thread def _update_messages(self): with self._mutex: messages = self._messages[:] self._messages = [] had_errors = False for message in messages: for status in message.status: was_selected = False if (self._name_to_item.has_key(status.name)): item = self._name_to_item[status.name] if item.tree_node.isSelected(): was_selected = True if (item.status.level == DiagnosticStatus.ERROR and status.level != DiagnosticStatus.ERROR): had_errors = True self._update_item(item, status, was_selected) else: self._create_item(status, was_selected, True) if (status.level == DiagnosticStatus.ERROR): had_errors = True if (had_errors and self._new_errors_callback != None): self._new_errors_callback() self._update_root_labels() self.update() self._refresh_selection() def _update_item(self, item, status, was_selected): change_parent = False if (item.status.level != status.level): change_parent = True if (change_parent): if (item.status.level == DiagnosticStatus.OK): self._ok_node.removeChild(item.tree_node) elif (item.status.level == DiagnosticStatus.WARN): self._warning_node.removeChild(item.tree_node) elif (item.status.level == -1) or (item.status.level == DiagnosticStatus.STALE): self._stale_node.removeChild(item.tree_node) else: # ERROR self._error_node.removeChild(item.tree_node) if (status.level == DiagnosticStatus.OK): parent_node = self._ok_node elif (status.level == DiagnosticStatus.WARN): parent_node = self._warning_node elif (status.level == -1) or (status.level == DiagnosticStatus.STALE): parent_node = self._stale_node else: # ERROR parent_node = self._error_node item.tree_node.setText(0, status.name + ": " + status.message) item.tree_node.setData(0, Qt.UserRole, item) parent_node.addChild(item.tree_node) # expand errors automatically if (status.level > 1 or status.level == -1): parent_node.setExpanded(True) parent_node.sortChildren(0, Qt.AscendingOrder) if (was_selected): self.tree_widget.setCurrentItem(item.tree_node) else: item.tree_node.setText(0, status.name + ": " + status.message) item.status = status if (was_selected): self._fillout_info(item.tree_node) item.mark = True def _create_item(self, status, select, expand_if_error): if (status.level == DiagnosticStatus.OK): parent_node = self._ok_node elif (status.level == DiagnosticStatus.WARN): parent_node = self._warning_node elif (status.level == -1) or (status.level == DiagnosticStatus.STALE): parent_node = self._stale_node else: # ERROR parent_node = self._error_node item = TreeItem(status, QTreeWidgetItem(parent_node, [status.name + ": " + status.message])) item.tree_node.setData(0, Qt.UserRole, item) parent_node.addChild(item.tree_node) self._name_to_item[status.name] = item parent_node.sortChildren(0, Qt.AscendingOrder) if (select): item.tree_node.setSelected(True) if (expand_if_error and (status.level > 1 or status.level == -1)): parent_node.setExpanded(True) item.mark = True return item def _fillout_info(self, node): item = node.data(0, Qt.UserRole) if not item: return scroll_value = self.html_browser.verticalScrollBar().value() status = item.status s = cStringIO.StringIO() s.write("<html><body>") s.write("<b>Component</b>: %s<br>\n" % (status.name)) s.write("<b>Message</b>: %s<br>\n" % (status.message)) s.write("<b>Hardware ID</b>: %s<br><br>\n\n" % (status.hardware_id)) s.write('<table border="1" cellpadding="2" cellspacing="0">') for value in status.values: value.value = value.value.replace("\n", "<br>") s.write("<tr><td><b>%s</b></td> <td>%s</td></tr>\n" % (value.key, value.value)) s.write("</table></body></html>") self.html_browser.setHtml(s.getvalue()) if self.html_browser.verticalScrollBar().maximum() < scroll_value: scroll_value = self.html_browser.verticalScrollBar().maximum() self.html_browser.verticalScrollBar().setValue(scroll_value) def _refresh_selection(self): current_item = self.tree_widget.selectedItems() if current_item: self._fillout_info(current_item[0]) def _on_key_press(self, event): key = event.key() if key == Qt.Key_Delete: nodes = self.tree_widget.selectedItems() if (nodes != [] and nodes[0] not in (self._ok_node, self._warning_node, self._stale_node, self._error_node)): item = nodes[0].data(0, Qt.UserRole) if (item.status.level == 0): self._ok_node.removeChild(item.tree_node) elif (item.status.level == 1): self._warning_node.removeChild(item.tree_node) elif (item.status.level == -1) or (item.status.level == DiagnosticStatus.STALE): self._stale_node.removeChild(item.tree_node) else: self._error_node.removeChild(item.tree_node) del self._name_to_item[item.status.name] self._update_root_labels() self.update() event.accept() else: event.ignore() def _on_timer(self): if self._previous_ros_time + rospy.Duration(5) > rospy.Time.now(): return self._previous_ros_time = rospy.Time.now() for name, item in self._name_to_item.iteritems(): node = item.tree_node if (item != None): if (not item.mark): was_selected = False selected = self.tree_widget.selectedItems() if selected != [] and selected[0] == node: was_selected = True new_status = copy.deepcopy(item.status) new_status.level = -1 # mark stale self._update_item(item, new_status, was_selected) item.mark = False self._update_root_labels() self.update() def set_new_errors_callback(self, callback): self._new_errors_callback = callback def _update_root_labels(self): self._stale_node.setText(0, "Stale (%s)" % (self._stale_node.childCount())) self._error_node.setText(0, "Errors (%s)" % (self._error_node.childCount())) self._warning_node.setText(0, "Warnings (%s)" % (self._warning_node.childCount())) self._ok_node.setText(0, "Ok (%s)" % (self._ok_node.childCount()))
class EnhancedLineEdit(QLineEdit): stop_signal = Signal() ''' stop button was pressed ''' refresh_signal = Signal(str) ''' sends a refresh signal with current text. This signal is emmited if text was changed or button was pressed ''' def __init__(self, parent=None): QLineEdit.__init__(self, parent=None) self.process_active = False # create a reload button with icon self.button_reload = button_reload = QToolButton(self) icon = QIcon.fromTheme("view-refresh", QIcon(":/icons/oxygen_view_refresh.png")) button_reload.setIcon(icon) button_reload.setCursor(Qt.ArrowCursor) button_reload.setStyleSheet( "QToolButton { border: none; padding: 0px; }") # create a stop button with icon self.button_stop = button_stop = QToolButton(self) icon = QIcon.fromTheme("process-stop", QIcon(":/icons/oxygen_view_refresh.png")) button_stop.setIcon(icon) button_stop.setCursor(Qt.ArrowCursor) button_stop.setStyleSheet( "QToolButton { border: none; padding: 0px; }") button_stop.hide() # signals, clear lineEdit if btn pressed; change btn visibility on input button_reload.clicked.connect(self._emit_refresh_text) self.textChanged[str].connect(self.update_close_button) button_stop.clicked.connect(self._process_stop) frameWidth = self.style().pixelMetric(QStyle.PM_DefaultFrameWidth) self.setStyleSheet("QLineEdit { padding-right: %dpx; } " % (button_reload.sizeHint().width() + frameWidth + 1)) msz = self.minimumSizeHint() self.setMinimumSize( max(msz.width(), button_reload.sizeHint().height() + frameWidth * 2 + 2), max(msz.height(), button_reload.sizeHint().height() + frameWidth * 2 + 2)) self._timer = QTimer(self) self._timer.setSingleShot(True) self._timer.setInterval(500) self._timer.timeout.connect(self._emit_refresh_text) def resizeEvent(self, event): sz = self.button_reload.sizeHint() frameWidth = self.style().pixelMetric(QStyle.PM_DefaultFrameWidth) self.button_reload.move(self.rect().right() - frameWidth - sz.width(), (self.rect().bottom() + 1 - sz.height()) / 2) self.button_stop.move(self.rect().right() - frameWidth - sz.width(), (self.rect().bottom() + 1 - sz.height()) / 2) def update_close_button(self, text): self._timer.stop() self._timer.start() # self.button_reload.setVisible(True if text else False) def set_process_active(self, state): if self.process_active != state: self.process_active = state self.button_reload.setVisible(not state) self.button_stop.setVisible(state) def _process_stop(self): self.stop_signal.emit() def _emit_refresh_text(self): self.set_process_active(True) self.refresh_signal.emit(self.text()) def keyPressEvent(self, event): ''' Enable the ESC handling ''' if event.key() == Qt.Key_Escape and self.text(): self.setText('') self._timer.stop() elif event.key() in [Qt.Key_Return, Qt.Key_Enter]: self._timer.stop() self._emit_refresh_text() else: event.accept() QLineEdit.keyPressEvent(self, event)
class TopicWidget(QWidget): """ main class inherits from the ui window class. You can specify the topics that the topic pane. TopicWidget.start must be called in order to update topic pane. """ SELECT_BY_NAME = 0 SELECT_BY_MSGTYPE = 1 _column_names = ['topic', 'type', 'bandwidth', 'rate', 'value'] def __init__(self, plugin=None, selected_topics=None, select_topic_type=SELECT_BY_NAME): """ @type selected_topics: list of tuples. @param selected_topics: [($NAME_TOPIC$, $TYPE_TOPIC$), ...] @type select_topic_type: int @param select_topic_type: Can specify either the name of topics or by the type of topic, to filter the topics to show. If 'select_topic_type' argument is None, this arg shouldn't be meaningful. """ super(TopicWidget, self).__init__() self._select_topic_type = select_topic_type rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_topic'), 'resource', 'TopicWidget.ui') loadUi(ui_file, self) self._plugin = plugin self.topics_tree_widget.sortByColumn(0, Qt.AscendingOrder) header = self.topics_tree_widget.header() header.setResizeMode(QHeaderView.ResizeToContents) header.customContextMenuRequested.connect(self.handle_header_view_customContextMenuRequested) header.setContextMenuPolicy(Qt.CustomContextMenu) # Whether to get all topics or only the topics that are set in advance. # Can be also set by the setter method "set_selected_topics". self._selected_topics = selected_topics self._current_topic_list = [] self._topics = {} self._tree_items = {} self._column_index = {} for column_name in self._column_names: self._column_index[column_name] = len(self._column_index) # self.refresh_topics() # init and start update timer self._timer_refresh_topics = QTimer(self) self._timer_refresh_topics.timeout.connect(self.refresh_topics) def set_topic_specifier(self, specifier): self._select_topic_type = specifier def start(self): """ This method needs to be called to start updating topic pane. """ self._timer_refresh_topics.start(1000) @Slot() def refresh_topics(self): """ refresh tree view items """ if self._selected_topics is None: topic_list = rospy.get_published_topics() if topic_list is None: rospy.logerr('Not even a single published topic found. Check network configuration') return else: # Topics to show are specified. topic_list = self._selected_topics topic_specifiers_server_all = None topic_specifiers_required = None rospy.logdebug('refresh_topics) self._selected_topics=%s' % (topic_list,)) if self._select_topic_type == self.SELECT_BY_NAME: topic_specifiers_server_all = [name for name, type in rospy.get_published_topics()] topic_specifiers_required = [name for name, type in topic_list] elif self._select_topic_type == self.SELECT_BY_MSGTYPE: # The topics that are required (by whoever uses this class). topic_specifiers_required = [type for name, type in topic_list] # The required topics that match with published topics. topics_match = [(name, type) for name, type in rospy.get_published_topics() if type in topic_specifiers_required] topic_list = topics_match rospy.logdebug('selected & published topic types=%s' % (topic_list,)) rospy.logdebug('server_all=%s\nrequired=%s\ntlist=%s' % (topic_specifiers_server_all, topic_specifiers_required, topic_list)) if len(topic_list) == 0: rospy.logerr('None of the following required topics are found.\n(NAME, TYPE): %s' % (self._selected_topics,)) return if self._current_topic_list != topic_list: self._current_topic_list = topic_list # start new topic dict new_topics = {} for topic_name, topic_type in topic_list: # if topic is new or has changed its type if topic_name not in self._topics or \ self._topics[topic_name]['type'] != topic_type: # create new TopicInfo topic_info = TopicInfo(topic_name, topic_type) message_instance = None if topic_info.message_class is not None: message_instance = topic_info.message_class() # add it to the dict and tree view topic_item = self._recursive_create_widget_items(self.topics_tree_widget, topic_name, topic_type, message_instance) new_topics[topic_name] = { 'item': topic_item, 'info': topic_info, 'type': topic_type, } else: # if topic has been seen before, copy it to new dict and # remove it from the old one new_topics[topic_name] = self._topics[topic_name] del self._topics[topic_name] # clean up old topics for topic_name in self._topics.keys(): self._topics[topic_name]['info'].stop_monitoring() index = self.topics_tree_widget.indexOfTopLevelItem( self._topics[topic_name]['item']) self.topics_tree_widget.takeTopLevelItem(index) del self._topics[topic_name] # switch to new topic dict self._topics = new_topics self._update_topics_data() def _update_topics_data(self): for topic in self._topics.values(): topic_info = topic['info'] if topic_info.monitoring: # update rate rate, _, _, _ = topic_info.get_hz() rate_text = '%1.2f' % rate if rate != None else 'unknown' # update bandwidth bytes_per_s, _, _, _ = topic_info.get_bw() if bytes_per_s is None: bandwidth_text = 'unknown' elif bytes_per_s < 1000: bandwidth_text = '%.2fB/s' % bytes_per_s elif bytes_per_s < 1000000: bandwidth_text = '%.2fKB/s' % (bytes_per_s / 1000.) else: bandwidth_text = '%.2fMB/s' % (bytes_per_s / 1000000.) # update values value_text = '' self.update_value(topic_info._topic_name, topic_info.last_message) else: rate_text = '' bandwidth_text = '' value_text = 'not monitored' if topic_info.error is None else topic_info.error self._tree_items[topic_info._topic_name].setText(self._column_index['rate'], rate_text) self._tree_items[topic_info._topic_name].setText(self._column_index['bandwidth'], bandwidth_text) self._tree_items[topic_info._topic_name].setText(self._column_index['value'], value_text) def update_value(self, topic_name, message): if hasattr(message, '__slots__') and hasattr(message, '_slot_types'): for slot_name in message.__slots__: self.update_value(topic_name + '/' + slot_name, getattr(message, slot_name)) elif type(message) in (list, tuple) and (len(message) > 0) and hasattr(message[0], '__slots__'): for index, slot in enumerate(message): if topic_name + '[%d]' % index in self._tree_items: self.update_value(topic_name + '[%d]' % index, slot) else: base_type_str, _ = self._extract_array_info(self._tree_items[topic_name].text(self._column_index['type'])) self._recursive_create_widget_items(self._tree_items[topic_name], topic_name + '[%d]' % index, base_type_str, slot) # remove obsolete children if len(message) < self._tree_items[topic_name].childCount(): for i in range(len(message), self._tree_items[topic_name].childCount()): item_topic_name = topic_name + '[%d]' % i self._recursive_delete_widget_items(self._tree_items[item_topic_name]) else: if topic_name in self._tree_items: self._tree_items[topic_name].setText(self._column_index['value'], repr(message)) def _extract_array_info(self, type_str): array_size = None if '[' in type_str and type_str[-1] == ']': type_str, array_size_str = type_str.split('[', 1) array_size_str = array_size_str[:-1] if len(array_size_str) > 0: array_size = int(array_size_str) else: array_size = 0 return type_str, array_size def _recursive_create_widget_items(self, parent, topic_name, type_name, message): if parent is self.topics_tree_widget: # show full topic name with preceding namespace on toplevel item topic_text = topic_name item = TreeWidgetItem(self._toggle_monitoring, topic_name, parent) else: topic_text = topic_name.split('/')[-1] if '[' in topic_text: topic_text = topic_text[topic_text.index('['):] item = QTreeWidgetItem(parent) item.setText(self._column_index['topic'], topic_text) item.setText(self._column_index['type'], type_name) item.setData(0, Qt.UserRole, topic_name) self._tree_items[topic_name] = item if hasattr(message, '__slots__') and hasattr(message, '_slot_types'): for slot_name, type_name in zip(message.__slots__, message._slot_types): self._recursive_create_widget_items(item, topic_name + '/' + slot_name, type_name, getattr(message, slot_name)) else: base_type_str, array_size = self._extract_array_info(type_name) try: base_instance = roslib.message.get_message_class(base_type_str)() except (ValueError, TypeError): base_instance = None if array_size is not None and hasattr(base_instance, '__slots__'): for index in range(array_size): self._recursive_create_widget_items(item, topic_name + '[%d]' % index, base_type_str, base_instance) return item def _toggle_monitoring(self, topic_name): item = self._tree_items[topic_name] if item.checkState(0): self._topics[topic_name]['info'].start_monitoring() else: self._topics[topic_name]['info'].stop_monitoring() def _recursive_delete_widget_items(self, item): def _recursive_remove_items_from_tree(item): for index in reversed(range(item.childCount())): _recursive_remove_items_from_tree(item.child(index)) topic_name = item.data(0, Qt.UserRole) del self._tree_items[topic_name] _recursive_remove_items_from_tree(item) item.parent().removeChild(item) @Slot('QPoint') def handle_header_view_customContextMenuRequested(self, pos): header = self.topics_tree_widget.header() # show context menu menu = QMenu(self) action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize') action = menu.exec_(header.mapToGlobal(pos)) # evaluate user action if action is action_toggle_auto_resize: if header.resizeMode(0) == QHeaderView.ResizeToContents: header.setResizeMode(QHeaderView.Interactive) else: header.setResizeMode(QHeaderView.ResizeToContents) @Slot('QPoint') def on_topics_tree_widget_customContextMenuRequested(self, pos): item = self.topics_tree_widget.itemAt(pos) if item is None: return # show context menu menu = QMenu(self) action_item_expand = menu.addAction(QIcon.fromTheme('zoom-in'), 'Expand All Children') action_item_collapse = menu.addAction(QIcon.fromTheme('zoom-out'), 'Collapse All Children') action = menu.exec_(self.topics_tree_widget.mapToGlobal(pos)) # evaluate user action if action in (action_item_expand, action_item_collapse): expanded = (action is action_item_expand) def recursive_set_expanded(item): item.setExpanded(expanded) for index in range(item.childCount()): recursive_set_expanded(item.child(index)) recursive_set_expanded(item) def shutdown_plugin(self): for topic in self._topics.values(): topic['info'].stop_monitoring() self._timer_refresh_topics.stop() def set_selected_topics(self, selected_topics): """ @param selected_topics: list of tuple. [(topic_name, topic_type)] @type selected_topics: [] """ rospy.logdebug('set_selected_topics topics={}'.format( len(selected_topics))) self._selected_topics = selected_topics
class PlotWidget(QWidget): _redraw_interval = 40 def __init__(self, arguments=None, initial_topics=None): super(PlotWidget, self).__init__() self.setObjectName('PlotWidget') ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'plot.ui') loadUi(ui_file, self) self.subscribe_topic_button.setIcon(QIcon.fromTheme('add')) self.remove_topic_button.setIcon(QIcon.fromTheme('remove')) self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) self.data_plot = None self.subscribe_topic_button.setEnabled(False) self._topic_completer = TopicCompleter(self.topic_edit) self._topic_completer.update_topics() self.topic_edit.setCompleter(self._topic_completer) self._start_time = rospy.get_time() self._rosdata = {} self._remove_topic_menu = QMenu() # init and start update timer for plot self._update_plot_timer = QTimer(self) self._update_plot_timer.timeout.connect(self.update_plot) # save command line arguments self._arguments = arguments self._initial_topics = initial_topics def switch_data_plot_widget(self, data_plot): self.enable_timer(enabled=False) self.data_plot_layout.removeWidget(self.data_plot) if self.data_plot is not None: self.data_plot.close() self.data_plot = data_plot self.data_plot_layout.addWidget(self.data_plot) # setup drag 'n drop self.data_plot.dropEvent = self.dropEvent self.data_plot.dragEnterEvent = self.dragEnterEvent if self._initial_topics: for topic_name in self._initial_topics: self.add_topic(topic_name) self._initial_topics = None else: for topic_name, rosdata in self._rosdata.items(): data_x, data_y = rosdata.next() self.data_plot.add_curve(topic_name, topic_name, data_x, data_y) self._subscribed_topics_changed() @Slot('QDragEnterEvent*') def dragEnterEvent(self, event): # get topic name if not event.mimeData().hasText(): if not hasattr(event.source(), 'selectedItems') or len( event.source().selectedItems()) == 0: qWarning( 'Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0' ) return item = event.source().selectedItems()[0] topic_name = item.data(0, Qt.UserRole) if topic_name == None: qWarning( 'Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)' ) return else: topic_name = str(event.mimeData().text()) # check for numeric field type is_numeric, is_array, message = is_slot_numeric(topic_name) if is_numeric and not is_array: event.acceptProposedAction() else: qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message)) @Slot('QDropEvent*') def dropEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) else: droped_item = event.source().selectedItems()[0] topic_name = str(droped_item.data(0, Qt.UserRole)) self.add_topic(topic_name) @Slot(str) def on_topic_edit_textChanged(self, topic_name): # on empty topic name, update topics if topic_name in ('', '/'): self._topic_completer.update_topics() is_numeric, is_array, message = is_slot_numeric(topic_name) self.subscribe_topic_button.setEnabled(is_numeric and not is_array) self.subscribe_topic_button.setToolTip(message) @Slot() def on_subscribe_topic_button_clicked(self): self.add_topic(str(self.topic_edit.text())) @Slot(bool) def on_pause_button_clicked(self, checked): self.enable_timer(not checked) @Slot() def on_clear_button_clicked(self): self.clean_up_subscribers() def update_plot(self): if self.data_plot is not None: for topic_name, rosdata in self._rosdata.items(): try: data_x, data_y = rosdata.next() self.data_plot.update_values(topic_name, data_x, data_y) except RosPlotException as e: qWarning('PlotWidget.update_plot(): error in rosplot: %s' % e) self.data_plot.redraw() def _subscribed_topics_changed(self): self._update_remove_topic_menu() if self._arguments: if self._arguments.start_paused: self.pause_button.setChecked(True) if not self.pause_button.isChecked(): # if pause button is not pressed, enable timer based on subscribed topics self.enable_timer(self._rosdata) def _update_remove_topic_menu(self): def make_remove_topic_function(x): return lambda: self.remove_topic(x) self._remove_topic_menu.clear() for topic_name in sorted(self._rosdata.keys()): action = QAction(topic_name, self._remove_topic_menu) action.triggered.connect(make_remove_topic_function(topic_name)) self._remove_topic_menu.addAction(action) self.remove_topic_button.setMenu(self._remove_topic_menu) def add_topic(self, topic_name): if topic_name in self._rosdata: qWarning('PlotWidget.add_topic(): topic already subscribed: %s' % topic_name) return self._rosdata[topic_name] = ROSData(topic_name, self._start_time) data_x, data_y = self._rosdata[topic_name].next() self.data_plot.add_curve(topic_name, topic_name, data_x, data_y) self._subscribed_topics_changed() def remove_topic(self, topic_name): self._rosdata[topic_name].close() del self._rosdata[topic_name] self.data_plot.remove_curve(topic_name) self._subscribed_topics_changed() def clean_up_subscribers(self): for topic_name, rosdata in self._rosdata.items(): rosdata.close() self.data_plot.remove_curve(topic_name) self._rosdata = {} self._subscribed_topics_changed() def enable_timer(self, enabled=True): if enabled: self._update_plot_timer.start(self._redraw_interval) else: self._update_plot_timer.stop()
class Control(Plugin): def __init__(self, context): super(Control, self).__init__(context) # Give QObjects reasonable names self.setObjectName('Control') # Create QWidget self._widget = QWidget() # Get path to UI file which should be in the "resource" folder of # this package ui_file = os.path.join(rospkg.RosPack().get_path('robosub'), 'src/rqt/rqt_control/resource', 'Control.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) self.control_timer = QTimer(self) self.control_timer.timeout.connect(self.control_missed) self.control_timer.start(1000) self.control_status_timer = QTimer(self) self.control_status_timer.timeout.connect(self.control_status_missed) self.control_status_timer.start(1000) # Give QObjects reasonable names self._widget.setObjectName('Control') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) # Add widget to the user interface context.add_widget(self._widget) self._widget.statusActive.hide() self._widget.controlActive.hide() self.con_sub = rospy.Subscriber('control', control, self.control_callback, queue_size=1) self.cs_sub = rospy.Subscriber('control_status', control_status, self.control_status_callback, queue_size=1) img_file = os.path.join(rospkg.RosPack().get_path('robosub'), 'src/rqt/resource/robosub_logo.png') self._widget.setStyleSheet(".QWidget {background-image: url(" + img_file + "); background-repeat: no-repeat;" + "background-position:bottom right}") def control_missed(self): if not self._widget.controlStale.isVisible(): self._widget.controlStale.show() self._widget.controlActive.hide() def control_status_missed(self): if not self._widget.statusStale.isVisible(): self._widget.statusStale.show() self._widget.statusActive.hide() def control_status_callback(self, m): try: self.control_status_timer.stop() except RuntimeError: pass if self._widget.statusStale.isVisible(): self._widget.statusStale.setVisible(False) self._widget.statusActive.setVisible(True) # Set the states self._widget.forwardStatusState.setText(m.forward_state) self._widget.strafeStatusState.setText(m.strafe_left_state) self._widget.diveStatusState.setText(m.dive_state) self._widget.rollStatusState.setText(m.roll_right_state) self._widget.pitchStatusState.setText(m.pitch_down_state) self._widget.yawStatusState.setText(m.yaw_left_state) self._widget.forwardGoal.setText("{:.4f}".format(m.forward_goal)) self._widget.strafeGoal.setText("{:.4f}".format(m.strafe_left_goal)) self._widget.diveGoal.setText("{:.4f}".format(m.dive_goal)) self._widget.rollGoal.setText("{:.4f}".format(m.roll_right_goal)) self._widget.pitchGoal.setText("{:.4f}".format(m.pitch_down_goal)) self._widget.yawGoal.setText("{:.4f}".format(m.yaw_left_goal)) self.control_status_timer.start(1000) def control_callback(self, m): try: self.control_timer.stop() except RuntimeError: pass if self._widget.controlStale.isVisible(): self._widget.controlStale.hide() self._widget.controlActive.show() # Set the states self._widget.forwardState.setText(state_types[m.forward_state]) self._widget.strafeState.setText(state_types[m.strafe_state]) self._widget.diveState.setText(state_types[m.dive_state]) self._widget.rollState.setText(state_types[m.roll_state]) self._widget.pitchState.setText(state_types[m.pitch_state]) self._widget.yawState.setText(state_types[m.yaw_state]) self._widget.forwardValue.setText("{:.4f}".format(m.forward)) self._widget.strafeValue.setText("{:.4f}".format(m.strafe_left)) self._widget.diveValue.setText("{:.4f}".format(m.dive)) self._widget.rollValue.setText("{:.4f}".format(m.roll_right)) self._widget.pitchValue.setText("{:.4f}".format(m.pitch_down)) self._widget.yawValue.setText("{:.4f}".format(m.yaw_left)) self.control_timer.start(1000) def shutdown_plugin(self): self.cs_sub.unregister() self.con_sub.unregister() self.control_timer.stop() self.control_status_timer.stop() def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class MonitorDashWidget(IconToolButton): """ A widget which brings up the rqt_robot_monitor. Times out after certain period of time (set as 5 sec as of Apr 2013) without receiving diagnostics msg ('/diagnostics_toplevel_state' of DiagnosticStatus type), status becomes as 'stale'. :param context: The plugin context to create the monitor in. :type context: qt_gui.plugin_context.PluginContext """ def __init__(self, context, icon_paths=[]): self._graveyard = [] ok_icon = ['bg-green.svg', 'ic-diagnostics.svg'] warn_icon = ['bg-yellow.svg', 'ic-diagnostics.svg', 'ol-warn-badge.svg'] err_icon = ['bg-red.svg', 'ic-diagnostics.svg', 'ol-err-badge.svg'] stale_icon = ['bg-grey.svg', 'ic-diagnostics.svg', 'ol-stale-badge.svg'] icons = [ok_icon, warn_icon, err_icon, stale_icon] super(MonitorDashWidget, self).__init__('MonitorWidget', icons, icon_paths=icon_paths) self.setFixedSize(self._icons[0].actualSize(QSize(50, 30))) self._monitor = None self._close_mutex = QMutex() self._show_mutex = QMutex() self._last_update = rospy.Time.now() self.context = context self.clicked.connect(self._show_monitor) self._monitor_shown = False self.setToolTip('Diagnostics') self._diagnostics_toplevel_state_sub = rospy.Subscriber( 'diagnostics_toplevel_state', DiagnosticStatus, self.toplevel_state_callback) self._top_level_state = -1 self._stall_timer = QTimer() self._stall_timer.timeout.connect(self._stalled) self._stalled() self._plugin_settings = None self._instance_settings = None def toplevel_state_callback(self, msg): self._is_stale = False self._stall_timer.start(5000) if self._top_level_state != msg.level: if (msg.level >= 2): self.update_state(2) self.setToolTip("Diagnostics: Error") elif (msg.level == 1): self.update_state(1) self.setToolTip("Diagnostics: Warning") else: self.update_state(0) self.setToolTip("Diagnostics: OK") self._top_level_state = msg.level def _stalled(self): self._stall_timer.stop() self._is_stale = True self.update_state(3) self._top_level_state = 3 self.setToolTip("Diagnostics: Stale\nNo message received on " + "dashboard_agg in the last 5 seconds") def _show_monitor(self): with QMutexLocker(self._show_mutex): try: if self._monitor_shown: self.context.remove_widget(self._monitor) self._monitor_close() self._monitor_shown = False else: self._monitor = RobotMonitorWidget(self.context, '/diagnostics_agg') if self._plugin_settings: self._monitor.restore_settings(self._plugin_settings, self._instance_settings) self.context.add_widget(self._monitor) self._monitor_shown = True except Exception: if self._monitor_shown == False: raise #TODO: when closeEvents is available fix this hack # (It ensures the button will toggle correctly) self._monitor_shown = False self._show_monitor() def _monitor_close(self): if self._monitor_shown: with QMutexLocker(self._close_mutex): if self._plugin_settings: self._monitor.save_settings(self._plugin_settings, self._instance_settings) self._monitor.shutdown() self._monitor.close() self._graveyard.append(self._monitor) self._monitor = None def shutdown_widget(self): self._stall_timer.stop() if self._monitor: self._monitor.shutdown() self._diagnostics_toplevel_state_sub.unregister() def save_settings(self, plugin_settings, instance_settings): if self._monitor_shown: self._monitor.save_settings(self._plugin_settings, self._instance_settings) def restore_settings(self, plugin_settings, instance_settings): self._plugin_settings = plugin_settings self._instance_settings = instance_settings
class RqtParamManagerPlugin(Plugin): """UIのメインクラス""" def __init__(self, context): """初期化処理""" super(RqtParamManagerPlugin, self).__init__(context) # クラス変数初期化 self._title = "rqt_param_manager" self._get_interval = 1000 self._monitor_timer = QTimer() self._conf_file_path_list = FILE_DEFAULT_PM_CONFS self._dump_yaml_file_path = "" self._config_items = [] self._ros_namespace = os.environ.get(KEY_ENV_ROS_NAMESPACE, "") self._table_input_item_map = {} self._topic_listeners = [] self._topic_data_map = {} self._prm_writer = None self._monitor_param_nms = [] self._param_values = {} self.setObjectName('RqtParamManagerPlugin') args = {} self._parse_args(sys.argv, args) if (ARG_CONF in args): self._conf_file_path_list = args[ARG_CONF] if (len(self._ros_namespace) > 0 and self._ros_namespace[:-1] != "/"): self._ros_namespace = self._ros_namespace + "/" # Create QWidget self._widget = QWidget() self.ui = Ui_rqt_param_manager_main() self.ui.setupUi(self._widget) context.add_widget(self._widget) tblMon = self.ui.tblMonitor tblMon.initUI() tblMon.invoke_topic_pub.connect(self._on_topic_publish_invoke) self._initEnv(args) result_load_conf = self._parse_conf_file(self._conf_file_path_list, self._config_items, self._topic_data_map) QTimer.singleShot(0, self._update_window_title) self.ui.btnClose.clicked.connect(self._app_close) if not result_load_conf: self.ui.btnSave.setEnabled(False) else: self.ui.btnSave.clicked.connect(self._on_exec_save) self._monitor_timer.timeout.connect(self._on_period_monitoring) tblMon.load_items(self._config_items) self._monitor_param_nms = tblMon.get_monitor_param_nms() # 定期監視処理の開始 if self._get_interval > 0: self._on_period_monitoring() rospy.loginfo("start monitor. interval =%d sec", self._get_interval) self._monitor_timer.start(self._get_interval) self._start_topic_listen(self._config_items) def _initEnv(self, args): if (ARG_DUMP in args): self._dump_yaml_file_path = args[ARG_DUMP] else: self.ui.btnSave.setVisible(False) if (ARG_WINDOW_SIZE in args): val = args[ARG_WINDOW_SIZE] result = re.match("([\d]+)[xX,-:]([\d]+)", val) if (result): win_width = int(result.group(1)) win_height = int(result.group(2)) parentWid = self._widget.parentWidget() while True: wkParentWid = parentWid.parentWidget() if (wkParentWid is None): break parentWid = wkParentWid parentWid.resize(win_width, win_height) if (ARG_COLUMN_WIDTH_LABEL in args): val = args[ARG_COLUMN_WIDTH_LABEL] try: table = self.ui.tblMonitor if (val.endswith("px")): table.colLabelWidthFixed = int(val[:-2]) elif (val.endswith("%")): table.colLabelWidthRatio = int(val[:-1]) / 100.0 else: table.colLabelWidthFixed = int(val) except Exception as e: rospy.logerr("label column width set failed. val=%s. cause=%s", val, e) def _update_window_title(self): """ウィンドウタイトルを変更する処理""" # 初期化処理内でsetWindowTitleを呼んでも変更されないので self._widget.setWindowTitle(self._title) def _app_close(self): self._monitor_timer.stop() QCoreApplication.quit() def shutdown_plugin(self): """シャットダウン処理""" for listener in self._topic_listeners: listener.doCancel() self._monitor_timer.stop() def _parse_args(self, argv, args): """引数パース処理""" for arg in sys.argv: tokens = arg.split(":=") if len(tokens) == 2: key = tokens[0] if (ARG_CONF == key): args[ARG_CONF] = [tokens[1]] else: args[key] = tokens[1] def _parse_conf_file(self, conf_file_path_list, items, topic_data_map): """PM設定ファイル解析処理""" result = False for conf_file_path in conf_file_path_list: rospy.loginfo("load conf file. path =%s", conf_file_path) try: file = open(conf_file_path, 'r') set_title = False for line in file: line = line.strip() if (len(line) == 0 or line[0] == "#"): continue item = ConfigItem() item.prefix = self._ros_namespace if (not item.parse(line, topic_data_map)): rospy.logerr("conf file wrong line. %s", line) else: if (ITEM_TYPE_TITLE == item.type and len(items) == 0 and not set_title): self._title = item.label set_title = True continue items.append(item) file.close() result = True except IOError as e: rospy.logerr("conf file load failed. %s", e) if result: break return result def _on_period_monitoring(self): """定期監視処理""" table = self.ui.tblMonitor param_values = {} for param_nm in self._monitor_param_nms: param_values[param_nm] = None try: val = rospy.get_param(param_nm) param_values[param_nm] = val except Exception as err: pass table.update_param_values(param_values) self._param_values = param_values def _start_topic_listen(self, items): listened_topics = [] for item in items: if (ITEM_TYPE_ECHO == item.type): try: listened_topics.index(item.topic) # もうすでに購読済みなので何もしない except ValueError as ve: # 未購読 listened_topics.append(item.topic) thread = RosTopicListener(item.topic) self._topic_listeners.append(thread) thread.received_topic_values.connect( self.ui.tblMonitor._on_update_topic_values) thread.start() print "thread start and sleep" sleep(0.001) except Except as err: rospy.logerr("conf file load failed. %s", e) def _on_exec_save(self): """パラメータ保存実行処理""" if (self._prm_writer is not None): QMessageBox.warning(self._widget, "警告", "パラメータ保存実行中です") return if (not os.path.isfile(self._dump_yaml_file_path)): QMessageBox.warning(self._widget, "警告", "出力対象のファイルが見つかりません") return param_config_items = [] for item in self._config_items: if (ITEM_TYPE_NUMBER == item.type or ITEM_TYPE_TEXT == item.type): param_config_items.append(item) if (len(self._dump_yaml_file_path) == 0): QMessageBox.information(self._widget, "お知らせ", "保存先が指定されていません") return if (len(param_config_items) == 0): QMessageBox.information(self._widget, "お知らせ", "保存対象がありません") return self._monitor_timer.stop() self._widget.setEnabled(False) self._prm_writer = RosParamWriter() self._prm_writer.work_finished.connect( self._on_param_writer_work_finished) self._prm_writer.finished.connect(self._prm_writer.deleteLater) self._prm_writer.param_config_items = param_config_items self._prm_writer.dump_file_path = self._dump_yaml_file_path self._prm_writer.start() def _on_param_writer_work_finished(self, result): self._prm_writer = None self._monitor_timer.start() self._widget.setEnabled(True) if (not result): QMessageBox.warning(self._widget, "警告", "パラメータが正常に保存できませんでした") else: QMessageBox.information(self._widget, "お知らせ", "パラメータを保存しました") def _on_topic_publish_invoke(self, topic_nm, val): try: confirm_msg = "トピック「{}」のパブリッシュを実行しますか?".format(topic_nm) if (not self._showdialog("確認", confirm_msg)): return # pub = rospy.Publisher( # topic_nm, # std_msgs.msg.Bool, # queue_size=1, # latch=False) # pub.publish(val) ret = subprocess.call( ["rostopic", "pub", "-1", topic_nm, "std_msgs/Bool", str(val)]) print("ret=%d" % ret) # QMessageBox.information( # self._widget, # "お知らせ", # "トピック「{}」のパブリッシュを実行しました。".format(topic_nm)) except Exception as err: print("err=%s" % err) rospy.logerr("topic publish failed. topic=%s err=%s", topic_nm, err) QMessageBox.critical(self._widget, "エラー", "トピック「{}」のパブリッシュに失敗しました。".format(topic_nm)) def _showdialog(self, title, msg): mbox = QMessageBox(self._widget) mbox.setIcon(QMessageBox.Question) mbox.setText(msg) mbox.setWindowTitle(title) mbox.setStandardButtons(QMessageBox.Ok | QMessageBox.Cancel) retval = mbox.exec_() return QMessageBox.Ok == retval