def __init__(self, ros_package):
super(GraphWidget, self).__init__()
self._current_graph = None
self._lock = Lock()
self._load_ui(ros_package)
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
factory = DmgItemFactory()
factory.set_color(QColor(50, 50, 50))
factory.set_highlighted_color(QColor(0, 150, 0))
self._dot_to_qt = DotToQtGenerator(factory)
self.initialized = False
self.setObjectName('GraphWidget')
self.graphics_view.setScene(self._scene)
self.open_button.setIcon(QIcon.fromTheme('document-open'))
self.open_button.pressed.connect(self._import)
self.export_button.setIcon(QIcon.fromTheme('document-export'))
self.export_button.pressed.connect(self._export)
self.fit_to_view_button.setIcon(QIcon.fromTheme('zoom-fit-best'))
self.fit_to_view_button.pressed.connect(self._fit_to_view)
self.decision_graphs_combo_box.setSizeAdjustPolicy(QComboBox.AdjustToMinimumContentsLength)
self.decision_graphs_combo_box.currentIndexChanged['QString'].connect(self._graph_item_changed)
self._dot_processor = DotProcessor(self._dot_to_qt)
self.decision_graphs = dict()
self.states = dict()
def __init__(self, parent=None):
super(QCameraView, self).__init__(parent)
self.scene = QGraphicsScene(self)
self.setScene(self.scene)
self._load_default_image()
self.connect(self, SIGNAL("load_default_image"),
self._load_default_image)
def __init__(self, context):
super(RosTfTree, self).__init__(context)
self.initialized = False
self.setObjectName('RosTfTree')
self._current_dotcode = None
self._widget = QWidget()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosTfTreeDotcodeGenerator()
self.tf2_buffer_ = tf2_ros.Buffer()
self.tf2_listener_ = tf2_ros.TransformListener(self.tf2_buffer_)
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_tf_tree'), 'resource', 'RosTfTree.ui')
loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosTfTreeUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(self._update_tf_graph)
self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view)
self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image-x-generic'))
self._widget.save_as_image_push_button.pressed.connect(self._save_image)
self._deferred_fit_in_view.connect(self._fit_in_view,
Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
self._force_refresh = False
def __init__(self, camera_topic='/image_raw'):
super(CameraView, self).__init__()
self._scene = QGraphicsScene()
self.bridge = CvBridge()
self._map_item = 0
self.image_changed.connect(self._update)
self._sub = rospy.Subscriber(camera_topic, Image, self.callback)
self.setScene(self._scene)
def __init__(self, context):
self._context = context
super(ConductorGraph, self).__init__(context)
self.initialised = False
self.setObjectName('Conductor Graph')
self._node_items = None
self._edge_items = None
self._node_item_events = {}
self._edge_item_events = {}
self._client_info_list = {}
self._widget = QWidget()
self.cur_selected_client_name = ""
self.pre_selected_client_name = ""
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory=PygraphvizFactory()
self.dotcode_generator = ConductorGraphDotcodeGenerator()
self.dot_to_qt = DotToQtGenerator()
self._graph = ConductorGraphInfo(self._update_conductor_graph_relay,
self._set_network_statisics)
rospack = rospkg.RosPack()
ui_file = os.path.join(rospack.get_path('concert_conductor_graph'),
'ui', 'conductor_graph.ui')
loadUi(ui_file, self._widget,
{'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('ConductorGraphUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.highlight_connections_check_box.toggled.connect(
self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(
self._redraw_graph_view)
self._widget.clusters_check_box.toggled.connect(
self._redraw_graph_view)
self.signal_deferred_fit_in_view.connect(self._fit_in_view,
Qt.QueuedConnection)
self.signal_deferred_fit_in_view.emit()
self._widget.tabWidget.currentChanged.connect(self._change_client_tab)
self.signal_update_conductor_graph.connect(
self._update_conductor_graph)
context.add_widget(self._widget)
def __init__(self,
map_topic='/map',
paths=[
'/move_base/SBPLLatticePlanner/plan',
'/move_base/TrajectoryPlannerROS/local_plan'
],
polygons=['/move_base/local_costmap/robot_footprint'],
tf=None,
parent=None):
super(NavView, self).__init__()
self._parent = parent
self.last_path = None
self.map_changed.connect(self._update)
self.destroyed.connect(self.close)
#ScrollHandDrag
self.setDragMode(QGraphicsView.ScrollHandDrag)
self._map = None
self._map_item = None
self.w = 0
self.h = 0
self._paths = {}
self._polygons = {}
self.path_changed.connect(self._update_path)
self.polygon_changed.connect(self._update_polygon)
self._colors = [(238, 34, 116), (68, 134, 252), (236, 228, 46),
(102, 224, 18), (242, 156, 6), (240, 64, 10),
(196, 30, 250)]
self._scene = QGraphicsScene()
if tf is None:
self._tf = tf.TransformListener()
else:
self._tf = tf
self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_cb)
for path in paths:
self.add_path(path)
for poly in polygons:
self.add_polygon(poly)
self.setScene(self._scene)
def set_timeline_data(self,
len_timeline=None,
color_callback=None,
pause_callback=None):
if len_timeline:
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_robot_monitor'),
'resource', 'timelinepane.ui')
loadUi(ui_file, self, {'TimelineView': TimelineView})
self._pause_callback = pause_callback
self._timeline_view.set_init_data(1, len_timeline, 5,
color_callback)
self._scene = QGraphicsScene(self._timeline_view)
self._timeline_view.setScene(self._scene)
self._timeline_view.show()
self._queue_diagnostic = deque()
self._len_timeline = len_timeline
self._paused = False
self._tracking_latest = True
self._last_sec_marker_at = 2
self._last_msg = None
self._pause_button.clicked[bool].connect(self._pause)
self.sig_update.connect(self._timeline_view.slot_redraw)
def __init__(self, ros_package):
super(GraphWidget, self).__init__()
self._current_graph = None
self._lock = Lock()
self._load_ui(ros_package)
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
factory = DmgItemFactory()
factory.set_color(QColor(50, 50, 50))
factory.set_highlighted_color(QColor(0, 150, 0))
self._dot_to_qt = DotToQtGenerator(factory)
self.initialized = False
self.setObjectName("GraphWidget")
self.graphics_view.setScene(self._scene)
self.open_button.setIcon(QIcon.fromTheme("document-open"))
self.open_button.pressed.connect(self._import)
self.export_button.setIcon(QIcon.fromTheme("document-export"))
self.export_button.pressed.connect(self._export)
self.fit_to_view_button.setIcon(QIcon.fromTheme("zoom-fit-best"))
self.fit_to_view_button.pressed.connect(self._fit_to_view)
self.decision_graphs_combo_box.setSizeAdjustPolicy(QComboBox.AdjustToMinimumContentsLength)
self.decision_graphs_combo_box.currentIndexChanged["QString"].connect(self._graph_item_changed)
self._dot_processor = DotProcessor(self._dot_to_qt)
self.decision_graphs = dict()
self.states = dict()
def __init__(self, parent):
super(TimelineWidget, self).__init__()
self.parent = parent
self._layout = QHBoxLayout()
#self._view = QGraphicsView()
self._view = TimelineWidget.TimelineView(self)
self._scene = QGraphicsScene()
self._colors = [QColor('green'), QColor('yellow'), QColor('red')]
self._messages = [None for x in range(20)]
self._mq = [1 for x in range(20)]
self._view.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
self._view.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
self._view.setScene(self._scene)
self._layout.addWidget(self._view, 1)
self.pause_button = QPushButton('Pause')
self.pause_button.setCheckable(True)
self.pause_button.clicked.connect(self.pause)
self._layout.addWidget(self.pause_button)
self.setLayout(self._layout)
self.update.connect(self.redraw)
def __init__(self, context):
super(CapabilityGraph, self).__init__(context)
self.setObjectName('CapabilityGraph')
self.__current_dotcode = None
self.__widget = QWidget()
self.__dot_to_qt = DotToQtGenerator()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_capabilities'), 'resources', 'CapabilityGraph.ui')
loadUi(ui_file, self.__widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self.__widget.setObjectName('CapabilityGraphUI')
if context.serial_number() > 1:
self.__widget.setWindowTitle(self.__widget.windowTitle() + (' (%d)' % context.serial_number()))
self.__scene = QGraphicsScene()
self.__scene.setBackgroundBrush(Qt.white)
self.__widget.graphics_view.setScene(self.__scene)
self.__widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self.__widget.refresh_graph_push_button.pressed.connect(self.__update_capabilities_graph)
self.__update_capabilities_graph()
self.__deferred_fit_in_view.connect(self.__fit_in_view, Qt.QueuedConnection)
self.__deferred_fit_in_view.emit()
context.add_widget(self.__widget)
def __init__(self, context):
super(CapabilityGraph, self).__init__(context)
self.setObjectName('CapabilityGraph')
self.__current_dotcode = None
self.__running_providers = []
self.__spec_index = None
self.__widget = QWidget()
self.__dot_to_qt = DotToQtGenerator()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_capabilities'), 'resources',
'CapabilityGraph.ui')
loadUi(
ui_file, self.__widget, {
'CapabilitiesInteractiveGraphicsView':
CapabilitiesInteractiveGraphicsView
})
self.__widget.setObjectName('CapabilityGraphUI')
if context.serial_number() > 1:
self.__widget.setWindowTitle(self.__widget.windowTitle() +
(' (%d)' % context.serial_number()))
self.__scene = QGraphicsScene()
self.__scene.setBackgroundBrush(Qt.white)
self.__widget.graphics_view.setScene(self.__scene)
self.__widget.refresh_graph_push_button.setIcon(
QIcon.fromTheme('view-refresh'))
self.__widget.refresh_graph_push_button.pressed.connect(
self.__refresh_view)
self.__refresh_view()
self.__deferred_fit_in_view.connect(self.__fit_in_view,
Qt.QueuedConnection)
self.__deferred_fit_in_view.emit()
self.__redraw_graph.connect(self.__update_capabilities_graph)
# TODO: use user provided server node name
rospy.Subscriber('/capability_server/events', CapabilityEvent,
self.__handle_event)
context.add_widget(self.__widget)
class CameraView(QGraphicsView):
image_changed = Signal()
def __init__(self, camera_topic='/image_raw'):
super(CameraView,self).__init__()
self._scene = QGraphicsScene()
self.bridge = CvBridge()
self._map_item = 0
self.image_changed.connect(self._update)
self._sub = rospy.Subscriber(camera_topic,Image,self.callback)
self.setScene(self._scene)
def callback(self, msg):
self.w = msg.width
self.h = msg.height
a = self.bridge.imgmsg_to_cv(msg, "rgb8")
a = numpy.array(a)
image = QImage(a, self.w, self.h, QImage.Format_RGB888)
self._map = image
self._scene.setSceneRect(0,0,self.w,self.h)
self.image_changed.emit()
def _update(self):
if self._map_item:
self._scene.removeItem(self._map_item)
pixmap = QPixmap.fromImage(self._map)
self._map_item = self._scene.addPixmap(pixmap)
self.centerOn(self._map_item)
self.show()
def _mirror(self, item):
item.scale(-1, 1)
item.translate(-self.w, 0)
def close(self):
if self._sub:
self._sub.unregister()
super(CameraView, self).close()
class CameraView(QGraphicsView):
image_changed = Signal()
def __init__(self, camera_topic='/image_raw'):
super(CameraView, self).__init__()
self._scene = QGraphicsScene()
self.bridge = CvBridge()
self._map_item = 0
self.image_changed.connect(self._update)
self._sub = rospy.Subscriber(camera_topic, Image, self.callback)
self.setScene(self._scene)
def callback(self, msg):
self.w = msg.width
self.h = msg.height
a = self.bridge.imgmsg_to_cv(msg, "rgb8")
a = numpy.array(a)
image = QImage(a, self.w, self.h, QImage.Format_RGB888)
self._map = image
self._scene.setSceneRect(0, 0, self.w, self.h)
self.image_changed.emit()
def _update(self):
if self._map_item:
self._scene.removeItem(self._map_item)
pixmap = QPixmap.fromImage(self._map)
self._map_item = self._scene.addPixmap(pixmap)
self.centerOn(self._map_item)
self.show()
def _mirror(self, item):
item.scale(-1, 1)
item.translate(-self.w, 0)
def close(self):
if self._sub:
self._sub.unregister()
super(CameraView, self).close()
class EsterelPlanViewerWidget(QWidget):
# plan view
_scene = QGraphicsScene()
_webview = QGraphicsWebView()
_svg = QGraphicsSvgItem()
_renderer = QSvgRenderer()
def __init__(self, plugin=None):
super(EsterelPlanViewerWidget, self).__init__()
# Create QWidget
ui_file = os.path.join(rospkg.RosPack().get_path('rosplan_rqt'),
'resource', 'esterel_plan_viewer.ui')
loadUi(ui_file, self)
self.setObjectName('ROSPlanEsterelPlanViewer')
self.graphicsView.setScene(self._scene)
self._scene.addItem(self._svg)
self.refreshButton.clicked[bool].connect(self._handle_refresh_clicked)
self._sub = rospy.Subscriber("/plan_graph", String, self.plan_received)
self._plugin = plugin
"""
updating plan view
"""
def plan_received(self, msg):
graph = pydot.graph_from_dot_data(msg.data)
svg_string = graph.create_svg()
self._renderer.load(QByteArray(svg_string))
self._svg.setSharedRenderer(self._renderer)
"""
called when the refresh button is clicked
"""
def _handle_refresh_clicked(self, checked):
self._sub.unregister()
self._sub = rospy.Subscriber(self.topicText.text(), String,
self.plan_received)
"""
Qt methods
"""
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
def __init__(self, timeline, parent):
super(ImageView, self).__init__(timeline, parent)
self._image = None
self._image_topic = None
self._image_stamp = None
self.quality = Image.NEAREST # quality hint for scaling
# TODO put the image_topic and image_stamp on the picture or display them in some fashion
self._overlay_font_size = 14.0
self._overlay_indent = (4, 4)
self._overlay_color = (0.2, 0.2, 1.0)
self._image_view = QGraphicsView(parent)
self._image_view.resizeEvent = self._resizeEvent
self._scene = QGraphicsScene()
self._image_view.setScene(self._scene)
parent.layout().addWidget(self._image_view)
def __init__(self, parent=None):
super(QMapView, self).__init__()
self._parent = parent
self._scene = QGraphicsScene()
self.map_changed.connect(self._update_map)
self.destroyed.connect(self.close)
# ScrollHandDrag
self.setDragMode(QGraphicsView.ScrollHandDrag)
self._map = None
self.w = 0
self.h = 0
self._map_item = {}
self._polygons = {}
self._colors = [(238, 34, 116), (68, 134, 252), (236, 228, 46), (102, 224, 18), (242, 156, 6), (240, 64, 10), (196, 30, 250)]
self.setScene(self._scene)
def __init__(self, camera_topic='/image_raw'):
super(CameraView,self).__init__()
self._scene = QGraphicsScene()
self.bridge = CvBridge()
self._map_item = 0
self.image_changed.connect(self._update)
self._sub = rospy.Subscriber(camera_topic,Image,self.callback)
self.setScene(self._scene)
def __init__(self, map_topic='/map',
paths=['/move_base/SBPLLatticePlanner/plan', '/move_base/TrajectoryPlannerROS/local_plan'],
polygons=['/move_base/local_costmap/robot_footprint'], tf=None, parent=None):
super(NavView, self).__init__()
self._parent = parent
self._pose_mode = False
self._goal_mode = False
self.last_path = None
self.map_changed.connect(self._update)
self.destroyed.connect(self.close)
#ScrollHandDrag
self.setDragMode(QGraphicsView.ScrollHandDrag)
self._map = None
self._map_item = None
self.w = 0
self.h = 0
self._paths = {}
self._polygons = {}
self.path_changed.connect(self._update_path)
self.polygon_changed.connect(self._update_polygon)
self._colors = [(238, 34, 116), (68, 134, 252), (236, 228, 46), (102, 224, 18), (242, 156, 6), (240, 64, 10), (196, 30, 250)]
self._scene = QGraphicsScene()
if tf is None:
self._tf = tf.TransformListener()
else:
self._tf = tf
self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_cb)
for path in paths:
self.add_path(path)
for poly in polygons:
self.add_polygon(poly)
try:
self._pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=100)
self._goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=100)
except TypeError:
self._pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped)
self._goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped)
self.setScene(self._scene)
def __init__(self, parent):
"""Cannot take args other than parent due to loadUi limitation."""
super(TimelineView, self).__init__()
self._parent = parent
self._timeline_marker = QIcon.fromTheme('system-search')
self._min = 0
self._max = 0
self._xpos_marker = 5
self._timeline_marker_width = 15
self._timeline_marker_height = 15
self._last_marker_at = 2
self.redraw.connect(self._slot_redraw)
self._timeline = None
self.setUpdatesEnabled(True)
self._scene = QGraphicsScene(self)
self.setScene(self._scene)
def _redraw_graph_view(self):
key = str(self.get_current_message().header.stamp)
if key in self._scene_cache:
new_scene = self._scene_cache[key]
else: # cache miss
new_scene = QGraphicsScene()
new_scene.setBackgroundBrush(Qt.white)
if self._widget.highlight_connections_check_box.isChecked():
highlight_level = 3
else:
highlight_level = 1
# (nodes, edges) = self.dot_to_qt.graph_to_qt_items(self.dotcode_generator.graph,
# highlight_level)
# this function is very expensive
(nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(
self._current_dotcode, highlight_level)
for node_item in iter(nodes.values()):
new_scene.addItem(node_item)
for edge_items in iter(edges.values()):
for edge_item in edge_items:
edge_item.add_to_scene(new_scene)
new_scene.setSceneRect(new_scene.itemsBoundingRect())
# put the scene in the cache
self._scene_cache[key] = new_scene
self._scene_cache_keys.append(key)
if len(self._scene_cache) > self._scene_cache_capacity:
oldest = self._scene_cache_keys[0]
del self._scene_cache[oldest]
self._scene_cache_keys.remove(oldest)
# after construction, set the scene and fit to the view
self._scene = new_scene
self._widget.graphics_view.setScene(self._scene)
self._widget.message_label.setText(self._tip_message)
if self._widget.auto_fit_graph_check_box.isChecked():
self._fit_in_view()
def __init__(self, parent, len_timeline,
msg_callback,
color_callback,
pause_callback=None):
"""
@param msg_callback: This can be a function that takes either
{ DiagnosticArray, DiagnosticsStatus } as an argument. If your timeline
is to show the status of all devices, choose the former.
Else (eg. your timeline only handles the status of an arbitrary
node) use the latter.
@param color_callback: Not directly used within this class. Instead,
this will be passed and used in TimelineView class.
@todo: pause_callback is MUST since it's used in both by
RobotMonitorWidget & InspectorWindow.
"""
super(TimelinePane, self).__init__()
self._parent = parent
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'rqt_robot_monitor_timelinepane.ui')
loadUi(ui_file, self, {'TimelineView': TimelineView})
# Initialization process for TimeLineView obj continues.
self._pause_callback = pause_callback
self._msg_callback = msg_callback
self._timeline_view.set_init_data(1, len_timeline, 5,
# self._get_color_for_value)
color_callback)
self._scene = QGraphicsScene(self._timeline_view)
# self._colors = [QColor('green'), QColor('yellow'), QColor('red')]
self._timeline_view.setScene(self._scene)
self._timeline_view.show()
# self._messages = [None for x in range(len_timeline)] # DiagnosticStatus
# self._queue_diagnostic = [1 for x in range(20)]
self._queue_diagnostic = deque()
self._len_timeline = len_timeline
self._paused = False
self._tracking_latest = True
self._last_sec_marker_at = 2
self._last_msg = None
self._pause_button.clicked[bool].connect(self._pause)
self._sig_update.connect(self._timeline_view.slot_redraw)
def __init__(self, timeline, parent, topic):
super(ImageView, self).__init__(timeline, parent, topic)
self._image = None
self._image_topic = None
self._image_stamp = None
self.quality = Image.NEAREST # quality hint for scaling
# TODO put the image_topic and image_stamp on the picture or display them in some fashion
self._overlay_font_size = 14.0
self._overlay_indent = (4, 4)
self._overlay_color = (0.2, 0.2, 1.0)
self._image_view = QGraphicsView(parent)
self._image_view.resizeEvent = self._resizeEvent
self._scene = QGraphicsScene()
self._image_view.setScene(self._scene)
parent.layout().addWidget(self._image_view)
def initBumperGraphics(self):
# Pens
self.blue_pen = QPen(QColor(0,0,255))
self.blue_pen.setWidth(10)
self.bumper_lines = []
# Text state labels
self.bumper_state_labels = [QGraphicsTextItem() for i in range(0,4)]
for i in range(len(self.bumper_state_labels)):
self.bumper_state_labels[i].setFont(QFont('Ubuntu', 14, QFont.Bold))
self.bumper_state_labels[i].setPlainText('00')
self.bumper_state_labels[0].setPos(self.bumper_fl_x-10, self.bumper_fl_y + 55)
self.bumper_state_labels[1].setPos(self.bumper_fr_x-10, self.bumper_fr_y + 55)
self.bumper_state_labels[2].setPos(self.bumper_rl_x-10, self.bumper_rl_y - 80)
self.bumper_state_labels[3].setPos(self.bumper_rr_x-10, self.bumper_rr_y - 80)
# Bumper indicator lines
self.bumperLine(self.bumper_fl_x - 20, self.bumper_fl_y - self.bumper_dy, True)
self.bumperLine(self.bumper_fl_x - self.bumper_dx, self.bumper_fl_y - 20, False)
self.bumperLine(self.bumper_fl_x + self.bumper_dx, self.bumper_fl_y - 20, False)
self.bumperLine(self.bumper_fl_x - 20, self.bumper_fl_y + self.bumper_dy, True)
self.bumperLine(self.bumper_fr_x - 20, self.bumper_fr_y - self.bumper_dy, True)
self.bumperLine(self.bumper_fr_x - self.bumper_dx, self.bumper_fr_y - 20, False)
self.bumperLine(self.bumper_fr_x + self.bumper_dx, self.bumper_fr_y - 20, False)
self.bumperLine(self.bumper_fr_x - 20, self.bumper_fr_y + self.bumper_dy, True)
self.bumperLine(self.bumper_rl_x - 20, self.bumper_rl_y - self.bumper_dy, True)
self.bumperLine(self.bumper_rl_x - self.bumper_dx, self.bumper_rl_y - 20, False)
self.bumperLine(self.bumper_rl_x + self.bumper_dx, self.bumper_rl_y - 20, False)
self.bumperLine(self.bumper_rl_x - 20, self.bumper_rl_y + self.bumper_dy, True)
self.bumperLine(self.bumper_rr_x - 20, self.bumper_rr_y - self.bumper_dy, True)
self.bumperLine(self.bumper_rr_x - self.bumper_dx, self.bumper_rr_y - 20, False)
self.bumperLine(self.bumper_rr_x + self.bumper_dx, self.bumper_rr_y - 20, False)
self.bumperLine(self.bumper_rr_x - 20, self.bumper_rr_y + self.bumper_dy, True)
# Populate scene
graphics_scene = QGraphicsScene()
for bumper in self.bumper_lines:
graphics_scene.addItem(bumper)
for label in self.bumper_state_labels:
graphics_scene.addItem(label)
graphics_scene.setSceneRect(0, 0, self._widget.bumperGraphicsView.width() - 4, self._widget.bumperGraphicsView.height() - 4)
self._widget.bumperGraphicsView.setScene(graphics_scene)
self._widget.bumperGraphicsView.setBackgroundBrush(QBrush(QImage(os.path.join(rospkg.RosPack().get_path('mobility_base_tools'), 'images', 'mb_top.png'))))
self._widget.bumperGraphicsView.show()
def __init__(self, topic='/map', tf=None, parent=None):
super(RoboMap, self).__init__()
self.parent = parent
self.tf = tf
self.topic = topic
self.map_change.connect(self.update)
self.destroyed.connect(self.close)
self.map = None
self.mapitem = None
self.w = 0
self.h = 0
self.resolution = None
self.origin = None
self.polygon = None
self.point = None
self.scene = QGraphicsScene()
self.subscriber = rospy.Subscriber(topic, OccupancyGrid, self.callback)
self.setScene(self.scene)
self.timer = None
def __init__(self, parent):
"""Cannot take args other than parent due to loadUi limitation."""
super(TimelineView, self).__init__()
self._parent = parent
self._timeline_marker = QIcon.fromTheme('system-search')
self._min = 0
self._max = 0
self._xpos_marker = 5
self._timeline_marker_width = 15
self._timeline_marker_height = 15
self._last_marker_at = 2
self.redraw.connect(self._slot_redraw)
self._timeline = None
self.setUpdatesEnabled(True)
self._scene = QGraphicsScene(self)
self.setScene(self._scene)
def __init__(self, context):
super(CapabilityGraph, self).__init__(context)
self.setObjectName('CapabilityGraph')
self.__current_dotcode = None
self.__running_providers = []
self.__spec_index = None
self.__widget = QWidget()
self.__dot_to_qt = DotToQtGenerator()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_capabilities'), 'resources', 'CapabilityGraph.ui')
loadUi(ui_file, self.__widget, {'CapabilitiesInteractiveGraphicsView': CapabilitiesInteractiveGraphicsView})
self.__widget.setObjectName('CapabilityGraphUI')
if context.serial_number() > 1:
self.__widget.setWindowTitle(self.__widget.windowTitle() + (' (%d)' % context.serial_number()))
self.__scene = QGraphicsScene()
self.__scene.setBackgroundBrush(Qt.white)
self.__widget.graphics_view.setScene(self.__scene)
self.__widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self.__widget.refresh_graph_push_button.pressed.connect(self.__refresh_view)
self.__refresh_view()
self.__deferred_fit_in_view.connect(self.__fit_in_view, Qt.QueuedConnection)
self.__deferred_fit_in_view.emit()
self.__redraw_graph.connect(self.__update_capabilities_graph)
# TODO: use user provided server node name
rospy.Subscriber('/capability_server/events', CapabilityEvent, self.__handle_event)
context.add_widget(self.__widget)
class CapabilityGraph(Plugin):
__deferred_fit_in_view = Signal()
__redraw_graph = Signal()
def __init__(self, context):
super(CapabilityGraph, self).__init__(context)
self.setObjectName('CapabilityGraph')
self.__current_dotcode = None
self.__running_providers = []
self.__spec_index = None
self.__widget = QWidget()
self.__dot_to_qt = DotToQtGenerator()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_capabilities'), 'resources', 'CapabilityGraph.ui')
loadUi(ui_file, self.__widget, {'CapabilitiesInteractiveGraphicsView': CapabilitiesInteractiveGraphicsView})
self.__widget.setObjectName('CapabilityGraphUI')
if context.serial_number() > 1:
self.__widget.setWindowTitle(self.__widget.windowTitle() + (' (%d)' % context.serial_number()))
self.__scene = QGraphicsScene()
self.__scene.setBackgroundBrush(Qt.white)
self.__widget.graphics_view.setScene(self.__scene)
self.__widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self.__widget.refresh_graph_push_button.pressed.connect(self.__refresh_view)
self.__refresh_view()
self.__deferred_fit_in_view.connect(self.__fit_in_view, Qt.QueuedConnection)
self.__deferred_fit_in_view.emit()
self.__redraw_graph.connect(self.__update_capabilities_graph)
# TODO: use user provided server node name
rospy.Subscriber('/capability_server/events', CapabilityEvent, self.__handle_event)
context.add_widget(self.__widget)
def __handle_event(self, msg):
if msg.type == CapabilityEvent.STOPPED:
return
if msg.type == CapabilityEvent.LAUNCHED and msg.provider not in self.__running_providers:
self.__running_providers.append(msg.provider)
if msg.type == CapabilityEvent.TERMINATED and msg.provider in self.__running_providers:
self.__running_providers.remove(msg.provider)
self.__redraw_graph.emit()
def __get_specs(self):
self.__spec_index, errors = spec_index_from_service()
assert not errors
def __get_running_providers(self):
# TODO: replace 'capability_server' with user provided server name
service_name = '/{0}/get_running_capabilities'.format('capability_server')
rospy.wait_for_service(service_name)
get_running_capabilities = rospy.ServiceProxy(service_name, GetRunningCapabilities)
response = get_running_capabilities()
self.__running_providers = []
for cap in response.running_capabilities:
self.__running_providers.append(cap.capability.provider)
def __refresh_view(self):
self.__get_specs()
self.__get_running_providers()
self.__update_capabilities_graph()
def __update_capabilities_graph(self):
self.__update_graph_view(self.__generate_dotcode())
def __generate_dotcode(self):
return generate_dotcode_from_capability_info(self.__spec_index, self.__running_providers)
def __update_graph_view(self, dotcode):
if dotcode == self.__current_dotcode:
return
self.__current_dotcode = dotcode
self.__redraw_graph_view()
def __fit_in_view(self):
self.__widget.graphics_view.fitInView(self.__scene.itemsBoundingRect(), Qt.KeepAspectRatio)
def __redraw_graph_view(self):
self.__widget.graphics_view._running_providers = self.__running_providers
self.__widget.graphics_view._spec_index = self.__spec_index
self.__scene.clear()
highlight_level = 1
# layout graph and create qt items
(nodes, edges) = self.__dot_to_qt.dotcode_to_qt_items(self.__current_dotcode,
highlight_level=highlight_level,
same_label_siblings=True)
for node_item in nodes.itervalues():
self.__scene.addItem(node_item)
for edge_items in edges.itervalues():
for edge_item in edge_items:
edge_item.add_to_scene(self.__scene)
self.__scene.setSceneRect(self.__scene.itemsBoundingRect())
self.__fit_in_view()
def initJoystickGraphics(self):
# Pens
self.cyan_pen = QPen(QColor(0, 255, 255))
self.magenta_pen = QPen(QColor(255, 0, 255))
self.red_pen = QPen(QColor(255, 0, 0))
self.cyan_pen.setWidth(3)
self.magenta_pen.setWidth(3)
self.red_pen.setWidth(3)
self.stick_ind_l = QGraphicsEllipseItem()
self.stick_ind_r = QGraphicsEllipseItem()
self.stick_line_l = QGraphicsLineItem()
self.stick_line_r = QGraphicsLineItem()
self.mode_ind = QGraphicsLineItem()
# Left joystick indicator circle
px_l = self.stick_ind_lox - self.stick_ind_radius
py_l = self.stick_ind_loy - self.stick_ind_radius
self.stick_ind_l.setRect(px_l, py_l, 2 * self.stick_ind_radius, 2 * self.stick_ind_radius)
self.stick_ind_l.setBrush(QBrush(QColor(255, 0, 0)))
self.stick_ind_l.setPen(QPen(QColor(0, 0, 0)))
# Right joystick indicator circle
px_r = self.stick_ind_rox - self.stick_ind_radius
py_r = self.stick_ind_roy - self.stick_ind_radius
self.stick_ind_r.setRect(px_r, py_r, 2 * self.stick_ind_radius, 2 * self.stick_ind_radius)
self.stick_ind_r.setBrush(QBrush(QColor(255, 0, 0)))
self.stick_ind_r.setPen(QPen(QColor(0, 0, 0)))
# Left joystick indicator line
line_pen = QPen(QColor(255,0,0))
line_pen.setWidth(4)
self.stick_line_l.setLine(self.stick_ind_lox, self.stick_ind_loy, self.stick_ind_lox, self.stick_ind_loy)
self.stick_line_l.setPen(line_pen)
# Right joystick indicator line
self.stick_line_r.setLine(self.stick_ind_rox, self.stick_ind_roy, self.stick_ind_rox, self.stick_ind_roy)
self.stick_line_r.setPen(line_pen)
# Mode indicator line
self.mode_ind.setLine(self.mode_ind_x1, self.mode_ind_y1, self.mode_ind_x2, self.mode_ind_y2)
self.mode_ind.setPen(self.cyan_pen)
# Joystick power indicator
self.joystick_power_ind = []
self.joystick_power_ind.append(QGraphicsLineItem(self.power_ind_x1, self.power_ind_y + 20, self.power_ind_x1, self.power_ind_y - 20))
self.joystick_power_ind.append(QGraphicsLineItem(self.power_ind_x1, self.power_ind_y - 20, self.power_ind_x1 + 50, self.power_ind_y - 20))
self.joystick_power_ind.append(QGraphicsLineItem(self.power_ind_x1+50, self.power_ind_y - 20, self.power_ind_x1+50, self.power_ind_y + 20))
self.joystick_power_ind.append(QGraphicsLineItem(self.power_ind_x1+50, self.power_ind_y + 20, self.power_ind_x1, self.power_ind_y + 20))
# Populate scene
graphics_scene = QGraphicsScene()
graphics_scene.addItem(self.stick_ind_l)
graphics_scene.addItem(self.stick_ind_r)
graphics_scene.addItem(self.stick_line_l)
graphics_scene.addItem(self.stick_line_r)
graphics_scene.addItem(self.mode_ind)
for l in self.joystick_power_ind:
l.setPen(self.red_pen)
graphics_scene.addItem(l)
graphics_scene.setSceneRect(0, 0, self._widget.joystickGraphicsView.width() - 4, self._widget.joystickGraphicsView.height() - 4)
self._widget.joystickGraphicsView.setScene(graphics_scene)
self._widget.joystickGraphicsView.setBackgroundBrush(QBrush(QImage(os.path.join(rospkg.RosPack().get_path('mobility_base_tools'), 'images', 'dx6ilabels.jpg'))))
self._widget.joystickGraphicsView.show()
class ConductorGraph(Plugin):
# pyqt signals are always defined as class attributes
signal_deferred_fit_in_view = Signal()
signal_update_conductor_graph = Signal()
# constants
# colour definitions from http://www.w3.org/TR/SVG/types.html#ColorKeywords
# see also http://qt-project.org/doc/qt-4.8/qcolor.html#setNamedColor
link_strength_colours = {
'very_strong': QColor("lime"),
'strong': QColor("chartreuse"),
'normal': QColor("yellow"),
'weak': QColor("orange"),
'very_weak': QColor("red"),
'missing': QColor("powderblue")
}
def __init__(self, context):
self._context = context
super(ConductorGraph, self).__init__(context)
self.initialised = False
self.setObjectName('Conductor Graph')
self._node_items = None
self._edge_items = None
self._node_item_events = {}
self._edge_item_events = {}
self._client_info_list = {}
self._widget = QWidget()
self.cur_selected_client_name = ""
self.pre_selected_client_name = ""
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory=PygraphvizFactory()
self.dotcode_generator = ConductorGraphDotcodeGenerator()
self.dot_to_qt = DotToQtGenerator()
self._graph = ConductorGraphInfo(self._update_conductor_graph_relay,
self._set_network_statisics)
rospack = rospkg.RosPack()
ui_file = os.path.join(rospack.get_path('concert_conductor_graph'),
'ui', 'conductor_graph.ui')
loadUi(ui_file, self._widget,
{'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('ConductorGraphUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.highlight_connections_check_box.toggled.connect(
self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(
self._redraw_graph_view)
self._widget.clusters_check_box.toggled.connect(
self._redraw_graph_view)
self.signal_deferred_fit_in_view.connect(self._fit_in_view,
Qt.QueuedConnection)
self.signal_deferred_fit_in_view.emit()
self._widget.tabWidget.currentChanged.connect(self._change_client_tab)
self.signal_update_conductor_graph.connect(
self._update_conductor_graph)
context.add_widget(self._widget)
def restore_settings(self, plugin_settings, instance_settings):
self.initialised = True
self._update_conductor_graph()
def shutdown_plugin(self):
self._graph.shutdown()
def _update_conductor_graph(self):
if self.initialised:
self._redraw_graph_view()
self._update_client_tab()
def _update_conductor_graph_relay(self):
"""
This seems a bit obtuse, but we can't just dump the _update_conductor_graph callback on the underlying
conductor graph info and trigger it from there since that trigger will operate from a ros thread and pyqt
will crash trying to co-ordinate gui changes from an external thread. We need to relay via a signal.
"""
self.signal_update_conductor_graph.emit()
def _update_client_tab(self):
print('[conductor graph]: _update_client_tab')
self.pre_selected_client_name = self.cur_selected_client_name
self._widget.tabWidget.clear()
for k in self._graph.concert_clients.values():
# Only pull in information from connected or connectable clients
if k.state not in [
concert_msgs.ConcertClientState.AVAILABLE,
concert_msgs.ConcertClientState.MISSING,
concert_msgs.ConcertClientState.UNINVITED
]:
continue
main_widget = QWidget()
ver_layout = QVBoxLayout(main_widget)
ver_layout.setContentsMargins(9, 9, 9, 9)
ver_layout.setSizeConstraint(ver_layout.SetDefaultConstraint)
#button layout
sub_widget = QWidget()
sub_widget.setAccessibleName('sub_widget')
ver_layout.addWidget(sub_widget)
#client information layout
context_label = QLabel()
context_label.setText("Client information")
ver_layout.addWidget(context_label)
app_context_widget = QPlainTextEdit()
app_context_widget.setObjectName(k.concert_alias + '_' +
'app_context_widget')
app_context_widget.setAccessibleName('app_context_widget')
app_context_widget.appendHtml(k.get_rapp_context())
app_context_widget.setReadOnly(True)
cursor = app_context_widget.textCursor()
cursor.movePosition(QTextCursor.Start, QTextCursor.MoveAnchor, 0)
app_context_widget.setTextCursor(cursor)
ver_layout.addWidget(app_context_widget)
# new icon
path = ""
if k.is_new:
# This only changes when the concert client changes topic publishes anew
path = os.path.join(os.path.dirname(os.path.abspath(__file__)),
"../../resources/images/new.gif")
#add tab
self._widget.tabWidget.addTab(main_widget, QIcon(path),
k.concert_alias)
#set previous selected tab
for k in range(self._widget.tabWidget.count()):
tab_text = self._widget.tabWidget.tabText(k)
if tab_text == self.pre_selected_client_name:
self._widget.tabWidget.setCurrentIndex(k)
def _change_client_tab(self, index):
self.cur_selected_client_name = self._widget.tabWidget.tabText(
self._widget.tabWidget.currentIndex())
def _set_network_statisics(self):
# we currently redraw every statistics update (expensive!) so passing for now, but we should
# reenable this and drop the change callback to be more efficient
#if self._edge_items == None:
# return
#else:
# for edge_items in self._edge_items.itervalues():
# for edge_item in edge_items:
# edge_dst_name = edge_item.to_node._label.text()
# edge_item.setToolTip(str(self._graph.concert_clients[edge_dst_name].msg.conn_stats))
pass
def _redraw_graph_view(self):
print("[conductor graph]: _redraw_graph_view")
# regenerate the dotcode
current_dotcode = self.dotcode_generator.generate_dotcode(
conductor_graph_instance=self._graph,
dotcode_factory=self.dotcode_factory,
clusters=self._widget.clusters_check_box.isChecked())
#print("Dotgraph: \n%s" % current_dotcode)
self._scene.clear()
self._node_item_events = {}
self._edge_item_events = {}
self._node_items = None
self._edge_items = None
highlight_level = 3 if self._widget.highlight_connections_check_box.isChecked(
) else 1
# layout graph and create qt items
(nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(
current_dotcode,
highlight_level=highlight_level,
same_label_siblings=True)
self._node_items = nodes
self._edge_items = edges
#nodes - if we wish to make special nodes, do that here (maybe subclass GraphItem, just like NodeItem does
for node_item in nodes.itervalues():
# redefine mouse event
#self._node_item_events[node_item._label.text()] = GraphEventHandler(self._widget.tabWidget, node_item, node_item.mouseDoubleClickEvent)
#node_item.mouseDoubleClickEvent = self._node_item_events[node_item._label.text()].NodeEvent
self._node_item_events[
node_item._label.text()] = GraphEventHandler(
self._widget.tabWidget, node_item,
node_item.hoverEnterEvent)
node_item.hoverEnterEvent = self._node_item_events[
node_item._label.text()].NodeEvent
self._scene.addItem(node_item)
#edges
for edge_items in edges.itervalues():
for edge_item in edge_items:
#redefine the edge hover event
self._edge_item_events[
edge_item._label.text()] = GraphEventHandler(
self._widget.tabWidget, edge_item,
edge_item._label.hoverEnterEvent)
edge_item._label.hoverEnterEvent = self._edge_item_events[
edge_item._label.text()].EdgeEvent
#self._edge_item_events[edge_item._label.text()]=GraphEventHandler(self._widget.tabWidget,edge_item,edge_item.mouseDoubleClickEvent);
#edge_item.mouseDoubleClickEvent=self._edge_item_events[edge_item._label.text()].EdgeEvent;
edge_item.add_to_scene(self._scene)
#set the color of node as connection strength one of red, yellow, green
edge_dst_name = edge_item.to_node._label.text()
if edge_dst_name in self._graph.concert_clients.keys():
link_strength_colour = ConductorGraph.link_strength_colours[
self._graph.concert_clients[edge_dst_name].
get_connection_strength()]
edge_item._default_color = link_strength_colour
edge_item.set_node_color(link_strength_colour)
#set the tooltip about network information
edge_item.setToolTip(
str(self._graph.concert_clients[edge_dst_name].msg.
conn_stats))
self._scene.setSceneRect(self._scene.itemsBoundingRect())
if self._widget.auto_fit_graph_check_box.isChecked():
self._fit_in_view()
def _fit_in_view(self):
self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(),
Qt.KeepAspectRatio)
class RosPackGraph(Plugin):
_deferred_fit_in_view = Signal()
def __init__(self, context):
super(RosPackGraph, self).__init__(context)
self.initialized = False
self._current_dotcode = None
self._update_thread = WorkerThread(self._update_thread_run, self._update_finished)
self._nodes = {}
self._edges = {}
self._options = {}
self._options_serialized = ''
self.setObjectName('RosPackGraph')
rospack = rospkg.RosPack()
rosstack = rospkg.RosStack()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosPackageGraphDotcodeGenerator(rospack, rosstack)
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_dep'), 'resource', 'RosPackGraph.ui')
loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosPackGraphUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.depth_combo_box.insertItem(0, self.tr('infinite'), -1)
self._widget.depth_combo_box.insertItem(1, self.tr('1'), 2)
self._widget.depth_combo_box.insertItem(2, self.tr('2'), 3)
self._widget.depth_combo_box.insertItem(3, self.tr('3'), 4)
self._widget.depth_combo_box.insertItem(4, self.tr('4'), 5)
self._widget.depth_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph)
self._widget.directions_combo_box.insertItem(0, self.tr('depends'), 0)
self._widget.directions_combo_box.insertItem(1, self.tr('depends_on'), 1)
self._widget.directions_combo_box.insertItem(2, self.tr('both'), 2)
self._widget.directions_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph)
self._widget.package_type_combo_box.insertItem(0, self.tr('wet & dry'), 3)
self._widget.package_type_combo_box.insertItem(1, self.tr('wet only'), 2)
self._widget.package_type_combo_box.insertItem(2, self.tr('dry only'), 1)
self._widget.package_type_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph)
completionmodel = StackageCompletionModel(self._widget.filter_line_edit, rospack, rosstack)
completer = RepeatedWordCompleter(completionmodel, self)
completer.setCompletionMode(QCompleter.PopupCompletion)
completer.setWrapAround(True)
completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.filter_line_edit.editingFinished.connect(self._refresh_rospackgraph)
self._widget.filter_line_edit.setCompleter(completer)
self._widget.filter_line_edit.selectionChanged.connect(self._clear_filter)
self._widget.with_stacks_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.mark_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.colorize_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.hide_transitives_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(self._update_rospackgraph)
self._widget.highlight_connections_check_box.toggled.connect(self._refresh_rospackgraph)
self._widget.auto_fit_graph_check_box.toggled.connect(self._refresh_rospackgraph)
self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(self._save_image)
self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
# If in either of following case, this turnes True
# - 1st filtering key is already input by user
# - filtering key is restored
self._filtering_started = False
def shutdown_plugin(self):
self._update_thread.kill()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('depth_combo_box_index', self._widget.depth_combo_box.currentIndex())
instance_settings.set_value('directions_combo_box_index', self._widget.directions_combo_box.currentIndex())
instance_settings.set_value('package_type_combo_box', self._widget.package_type_combo_box.currentIndex())
instance_settings.set_value('filter_line_edit_text', self._widget.filter_line_edit.text())
instance_settings.set_value('with_stacks_state', self._widget.with_stacks_check_box.isChecked())
instance_settings.set_value('hide_transitives_state', self._widget.hide_transitives_check_box.isChecked())
instance_settings.set_value('mark_state', self._widget.mark_check_box.isChecked())
instance_settings.set_value('colorize_state', self._widget.colorize_check_box.isChecked())
instance_settings.set_value('auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked())
instance_settings.set_value('highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked())
def restore_settings(self, plugin_settings, instance_settings):
_str_filter = instance_settings.value('filter_line_edit_text', '')
if (_str_filter == None or _str_filter == '') and \
not self._filtering_started:
_str_filter = '(Separate pkgs by comma)'
else:
self._filtering_started = True
self._widget.depth_combo_box.setCurrentIndex(int(instance_settings.value('depth_combo_box_index', 0)))
self._widget.directions_combo_box.setCurrentIndex(int(instance_settings.value('directions_combo_box_index', 0)))
self._widget.package_type_combo_box.setCurrentIndex(int(instance_settings.value('package_type_combo_box', 0)))
self._widget.filter_line_edit.setText(_str_filter)
self._widget.with_stacks_check_box.setChecked(instance_settings.value('with_stacks_state', True) in [True, 'true'])
self._widget.mark_check_box.setChecked(instance_settings.value('mark_state', True) in [True, 'true'])
self._widget.colorize_check_box.setChecked(instance_settings.value('colorize_state', False) in [True, 'true'])
self._widget.hide_transitives_check_box.setChecked(instance_settings.value('hide_transitives_state', False) in [True, 'true'])
self._widget.auto_fit_graph_check_box.setChecked(instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true'])
self._widget.highlight_connections_check_box.setChecked(instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true'])
self.initialized = True
self._refresh_rospackgraph()
def _update_rospackgraph(self):
# re-enable controls customizing fetched ROS graph
self._widget.depth_combo_box.setEnabled(True)
self._widget.directions_combo_box.setEnabled(True)
self._widget.package_type_combo_box.setEnabled(True)
self._widget.filter_line_edit.setEnabled(True)
self._widget.with_stacks_check_box.setEnabled(True)
self._widget.mark_check_box.setEnabled(True)
self._widget.colorize_check_box.setEnabled(True)
self._widget.hide_transitives_check_box.setEnabled(True)
self._refresh_rospackgraph(force_update=True)
def _update_options(self):
self._options['depth'] = self._widget.depth_combo_box.itemData(self._widget.depth_combo_box.currentIndex())
self._options['directions'] = self._widget.directions_combo_box.itemData(self._widget.directions_combo_box.currentIndex())
self._options['package_types'] = self._widget.package_type_combo_box.itemData(self._widget.package_type_combo_box.currentIndex())
self._options['with_stacks'] = self._widget.with_stacks_check_box.isChecked()
self._options['mark_selected'] = self._widget.mark_check_box.isChecked()
self._options['hide_transitives'] = self._widget.hide_transitives_check_box.isChecked()
# TODO: Allow different color themes
self._options['colortheme'] = True if self._widget.colorize_check_box.isChecked() else None
self._options['names'] = self._widget.filter_line_edit.text().split(',')
if self._options['names'] == [u'None']:
self._options['names'] = []
self._options['highlight_level'] = 3 if self._widget.highlight_connections_check_box.isChecked() else 1
self._options['auto_fit'] = self._widget.auto_fit_graph_check_box.isChecked()
def _refresh_rospackgraph(self, force_update=False):
if not self.initialized:
return
self._update_thread.kill()
self._update_options()
# avoid update if options did not change and force_update is not set
new_options_serialized = pickle.dumps(self._options)
if new_options_serialized == self._options_serialized and not force_update:
return
self._options_serialized = pickle.dumps(self._options)
self._scene.setBackgroundBrush(Qt.lightGray)
self._update_thread.start()
# this runs in a non-gui thread, so don't access widgets here directly
def _update_thread_run(self):
self._update_graph(self._generate_dotcode())
@Slot()
def _update_finished(self):
self._scene.setBackgroundBrush(Qt.white)
self._redraw_graph_scene()
# this runs in a non-gui thread, so don't access widgets here directly
def _generate_dotcode(self):
includes = []
excludes = []
for name in self._options['names']:
if name.strip().startswith('-'):
excludes.append(name.strip()[1:])
else:
includes.append(name.strip())
# orientation = 'LR'
descendants = True
ancestors = True
if self._options['directions'] == 1:
descendants = False
if self._options['directions'] == 0:
ancestors = False
return self.dotcode_generator.generate_dotcode(dotcode_factory=self.dotcode_factory,
selected_names=includes,
excludes=excludes,
depth=self._options['depth'],
with_stacks=self._options['with_stacks'],
descendants=descendants,
ancestors=ancestors,
mark_selected=self._options['mark_selected'],
colortheme=self._options['colortheme'],
hide_transitives=self._options['hide_transitives'],
hide_wet=self._options['package_types'] == 1,
hide_dry=self._options['package_types'] == 2)
# this runs in a non-gui thread, so don't access widgets here directly
def _update_graph(self, dotcode):
self._current_dotcode = dotcode
self._nodes, self._edges = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode, self._options['highlight_level'])
def _generate_tool_tip(self, url):
if url is not None and ':' in url:
item_type, item_path = url.split(':', 1)
if item_type == 'node':
tool_tip = 'Node:\n %s' % (item_path)
service_names = rosservice.get_service_list(node=item_path)
if service_names:
tool_tip += '\nServices:'
for service_name in service_names:
try:
service_type = rosservice.get_service_type(service_name)
tool_tip += '\n %s [%s]' % (service_name, service_type)
except rosservice.ROSServiceIOException, e:
tool_tip += '\n %s' % (e)
return tool_tip
elif item_type == 'topic':
topic_type, topic_name, _ = rostopic.get_topic_type(item_path)
return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type)
return url
class RoboMap(QGraphicsView):
map_change = Signal()
def __init__(self, topic='/map', tf=None, parent=None):
super(RoboMap, self).__init__()
self.parent = parent
self.tf = tf
self.topic = topic
self.map_change.connect(self.update)
self.destroyed.connect(self.close)
self.map = None
self.mapitem = None
self.w = 0
self.h = 0
self.resolution = None
self.origin = None
self.polygon = None
self.point = None
self.scene = QGraphicsScene()
self.subscriber = rospy.Subscriber(topic, OccupancyGrid, self.callback)
self.setScene(self.scene)
self.timer = None
def update(self):
if self.mapitem:
self.scene.removeItem(self.mapitem)
qpix = QPixmap.fromImage(self.map)
self.mapitem = self.scene.addPixmap(qpix)
self.mirror(self.mapitem)
self.show()
def mirror(self, item):
item.scale(-1, 1)
item.translate(-self.w, 0)
def callback(self, msg):
self.w = msg.info.width
self.h = msg.info.height
self.resolution = msg.info.resolution
self.origin = (msg.info.origin.position.x, msg.info.origin.position.y)
arr = np.array(msg.data, dtype=np.uint8, copy=False, order='C')
arr = arr.reshape((self.h, self.w))
img = QImage(arr.reshape((arr.shape[0] * arr.shape[1])), self.w, self.h, QImage.Format_Indexed8)
#need to invert some colors :)
for z in reversed(range(101)):
img.setColor(100 - z, qRgb(z*2.55, z*2.55, z*2.55))
img.setColor(101, qRgb(255, 0, 0))
img.setColor(255, qRgb(100, 100, 100))
self.map = img
self.setSceneRect(0, 0, self.w, self.h)
self.map_change.emit()
# Explorer laser callback
self.parent.taskplanner.explorer.laser_callback(msg)
def update_map(self, dead_pirates):
tmp = []
for z in dead_pirates:
x = z.pose.position.x
y = z.pose.position.y
#transform pose coordinates to map coordinates
map_y = (y - self.origin[1])/self.resolution
map_x = -((x - self.origin[0])/self.resolution) + self.w
tmp.append(self.draw_point(map_x, map_y, color=Qt.red))
print 'Added dead'
self.parent.dead_pirate_objects = tmp
def draw_point(self, x, y, color=Qt.magenta, rad=1.0, message=None):
ell = self.scene.addEllipse(x-rad, y-rad, rad*2.0, rad*2.0, color, QBrush(Qt.SolidPattern))
if message:
self.parent.update_textbox(message, (str(x) + ' ' + str(y)))
ell.setZValue(2000.0)
return ell
def mousePressEvent(self, e):
point = self.mapToScene(e.x(), e.y())
if self.point:
self.scene.removeItem(self.point)
self.point = None
self.point = self.draw_point(point.x(), point.y(), Qt.yellow, 1.0)
def wheelEvent(self, e):
e.ignore()
if e.delta() > 0:
self.scale(1.30, 1.30)
else:
self.scale(0.7, 0.7)
class NavView(QGraphicsView):
map_changed = Signal()
path_changed = Signal(str)
polygon_changed = Signal(str)
def __init__(self, map_topic='/map',
paths=['/move_base/SBPLLatticePlanner/plan', '/move_base/TrajectoryPlannerROS/local_plan'],
polygons=['/move_base/local_costmap/robot_footprint'], tf=None, parent=None):
super(NavView, self).__init__()
self._parent = parent
self._pose_mode = False
self._goal_mode = False
self.last_path = None
self.map_changed.connect(self._update)
self.destroyed.connect(self.close)
#ScrollHandDrag
self.setDragMode(QGraphicsView.ScrollHandDrag)
self._map = None
self._map_item = None
self.w = 0
self.h = 0
self._paths = {}
self._polygons = {}
self.path_changed.connect(self._update_path)
self.polygon_changed.connect(self._update_polygon)
self._colors = [(238, 34, 116), (68, 134, 252), (236, 228, 46), (102, 224, 18), (242, 156, 6), (240, 64, 10), (196, 30, 250)]
self._scene = QGraphicsScene()
if tf is None:
self._tf = tf.TransformListener()
else:
self._tf = tf
self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_cb)
for path in paths:
self.add_path(path)
for poly in polygons:
self.add_polygon(poly)
try:
self._pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=100)
self._goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=100)
except TypeError:
self._pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped)
self._goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped)
self.setScene(self._scene)
def add_dragdrop(self, item):
# Add drag and drop functionality to all the items in the view
def c(x, e):
self.dragEnterEvent(e)
def d(x, e):
self.dropEvent(e)
item.setAcceptDrops(True)
item.dragEnterEvent = c
item.dropEvent = d
def dragEnterEvent(self, e):
if self._parent:
self._parent.dragEnterEvent(e)
def dropEvent(self, e):
if self._parent:
self._parent.dropEvent(e)
def wheelEvent(self, event):
event.ignore()
if event.delta() > 0:
self.scale(1.15, 1.15)
else:
self.scale(0.85, 0.85)
def map_cb(self, msg):
self.resolution = msg.info.resolution
self.w = msg.info.width
self.h = msg.info.height
a = numpy.array(msg.data, dtype=numpy.uint8, copy=False, order='C')
a = a.reshape((self.h, self.w))
if self.w % 4:
e = numpy.empty((self.h, 4 - self.w % 4), dtype=a.dtype, order='C')
a = numpy.append(a, e, axis=1)
image = QImage(a.reshape((a.shape[0] * a.shape[1])), self.w, self.h, QImage.Format_Indexed8)
for i in reversed(range(101)):
image.setColor(100 - i, qRgb(i* 2.55, i * 2.55, i * 2.55))
image.setColor(101, qRgb(255, 0, 0)) # not used indices
image.setColor(255, qRgb(200, 200, 200)) # color for unknown value -1
self._map = image
self.setSceneRect(0, 0, self.w, self.h)
self.map_changed.emit()
def add_path(self, name):
path = PathInfo(name)
def c(msg):
if not self._map:
return
pp = QPainterPath()
# Transform everything in to the map frame
if not (msg.header.frame_id == '/map' or msg.header.frame_id == ''):
try:
self._tf.waitForTransform(msg.header.frame_id, '/map', rospy.Time(), rospy.Duration(10))
data = [self._tf.transformPose('/map', pose) for pose in msg.poses]
except tf.Exception:
rospy.logerr("TF Error")
data = []
else:
data = msg.poses
if len(data) > 0:
start = data[0].pose.position
pp.moveTo(start.x / self.resolution, start.y / self.resolution)
for pose in data:
pt = pose.pose.position
pp.lineTo(pt.x / self.resolution, pt.y / self.resolution)
path.path = pp
self.path_changed.emit(name)
path.color = random.choice(self._colors)
self._colors.remove(path.color)
path.cb = c
path.sub = rospy.Subscriber(path.name, Path, path.cb)
self._paths[name] = path
def add_polygon(self, name):
poly = PathInfo(name)
def c(msg):
if not self._map:
return
if not (msg.header.frame_id == '/map' or msg.header.frame_id == ''):
try:
self._tf.waitForTransform(msg.header.frame_id, '/map', rospy.Time(), rospy.Duration(10))
points_stamped = []
for pt in msg.polygon.points:
ps = PointStamped()
ps.header.frame_id = msg.header.frame_id
ps.point.x = pt.x
ps.point.y = pt.y
points_stamped.append(ps)
trans_pts = [self._tf.transformPoint('/map', pt).point for pt in points_stamped]
except tf.Exception:
rospy.logerr("TF Error")
trans_pts = []
else:
trans_pts = [pt for pt in msg.polygon.points]
if len(trans_pts) > 0:
pts = [QPointF(pt.x / self.resolution, pt.y / self.resolution) for pt in trans_pts]
close = trans_pts[0]
pts.append(QPointF(close.x / self.resolution, close.y / self.resolution))
poly.path = QPolygonF(pts)
self.polygon_changed.emit(name)
poly.color = random.choice(self._colors)
self._colors.remove(poly.color)
poly.cb = c
poly.sub = rospy.Subscriber(poly.name, PolygonStamped, poly.cb)
self._polygons[name] = poly
def pose_mode(self):
if not self._pose_mode:
self._pose_mode = True
self._goal_mode = False
self.setDragMode(QGraphicsView.NoDrag)
elif self._pose_mode:
self._pose_mode = False
self.setDragMode(QGraphicsView.ScrollHandDrag)
def goal_mode(self):
if not self._goal_mode:
self._goal_mode = True
self._pose_mode = False
self.setDragMode(QGraphicsView.NoDrag)
elif self._goal_mode:
self._goal_mode = False
self.setDragMode(QGraphicsView.ScrollHandDrag)
def draw_position(self, e, mirror=True):
p = self.mapToScene(e.x(), e.y())
v = (p.x() - self.drag_start[0], p.y() - self.drag_start[1])
mag = sqrt(pow(v[0], 2) + pow(v[1], 2))
u = (v[0]/mag, v[1]/mag)
res = (u[0]*20, u[1]*20)
path = self._scene.addLine(self.drag_start[0], self.drag_start[1],
self.drag_start[0] + res[0], self.drag_start[1] + res[1])
if self.last_path:
self._scene.removeItem(self.last_path)
self.last_path = None
self.last_path = path
if mirror:
# Mirror point over x axis
x = ((self.w / 2) - self.drag_start[0]) + (self.w /2)
else:
x = self.drag_start[0]
map_p = [x * self.resolution, self.drag_start[1] * self.resolution]
angle = atan(u[1] / u[0])
quat = quaternion_from_euler(0, 0, degrees(angle))
return map_p, quat
def mousePressEvent(self, e):
if self._goal_mode or self._pose_mode:
p = self.mapToScene(e.x(), e.y())
self.drag_start = (p.x(), p.y())
else:
super(NavView, self).mousePressEvent(e)
def mouseReleaseEvent(self, e):
if self._goal_mode:
self._goal_mode = False
map_p, quat = self.draw_position(e)
msg = PoseStamped()
msg.header.frame_id = '/map'
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = map_p[0]
msg.pose.position.y = map_p[1]
msg.pose.orientation.w = quat[0]
msg.pose.orientation.z = quat[3]
self._goal_pub.publish(msg)
elif self._pose_mode:
self._pose_mode = False
map_p, quat = self.draw_position(e)
msg = PoseWithCovarianceStamped()
msg.header.frame_id = '/map'
msg.header.stamp = rospy.Time.now()
#TODO: Is it ok to just ignore the covariance matrix here?
msg.pose.pose.orientation.w = quat[0]
msg.pose.pose.orientation.z = quat[3]
msg.pose.pose.position.x = map_p[0]
msg.pose.pose.position.y = map_p[1]
self._pose_pub.publish(msg)
# Clean up the path
if self.last_path:
self._scene.removeItem(self.last_path)
self.last_path = None
#def mouseMoveEvent(self, e):
# if e.buttons() == Qt.LeftButton and (self._pose_mode or self._goal_mode):
# map_p, quat = self.draw_position(e)
def close(self):
if self.map_sub:
self.map_sub.unregister()
for p in self._paths.itervalues():
if p.sub:
p.sub.unregister()
for p in self._polygons.itervalues():
if p.sub:
p.sub.unregister()
super(NavView, self).close()
def _update(self):
if self._map_item:
self._scene.removeItem(self._map_item)
pixmap = QPixmap.fromImage(self._map)
self._map_item = self._scene.addPixmap(pixmap)
# Everything must be mirrored
self._mirror(self._map_item)
# Add drag and drop functionality
self.add_dragdrop(self._map_item)
self.centerOn(self._map_item)
self.show()
def _update_path(self, name):
old_item = None
if name in self._paths.keys():
old_item = self._paths[name].item
self._paths[name].item = self._scene.addPath(self._paths[name].path, pen=QPen(QColor(*self._paths[name].color)))
# Everything must be mirrored
self._mirror(self._paths[name].item)
if old_item:
self._scene.removeItem(old_item)
def _update_polygon(self, name):
old_item = None
if name in self._polygons.keys():
old_item = self._polygons[name].item
self._polygons[name].item = self._scene.addPolygon(self._polygons[name].path, pen=QPen(QColor(*self._polygons[name].color)))
# Everything must be mirrored
self._mirror(self._polygons[name].item)
if old_item:
self._scene.removeItem(old_item)
def _mirror(self, item):
item.scale(-1, 1)
item.translate(-self.w, 0)
def save_settings(self, plugin_settings, instance_settings):
# TODO add any settings to be saved
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO add any settings to be restored
pass
class NavView(QGraphicsView):
map_changed = Signal()
path_changed = Signal(str)
polygon_changed = Signal(str)
def __init__(self, map_topic='/map',
paths=['/move_base/SBPLLatticePlanner/plan', '/move_base/TrajectoryPlannerROS/local_plan'],
polygons=['/move_base/local_costmap/robot_footprint'], tf=None, parent=None):
super(NavView, self).__init__()
self._parent = parent
self._pose_mode = False
self._goal_mode = False
self.last_path = None
self.map_changed.connect(self._update)
self.destroyed.connect(self.close)
#ScrollHandDrag
self.setDragMode(QGraphicsView.ScrollHandDrag)
self._map = None
self._map_item = None
self.w = 0
self.h = 0
self._paths = {}
self._polygons = {}
self.path_changed.connect(self._update_path)
self.polygon_changed.connect(self._update_polygon)
self._colors = [(238, 34, 116), (68, 134, 252), (236, 228, 46), (102, 224, 18), (242, 156, 6), (240, 64, 10), (196, 30, 250)]
self._scene = QGraphicsScene()
if tf is None:
self._tf = tf.TransformListener()
else:
self._tf = tf
self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_cb)
for path in paths:
self.add_path(path)
for poly in polygons:
self.add_polygon(poly)
self._pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped)
self._goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped)
self.setScene(self._scene)
def add_dragdrop(self, item):
# Add drag and drop functionality to all the items in the view
def c(x, e):
self.dragEnterEvent(e)
def d(x, e):
self.dropEvent(e)
item.setAcceptDrops(True)
item.dragEnterEvent = c
item.dropEvent = d
def dragEnterEvent(self, e):
if self._parent:
self._parent.dragEnterEvent(e)
def dropEvent(self, e):
if self._parent:
self._parent.dropEvent(e)
def wheelEvent(self, event):
event.ignore()
if event.delta() > 0:
self.scale(1.15, 1.15)
else:
self.scale(0.85, 0.85)
def map_cb(self, msg):
self.resolution = msg.info.resolution
self.w = msg.info.width
self.h = msg.info.height
a = numpy.array(msg.data, dtype=numpy.uint8, copy=False, order='C')
a = a.reshape((self.h, self.w))
if self.w % 4:
e = numpy.empty((self.h, 4 - self.w % 4), dtype=a.dtype, order='C')
a = numpy.append(a, e, axis=1)
image = QImage(a.reshape((a.shape[0] * a.shape[1])), self.w, self.h, QImage.Format_Indexed8)
for i in reversed(range(101)):
image.setColor(100 - i, qRgb(i* 2.55, i * 2.55, i * 2.55))
image.setColor(101, qRgb(255, 0, 0)) # not used indices
image.setColor(255, qRgb(200, 200, 200)) # color for unknown value -1
self._map = image
self.setSceneRect(0, 0, self.w, self.h)
self.map_changed.emit()
def add_path(self, name):
path = PathInfo(name)
def c(msg):
if not self._map:
return
pp = QPainterPath()
# Transform everything in to the map frame
if not (msg.header.frame_id == '/map' or msg.header.frame_id == ''):
try:
self._tf.waitForTransform(msg.header.frame_id, '/map', rospy.Time(), rospy.Duration(10))
data = [self._tf.transformPose('/map', pose) for pose in msg.poses]
except tf.Exception:
rospy.logerr("TF Error")
data = []
else:
data = msg.poses
if len(data) > 0:
start = data[0].pose.position
pp.moveTo(start.x / self.resolution, start.y / self.resolution)
for pose in data:
pt = pose.pose.position
pp.lineTo(pt.x / self.resolution, pt.y / self.resolution)
path.path = pp
self.path_changed.emit(name)
path.color = random.choice(self._colors)
self._colors.remove(path.color)
path.cb = c
path.sub = rospy.Subscriber(path.name, Path, path.cb)
self._paths[name] = path
def add_polygon(self, name):
poly = PathInfo(name)
def c(msg):
if not self._map:
return
if not (msg.header.frame_id == '/map' or msg.header.frame_id == ''):
try:
self._tf.waitForTransform(msg.header.frame_id, '/map', rospy.Time(), rospy.Duration(10))
points_stamped = []
for pt in msg.polygon.points:
ps = PointStamped()
ps.header.frame_id = msg.header.frame_id
ps.point.x = pt.x
ps.point.y = pt.y
points_stamped.append(ps)
trans_pts = [self._tf.transformPoint('/map', pt).point for pt in points_stamped]
except tf.Exception:
rospy.logerr("TF Error")
trans_pts = []
else:
trans_pts = [pt for pt in msg.polygon.points]
if len(trans_pts) > 0:
pts = [QPointF(pt.x / self.resolution, pt.y / self.resolution) for pt in trans_pts]
close = trans_pts[0]
pts.append(QPointF(close.x / self.resolution, close.y / self.resolution))
poly.path = QPolygonF(pts)
self.polygon_changed.emit(name)
poly.color = random.choice(self._colors)
self._colors.remove(poly.color)
poly.cb = c
poly.sub = rospy.Subscriber(poly.name, PolygonStamped, poly.cb)
self._polygons[name] = poly
def pose_mode(self):
if not self._pose_mode:
self._pose_mode = True
self._goal_mode = False
self.setDragMode(QGraphicsView.NoDrag)
elif self._pose_mode:
self._pose_mode = False
self.setDragMode(QGraphicsView.ScrollHandDrag)
def goal_mode(self):
if not self._goal_mode:
self._goal_mode = True
self._pose_mode = False
self.setDragMode(QGraphicsView.NoDrag)
elif self._goal_mode:
self._goal_mode = False
self.setDragMode(QGraphicsView.ScrollHandDrag)
def draw_position(self, e, mirror=True):
p = self.mapToScene(e.x(), e.y())
v = (p.x() - self.drag_start[0], p.y() - self.drag_start[1])
mag = sqrt(pow(v[0], 2) + pow(v[1], 2))
u = (v[0]/mag, v[1]/mag)
res = (u[0]*20, u[1]*20)
path = self._scene.addLine(self.drag_start[0], self.drag_start[1],
self.drag_start[0] + res[0], self.drag_start[1] + res[1])
if self.last_path:
self._scene.removeItem(self.last_path)
self.last_path = None
self.last_path = path
if mirror:
# Mirror point over x axis
x = ((self.w / 2) - self.drag_start[0]) + (self.w /2)
else:
x = self.drag_start[0]
map_p = [x * self.resolution, self.drag_start[1] * self.resolution]
angle = atan(u[1] / u[0])
quat = quaternion_from_euler(0, 0, degrees(angle))
return map_p, quat
def mousePressEvent(self, e):
if self._goal_mode or self._pose_mode:
p = self.mapToScene(e.x(), e.y())
self.drag_start = (p.x(), p.y())
else:
super(NavView, self).mousePressEvent(e)
def mouseReleaseEvent(self, e):
if self._goal_mode:
self._goal_mode = False
map_p, quat = self.draw_position(e)
msg = PoseStamped()
msg.header.frame_id = '/map'
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = map_p[0]
msg.pose.position.y = map_p[1]
msg.pose.orientation.w = quat[0]
msg.pose.orientation.z = quat[3]
self._goal_pub.publish(msg)
elif self._pose_mode:
self._pose_mode = False
map_p, quat = self.draw_position(e)
msg = PoseWithCovarianceStamped()
msg.header.frame_id = '/map'
msg.header.stamp = rospy.Time.now()
#TODO: Is it ok to just ignore the covariance matrix here?
msg.pose.pose.orientation.w = quat[0]
msg.pose.pose.orientation.z = quat[3]
msg.pose.pose.position.x = map_p[0]
msg.pose.pose.position.y = map_p[1]
self._pose_pub.publish(msg)
# Clean up the path
if self.last_path:
self._scene.removeItem(self.last_path)
self.last_path = None
#def mouseMoveEvent(self, e):
# if e.buttons() == Qt.LeftButton and (self._pose_mode or self._goal_mode):
# map_p, quat = self.draw_position(e)
def close(self):
if self.map_sub:
self.map_sub.unregister()
for p in self._paths.itervalues():
if p.sub:
p.sub.unregister()
for p in self._polygons.itervalues():
if p.sub:
p.sub.unregister()
super(NavView, self).close()
def _update(self):
if self._map_item:
self._scene.removeItem(self._map_item)
pixmap = QPixmap.fromImage(self._map)
self._map_item = self._scene.addPixmap(pixmap)
# Everything must be mirrored
self._mirror(self._map_item)
# Add drag and drop functionality
self.add_dragdrop(self._map_item)
self.centerOn(self._map_item)
self.show()
def _update_path(self, name):
old_item = None
if name in self._paths.keys():
old_item = self._paths[name].item
self._paths[name].item = self._scene.addPath(self._paths[name].path, pen=QPen(QColor(*self._paths[name].color)))
# Everything must be mirrored
self._mirror(self._paths[name].item)
if old_item:
self._scene.removeItem(old_item)
def _update_polygon(self, name):
old_item = None
if name in self._polygons.keys():
old_item = self._polygons[name].item
self._polygons[name].item = self._scene.addPolygon(self._polygons[name].path, pen=QPen(QColor(*self._polygons[name].color)))
# Everything must be mirrored
self._mirror(self._polygons[name].item)
if old_item:
self._scene.removeItem(old_item)
def _mirror(self, item):
item.scale(-1, 1)
item.translate(-self.w, 0)
def save_settings(self, plugin_settings, instance_settings):
# TODO add any settings to be saved
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO add any settings to be restored
pass
class ImageView(TopicMessageView):
"""
Popup image viewer
"""
name = 'Image'
def __init__(self, timeline, parent):
super(ImageView, self).__init__(timeline, parent)
self._image = None
self._image_topic = None
self._image_stamp = None
self.quality = Image.NEAREST # quality hint for scaling
# TODO put the image_topic and image_stamp on the picture or display them in some fashion
self._overlay_font_size = 14.0
self._overlay_indent = (4, 4)
self._overlay_color = (0.2, 0.2, 1.0)
self._image_view = QGraphicsView(parent)
self._image_view.resizeEvent = self._resizeEvent
self._scene = QGraphicsScene()
self._image_view.setScene(self._scene)
parent.layout().addWidget(self._image_view)
# MessageView implementation
def _resizeEvent(self, event):
# TODO make this smarter. currently there will be no scrollbar even if the timeline extends beyond the viewable area
self._scene.setSceneRect(0, 0,
self._image_view.size().width() - 2,
self._image_view.size().height() - 2)
self.put_image_into_scene()
def message_viewed(self, bag, msg_details):
"""
refreshes the image
"""
TopicMessageView.message_viewed(self, bag, msg_details)
topic, msg, t = msg_details[:3]
if not msg:
self.set_image(None, topic, stamp)
else:
self.set_image(msg, topic, msg.header.stamp)
def message_cleared(self):
TopicMessageView.message_cleared(self)
self.set_image(None, None, None)
# End MessageView implementation
def put_image_into_scene(self):
if self._image:
resized_image = self._image.resize(
(self._image_view.size().width() - 2,
self._image_view.size().height() - 2), self.quality)
QtImage = ImageQt.ImageQt(resized_image)
pixmap = QPixmap.fromImage(QtImage)
self._scene.clear()
self._scene.addPixmap(pixmap)
def set_image(self, image_msg, image_topic, image_stamp):
self._image_msg = image_msg
if image_msg:
self._image = image_helper.imgmsg_to_pil(image_msg)
else:
self._image = None
self._image_topic = image_topic
self._image_stamp = image_stamp
self.put_image_into_scene()
class ImageView(TopicMessageView):
"""
Popup image viewer
"""
name = 'Image'
def __init__(self, timeline, parent, topic):
super(ImageView, self).__init__(timeline, parent, topic)
self._image = None
self._image_topic = None
self._image_stamp = None
self.quality = Image.NEAREST # quality hint for scaling
# TODO put the image_topic and image_stamp on the picture or display them in some fashion
self._overlay_font_size = 14.0
self._overlay_indent = (4, 4)
self._overlay_color = (0.2, 0.2, 1.0)
self._image_view = QGraphicsView(parent)
self._image_view.resizeEvent = self._resizeEvent
self._scene = QGraphicsScene()
self._image_view.setScene(self._scene)
parent.layout().addWidget(self._image_view)
# MessageView implementation
def _resizeEvent(self, event):
# TODO make this smarter. currently there will be no scrollbar even if the timeline extends beyond the viewable area
self._scene.setSceneRect(0, 0, self._image_view.size().width() - 2, self._image_view.size().height() - 2)
self.put_image_into_scene()
def message_viewed(self, bag, msg_details):
"""
refreshes the image
"""
TopicMessageView.message_viewed(self, bag, msg_details)
topic, msg, t = msg_details[:3]
if not msg:
self.set_image(None, topic, 'no message')
else:
self.set_image(msg, topic, msg.header.stamp)
def message_cleared(self):
TopicMessageView.message_cleared(self)
self.set_image(None, None, None)
# End MessageView implementation
def put_image_into_scene(self):
if self._image:
resized_image = self._image.resize((self._image_view.size().width() - 2, self._image_view.size().height() - 2), self.quality)
QtImage = ImageQt(resized_image)
pixmap = QPixmap.fromImage(QtImage)
self._scene.clear()
self._scene.addPixmap(pixmap)
def set_image(self, image_msg, image_topic, image_stamp):
self._image_msg = image_msg
if image_msg:
self._image = image_helper.imgmsg_to_pil(image_msg)
else:
self._image = None
self._image_topic = image_topic
self._image_stamp = image_stamp
self.put_image_into_scene()
class NavView(QGraphicsView):
map_changed = Signal()
path_changed = Signal(str)
polygon_changed = Signal(str)
def __init__(self, map_topic='/map',
paths=['/move_base/SBPLLatticePlanner/plan', '/move_base/TrajectoryPlannerROS/local_plan'],
polygons=['/move_base/local_costmap/robot_footprint']):
super(NavView, self).__init__()
self.map_changed.connect(self._update)
self.destroyed.connect(self.close)
#ScrollHandDrag
self.setDragMode(QGraphicsView.ScrollHandDrag)
self._map = None
self._map_item = None
self.w = 0
self.h = 0
self._paths = {}
self._polygons = {}
self.path_changed.connect(self._update_path)
self.polygon_changed.connect(self._update_polygon)
self._colors = [(238, 34, 116), (68, 134, 252), (236, 228, 46), (102, 224, 18), (242, 156, 6), (240, 64, 10), (196, 30, 250)]
self._scene = QGraphicsScene()
self._tf = tf.TransformListener()
self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_cb)
for path in paths:
self.add_path(path)
for poly in polygons:
self.add_polygon(poly)
self.setScene(self._scene)
def wheelEvent(self, event):
event.ignore()
if event.delta() > 0:
self.scale(1.15, 1.15)
else:
self.scale(0.85, 0.85)
def map_cb(self, msg):
self.resolution = msg.info.resolution
self.w = msg.info.width
self.h = msg.info.height
a = numpy.array(msg.data, dtype=numpy.uint8, copy=False, order='C')
a = a.reshape((self.h, self.w))
if self.w % 4:
e = numpy.empty((self.h, 4 - self.w % 4), dtype=a.dtype, order='C')
a = numpy.append(a, e, axis=1)
image = QImage(a.reshape((a.shape[0] * a.shape[1])), self.w, self.h, QImage.Format_Indexed8)
for i in range(101):
image.setColor(i, qRgb(i * 2.55, i * 2.55, i * 2.55))
image.setColor(101, qRgb(255, 0, 0)) # not used indices
image.setColor(255, qRgb(0, 0, 150)) # color for unknown value -1
self._map = image
self.setSceneRect(0, 0, self.w, self.h)
self.map_changed.emit()
def add_path(self, name):
path = PathInfo(name)
def c(msg):
pp = QPainterPath()
# Transform everything in to the map frame
if not (msg.header.frame_id == '/map' or msg.header.frame_id == ''):
try:
self._tf.waitForTransform(msg.header.frame_id, '/map', rospy.Time(), rospy.Duration(10))
data = [self._tf.transformPose('/map', pose) for pose in msg.poses]
except tf.Exception:
rospy.logerr("TF Error")
data = []
else:
data = msg.poses
if len(data) > 0:
start = data[0].pose.position
pp.moveTo(start.x / self.resolution, start.y / self.resolution)
for pose in data:
pt = pose.pose.position
pp.lineTo(pt.x / self.resolution, pt.y / self.resolution)
path.path = pp
self.path_changed.emit(name)
path.color = random.choice(self._colors)
self._colors.remove(path.color)
path.cb = c
path.sub = rospy.Subscriber(path.name, Path, path.cb)
self._paths[name] = path
def add_polygon(self, name):
poly = PathInfo(name)
def c(msg):
if not (msg.header.frame_id == '/map' or msg.header.frame_id == ''):
try:
self._tf.waitForTransform(msg.header.frame_id, '/map', rospy.Time(), rospy.Duration(10))
points_stamped = []
for pt in msg.polygon.points:
ps = PointStamped()
ps.header.frame_id = msg.header.frame_id
ps.point.x = pt.x
ps.point.y = pt.y
points_stamped.append(ps)
trans_pts = [self._tf.transformPoint('/map', pt).point for pt in points_stamped]
except tf.Exception:
rospy.logerr("TF Error")
trans_pts = []
else:
trans_pts = [pt for pt in msg.polygon.points]
if len(trans_pts) > 0:
pts = [QPointF(pt.x / self.resolution, pt.y / self.resolution) for pt in trans_pts]
close = trans_pts[0]
pts.append(QPointF(close.x / self.resolution, close.y / self.resolution))
poly.path = QPolygonF(pts)
self.polygon_changed.emit(name)
poly.color = random.choice(self._colors)
self._colors.remove(poly.color)
poly.cb = c
poly.sub = rospy.Subscriber(poly.name, PolygonStamped, poly.cb)
self._polygons[name] = poly
def close(self):
if self.map_sub:
self.map_sub.unregister()
for p in self._paths.itervalues():
if p.sub:
p.sub.unregister()
for p in self._polygons.itervalues():
if p.sub:
p.sub.unregister()
def _update(self):
if self._map_item:
self._scene.removeItem(self._map_item)
pixmap = QPixmap.fromImage(self._map)
self._map_item = self._scene.addPixmap(pixmap)
# Everything must be mirrored
self._mirror(self._map_item)
self.centerOn(self._map_item)
self.show()
def _update_path(self, name):
old_item = None
if name in self._paths.keys():
old_item = self._paths[name].item
self._paths[name].item = self._scene.addPath(self._paths[name].path, pen=QPen(QColor(*self._paths[name].color)))
# Everything must be mirrored
self._mirror(self._paths[name].item)
if old_item:
self._scene.removeItem(old_item)
def _update_polygon(self, name):
old_item = None
if name in self._polygons.keys():
old_item = self._polygons[name].item
self._polygons[name].item = self._scene.addPolygon(self._polygons[name].path, pen=QPen(QColor(*self._polygons[name].color)))
# Everything must be mirrored
self._mirror(self._polygons[name].item)
if old_item:
self._scene.removeItem(old_item)
def _mirror(self, item):
item.scale(-1, 1)
item.translate(-self.w, 0)
def __init__(self, context):
super(RosPackGraph, self).__init__(context)
self.initialized = False
self._current_dotcode = None
self._update_thread = WorkerThread(self._update_thread_run, self._update_finished)
self._nodes = {}
self._edges = {}
self._options = {}
self._options_serialized = ''
self.setObjectName('RosPackGraph')
rospack = rospkg.RosPack()
rosstack = rospkg.RosStack()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosPackageGraphDotcodeGenerator(rospack, rosstack)
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_dep'), 'resource', 'RosPackGraph.ui')
loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosPackGraphUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.depth_combo_box.insertItem(0, self.tr('infinite'), -1)
self._widget.depth_combo_box.insertItem(1, self.tr('1'), 2)
self._widget.depth_combo_box.insertItem(2, self.tr('2'), 3)
self._widget.depth_combo_box.insertItem(3, self.tr('3'), 4)
self._widget.depth_combo_box.insertItem(4, self.tr('4'), 5)
self._widget.depth_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph)
self._widget.directions_combo_box.insertItem(0, self.tr('depends'), 0)
self._widget.directions_combo_box.insertItem(1, self.tr('depends_on'), 1)
self._widget.directions_combo_box.insertItem(2, self.tr('both'), 2)
self._widget.directions_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph)
self._widget.package_type_combo_box.insertItem(0, self.tr('wet & dry'), 3)
self._widget.package_type_combo_box.insertItem(1, self.tr('wet only'), 2)
self._widget.package_type_combo_box.insertItem(2, self.tr('dry only'), 1)
self._widget.package_type_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph)
completionmodel = StackageCompletionModel(self._widget.filter_line_edit, rospack, rosstack)
completer = RepeatedWordCompleter(completionmodel, self)
completer.setCompletionMode(QCompleter.PopupCompletion)
completer.setWrapAround(True)
completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.filter_line_edit.editingFinished.connect(self._refresh_rospackgraph)
self._widget.filter_line_edit.setCompleter(completer)
self._widget.filter_line_edit.selectionChanged.connect(self._clear_filter)
self._widget.with_stacks_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.mark_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.colorize_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.hide_transitives_check_box.clicked.connect(self._refresh_rospackgraph)
self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(self._update_rospackgraph)
self._widget.highlight_connections_check_box.toggled.connect(self._refresh_rospackgraph)
self._widget.auto_fit_graph_check_box.toggled.connect(self._refresh_rospackgraph)
self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(self._save_image)
self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
# If in either of following case, this turnes True
# - 1st filtering key is already input by user
# - filtering key is restored
self._filtering_started = False
class GraphWidget(QWidget):
@staticmethod
def get_unique_name(context):
return ("Decision Graph (%d)" % context.serial_number()) if context.serial_number() > 1 else "Decision Graph"
@staticmethod
def get_file_name(absolute_path):
return ".".join(path.basename(absolute_path).split(".")[:-1])
def __init__(self, ros_package):
super(GraphWidget, self).__init__()
self._current_graph = None
self._lock = Lock()
self._load_ui(ros_package)
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
factory = DmgItemFactory()
factory.set_color(QColor(50, 50, 50))
factory.set_highlighted_color(QColor(0, 150, 0))
self._dot_to_qt = DotToQtGenerator(factory)
self.initialized = False
self.setObjectName("GraphWidget")
self.graphics_view.setScene(self._scene)
self.open_button.setIcon(QIcon.fromTheme("document-open"))
self.open_button.pressed.connect(self._import)
self.export_button.setIcon(QIcon.fromTheme("document-export"))
self.export_button.pressed.connect(self._export)
self.fit_to_view_button.setIcon(QIcon.fromTheme("zoom-fit-best"))
self.fit_to_view_button.pressed.connect(self._fit_to_view)
self.decision_graphs_combo_box.setSizeAdjustPolicy(QComboBox.AdjustToMinimumContentsLength)
self.decision_graphs_combo_box.currentIndexChanged["QString"].connect(self._graph_item_changed)
self._dot_processor = DotProcessor(self._dot_to_qt)
self.decision_graphs = dict()
self.states = dict()
def update(self, message):
data = self._get_data_from_message(message)
key = self._get_key(data)
if key not in self.decision_graphs:
try:
self._add_graph(key, data)
print "INFO: Graph has been added"
except GraphParseException as ex:
print "ERROR: Failed to load graph: %s", ex.message
else:
self.states[key] = data["name"], data["status"]
if self.decision_graphs[key].graph_id != message.status[0].values[-1].value:
self.decision_graphs[key].graph_id = message.status[0].values[-1].value
print "INFO: Graph id has been changed"
elif self._current_graph == self.decision_graphs[key]:
if not self._update_graph(data["name"], data["status"]):
print "WARNING: Failed to find appropriate graph for update"
def _load_ui(self, ros_package):
user_interface_file = path.join(ros_package.get_path("rqt_decision_graph"), "resource", "DecisionGraph.ui")
loadUi(user_interface_file, self, {"InteractiveGraphicsView": InteractiveGraphicsView})
def _import(self):
file_path, _ = QFileDialog.getOpenFileName(
self, self.tr("Import custom graph"), None, self.tr("DOT graph (*.dot)")
)
if file_path is None or file_path == "":
return
custom_graph = Graph(self._dot_processor, file_path, file_path)
self.decision_graphs[custom_graph.source] = custom_graph
self._current_graph = custom_graph
self.decision_graphs_combo_box.addItem(custom_graph.source)
self.decision_graphs_combo_box.setCurrentIndex(self.decision_graphs_combo_box.findText(custom_graph.source))
# Export graph as image
def _export(self):
file_name, _ = QFileDialog.getSaveFileName(
self, self.tr("Save as image"), "graph.png", self.tr("Image (*.bmp *.jpg *.png *.tiff)")
)
if file_name is None or file_name == "":
return
img = QImage((self._scene.sceneRect().size() * 2.0).toSize(), QImage.Format_ARGB32_Premultiplied)
painter = QPainter(img)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
img.save(file_name)
def _add_graph(self, key, data):
self._lock.acquire()
decision_graph = DecisionGraph(
data["name"].split("/")[1],
data["node_run_id"],
data["node_name"],
data["node_exe_file"],
data["node_exe_dir"],
self._dot_processor,
key,
)
self.decision_graphs[key] = decision_graph
self.decision_graphs_combo_box.addItem(key)
self._lock.release()
def _reset_graph_state(self, name, status):
if self._current_graph is not None:
for node in self._current_graph.nodes.values():
if name[: len(node.url)] == node.url:
node.highlight(True) if "started" == status else node.highlight(False)
def _update_graph(self, name, status):
self._lock.acquire()
is_updated = False
if self._current_graph is not None:
for node in self._current_graph.nodes.values():
if "started" == status and name[: len(node.url)] == node.url:
node.highlight(True)
is_updated = True
elif "stopped" == status and name == node.url:
node.highlight(False)
is_updated = True
self._lock.release()
return is_updated
def _graph_item_changed(self, event):
self._lock.acquire()
if event in self.decision_graphs:
self._current_graph = self.decision_graphs[event]
self._redraw_graph_view()
self._fit_to_view()
if isinstance(self._current_graph, DecisionGraph):
state = self.states.get(self._current_graph.key, None)
if state is not None:
self._reset_graph_state(state[0], state[1])
self._lock.release()
def _get_data_from_message(self, message):
return {value.key: value.value for value in message.status[0].values}
def _get_key(self, data):
return data["name"].split("/")[1] + data["node_name"]
def _redraw_graph_view(self):
self._current_graph.load()
self._scene.clear()
for node_item in self._current_graph.nodes.itervalues():
self._scene.addItem(node_item)
for edge_items in self._current_graph.edges.itervalues():
for edge_item in edge_items:
edge_item.add_to_scene(self._scene)
self._scene.setSceneRect(self._scene.itemsBoundingRect())
def _fit_to_view(self):
self.graphics_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio)
class QMapView(QGraphicsView):
map_changed = Signal()
def __init__(self, parent=None):
super(QMapView, self).__init__()
self._parent = parent
self._scene = QGraphicsScene()
self.map_changed.connect(self._update_map)
self.destroyed.connect(self.close)
# ScrollHandDrag
self.setDragMode(QGraphicsView.ScrollHandDrag)
self._map = None
self.w = 0
self.h = 0
self._map_item = {}
self._polygons = {}
self._colors = [(238, 34, 116), (68, 134, 252), (236, 228, 46), (102, 224, 18), (242, 156, 6), (240, 64, 10), (196, 30, 250)]
self.setScene(self._scene)
def add_dragdrop(self, item):
# Add drag and drop functionality to all the items in the view
def c(x, e):
self.dragEnterEvent(e)
def d(x, e):
self.dropEvent(e)
item.setAcceptDrops(True)
item.dragEnterEvent = c
item.dropEvent = d
def dragEnterEvent(self, e):
if self._parent:
self._parent.dragEnterEvent(e)
def dropEvent(self, e):
if self._parent:
self._parent.dropEvent(e)
def wheelEvent(self, event):
event.ignore()
if event.delta() > 0:
self.scale(1.15, 1.15)
else:
self.scale(0.85, 0.85)
@pyqtSlot(nav_msgs.OccupancyGrid, name='map_received')
def map_cb(self, msg):
self.resolution = msg.info.resolution
self.w = msg.info.width
self.h = msg.info.height
a = numpy.array(msg.data, dtype=numpy.uint8, copy=False, order='C')
a = a.reshape((self.h, self.w))
if self.w % 4:
e = numpy.empty((self.h, 4 - self.w % 4), dtype=a.dtype, order='C')
a = numpy.append(a, e, axis=1)
image = QImage(a.reshape((a.shape[0] * a.shape[1])), self.w, self.h, QImage.Format_Indexed8)
for i in reversed(range(101)):
image.setColor(100 - i, qRgb(i * 2.55, i * 2.55, i * 2.55))
image.setColor(101, qRgb(255, 0, 0)) # not used indices
image.setColor(255, qRgb(200, 200, 200)) # color for unknown value -1
self._map = image
self.setSceneRect(0, 0, self.w, self.h)
self.map_changed.emit()
def _update_map(self):
if self._map_item:
self._scene.removeItem(self._map_item)
pixmap = QPixmap.fromImage(self._map)
self._map_item = self._scene.addPixmap(pixmap)
# Everything must be mirrored
self._mirror(self._map_item)
# Add drag and drop functionality
self.add_dragdrop(self._map_item)
self.centerOn(self._map_item)
self.show()
def _mirror(self, item):
item.scale(-1, 1)
item.translate(-self.w, 0)
def save_settings(self, plugin_settings, instance_settings):
# TODO add any settings to be saved
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO add any settings to be restored
pass
class RosTfTree(QObject):
_deferred_fit_in_view = Signal()
def __init__(self, context):
super(RosTfTree, self).__init__(context)
self.initialized = False
self.setObjectName('RosTfTree')
self._current_dotcode = None
self._widget = QWidget()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosTfTreeDotcodeGenerator()
self.tf2_buffer_ = tf2_ros.Buffer()
self.tf2_listener_ = tf2_ros.TransformListener(self.tf2_buffer_)
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_tf_tree'), 'resource', 'RosTfTree.ui')
loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosTfTreeUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(self._update_tf_graph)
self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view)
self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image-x-generic'))
self._widget.save_as_image_push_button.pressed.connect(self._save_image)
self._deferred_fit_in_view.connect(self._fit_in_view,
Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
self._force_refresh = False
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('auto_fit_graph_check_box_state',
self._widget.auto_fit_graph_check_box.isChecked())
instance_settings.set_value('highlight_connections_check_box_state',
self._widget.highlight_connections_check_box.isChecked())
def restore_settings(self, plugin_settings, instance_settings):
self._widget.auto_fit_graph_check_box.setChecked(
instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true'])
self._widget.highlight_connections_check_box.setChecked(
instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true'])
self.initialized = True
self._refresh_tf_graph()
def _update_tf_graph(self):
self._force_refresh = True
self._refresh_tf_graph()
def _refresh_tf_graph(self):
if not self.initialized:
return
self._update_graph_view(self._generate_dotcode())
def _generate_dotcode(self):
force_refresh = self._force_refresh
self._force_refresh = False
rospy.wait_for_service('~tf2_frames')
tf2_frame_srv = rospy.ServiceProxy('~tf2_frames', FrameGraph)
return self.dotcode_generator.generate_dotcode(dotcode_factory=self.dotcode_factory,
tf2_frame_srv=tf2_frame_srv,
force_refresh=force_refresh)
def _update_graph_view(self, dotcode):
if dotcode == self._current_dotcode:
return
self._current_dotcode = dotcode
self._redraw_graph_view()
def _generate_tool_tip(self, url):
return url
def _redraw_graph_view(self):
self._scene.clear()
if self._widget.highlight_connections_check_box.isChecked():
highlight_level = 3
else:
highlight_level = 1
(nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode,
highlight_level)
for node_item in nodes.itervalues():
self._scene.addItem(node_item)
for edge_items in edges.itervalues():
for edge_item in edge_items:
edge_item.add_to_scene(self._scene)
self._scene.setSceneRect(self._scene.itemsBoundingRect())
if self._widget.auto_fit_graph_check_box.isChecked():
self._fit_in_view()
def _load_dot(self, file_name=None):
if file_name is None:
file_name, _ = QFileDialog.getOpenFileName(
self._widget,
self.tr('Open graph from file'),
None,
self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
try:
fhandle = open(file_name, 'rb')
dotcode = fhandle.read()
fhandle.close()
except IOError:
return
self._update_graph_view(dotcode)
def _fit_in_view(self):
self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(),
Qt.KeepAspectRatio)
def _save_dot(self):
file_name, _ = QFileDialog.getSaveFileName(self._widget,
self.tr('Save as DOT'),
'frames.dot',
self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
file = QFile(file_name)
if not file.open(QIODevice.WriteOnly | QIODevice.Text):
return
file.write(self._current_dotcode)
file.close()
def _save_svg(self):
file_name, _ = QFileDialog.getSaveFileName(
self._widget,
self.tr('Save as SVG'),
'frames.svg',
self.tr('Scalable Vector Graphic (*.svg)'))
if file_name is None or file_name == '':
return
generator = QSvgGenerator()
generator.setFileName(file_name)
generator.setSize((self._scene.sceneRect().size() * 2.0).toSize())
painter = QPainter(generator)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
def _save_image(self):
file_name, _ = QFileDialog.getSaveFileName(
self._widget,
self.tr('Save as image'),
'frames.png',
self.tr('Image (*.bmp *.jpg *.png *.tiff)'))
if file_name is None or file_name == '':
return
img = QImage((self._scene.sceneRect().size() * 2.0).toSize(),
QImage.Format_ARGB32_Premultiplied)
painter = QPainter(img)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
img.save(file_name)
class TimelineWidget(QWidget):
class TimelineView(QGraphicsView):
def __init__(self, parent):
super(TimelineWidget.TimelineView, self).__init__()
self.parent = parent
def mouseReleaseEvent(self, event):
self.parent.mouse_release(event)
update = pyqtSignal()
def __init__(self, parent):
super(TimelineWidget, self).__init__()
self.parent = parent
self._layout = QHBoxLayout()
#self._view = QGraphicsView()
self._view = TimelineWidget.TimelineView(self)
self._scene = QGraphicsScene()
self._colors = [QColor('green'), QColor('yellow'), QColor('red')]
self._messages = [None for x in range(20)]
self._mq = [1 for x in range(20)]
self._view.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
self._view.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
self._view.setScene(self._scene)
self._layout.addWidget(self._view, 1)
self.pause_button = QPushButton('Pause')
self.pause_button.setCheckable(True)
self.pause_button.clicked.connect(self.pause)
self._layout.addWidget(self.pause_button)
self.setLayout(self._layout)
self.update.connect(self.redraw)
def redraw(self):
self._scene.clear()
self._scene
for i, m in enumerate(self._mq):
w = float(self._view.viewport().width())/len(self._mq)
h = self._view.viewport().height()
rect = self._scene.addRect(w*i, 0, w, h, QColor('black'), self._colors[m])
def mouse_release(self, event):
i = int(floor(event.x()/(float(self._view.viewport().width())/len(self._mq))))
msg = self._messages[i]
if msg:
self.parent.pause(msg)
if not self.pause_button.isChecked():
self.pause_button.toggle()
def resizeEvent(self, event):
self.redraw()
def get_worst(self, msg):
lvl = 0
for status in msg.status:
if status.level > lvl:
lvl = status.level
return lvl
def add_message(self, msg):
self._messages = self._messages[1:]
self._messages.append(msg)
self._mq = self._mq[1:]
try:
lvl = msg.level
except AttributeError:
lvl = self.get_worst(msg)
if lvl > 2:
lvl = 2
self._mq.append(lvl)
self.update.emit()
def pause(self, state):
if state:
self.parent.pause(self._messages[-1])
else:
self.parent.unpause()
def __init__(self, context):
super(RosBehaviourTree, self).__init__(context)
self.setObjectName('RosBehaviourTree')
parser = argparse.ArgumentParser()
RosBehaviourTree.add_arguments(parser, False)
# if the context doesn't have an argv attribute then assume we're running with --no-roscore
if not hasattr(context, 'argv'):
args = sys.argv[1:]
# Can run the viewer with or without live updating. Running without is
# intended for viewing of bags only
self.live_update = False
else:
args = context.argv()
self.live_update = True
parsed_args = parser.parse_args(args)
self.context = context
self.initialized = False
self._current_dotcode = None # dotcode for the tree that is currently displayed
self._viewing_bag = False # true if a bag file is loaded
# True if next or previous buttons are pressed. Reset if the tree being
# viewed is the last one in the list.
self._browsing_timeline = False
self._widget = QWidget()
# factory builds generic dotcode items
self.dotcode_factory = PygraphvizFactory() # PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosBehaviourTreeDotcodeGenerator()
self.current_topic = None
self.behaviour_sub = None
self._tip_message = None # message of the tip of the tree
self._saved_settings_topic = None # topic subscribed to by previous instance
self.visibility_level = py_trees.common.VisibilityLevel.DETAIL
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_py_trees'), 'resource',
'RosBehaviourTree.ui')
loadUi(ui_file, self._widget,
{'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosBehaviourTreeUi')
if hasattr(context, 'serial_number') and context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.highlight_connections_check_box.toggled.connect(
self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(
self._redraw_graph_view)
self._widget.fit_in_view_push_button.setIcon(
QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_bag_push_button.setIcon(
QIcon.fromTheme('document-open'))
self._widget.load_bag_push_button.pressed.connect(self._load_bag)
self._widget.load_dot_push_button.setIcon(
QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(
QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(
self._save_image)
for text in visibility.combo_to_py_trees:
self._widget.visibility_level_combo_box.addItem(text)
self._widget.visibility_level_combo_box.setCurrentIndex(
self.visibility_level)
self._widget.visibility_level_combo_box.currentIndexChanged[
'QString'].connect(self._update_visibility_level)
# set up the function that is called whenever the box is resized -
# ensures that the timeline is correctly drawn.
self._widget.resizeEvent = self._resize_event
self._timeline = None
self._timeline_listener = None
# Connect the message changed function of this object to a corresponding
# signal. This signal will be activated whenever the message being
# viewed changes.
self._message_changed.connect(self.message_changed)
self._message_cleared.connect(self.message_cleared)
# Set up combo box for topic selection
# when the refresh_combo signal happens, update the combo topics available
self._refresh_combo.connect(self._update_combo_topics)
# filter events to catch the event which opens the combo box
self._combo_event_filter = RosBehaviourTree.ComboBoxEventFilter(
self._refresh_combo)
self._widget.topic_combo_box.installEventFilter(
self._combo_event_filter)
self._widget.topic_combo_box.activated.connect(self._choose_topic)
self._update_combo_topics()
# Set up navigation buttons
self._widget.previous_tool_button.pressed.connect(self._previous)
self._widget.previous_tool_button.setIcon(
QIcon.fromTheme('go-previous'))
self._widget.next_tool_button.pressed.connect(self._next)
self._widget.next_tool_button.setIcon(QIcon.fromTheme('go-next'))
self._widget.first_tool_button.pressed.connect(self._first)
self._widget.first_tool_button.setIcon(QIcon.fromTheme('go-first'))
self._widget.last_tool_button.pressed.connect(self._last)
self._widget.last_tool_button.setIcon(QIcon.fromTheme('go-last'))
# play, pause and stop buttons
self._widget.play_tool_button.pressed.connect(self._play)
self._widget.play_tool_button.setIcon(
QIcon.fromTheme('media-playback-start'))
self._widget.stop_tool_button.pressed.connect(self._stop)
self._widget.stop_tool_button.setIcon(
QIcon.fromTheme('media-playback-stop'))
# also connect the navigation buttons so that they stop the timer when
# pressed while the tree is playing.
self._widget.first_tool_button.pressed.connect(self._stop)
self._widget.previous_tool_button.pressed.connect(self._stop)
self._widget.last_tool_button.pressed.connect(self._stop)
self._widget.next_tool_button.pressed.connect(self._stop)
# set up shortcuts for navigation (vim)
next_shortcut_vi = QShortcut(QKeySequence("l"), self._widget)
next_shortcut_vi.activated.connect(
self._widget.next_tool_button.pressed)
previous_shortcut_vi = QShortcut(QKeySequence("h"), self._widget)
previous_shortcut_vi.activated.connect(
self._widget.previous_tool_button.pressed)
first_shortcut_vi = QShortcut(QKeySequence("^"), self._widget)
first_shortcut_vi.activated.connect(
self._widget.first_tool_button.pressed)
last_shortcut_vi = QShortcut(QKeySequence("$"), self._widget)
last_shortcut_vi.activated.connect(
self._widget.last_tool_button.pressed)
# shortcuts for emacs
next_shortcut_emacs = QShortcut(QKeySequence("Ctrl+f"), self._widget)
next_shortcut_emacs.activated.connect(
self._widget.next_tool_button.pressed)
previous_shortcut_emacs = QShortcut(QKeySequence("Ctrl+b"),
self._widget)
previous_shortcut_emacs.activated.connect(
self._widget.previous_tool_button.pressed)
first_shortcut_emacs = QShortcut(QKeySequence("Ctrl+a"), self._widget)
first_shortcut_emacs.activated.connect(
self._widget.first_tool_button.pressed)
last_shortcut_emacs = QShortcut(QKeySequence("Ctrl+e"), self._widget)
last_shortcut_emacs.activated.connect(
self._widget.last_tool_button.pressed)
# set up stuff for dotcode cache
self._dotcode_cache_capacity = 50
self._dotcode_cache = {}
# cache is ordered on timestamps from messages, but earliest timestamp
# isn't necessarily the message that was viewed the longest time ago, so
# need to store keys
self._dotcode_cache_keys = []
# set up stuff for scene cache (dotcode cache doesn't seem to make much difference)
self._scene_cache_capacity = 50
self._scene_cache = {}
self._scene_cache_keys = []
# Update the timeline buttons to correspond with a completely
# uninitialised state.
self._set_timeline_buttons(first_snapshot=False,
previous_snapshot=False,
next_snapshot=False,
last_snapshot=False)
self._deferred_fit_in_view.connect(self._fit_in_view,
Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
# This is used to store a timer which controls how fast updates happen when the play button is pressed.
self._play_timer = None
# updates the view
self._refresh_view.connect(self._refresh_tree_graph)
self._force_refresh = False
if self.live_update:
context.add_widget(self._widget)
else:
self.initialized = True # this needs to be set for trees to be displayed
context.setCentralWidget(self._widget)
if parsed_args.bag:
self._load_bag(parsed_args.bag)
elif parsed_args.latest_bag:
# if the latest bag is requested, load it from the default directory, or
# the one specified in the args
bag_dir = parsed_args.bag_dir or os.getenv(
'ROS_HOME', os.path.expanduser('~/.ros')) + '/behaviour_trees'
self.open_latest_bag(bag_dir, parsed_args.by_time)
class CapabilityGraph(Plugin):
__deferred_fit_in_view = Signal()
def __init__(self, context):
super(CapabilityGraph, self).__init__(context)
self.setObjectName('CapabilityGraph')
self.__current_dotcode = None
self.__widget = QWidget()
self.__dot_to_qt = DotToQtGenerator()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_capabilities'), 'resources', 'CapabilityGraph.ui')
loadUi(ui_file, self.__widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self.__widget.setObjectName('CapabilityGraphUI')
if context.serial_number() > 1:
self.__widget.setWindowTitle(self.__widget.windowTitle() + (' (%d)' % context.serial_number()))
self.__scene = QGraphicsScene()
self.__scene.setBackgroundBrush(Qt.white)
self.__widget.graphics_view.setScene(self.__scene)
self.__widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self.__widget.refresh_graph_push_button.pressed.connect(self.__update_capabilities_graph)
self.__update_capabilities_graph()
self.__deferred_fit_in_view.connect(self.__fit_in_view, Qt.QueuedConnection)
self.__deferred_fit_in_view.emit()
context.add_widget(self.__widget)
def __update_capabilities_graph(self):
self.__update_graph_view(self.__generate_dotcode())
def __generate_dotcode(self):
return generate_dotcode_from_capability_info()
def __update_graph_view(self, dotcode):
if dotcode == self.__current_dotcode:
return
self.__current_dotcode = dotcode
self.__redraw_graph_view()
def __fit_in_view(self):
self.__widget.graphics_view.fitInView(self.__scene.itemsBoundingRect(), Qt.KeepAspectRatio)
def __redraw_graph_view(self):
self.__scene.clear()
highlight_level = 1
# layout graph and create qt items
(nodes, edges) = self.__dot_to_qt.dotcode_to_qt_items(self.__current_dotcode,
highlight_level=highlight_level,
same_label_siblings=True)
for node_item in nodes.itervalues():
self.__scene.addItem(node_item)
for edge_items in edges.itervalues():
for edge_item in edge_items:
edge_item.add_to_scene(self.__scene)
self.__scene.setSceneRect(self.__scene.itemsBoundingRect())
self.__fit_in_view()
class RosBehaviourTree(QObject):
_deferred_fit_in_view = Signal()
_refresh_view = Signal()
_refresh_combo = Signal()
_message_changed = Signal()
_message_cleared = Signal()
_expected_type = py_trees_msgs.BehaviourTree()._type
_empty_topic = "No valid topics available"
_unselected_topic = "Not subscribing"
no_roscore_switch = "--no-roscore"
class ComboBoxEventFilter(QObject):
"""Event filter for the combo box. Will filter left mouse button presses,
calling a signal when they happen
"""
def __init__(self, signal):
"""
:param Signal signal: signal that is emitted when a left mouse button press happens
"""
super(RosBehaviourTree.ComboBoxEventFilter, self).__init__()
self.signal = signal
def eventFilter(self, obj, event):
if event.type() == QEvent.MouseButtonPress and event.button(
) == Qt.LeftButton:
self.signal.emit()
return False
def __init__(self, context):
super(RosBehaviourTree, self).__init__(context)
self.setObjectName('RosBehaviourTree')
parser = argparse.ArgumentParser()
RosBehaviourTree.add_arguments(parser, False)
# if the context doesn't have an argv attribute then assume we're running with --no-roscore
if not hasattr(context, 'argv'):
args = sys.argv[1:]
# Can run the viewer with or without live updating. Running without is
# intended for viewing of bags only
self.live_update = False
else:
args = context.argv()
self.live_update = True
parsed_args = parser.parse_args(args)
self.context = context
self.initialized = False
self._current_dotcode = None # dotcode for the tree that is currently displayed
self._viewing_bag = False # true if a bag file is loaded
# True if next or previous buttons are pressed. Reset if the tree being
# viewed is the last one in the list.
self._browsing_timeline = False
self._widget = QWidget()
# factory builds generic dotcode items
self.dotcode_factory = PygraphvizFactory() # PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosBehaviourTreeDotcodeGenerator()
self.current_topic = None
self.behaviour_sub = None
self._tip_message = None # message of the tip of the tree
self._saved_settings_topic = None # topic subscribed to by previous instance
self.visibility_level = py_trees.common.VisibilityLevel.DETAIL
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_py_trees'), 'resource',
'RosBehaviourTree.ui')
loadUi(ui_file, self._widget,
{'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosBehaviourTreeUi')
if hasattr(context, 'serial_number') and context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.highlight_connections_check_box.toggled.connect(
self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(
self._redraw_graph_view)
self._widget.fit_in_view_push_button.setIcon(
QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_bag_push_button.setIcon(
QIcon.fromTheme('document-open'))
self._widget.load_bag_push_button.pressed.connect(self._load_bag)
self._widget.load_dot_push_button.setIcon(
QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(
QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(
self._save_image)
for text in visibility.combo_to_py_trees:
self._widget.visibility_level_combo_box.addItem(text)
self._widget.visibility_level_combo_box.setCurrentIndex(
self.visibility_level)
self._widget.visibility_level_combo_box.currentIndexChanged[
'QString'].connect(self._update_visibility_level)
# set up the function that is called whenever the box is resized -
# ensures that the timeline is correctly drawn.
self._widget.resizeEvent = self._resize_event
self._timeline = None
self._timeline_listener = None
# Connect the message changed function of this object to a corresponding
# signal. This signal will be activated whenever the message being
# viewed changes.
self._message_changed.connect(self.message_changed)
self._message_cleared.connect(self.message_cleared)
# Set up combo box for topic selection
# when the refresh_combo signal happens, update the combo topics available
self._refresh_combo.connect(self._update_combo_topics)
# filter events to catch the event which opens the combo box
self._combo_event_filter = RosBehaviourTree.ComboBoxEventFilter(
self._refresh_combo)
self._widget.topic_combo_box.installEventFilter(
self._combo_event_filter)
self._widget.topic_combo_box.activated.connect(self._choose_topic)
self._update_combo_topics()
# Set up navigation buttons
self._widget.previous_tool_button.pressed.connect(self._previous)
self._widget.previous_tool_button.setIcon(
QIcon.fromTheme('go-previous'))
self._widget.next_tool_button.pressed.connect(self._next)
self._widget.next_tool_button.setIcon(QIcon.fromTheme('go-next'))
self._widget.first_tool_button.pressed.connect(self._first)
self._widget.first_tool_button.setIcon(QIcon.fromTheme('go-first'))
self._widget.last_tool_button.pressed.connect(self._last)
self._widget.last_tool_button.setIcon(QIcon.fromTheme('go-last'))
# play, pause and stop buttons
self._widget.play_tool_button.pressed.connect(self._play)
self._widget.play_tool_button.setIcon(
QIcon.fromTheme('media-playback-start'))
self._widget.stop_tool_button.pressed.connect(self._stop)
self._widget.stop_tool_button.setIcon(
QIcon.fromTheme('media-playback-stop'))
# also connect the navigation buttons so that they stop the timer when
# pressed while the tree is playing.
self._widget.first_tool_button.pressed.connect(self._stop)
self._widget.previous_tool_button.pressed.connect(self._stop)
self._widget.last_tool_button.pressed.connect(self._stop)
self._widget.next_tool_button.pressed.connect(self._stop)
# set up shortcuts for navigation (vim)
next_shortcut_vi = QShortcut(QKeySequence("l"), self._widget)
next_shortcut_vi.activated.connect(
self._widget.next_tool_button.pressed)
previous_shortcut_vi = QShortcut(QKeySequence("h"), self._widget)
previous_shortcut_vi.activated.connect(
self._widget.previous_tool_button.pressed)
first_shortcut_vi = QShortcut(QKeySequence("^"), self._widget)
first_shortcut_vi.activated.connect(
self._widget.first_tool_button.pressed)
last_shortcut_vi = QShortcut(QKeySequence("$"), self._widget)
last_shortcut_vi.activated.connect(
self._widget.last_tool_button.pressed)
# shortcuts for emacs
next_shortcut_emacs = QShortcut(QKeySequence("Ctrl+f"), self._widget)
next_shortcut_emacs.activated.connect(
self._widget.next_tool_button.pressed)
previous_shortcut_emacs = QShortcut(QKeySequence("Ctrl+b"),
self._widget)
previous_shortcut_emacs.activated.connect(
self._widget.previous_tool_button.pressed)
first_shortcut_emacs = QShortcut(QKeySequence("Ctrl+a"), self._widget)
first_shortcut_emacs.activated.connect(
self._widget.first_tool_button.pressed)
last_shortcut_emacs = QShortcut(QKeySequence("Ctrl+e"), self._widget)
last_shortcut_emacs.activated.connect(
self._widget.last_tool_button.pressed)
# set up stuff for dotcode cache
self._dotcode_cache_capacity = 50
self._dotcode_cache = {}
# cache is ordered on timestamps from messages, but earliest timestamp
# isn't necessarily the message that was viewed the longest time ago, so
# need to store keys
self._dotcode_cache_keys = []
# set up stuff for scene cache (dotcode cache doesn't seem to make much difference)
self._scene_cache_capacity = 50
self._scene_cache = {}
self._scene_cache_keys = []
# Update the timeline buttons to correspond with a completely
# uninitialised state.
self._set_timeline_buttons(first_snapshot=False,
previous_snapshot=False,
next_snapshot=False,
last_snapshot=False)
self._deferred_fit_in_view.connect(self._fit_in_view,
Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
# This is used to store a timer which controls how fast updates happen when the play button is pressed.
self._play_timer = None
# updates the view
self._refresh_view.connect(self._refresh_tree_graph)
self._force_refresh = False
if self.live_update:
context.add_widget(self._widget)
else:
self.initialized = True # this needs to be set for trees to be displayed
context.setCentralWidget(self._widget)
if parsed_args.bag:
self._load_bag(parsed_args.bag)
elif parsed_args.latest_bag:
# if the latest bag is requested, load it from the default directory, or
# the one specified in the args
bag_dir = parsed_args.bag_dir or os.getenv(
'ROS_HOME', os.path.expanduser('~/.ros')) + '/behaviour_trees'
self.open_latest_bag(bag_dir, parsed_args.by_time)
@Slot(str)
def _update_visibility_level(self, visibility_level):
"""
We match the combobox index to the visibility levels defined in py_trees.common.VisibilityLevel.
"""
self.visibility_level = visibility.combo_to_py_trees[visibility_level]
self._refresh_tree_graph()
@staticmethod
def add_arguments(parser, group=True):
"""Allows for the addition of arguments to the rqt_gui loading method
:param bool group: If set to false, this indicates that the function is
being called from the rqt_py_trees script as opposed to the inside
of rqt_gui.main. We use this to ensure that the same arguments can
be passed with and without the --no-roscore argument set. If it is
set, the rqt_gui code is bypassed. We need to make sure that all the
arguments are displayed with -h.
"""
operate_object = parser
if group:
operate_object = parser.add_argument_group(
'Options for the rqt_py_trees viewer')
operate_object.add_argument(
'bag',
action='store',
nargs='?',
help='Load this bag when the viewer starts')
operate_object.add_argument(
'-l',
'--latest-bag',
action='store_true',
help=
'Load the latest bag available in the bag directory. Bag files are expected to be under the bag directory in the following structure: year-month-day/behaviour_tree_hour-minute-second.bag. If this structure is not followed, the bag file which was most recently modified is used.'
)
operate_object.add_argument(
'--bag-dir',
action='store',
help=
'Specify the directory in which to look for bag files. The default is $ROS_HOME/behaviour_trees, if $ROS_HOME is set, or ~/.ros/behaviour_trees otherwise.'
)
operate_object.add_argument(
'-m',
'--by-time',
action='store_true',
help=
'The latest bag is defined by the time at which the file was last modified, rather than the date and time specified in the filename.'
)
operate_object.add_argument(
RosBehaviourTree.no_roscore_switch,
action='store_true',
help=
'Run the viewer without roscore. It is only possible to view bag files if this is set.'
)
def open_latest_bag(self, bag_dir, by_time=False):
"""Open the latest bag in the given directory
:param str bag_dir: the directory in which to look for bags
:param bool by_time: if true, the latest bag is the one with the latest
modification time, not the latest date-time specified by its filename
"""
if not os.path.isdir(bag_dir):
rospy.logwarn(
"Requested bag directory {0} is invalid. Latest bag will not be loaded."
.format(bag_dir))
return
files = []
for root, unused_dirnames, filenames in os.walk(bag_dir, topdown=True):
files.extend(
fnmatch.filter(map(lambda p: os.path.join(root, p), filenames),
'*.bag'))
if not files:
rospy.logwarn(
"No files with extension .bag found in directory {0}".format(
bag_dir))
return
if not by_time:
# parse the file list with a regex to get only those which have the
# format year-month-day/behaviour_tree_hour-minute-second.bag
re_str = '.*\/\d{4}-\d{2}-\d{2}\/behaviour_tree_\d{2}-\d{2}-\d{2}.bag'
expr = re.compile(re_str)
valid = filter(lambda f: expr.match(f), files)
# if no files match the regex, use modification time instead
if not valid:
by_time = True
else:
# dates are monotonically increasing, so the last one is the latest
latest_bag = sorted(valid)[-1]
if by_time:
latest_bag = sorted(files,
cmp=lambda x, y: cmp(os.path.getctime(x),
os.path.getctime(y)))[-1]
self._load_bag(latest_bag)
def get_current_message(self):
"""
Get the message in the list or bag that is being viewed that should be
displayed.
"""
msg = None
if self._timeline_listener:
try:
msg = self._timeline_listener.msg
except KeyError:
pass
return py_trees_msgs.BehaviourTree() if msg is None else msg
def _choose_topic(self, index):
"""Updates the topic that is subscribed to based on changes to the combo box
text. If the topic is unchanged, nothing will happnen. Otherwise, the
old subscriber will be unregistered, and a new one initialised for the
updated topic. If the selected topic corresponds to the unselected
topic, the subscriber will be unregistered and a new one will not be
created.
"""
selected_topic = self._widget.topic_combo_box.currentText()
if selected_topic != self._empty_topic and self.current_topic != selected_topic:
self.current_topic = selected_topic
# destroy the old timeline and clear the scene
if self._timeline:
self._timeline.handle_close()
self._widget.timeline_graphics_view.setScene(None)
if selected_topic != self._unselected_topic:
# set up a timeline to track the messages coming from the subscriber
self._set_dynamic_timeline()
def _update_combo_topics(self):
"""
Update the topics displayed in the combo box that the user can use to select
which topic they want to listen on for trees, filtered so that only
topics with the correct message type are shown.
"""
# Only update topics if we're running with live updating
if not self.live_update:
self._widget.topic_combo_box.setEnabled(False)
return
self._widget.topic_combo_box.clear()
topic_list = rospy.get_published_topics()
valid_topics = []
for topic_path, topic_type in topic_list:
if topic_type == RosBehaviourTree._expected_type:
valid_topics.append(topic_path)
if not valid_topics:
self._widget.topic_combo_box.addItem(RosBehaviourTree._empty_topic)
return
# always add an item which does nothing so that it is possible to listen to nothing.
self._widget.topic_combo_box.addItem(
RosBehaviourTree._unselected_topic)
for topic in valid_topics:
self._widget.topic_combo_box.addItem(topic)
# if the topic corresponds to the one that was active the last time
# the viewer was run, automatically set that one as the one we look
# at
if topic == self._saved_settings_topic:
self._widget.topic_combo_box.setCurrentIndex(
self._widget.topic_combo_box.count() - 1)
self._choose_topic(self._widget.topic_combo_box.currentIndex())
def _set_timeline_buttons(self,
first_snapshot=None,
previous_snapshot=None,
next_snapshot=None,
last_snapshot=None):
"""
Allows timeline buttons to be enabled and disabled.
"""
if first_snapshot is not None:
self._widget.first_tool_button.setEnabled(first_snapshot)
if previous_snapshot is not None:
self._widget.previous_tool_button.setEnabled(previous_snapshot)
if next_snapshot is not None:
self._widget.next_tool_button.setEnabled(next_snapshot)
if last_snapshot is not None:
self._widget.last_tool_button.setEnabled(last_snapshot)
def _play(self):
"""
Start a timer which will automatically call the next function every time its
duration is up. Only works if the current message is not the final one.
"""
if not self._play_timer:
self._play_timer = rospy.Timer(rospy.Duration(1), self._timer_next)
def _timer_next(self, timer):
"""
Helper function for the timer so that it can call the next function without
breaking.
"""
self._next()
def _stop(self):
"""Stop the play timer, if it exists.
"""
if self._play_timer:
self._play_timer.shutdown()
self._play_timer = None
def _first(self):
"""Navigate to the first message. Activates the next and last buttons, disables
first and previous, and refreshes the view. Also changes the state to be
browsing the timeline.
"""
self._timeline.navigate_start()
self._set_timeline_buttons(first_snapshot=False,
previous_snapshot=False,
next_snapshot=True,
last_snapshot=True)
self._refresh_view.emit()
self._browsing_timeline = True
def _previous(self):
"""Navigate to the previous message. Activates the next and last buttons, and
refreshes the view. If the current message is the second message, then
the first and previous buttons are disabled. Changes the state to be
browsing the timeline.
"""
# if already at the beginning, do nothing
if self._timeline._timeline_frame.playhead == self._timeline._get_start_stamp(
):
return
# otherwise, go to the previous message
self._timeline.navigate_previous()
# now browsing the timeline
self._browsing_timeline = True
self._set_timeline_buttons(last_snapshot=True, next_snapshot=True)
# if now at the beginning, disable timeline buttons.
if self._timeline._timeline_frame.playhead == self._timeline._get_end_stamp(
):
self._set_timeline_buttons(next_snapshot=False,
last_snapshot=False)
self._refresh_view.emit()
def _last(self):
"""Navigate to the last message. Activates the first and previous buttons,
disables next and last, and refreshes the view. The user is no longer
browsing the timeline after this is called.
"""
self._timeline.navigate_end()
self._set_timeline_buttons(first_snapshot=True,
previous_snapshot=True,
next_snapshot=False,
last_snapshot=False)
self._refresh_view.emit()
self._browsing_timeline = False
self._new_messages = 0
def _next(self):
"""Navigate to the next message. Activates the first and previous buttons. If
the current message is the second from last, disables the next and last
buttons, and stops browsing the timeline.
"""
# if already at the end, do nothing
if self._timeline._timeline_frame.playhead == self._timeline._get_end_stamp(
):
return
# otherwise, go to the next message
self._timeline.navigate_next()
self._set_timeline_buttons(first_snapshot=True, previous_snapshot=True)
# if now at the end, disable timeline buttons and shutdown the play timer if active
if self._timeline._timeline_frame.playhead == self._timeline._get_end_stamp(
):
self._set_timeline_buttons(next_snapshot=False,
last_snapshot=False)
self._browsing_timeline = False
if self._play_timer:
self._play_timer.shutdown()
self._refresh_view.emit()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('visibility_level', self.visibility_level)
instance_settings.set_value(
'auto_fit_graph_check_box_state',
self._widget.auto_fit_graph_check_box.isChecked())
instance_settings.set_value(
'highlight_connections_check_box_state',
self._widget.highlight_connections_check_box.isChecked())
combo_text = self._widget.topic_combo_box.currentText()
if combo_text not in [self._empty_topic, self._unselected_topic]:
instance_settings.set_value('combo_box_subscribed_topic',
combo_text)
def restore_settings(self, plugin_settings, instance_settings):
try:
self._widget.auto_fit_graph_check_box.setChecked(
instance_settings.value('auto_fit_graph_check_box_state', True)
in [True, 'true'])
self._widget.highlight_connections_check_box.setChecked(
instance_settings.value(
'highlight_connections_check_box_state', True) in
[True, 'true'])
self._saved_settings_topic = instance_settings.value(
'combo_box_subscribed_topic', None)
saved_visibility_level = instance_settings.value(
'visibility_level', 1)
except TypeError as e:
self._widget.auto_fit_graph_check_box.setChecked(True)
self._widget.highlight_connections_check_box.setChecked(True)
self._saved_settings_topic = None
saved_visibility_level = 1
rospy.logerr(
"Rqt PyTrees: incompatible qt app configuration found, try removing ~/.config/ros.org/rqt_gui.ini"
)
rospy.logerr("Rqt PyTrees: %s" % e)
self._widget.visibility_level_combo_box.setCurrentIndex(
visibility.saved_setting_to_combo_index[saved_visibility_level])
self.initialized = True
self._update_combo_topics()
self._refresh_tree_graph()
def _refresh_tree_graph(self):
"""Refresh the graph view by regenerating the dotcode from the current message.
"""
if not self.initialized:
return
self._update_graph_view(
self._generate_dotcode(self.get_current_message()))
def _generate_dotcode(self, message):
"""
Get the dotcode for the given message, checking the cache for dotcode that
was previously generated, and adding to the cache if it wasn't there.
Cache replaces LRU.
Mostly stolen from rqt_bag.MessageLoaderThread
:param py_trees_msgs.BehavoiurTree message
"""
if message is None:
return ""
#######################################################
# Get the tip, from the perspective of the root
#######################################################
# this is pretty inefficient, and ignores caching
tip_id = None
self._tip_message = None
# reverse behaviour list - construction puts the root at the end (with
# visitor, at least)
for behaviour in reversed(message.behaviours):
# root has empty parent ID
if str(behaviour.parent_id) == str(uuid_msgs.UniqueID()):
# parent is the root behaviour, so
tip_id = behaviour.tip_id
# Run through the behaviours and do a couple of things:
# - get the tip
# - protect against feedback messages with quotes (https://bitbucket.org/yujinrobot/gopher_crazy_hospital/issues/72/rqt_py_trees-fails-to-display-tree)
if self._tip_message is None:
for behaviour in message.behaviours:
if str(behaviour.own_id) == str(tip_id):
self._tip_message = behaviour.message
if '"' in behaviour.message:
print("%s" % termcolor.colored(
'[ERROR] found double quotes in the feedback message [%s]'
% behaviour.message, 'red'))
behaviour.message = behaviour.message.replace('"', '')
print("%s" % termcolor.colored(
'[ERROR] stripped to stop from crashing, but do catch the culprit! [%s]'
% behaviour.message, 'red'))
key = str(message.header.stamp) # stamps are unique
if key in self._dotcode_cache:
return self._dotcode_cache[key]
force_refresh = self._force_refresh
self._force_refresh = False
visible_behaviours = visibility.filter_behaviours_by_visibility_level(
message.behaviours, self.visibility_level)
# cache miss
dotcode = self.dotcode_generator.generate_dotcode(
dotcode_factory=self.dotcode_factory,
behaviours=visible_behaviours,
timestamp=message.header.stamp,
force_refresh=force_refresh)
self._dotcode_cache[key] = dotcode
self._dotcode_cache_keys.append(key)
if len(self._dotcode_cache) > self._dotcode_cache_capacity:
oldest = self._dotcode_cache_keys[0]
del self._dotcode_cache[oldest]
self._dotcode_cache_keys.remove(oldest)
return dotcode
def _update_graph_view(self, dotcode):
if dotcode == self._current_dotcode:
return
self._current_dotcode = dotcode
self._redraw_graph_view()
def _redraw_graph_view(self):
key = str(self.get_current_message().header.stamp)
if key in self._scene_cache:
new_scene = self._scene_cache[key]
else: # cache miss
new_scene = QGraphicsScene()
new_scene.setBackgroundBrush(Qt.white)
if self._widget.highlight_connections_check_box.isChecked():
highlight_level = 3
else:
highlight_level = 1
# (nodes, edges) = self.dot_to_qt.graph_to_qt_items(self.dotcode_generator.graph,
# highlight_level)
# this function is very expensive
(nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(
self._current_dotcode, highlight_level)
for node_item in iter(nodes.values()):
new_scene.addItem(node_item)
for edge_items in iter(edges.values()):
for edge_item in edge_items:
edge_item.add_to_scene(new_scene)
new_scene.setSceneRect(new_scene.itemsBoundingRect())
# put the scene in the cache
self._scene_cache[key] = new_scene
self._scene_cache_keys.append(key)
if len(self._scene_cache) > self._scene_cache_capacity:
oldest = self._scene_cache_keys[0]
del self._scene_cache[oldest]
self._scene_cache_keys.remove(oldest)
# after construction, set the scene and fit to the view
self._scene = new_scene
self._widget.graphics_view.setScene(self._scene)
self._widget.message_label.setText(self._tip_message)
if self._widget.auto_fit_graph_check_box.isChecked():
self._fit_in_view()
def _resize_event(self, event):
"""Activated when the window is resized. Will re-fit the behaviour tree in the
window, and update the size of the timeline scene rectangle so that it
is correctly drawn.
"""
self._fit_in_view()
if self._timeline:
self._timeline.setSceneRect(
0, 0,
self._widget.timeline_graphics_view.width() - 2,
max(self._widget.timeline_graphics_view.height() - 2,
self._timeline._timeline_frame._history_bottom))
def timeline_changed(self):
"""Should be called whenever the timeline changes. At the moment this is only
used to ensure that the first and previous buttons are correctly
disabled when a new message coming in on the timeline pushes the
playhead to be at the first message
"""
if self._timeline._timeline_frame.playhead == self._timeline._get_start_stamp(
):
self._set_timeline_buttons(first_snapshot=False,
previous_snapshot=False)
else:
self._set_timeline_buttons(first_snapshot=True,
previous_snapshot=True)
def message_changed(self):
"""
This function should be called when the message being viewed changes. Will
change the current message and update the view. Also ensures that the
timeline buttons are correctly set for the current position of the
playhead on the timeline.
"""
if self._timeline._timeline_frame.playhead == self._timeline._get_end_stamp(
):
self._set_timeline_buttons(last_snapshot=False,
next_snapshot=False)
else:
self._set_timeline_buttons(last_snapshot=True, next_snapshot=True)
if self._timeline._timeline_frame.playhead == self._timeline._get_start_stamp(
):
self._set_timeline_buttons(first_snapshot=False,
previous_snapshot=False)
else:
self._set_timeline_buttons(first_snapshot=True,
previous_snapshot=True)
self._refresh_view.emit()
def message_cleared(self):
"""
This function should be called when the message being viewed was cleared.
Currently no situation where this happens?
"""
pass
def no_right_click_press_event(self, func):
"""Decorator for ignoring right click events on mouse press
"""
@functools.wraps(func)
def wrapper(event):
if event.type() == QEvent.MouseButtonPress and event.button(
) == Qt.RightButton:
event.ignore()
else:
func(event)
return wrapper
def _set_dynamic_timeline(self):
"""
Set the timeline to a dynamic timeline, listening to messages on the topic
selected in the combo box.
"""
self._timeline = DynamicTimeline(self, publish_clock=False)
# connect timeline events so that the timeline will update when events happen
self._widget.timeline_graphics_view.mousePressEvent = self.no_right_click_press_event(
self._timeline.on_mouse_down)
self._widget.timeline_graphics_view.mouseReleaseEvent = self._timeline.on_mouse_up
self._widget.timeline_graphics_view.mouseMoveEvent = self._timeline.on_mouse_move
self._widget.timeline_graphics_view.wheelEvent = self._timeline.on_mousewheel
self._widget.timeline_graphics_view.setScene(self._timeline)
# Don't show scrollbars - the timeline adjusts to the size of the view
self._widget.timeline_graphics_view.setHorizontalScrollBarPolicy(
Qt.ScrollBarAlwaysOff)
self._widget.timeline_graphics_view.setVerticalScrollBarPolicy(
Qt.ScrollBarAlwaysOff)
# Send a resize event so that the timeline knows the size of the view it's in
self._resize_event(None)
self._timeline.add_topic(self.current_topic,
py_trees_msgs.BehaviourTree)
# Create a listener for the timeline which will call the emit function
# on the given signals when the message being viewed changes or is
# cleared. The message being viewed changing generally happens when the
# user moves the slider around.
self._timeline_listener = DynamicTimelineListener(
self._timeline, self.current_topic, self._message_changed,
self._message_cleared)
# Need to add a listener to make sure that we can get information about
# messages that are on the topic that we're interested in.
self._timeline.add_listener(self.current_topic,
self._timeline_listener)
self._timeline.navigate_end()
self._timeline._redraw_timeline(None)
self._timeline.timeline_updated.connect(self.timeline_changed)
def _set_bag_timeline(self, bag):
"""Set the timeline of this object to a bag timeline, hooking the graphics view
into mouse and wheel functions of the timeline.
"""
self._timeline = BagTimeline(self, publish_clock=False)
# connect timeline events so that the timeline will update when events happen
self._widget.timeline_graphics_view.mousePressEvent = self.no_right_click_press_event(
self._timeline.on_mouse_down)
self._widget.timeline_graphics_view.mouseReleaseEvent = self._timeline.on_mouse_up
self._widget.timeline_graphics_view.mouseMoveEvent = self._timeline.on_mouse_move
self._widget.timeline_graphics_view.wheelEvent = self._timeline.on_mousewheel
self._widget.timeline_graphics_view.setScene(self._timeline)
# Don't show scrollbars - the timeline adjusts to the size of the view
self._widget.timeline_graphics_view.setHorizontalScrollBarPolicy(
Qt.ScrollBarAlwaysOff)
self._widget.timeline_graphics_view.setVerticalScrollBarPolicy(
Qt.ScrollBarAlwaysOff)
# Send a resize event so that the timeline knows the size of the view it's in
self._resize_event(None)
self._timeline.add_bag(bag)
# Create a listener for the timeline which will call the emit function
# on the given signals when the message being viewed changes or is
# cleared. The message being viewed changing generally happens when the
# user moves the slider around.
self._timeline_listener = TimelineListener(self._timeline,
self.current_topic,
self._message_changed,
self._message_cleared)
# Need to add a listener to make sure that we can get information about
# messages that are on the topic that we're interested in.
self._timeline.add_listener(self.current_topic,
self._timeline_listener)
# Go to the first message in the timeline of the bag.
self._timeline.navigate_start()
def _load_bag(self, file_name=None):
"""Load a bag from file. If no file name is given, a dialogue will pop up and
the user will be asked to select a file. If the bag file selected
doesn't have any valid topic, nothing will happen. If there are valid
topics, we load the bag and add a timeline for managing it.
"""
if file_name is None:
file_name, _ = QFileDialog.getOpenFileName(
self._widget, self.tr('Open trees from bag file'), None,
self.tr('ROS bag (*.bag)'))
if file_name is None or file_name == "":
return
rospy.loginfo("Reading bag from {0}".format(file_name))
bag = rosbag.Bag(file_name, 'r')
# ugh...
topics = list(bag.get_type_and_topic_info()[1].keys())
types = []
for i in range(0, len(bag.get_type_and_topic_info()[1].values())):
types.append(list(bag.get_type_and_topic_info()[1].values())[i][0])
tree_topics = [] # only look at the first matching topic
for ind, tp in enumerate(types):
if tp == 'py_trees_msgs/BehaviourTree':
tree_topics.append(topics[ind])
if len(tree_topics) == 0:
rospy.logerr('Requested bag did not contain any valid topics.')
return
self.message_list = []
self._viewing_bag = True
rospy.loginfo('Reading behaviour trees from topic {0}'.format(
tree_topics[0]))
for unused_topic, msg, unused_t in bag.read_messages(
topics=[tree_topics[0]]):
self.message_list.append(msg)
self.current_topic = tree_topics[0]
self._set_timeline_buttons(first_snapshot=True,
previous_snapshot=True,
next_snapshot=False,
last_snapshot=False)
self._set_bag_timeline(bag)
self._refresh_view.emit()
def _load_dot(self, file_name=None):
if file_name is None:
file_name, _ = QFileDialog.getOpenFileName(
self._widget, self.tr('Open tree from DOT file'), None,
self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
try:
fhandle = open(file_name, 'rb')
dotcode = fhandle.read()
fhandle.close()
except IOError:
return
self._update_graph_view(dotcode)
def _fit_in_view(self):
self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(),
Qt.KeepAspectRatio)
def _save_dot(self):
file_name, _ = QFileDialog.getSaveFileName(
self._widget, self.tr('Save as DOT'), 'frames.dot',
self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
dot_file = QFile(file_name)
if not dot_file.open(QIODevice.WriteOnly | QIODevice.Text):
return
dot_file.write(self._current_dotcode)
dot_file.close()
def _save_svg(self):
file_name, _ = QFileDialog.getSaveFileName(
self._widget, self.tr('Save as SVG'), 'frames.svg',
self.tr('Scalable Vector Graphic (*.svg)'))
if file_name is None or file_name == '':
return
generator = QSvgGenerator()
generator.setFileName(file_name)
generator.setSize((self._scene.sceneRect().size() * 2.0).toSize())
painter = QPainter(generator)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
def _save_image(self):
file_name, _ = QFileDialog.getSaveFileName(
self._widget, self.tr('Save as image'), 'frames.png',
self.tr('Image (*.bmp *.jpg *.png *.tiff)'))
if file_name is None or file_name == '':
return
img = QImage((self._scene.sceneRect().size() * 2.0).toSize(),
QImage.Format_ARGB32_Premultiplied)
painter = QPainter(img)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
img.save(file_name)
class QCameraView(QGraphicsView):
"""
Accepts an image of a teleop compressed image type and draws that in the
scene/view.
"""
def __init__(self, parent=None):
super(QCameraView, self).__init__(parent)
self.scene = QGraphicsScene(self)
self.setScene(self.scene)
self._load_default_image()
def _load_default_image(self):
joystick_icon = os.path.join(
rospkg.RosPack().get_path('rocon_bubble_icons'), 'icons',
'joystick.png')
pixmap = QPixmap(joystick_icon, format="png")
self.scene.addPixmap(pixmap)
self.scene.update()
self.fitInView(QRectF(0, 0, self.scene.width(), self.scene.height()),
Qt.KeepAspectRatio)
@pyqtSlot(sensor_msgs.CompressedImage, name='image_received')
def on_compressed_image_received(self, image):
'''
:param sensor_msgs.CompressedImage image: convert and display this in the QGraphicsView.
'''
if len(self.scene.items()) > 1:
self.scene.removeItem(self.scene.items()[0])
pixmap = QPixmap()
pixmap.loadFromData(image.data, format=image.format)
self.scene.addPixmap(pixmap)
self.scene.update()
# setting fitInvView seems sensitive to here or prior to scene update
self.fitInView(QRectF(0, 0, self.scene.width(), self.scene.height()),
Qt.KeepAspectRatio)
def resizeEvent(self, evt=None):
self.fitInView(QRectF(0, 0, self.scene.width(), self.scene.height()),
Qt.KeepAspectRatio)
def __init__(self, context):
super(RosGraph, self).__init__(context)
self.initialized = False
self.setObjectName('RosGraph')
self._graph = None
self._current_dotcode = None
self._widget = QWidget()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosGraphDotcodeGenerator()
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_graph'), 'resource', 'RosGraph.ui')
loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosGraphUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.graph_type_combo_box.insertItem(0, self.tr('Nodes only'), NODE_NODE_GRAPH)
self._widget.graph_type_combo_box.insertItem(1, self.tr('Nodes/Topics (active)'), NODE_TOPIC_GRAPH)
self._widget.graph_type_combo_box.insertItem(2, self.tr('Nodes/Topics (all)'), NODE_TOPIC_ALL_GRAPH)
self._widget.graph_type_combo_box.setCurrentIndex(0)
self._widget.graph_type_combo_box.currentIndexChanged.connect(self._refresh_rosgraph)
self.node_completionmodel = NamespaceCompletionModel(self._widget.filter_line_edit, False)
completer = RepeatedWordCompleter(self.node_completionmodel, self)
completer.setCompletionMode(QCompleter.PopupCompletion)
completer.setWrapAround(True)
completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.filter_line_edit.editingFinished.connect(self._refresh_rosgraph)
self._widget.filter_line_edit.setCompleter(completer)
self.topic_completionmodel = NamespaceCompletionModel(self._widget.topic_filter_line_edit, False)
topic_completer = RepeatedWordCompleter(self.topic_completionmodel, self)
topic_completer.setCompletionMode(QCompleter.PopupCompletion)
topic_completer.setWrapAround(True)
topic_completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.topic_filter_line_edit.editingFinished.connect(self._refresh_rosgraph)
self._widget.topic_filter_line_edit.setCompleter(topic_completer)
self._widget.namespace_cluster_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.actionlib_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.dead_sinks_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.leaf_topics_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.quiet_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(self._update_rosgraph)
self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view)
self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(self._save_image)
self._update_rosgraph()
self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
class GraphWidget(QWidget):
@staticmethod
def get_unique_name(context):
return ('Decision Graph (%d)' % context.serial_number()) if context.serial_number() > 1 else 'Decision Graph'
@staticmethod
def get_file_name(absolute_path):
return ".".join(path.basename(absolute_path).split('.')[:-1])
def __init__(self, ros_package):
super(GraphWidget, self).__init__()
self._current_graph = None
self._lock = Lock()
self._load_ui(ros_package)
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
factory = DmgItemFactory()
factory.set_color(QColor(50, 50, 50))
factory.set_highlighted_color(QColor(0, 150, 0))
self._dot_to_qt = DotToQtGenerator(factory)
self.initialized = False
self.setObjectName('GraphWidget')
self.graphics_view.setScene(self._scene)
self.open_button.setIcon(QIcon.fromTheme('document-open'))
self.open_button.pressed.connect(self._import)
self.export_button.setIcon(QIcon.fromTheme('document-export'))
self.export_button.pressed.connect(self._export)
self.fit_to_view_button.setIcon(QIcon.fromTheme('zoom-fit-best'))
self.fit_to_view_button.pressed.connect(self._fit_to_view)
self.decision_graphs_combo_box.setSizeAdjustPolicy(QComboBox.AdjustToMinimumContentsLength)
self.decision_graphs_combo_box.currentIndexChanged['QString'].connect(self._graph_item_changed)
self._dot_processor = DotProcessor(self._dot_to_qt)
self.decision_graphs = dict()
self.states = dict()
def update(self, message):
data = self._get_data_from_message(message)
key = self._get_key(data)
if key not in self.decision_graphs:
try:
self._add_graph(key, data)
print 'INFO: Graph has been added'
except GraphParseException as ex:
print 'ERROR: Failed to load graph: %s', ex.message
else:
self.states[key] = data['name'], data['status']
if self.decision_graphs[key].graph_id != message.status[0].values[-1].value:
self.decision_graphs[key].graph_id = message.status[0].values[-1].value
print 'INFO: Graph id has been changed'
elif self._current_graph == self.decision_graphs[key]:
if not self._update_graph(data['name'], data['status']):
print 'WARNING: Failed to find appropriate graph for update'
def _load_ui(self, ros_package):
user_interface_file = path.join(ros_package.get_path('rqt_decision_graph'), 'resource', 'DecisionGraph.ui')
loadUi(user_interface_file, self, {'InteractiveGraphicsView': InteractiveGraphicsView})
def _import(self):
file_path, _ = QFileDialog.getOpenFileName(self, self.tr('Import custom graph'),
None, self.tr('DOT graph (*.dot)'))
if file_path is None or file_path == '':
return
custom_graph = Graph(self._dot_processor, file_path, file_path)
self.decision_graphs[custom_graph.source] = custom_graph
self._current_graph = custom_graph
self.decision_graphs_combo_box.addItem(custom_graph.source)
self.decision_graphs_combo_box.setCurrentIndex(self.decision_graphs_combo_box.findText(custom_graph.source))
# Export graph as image
def _export(self):
file_name, _ = QFileDialog.getSaveFileName(self,
self.tr('Save as image'),
'graph.png',
self.tr('Image (*.bmp *.jpg *.png *.tiff)'))
if file_name is None or file_name == '':
return
img = QImage((self._scene.sceneRect().size() * 2.0).toSize(), QImage.Format_ARGB32_Premultiplied)
painter = QPainter(img)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
img.save(file_name)
def _add_graph(self, key, data):
self._lock.acquire()
decision_graph = DecisionGraph(data['name'].split('/')[1],
data['node_run_id'],
data['node_name'],
data['node_exe_file'],
data['node_exe_dir'],
self._dot_processor,
key)
self.decision_graphs[key] = decision_graph
self.decision_graphs_combo_box.addItem(key)
self._lock.release()
def _reset_graph_state(self, name, status):
if self._current_graph is not None:
for node in self._current_graph.nodes.values():
if name[:len(node.url)] == node.url:
node.highlight(True) if 'started' == status else node.highlight(False)
def _update_graph(self, name, status):
self._lock.acquire()
is_updated = False
if self._current_graph is not None:
for node in self._current_graph.nodes.values():
if 'started' == status and name[:len(node.url)] == node.url:
node.highlight(True)
is_updated = True
elif 'stopped' == status and name == node.url:
node.highlight(False)
is_updated = True
self._lock.release()
return is_updated
def _graph_item_changed(self, event):
self._lock.acquire()
if event in self.decision_graphs:
self._current_graph = self.decision_graphs[event]
self._redraw_graph_view()
self._fit_to_view()
if isinstance(self._current_graph, DecisionGraph):
state = self.states.get(self._current_graph.key, None)
if state is not None:
self._reset_graph_state(state[0], state[1])
self._lock.release()
def _get_data_from_message(self, message):
return {value.key: value.value for value in message.status[0].values}
def _get_key(self, data):
return data['name'].split('/')[1] + data['node_name']
def _redraw_graph_view(self):
self._current_graph.load()
self._scene.clear()
for node_item in self._current_graph.nodes.itervalues():
self._scene.addItem(node_item)
for edge_items in self._current_graph.edges.itervalues():
for edge_item in edge_items:
edge_item.add_to_scene(self._scene)
self._scene.setSceneRect(self._scene.itemsBoundingRect())
def _fit_to_view(self):
self.graphics_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio)
class RosGraph(Plugin):
_deferred_fit_in_view = Signal()
def __init__(self, context):
super(RosGraph, self).__init__(context)
self.initialized = False
self.setObjectName('RosGraph')
self._graph = None
self._current_dotcode = None
self._widget = QWidget()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosGraphDotcodeGenerator()
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'RosGraph.ui')
loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosGraphUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.graph_type_combo_box.insertItem(0, self.tr('Nodes only'), NODE_NODE_GRAPH)
self._widget.graph_type_combo_box.insertItem(1, self.tr('Nodes/Topics (active)'), NODE_TOPIC_GRAPH)
self._widget.graph_type_combo_box.insertItem(2, self.tr('Nodes/Topics (all)'), NODE_TOPIC_ALL_GRAPH)
self._widget.graph_type_combo_box.setCurrentIndex(0)
self._widget.graph_type_combo_box.currentIndexChanged.connect(self._refresh_rosgraph)
self.node_completionmodel = NamespaceCompletionModel(self._widget.filter_line_edit, False)
completer = RepeatedWordCompleter(self.node_completionmodel, self)
completer.setCompletionMode(QCompleter.PopupCompletion)
completer.setWrapAround(True)
completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.filter_line_edit.editingFinished.connect(self._refresh_rosgraph)
self._widget.filter_line_edit.setCompleter(completer)
self.topic_completionmodel = NamespaceCompletionModel(self._widget.topic_filter_line_edit, False)
topic_completer = RepeatedWordCompleter(self.topic_completionmodel, self)
topic_completer.setCompletionMode(QCompleter.PopupCompletion)
topic_completer.setWrapAround(True)
topic_completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.topic_filter_line_edit.editingFinished.connect(self._refresh_rosgraph)
self._widget.topic_filter_line_edit.setCompleter(topic_completer)
self._widget.namespace_cluster_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.actionlib_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.dead_sinks_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.leaf_topics_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.quiet_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(self._update_rosgraph)
self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view)
self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(self._save_image)
self._update_rosgraph()
self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('graph_type_combo_box_index', self._widget.graph_type_combo_box.currentIndex())
instance_settings.set_value('filter_line_edit_text', self._widget.filter_line_edit.text())
instance_settings.set_value('topic_filter_line_edit_text', self._widget.topic_filter_line_edit.text())
instance_settings.set_value('namespace_cluster_check_box_state', self._widget.namespace_cluster_check_box.isChecked())
instance_settings.set_value('actionlib_check_box_state', self._widget.actionlib_check_box.isChecked())
instance_settings.set_value('dead_sinks_check_box_state', self._widget.dead_sinks_check_box.isChecked())
instance_settings.set_value('leaf_topics_check_box_state', self._widget.leaf_topics_check_box.isChecked())
instance_settings.set_value('quiet_check_box_state', self._widget.quiet_check_box.isChecked())
instance_settings.set_value('auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked())
instance_settings.set_value('highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked())
def restore_settings(self, plugin_settings, instance_settings):
self._widget.graph_type_combo_box.setCurrentIndex(int(instance_settings.value('graph_type_combo_box_index', 0)))
self._widget.filter_line_edit.setText(instance_settings.value('filter_line_edit_text', '/'))
self._widget.topic_filter_line_edit.setText(instance_settings.value('topic_filter_line_edit_text', '/'))
self._widget.namespace_cluster_check_box.setChecked(instance_settings.value('namespace_cluster_check_box_state', True) in [True, 'true'])
self._widget.actionlib_check_box.setChecked(instance_settings.value('actionlib_check_box_state', True) in [True, 'true'])
self._widget.dead_sinks_check_box.setChecked(instance_settings.value('dead_sinks_check_box_state', True) in [True, 'true'])
self._widget.leaf_topics_check_box.setChecked(instance_settings.value('leaf_topics_check_box_state', True) in [True, 'true'])
self._widget.quiet_check_box.setChecked(instance_settings.value('quiet_check_box_state', True) in [True, 'true'])
self._widget.auto_fit_graph_check_box.setChecked(instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true'])
self._widget.highlight_connections_check_box.setChecked(instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true'])
self.initialized = True
self._refresh_rosgraph()
def _update_rosgraph(self):
# re-enable controls customizing fetched ROS graph
self._widget.graph_type_combo_box.setEnabled(True)
self._widget.filter_line_edit.setEnabled(True)
self._widget.topic_filter_line_edit.setEnabled(True)
self._widget.namespace_cluster_check_box.setEnabled(True)
self._widget.actionlib_check_box.setEnabled(True)
self._widget.dead_sinks_check_box.setEnabled(True)
self._widget.leaf_topics_check_box.setEnabled(True)
self._widget.quiet_check_box.setEnabled(True)
self._graph = rosgraph.impl.graph.Graph()
self._graph.set_master_stale(5.0)
self._graph.set_node_stale(5.0)
self._graph.update()
self.node_completionmodel.refresh(self._graph.nn_nodes)
self.topic_completionmodel.refresh(self._graph.nt_nodes)
self._refresh_rosgraph()
def _refresh_rosgraph(self):
if not self.initialized:
return
self._update_graph_view(self._generate_dotcode())
def _generate_dotcode(self):
ns_filter = self._widget.filter_line_edit.text()
topic_filter = self._widget.topic_filter_line_edit.text()
graph_mode = self._widget.graph_type_combo_box.itemData(self._widget.graph_type_combo_box.currentIndex())
orientation = 'LR'
if self._widget.namespace_cluster_check_box.isChecked():
namespace_cluster = 1
else:
namespace_cluster = 0
accumulate_actions = self._widget.actionlib_check_box.isChecked()
hide_dead_end_topics = self._widget.dead_sinks_check_box.isChecked()
hide_single_connection_topics = self._widget.leaf_topics_check_box.isChecked()
quiet = self._widget.quiet_check_box.isChecked()
return self.dotcode_generator.generate_dotcode(
rosgraphinst=self._graph,
ns_filter=ns_filter,
topic_filter=topic_filter,
graph_mode=graph_mode,
hide_single_connection_topics=hide_single_connection_topics,
hide_dead_end_topics=hide_dead_end_topics,
cluster_namespaces_level=namespace_cluster,
accumulate_actions=accumulate_actions,
dotcode_factory=self.dotcode_factory,
orientation=orientation,
quiet=quiet)
def _update_graph_view(self, dotcode):
if dotcode == self._current_dotcode:
return
self._current_dotcode = dotcode
self._redraw_graph_view()
def _generate_tool_tip(self, url):
if url is not None and ':' in url:
item_type, item_path = url.split(':', 1)
if item_type == 'node':
tool_tip = 'Node:\n %s' % (item_path)
service_names = rosservice.get_service_list(node=item_path)
if service_names:
tool_tip += '\nServices:'
for service_name in service_names:
try:
service_type = rosservice.get_service_type(service_name)
tool_tip += '\n %s [%s]' % (service_name, service_type)
except rosservice.ROSServiceIOException as e:
tool_tip += '\n %s' % (e)
return tool_tip
elif item_type == 'topic':
topic_type, topic_name, _ = rostopic.get_topic_type(item_path)
return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type)
return url
def _redraw_graph_view(self):
self._scene.clear()
if self._widget.highlight_connections_check_box.isChecked():
highlight_level = 3
else:
highlight_level = 1
# layout graph and create qt items
(nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode,
highlight_level=highlight_level,
same_label_siblings=True)
for node_item in nodes.itervalues():
self._scene.addItem(node_item)
for edge_items in edges.itervalues():
for edge_item in edge_items:
edge_item.add_to_scene(self._scene)
self._scene.setSceneRect(self._scene.itemsBoundingRect())
if self._widget.auto_fit_graph_check_box.isChecked():
self._fit_in_view()
def _load_dot(self, file_name=None):
if file_name is None:
file_name, _ = QFileDialog.getOpenFileName(self._widget, self.tr('Open graph from file'), None, self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
try:
fh = open(file_name, 'rb')
dotcode = fh.read()
fh.close()
except IOError:
return
# disable controls customizing fetched ROS graph
self._widget.graph_type_combo_box.setEnabled(False)
self._widget.filter_line_edit.setEnabled(False)
self._widget.topic_filter_line_edit.setEnabled(False)
self._widget.namespace_cluster_check_box.setEnabled(False)
self._widget.actionlib_check_box.setEnabled(False)
self._widget.dead_sinks_check_box.setEnabled(False)
self._widget.leaf_topics_check_box.setEnabled(False)
self._widget.quiet_check_box.setEnabled(False)
self._update_graph_view(dotcode)
def _fit_in_view(self):
self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio)
def _save_dot(self):
file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as DOT'), 'rosgraph.dot', self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
handle = QFile(file_name)
if not handle.open(QIODevice.WriteOnly | QIODevice.Text):
return
handle.write(self._current_dotcode)
handle.close()
def _save_svg(self):
file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as SVG'), 'rosgraph.svg', self.tr('Scalable Vector Graphic (*.svg)'))
if file_name is None or file_name == '':
return
generator = QSvgGenerator()
generator.setFileName(file_name)
generator.setSize((self._scene.sceneRect().size() * 2.0).toSize())
painter = QPainter(generator)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
def _save_image(self):
file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as image'), 'rosgraph.png', self.tr('Image (*.bmp *.jpg *.png *.tiff)'))
if file_name is None or file_name == '':
return
img = QImage((self._scene.sceneRect().size() * 2.0).toSize(), QImage.Format_ARGB32_Premultiplied)
painter = QPainter(img)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
img.save(file_name)
class EsterelPlanViewerWidget(QWidget):
# plan view
_scene = QGraphicsScene()
_webview = QGraphicsWebView()
_svg = QGraphicsSvgItem()
_renderer = QSvgRenderer()
def __init__(self, plugin=None):
super(EsterelPlanViewerWidget, self).__init__()
# Create QWidget
ui_file = os.path.join(rospkg.RosPack().get_path('rosplan_rqt'),
'resource', 'esterel_plan_viewer.ui')
loadUi(ui_file, self,
{'InteractiveGraphicsView': InteractiveGraphicsView})
self.setObjectName('ROSPlanEsterelPlanViewer')
self.graphicsView.setScene(self._scene)
self._scene.addItem(self._svg)
self.refreshButton.clicked[bool].connect(self._handle_refresh_clicked)
self.saveButton.clicked[bool].connect(self._handle_save_button_clicked)
self._sub = rospy.Subscriber('/rosplan_plan_dispatcher/plan_graph',
String, self.planReceivedCallback)
self._plugin = plugin
# to store the graph msg received in callback, later on is used to save the graph if needed
self.graph = None
def planReceivedCallback(self, msg):
'''
updating plan view
'''
# save graph in member variable in case user clicks save button later
self.graph = msg.data
# render graph
temp_buffer_graph = pydot.graph_from_dot_data(self.graph)
svg_string = temp_buffer_graph.create_svg()
self._renderer.load(QByteArray(svg_string))
self._svg.setSharedRenderer(self._renderer)
def _handle_refresh_clicked(self, checked):
'''
called when the refresh button is clicked
'''
self._sub.unregister()
self._sub = rospy.Subscriber(self.topicText.text(), String,
self.planReceivedCallback)
def save_graph(self, full_path):
'''
check if last graph msg received is valid (non empty), then save in file.dot
'''
if self.graph:
dot_file = open(full_path, 'w')
dot_file.write(self.graph)
dot_file.close()
rospy.loginfo('graph saved succesfully in %s', full_path)
else:
# if self.graph is None it will fall in this case
rospy.logerr('Could not save Graph: is empty, currently subscribing to: %s, try' +\
' clicking "Update subscriber" button and make sure graph is published at least one time', self.topicText.text())
def _handle_save_button_clicked(self, checked):
'''
called when the save button is clicked
'''
rospy.loginfo('Saving esterel plan to dot file')
fileName = QFileDialog.getSaveFileName(
self, 'Save Esterel plan to dot file', '',
'Graph xdot Files (*.dot)')
if fileName[0] == '':
rospy.loginfo("User has cancelled saving process")
else:
# add .dot at the end of the filename
full_dot_path = fileName[0]
if not '.dot' in full_dot_path:
full_dot_path += '.dot'
rospy.loginfo("path to save dot file: %s", full_dot_path)
self.save_graph(full_dot_path)
# Qt methods
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
def __init__(self, context):
super(RosGraph, self).__init__(context)
self.initialized = False
self.setObjectName('RosGraph')
self._graph = None
self._current_dotcode = None
self._widget = QWidget()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosGraphDotcodeGenerator()
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'RosGraph.ui')
loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosGraphUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.graph_type_combo_box.insertItem(0, self.tr('Nodes only'), NODE_NODE_GRAPH)
self._widget.graph_type_combo_box.insertItem(1, self.tr('Nodes/Topics (active)'), NODE_TOPIC_GRAPH)
self._widget.graph_type_combo_box.insertItem(2, self.tr('Nodes/Topics (all)'), NODE_TOPIC_ALL_GRAPH)
self._widget.graph_type_combo_box.setCurrentIndex(0)
self._widget.graph_type_combo_box.currentIndexChanged.connect(self._refresh_rosgraph)
self.node_completionmodel = NamespaceCompletionModel(self._widget.filter_line_edit, False)
completer = RepeatedWordCompleter(self.node_completionmodel, self)
completer.setCompletionMode(QCompleter.PopupCompletion)
completer.setWrapAround(True)
completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.filter_line_edit.editingFinished.connect(self._refresh_rosgraph)
self._widget.filter_line_edit.setCompleter(completer)
self.topic_completionmodel = NamespaceCompletionModel(self._widget.topic_filter_line_edit, False)
topic_completer = RepeatedWordCompleter(self.topic_completionmodel, self)
topic_completer.setCompletionMode(QCompleter.PopupCompletion)
topic_completer.setWrapAround(True)
topic_completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.topic_filter_line_edit.editingFinished.connect(self._refresh_rosgraph)
self._widget.topic_filter_line_edit.setCompleter(topic_completer)
self._widget.namespace_cluster_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.actionlib_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.dead_sinks_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.leaf_topics_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.quiet_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(self._update_rosgraph)
self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view)
self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(self._save_image)
self._update_rosgraph()
self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
class RosGraph(Plugin):
_deferred_fit_in_view = Signal()
def __init__(self, context):
super(RosGraph, self).__init__(context)
self.initialized = False
self.setObjectName('RosGraph')
self._graph = None
self._current_dotcode = None
self._widget = QWidget()
# factory builds generic dotcode items
self.dotcode_factory = PydotFactory()
# self.dotcode_factory = PygraphvizFactory()
# generator builds rosgraph
self.dotcode_generator = RosGraphDotcodeGenerator()
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_graph'), 'resource', 'RosGraph.ui')
loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('RosGraphUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.graph_type_combo_box.insertItem(0, self.tr('Nodes only'), NODE_NODE_GRAPH)
self._widget.graph_type_combo_box.insertItem(1, self.tr('Nodes/Topics (active)'), NODE_TOPIC_GRAPH)
self._widget.graph_type_combo_box.insertItem(2, self.tr('Nodes/Topics (all)'), NODE_TOPIC_ALL_GRAPH)
self._widget.graph_type_combo_box.setCurrentIndex(0)
self._widget.graph_type_combo_box.currentIndexChanged.connect(self._refresh_rosgraph)
self.node_completionmodel = NamespaceCompletionModel(self._widget.filter_line_edit, False)
completer = RepeatedWordCompleter(self.node_completionmodel, self)
completer.setCompletionMode(QCompleter.PopupCompletion)
completer.setWrapAround(True)
completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.filter_line_edit.editingFinished.connect(self._refresh_rosgraph)
self._widget.filter_line_edit.setCompleter(completer)
self.topic_completionmodel = NamespaceCompletionModel(self._widget.topic_filter_line_edit, False)
topic_completer = RepeatedWordCompleter(self.topic_completionmodel, self)
topic_completer.setCompletionMode(QCompleter.PopupCompletion)
topic_completer.setWrapAround(True)
topic_completer.setCaseSensitivity(Qt.CaseInsensitive)
self._widget.topic_filter_line_edit.editingFinished.connect(self._refresh_rosgraph)
self._widget.topic_filter_line_edit.setCompleter(topic_completer)
self._widget.namespace_cluster_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.actionlib_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.dead_sinks_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.leaf_topics_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.quiet_check_box.clicked.connect(self._refresh_rosgraph)
self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.refresh_graph_push_button.pressed.connect(self._update_rosgraph)
self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view)
self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(self._save_image)
self._update_rosgraph()
self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('graph_type_combo_box_index', self._widget.graph_type_combo_box.currentIndex())
instance_settings.set_value('filter_line_edit_text', self._widget.filter_line_edit.text())
instance_settings.set_value('topic_filter_line_edit_text', self._widget.topic_filter_line_edit.text())
instance_settings.set_value('namespace_cluster_check_box_state', self._widget.namespace_cluster_check_box.isChecked())
instance_settings.set_value('actionlib_check_box_state', self._widget.actionlib_check_box.isChecked())
instance_settings.set_value('dead_sinks_check_box_state', self._widget.dead_sinks_check_box.isChecked())
instance_settings.set_value('leaf_topics_check_box_state', self._widget.leaf_topics_check_box.isChecked())
instance_settings.set_value('quiet_check_box_state', self._widget.quiet_check_box.isChecked())
instance_settings.set_value('auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked())
instance_settings.set_value('highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked())
def restore_settings(self, plugin_settings, instance_settings):
self._widget.graph_type_combo_box.setCurrentIndex(int(instance_settings.value('graph_type_combo_box_index', 0)))
self._widget.filter_line_edit.setText(instance_settings.value('filter_line_edit_text', '/'))
self._widget.topic_filter_line_edit.setText(instance_settings.value('topic_filter_line_edit_text', '/'))
self._widget.namespace_cluster_check_box.setChecked(instance_settings.value('namespace_cluster_check_box_state', True) in [True, 'true'])
self._widget.actionlib_check_box.setChecked(instance_settings.value('actionlib_check_box_state', True) in [True, 'true'])
self._widget.dead_sinks_check_box.setChecked(instance_settings.value('dead_sinks_check_box_state', True) in [True, 'true'])
self._widget.leaf_topics_check_box.setChecked(instance_settings.value('leaf_topics_check_box_state', True) in [True, 'true'])
self._widget.quiet_check_box.setChecked(instance_settings.value('quiet_check_box_state', True) in [True, 'true'])
self._widget.auto_fit_graph_check_box.setChecked(instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true'])
self._widget.highlight_connections_check_box.setChecked(instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true'])
self.initialized = True
self._refresh_rosgraph()
def _update_rosgraph(self):
# re-enable controls customizing fetched ROS graph
self._widget.graph_type_combo_box.setEnabled(True)
self._widget.filter_line_edit.setEnabled(True)
self._widget.topic_filter_line_edit.setEnabled(True)
self._widget.namespace_cluster_check_box.setEnabled(True)
self._widget.actionlib_check_box.setEnabled(True)
self._widget.dead_sinks_check_box.setEnabled(True)
self._widget.leaf_topics_check_box.setEnabled(True)
self._widget.quiet_check_box.setEnabled(True)
self._graph = rosgraph.impl.graph.Graph()
self._graph.set_master_stale(5.0)
self._graph.set_node_stale(5.0)
self._graph.update()
self.node_completionmodel.refresh(self._graph.nn_nodes)
self.topic_completionmodel.refresh(self._graph.nt_nodes)
self._refresh_rosgraph()
def _refresh_rosgraph(self):
if not self.initialized:
return
self._update_graph_view(self._generate_dotcode())
def _generate_dotcode(self):
ns_filter = self._widget.filter_line_edit.text()
topic_filter = self._widget.topic_filter_line_edit.text()
graph_mode = self._widget.graph_type_combo_box.itemData(self._widget.graph_type_combo_box.currentIndex())
orientation = 'LR'
if self._widget.namespace_cluster_check_box.isChecked():
namespace_cluster = 1
else:
namespace_cluster = 0
accumulate_actions = self._widget.actionlib_check_box.isChecked()
hide_dead_end_topics = self._widget.dead_sinks_check_box.isChecked()
hide_single_connection_topics = self._widget.leaf_topics_check_box.isChecked()
quiet = self._widget.quiet_check_box.isChecked()
return self.dotcode_generator.generate_dotcode(
rosgraphinst=self._graph,
ns_filter=ns_filter,
topic_filter=topic_filter,
graph_mode=graph_mode,
hide_single_connection_topics=hide_single_connection_topics,
hide_dead_end_topics=hide_dead_end_topics,
cluster_namespaces_level=namespace_cluster,
accumulate_actions=accumulate_actions,
dotcode_factory=self.dotcode_factory,
orientation=orientation,
quiet=quiet)
def _update_graph_view(self, dotcode):
if dotcode == self._current_dotcode:
return
self._current_dotcode = dotcode
self._redraw_graph_view()
def _generate_tool_tip(self, url):
if url is not None and ':' in url:
item_type, item_path = url.split(':', 1)
if item_type == 'node':
tool_tip = 'Node:\n %s' % (item_path)
service_names = rosservice.get_service_list(node=item_path)
if service_names:
tool_tip += '\nServices:'
for service_name in service_names:
try:
service_type = rosservice.get_service_type(service_name)
tool_tip += '\n %s [%s]' % (service_name, service_type)
except rosservice.ROSServiceIOException as e:
tool_tip += '\n %s' % (e)
return tool_tip
elif item_type == 'topic':
topic_type, topic_name, _ = rostopic.get_topic_type(item_path)
return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type)
return url
def _redraw_graph_view(self):
self._scene.clear()
if self._widget.highlight_connections_check_box.isChecked():
highlight_level = 3
else:
highlight_level = 1
# layout graph and create qt items
(nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode,
highlight_level=highlight_level,
same_label_siblings=True)
for node_item in nodes.itervalues():
self._scene.addItem(node_item)
for edge_items in edges.itervalues():
for edge_item in edge_items:
edge_item.add_to_scene(self._scene)
self._scene.setSceneRect(self._scene.itemsBoundingRect())
if self._widget.auto_fit_graph_check_box.isChecked():
self._fit_in_view()
def _load_dot(self, file_name=None):
if file_name is None:
file_name, _ = QFileDialog.getOpenFileName(self._widget, self.tr('Open graph from file'), None, self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
try:
fh = open(file_name, 'rb')
dotcode = fh.read()
fh.close()
except IOError:
return
# disable controls customizing fetched ROS graph
self._widget.graph_type_combo_box.setEnabled(False)
self._widget.filter_line_edit.setEnabled(False)
self._widget.topic_filter_line_edit.setEnabled(False)
self._widget.namespace_cluster_check_box.setEnabled(False)
self._widget.actionlib_check_box.setEnabled(False)
self._widget.dead_sinks_check_box.setEnabled(False)
self._widget.leaf_topics_check_box.setEnabled(False)
self._widget.quiet_check_box.setEnabled(False)
self._update_graph_view(dotcode)
def _fit_in_view(self):
self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio)
def _save_dot(self):
file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as DOT'), 'rosgraph.dot', self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
handle = QFile(file_name)
if not handle.open(QIODevice.WriteOnly | QIODevice.Text):
return
handle.write(self._current_dotcode)
handle.close()
def _save_svg(self):
file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as SVG'), 'rosgraph.svg', self.tr('Scalable Vector Graphic (*.svg)'))
if file_name is None or file_name == '':
return
generator = QSvgGenerator()
generator.setFileName(file_name)
generator.setSize((self._scene.sceneRect().size() * 2.0).toSize())
painter = QPainter(generator)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
def _save_image(self):
file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as image'), 'rosgraph.png', self.tr('Image (*.bmp *.jpg *.png *.tiff)'))
if file_name is None or file_name == '':
return
img = QImage((self._scene.sceneRect().size() * 2.0).toSize(), QImage.Format_ARGB32_Premultiplied)
painter = QPainter(img)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
img.save(file_name)
class TimelineView(QGraphicsView):
"""
This class draws a graphical representation of a timeline.
This is ONLY the bar and colored boxes.
When you instantiate this class, do NOT forget to call set_init_data to
set necessary data.
"""
redraw = Signal()
def __init__(self, parent):
"""Cannot take args other than parent due to loadUi limitation."""
super(TimelineView, self).__init__()
self._parent = parent
self._timeline_marker = QIcon.fromTheme('system-search')
self._min = 0
self._max = 0
self._xpos_marker = 5
self._timeline_marker_width = 15
self._timeline_marker_height = 15
self._last_marker_at = 2
self.redraw.connect(self._slot_redraw)
self._timeline = None
self.setUpdatesEnabled(True)
self._scene = QGraphicsScene(self)
self.setScene(self._scene)
def set_timeline(self, timeline, name=None):
assert(self._timeline is None)
self._name = name
self._timeline = timeline
self._timeline.message_updated.connect(self._updated)
@Slot()
def _updated(self):
"""
Update the widget whenever we receive a new message
"""
# update the limits
self._min = 0
self._max = len(self._timeline)-1
# update the marker position
self._xpos_marker = self._timeline.get_position()
# redraw
self.redraw.emit()
def mouseReleaseEvent(self, event):
"""
:type event: QMouseEvent
"""
xpos = self.pos_from_x(event.x())
self.set_marker_pos(xpos)
def mousePressEvent(self, event):
"""
:type event: QMouseEvent
"""
assert(self._timeline is not None)
# Pause the timeline
self._timeline.set_paused(True)
xpos = self.pos_from_x(event.x())
self.set_marker_pos(xpos)
def mouseMoveEvent(self, event):
"""
:type event: QMouseEvent
"""
xpos = self.pos_from_x(event.x())
self.set_marker_pos(xpos)
def pos_from_x(self, x):
"""
Get the index in the timeline from the mouse click position
:param x: Position relative to self widget.
:return: Index
"""
width = self.size().width()
# determine value from mouse click
width_cell = width / float(len(self._timeline))
return int(floor(x / width_cell))
def set_marker_pos(self, xpos):
"""
Set marker position from index
:param xpos: Marker index
"""
assert(self._timeline is not None)
self._xpos_marker = self._clamp(xpos, self._min, self._max)
if self._xpos_marker == self._last_marker_at:
# Clicked the same pos as last time.
return
elif self._xpos_marker >= len(self._timeline):
# When clicked out-of-region
return
self._last_marker_at = self._xpos_marker
# Set timeline position. This broadcasts the message at that position
# to all of the other viewers
self._timeline.set_position(self._xpos_marker)
# redraw
self.redraw.emit()
def _clamp(self, val, min, max):
"""
Judge if val is within the range given by min & max.
If not, return either min or max.
:type val: any number format
:type min: any number format
:type max: any number format
:rtype: int
"""
if (val < min):
return min
if (val > max):
return max
return val
@Slot()
def _slot_redraw(self):
"""
Gets called either when new msg comes in or when marker is moved by
user.
"""
self._scene.clear()
qsize = self.size()
width_tl = qsize.width()
w = width_tl / float(max(len(self._timeline), 1))
is_enabled = self.isEnabled()
if self._timeline is not None:
for i, m in enumerate(self._timeline):
h = self.viewport().height()
# Figure out each cell's color.
qcolor = QColor('grey')
if is_enabled:
qcolor = self._get_color_for_value(m)
# TODO Use this code for adding gradation to the cell color.
# end_color = QColor(0.5 * QColor('red').value(),
# 0.5 * QColor('green').value(),
# 0.5 * QColor('blue').value())
self._scene.addRect(w * i, 0, w, h, QColor('white'), qcolor)
# Setting marker.
xpos_marker = (self._xpos_marker * w +
(w / 2.0) - (self._timeline_marker_width / 2.0))
pos_marker = QPointF(xpos_marker, 0)
# Need to instantiate marker everytime since it gets deleted
# in every loop by scene.clear()
timeline_marker = self._instantiate_tl_icon()
timeline_marker.setPos(pos_marker)
self._scene.addItem(timeline_marker)
def _instantiate_tl_icon(self):
timeline_marker_icon = QIcon.fromTheme('system-search')
timeline_marker_icon_pixmap = timeline_marker_icon.pixmap(
self._timeline_marker_width,
self._timeline_marker_height)
return QGraphicsPixmapItem(timeline_marker_icon_pixmap)
def _get_color_for_value(self, msg):
"""
:type msg: DiagnosticArray
"""
if self._name is not None:
# look up name in msg; return grey if not found
status = util.get_status_by_name(msg, self._name)
if status is not None:
return util.level_to_color(status.level)
else:
return QColor('grey')
return util.get_color_for_message(msg)