00001 """
00002 Copyright (c) 2013, Cogniteam
00003 All rights reserved.
00004
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007
00008 * Redistributions of source code must retain the above copyright
00009 notice, this list of conditions and the following disclaimer.
00010
00011 * Redistributions in binary form must reproduce the above copyright
00012 notice, this list of conditions and the following disclaimer in the
00013 documentation and/or other materials provided with the distribution.
00014
00015 * Neither the name of the Cogniteam nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 """
00030
00031 from __future__ import division
00032 from os import path
00033 from threading import Lock
00034 from python_qt_binding import loadUi
00035 from python_qt_binding.QtCore import Qt
00036 from python_qt_binding.QtGui import QFileDialog, QGraphicsScene, QIcon, QImage, QPainter, QWidget, QColor, QComboBox
00037 from collections import namedtuple
00038 from .interactive_graphics_view import InteractiveGraphicsView
00039 from .decision_graph import DecisionGraph
00040 from .graph import Graph, GraphParseException
00041 from .dot_to_qt import DotToQtGenerator
00042 from .dmg_item_factory import DmgItemFactory
00043
00044 from .dot_processor import DotProcessor
00045
00046
00047 class GraphWidget(QWidget):
00048 @staticmethod
00049 def get_unique_name(context):
00050 return ('Decision Graph (%d)' % context.serial_number()) if context.serial_number() > 1 else 'Decision Graph'
00051
00052 @staticmethod
00053 def get_file_name(absolute_path):
00054 return ".".join(path.basename(absolute_path).split('.')[:-1])
00055
00056 def __init__(self, ros_package):
00057 super(GraphWidget, self).__init__()
00058
00059 self._current_graph = None
00060 self._lock = Lock()
00061
00062 self._load_ui(ros_package)
00063
00064 self._scene = QGraphicsScene()
00065 self._scene.setBackgroundBrush(Qt.white)
00066 factory = DmgItemFactory()
00067 factory.set_color(QColor(50, 50, 50))
00068 factory.set_highlighted_color(QColor(0, 150, 0))
00069 self._dot_to_qt = DotToQtGenerator(factory)
00070
00071 self.initialized = False
00072 self.setObjectName('GraphWidget')
00073
00074 self.graphics_view.setScene(self._scene)
00075 self.open_button.setIcon(QIcon.fromTheme('document-open'))
00076 self.open_button.pressed.connect(self._import)
00077 self.export_button.setIcon(QIcon.fromTheme('document-export'))
00078 self.export_button.pressed.connect(self._export)
00079 self.fit_to_view_button.setIcon(QIcon.fromTheme('zoom-fit-best'))
00080 self.fit_to_view_button.pressed.connect(self._fit_to_view)
00081
00082 self.decision_graphs_combo_box.setSizeAdjustPolicy(QComboBox.AdjustToMinimumContentsLength)
00083 self.decision_graphs_combo_box.currentIndexChanged['QString'].connect(self._graph_item_changed)
00084
00085 self._dot_processor = DotProcessor(self._dot_to_qt)
00086
00087 self.decision_graphs = dict()
00088 self.states = dict()
00089
00090 def update(self, message):
00091 data = self._get_data_from_message(message)
00092 key = self._get_key(data)
00093
00094 if key not in self.decision_graphs:
00095 try:
00096 self._add_graph(key, data)
00097 print 'INFO: Graph has been added'
00098 except GraphParseException as ex:
00099 print 'ERROR: Failed to load graph: %s', ex.message
00100 else:
00101 self.states[key] = data['name'], data['status']
00102
00103 if self.decision_graphs[key].graph_id != message.status[0].values[-1].value:
00104 self.decision_graphs[key].graph_id = message.status[0].values[-1].value
00105 print 'INFO: Graph id has been changed'
00106 elif self._current_graph == self.decision_graphs[key]:
00107 if not self._update_graph(data['name'], data['status']):
00108 print 'WARNING: Failed to find appropriate graph for update'
00109
00110 def _load_ui(self, ros_package):
00111 user_interface_file = path.join(ros_package.get_path('rqt_decision_graph'), 'resource', 'DecisionGraph.ui')
00112
00113 loadUi(user_interface_file, self, {'InteractiveGraphicsView': InteractiveGraphicsView})
00114
00115 def _import(self):
00116 file_path, _ = QFileDialog.getOpenFileName(self, self.tr('Import custom graph'),
00117 None, self.tr('DOT graph (*.dot)'))
00118
00119 if file_path is None or file_path == '':
00120 return
00121
00122 custom_graph = Graph(self._dot_processor, file_path, file_path)
00123 self.decision_graphs[custom_graph.source] = custom_graph
00124 self._current_graph = custom_graph
00125
00126 self.decision_graphs_combo_box.addItem(custom_graph.source)
00127 self.decision_graphs_combo_box.setCurrentIndex(self.decision_graphs_combo_box.findText(custom_graph.source))
00128
00129
00130 def _export(self):
00131 file_name, _ = QFileDialog.getSaveFileName(self,
00132 self.tr('Save as image'),
00133 'graph.png',
00134 self.tr('Image (*.bmp *.jpg *.png *.tiff)'))
00135
00136 if file_name is None or file_name == '':
00137 return
00138
00139 img = QImage((self._scene.sceneRect().size() * 2.0).toSize(), QImage.Format_ARGB32_Premultiplied)
00140 painter = QPainter(img)
00141 painter.setRenderHint(QPainter.Antialiasing)
00142 self._scene.render(painter)
00143 painter.end()
00144 img.save(file_name)
00145
00146 def _add_graph(self, key, data):
00147 self._lock.acquire()
00148
00149 decision_graph = DecisionGraph(data['name'].split('/')[1],
00150 data['node_run_id'],
00151 data['node_name'],
00152 data['node_exe_file'],
00153 data['node_exe_dir'],
00154 self._dot_processor,
00155 key)
00156
00157 self.decision_graphs[key] = decision_graph
00158 self.decision_graphs_combo_box.addItem(key)
00159
00160 self._lock.release()
00161
00162 def _reset_graph_state(self, name, status):
00163 if self._current_graph is not None:
00164 for node in self._current_graph.nodes.values():
00165 if name[:len(node.url)] == node.url:
00166 node.highlight(True) if 'started' == status else node.highlight(False)
00167
00168 def _update_graph(self, name, status):
00169 self._lock.acquire()
00170 is_updated = False
00171 if self._current_graph is not None:
00172 for node in self._current_graph.nodes.values():
00173 if 'started' == status and name[:len(node.url)] == node.url:
00174 node.highlight(True)
00175 is_updated = True
00176 elif 'stopped' == status and name == node.url:
00177 node.highlight(False)
00178 is_updated = True
00179
00180 self._lock.release()
00181
00182 return is_updated
00183
00184 def _graph_item_changed(self, event):
00185 self._lock.acquire()
00186 if event in self.decision_graphs:
00187 self._current_graph = self.decision_graphs[event]
00188 self._redraw_graph_view()
00189 self._fit_to_view()
00190
00191 if isinstance(self._current_graph, DecisionGraph):
00192 state = self.states.get(self._current_graph.key, None)
00193 if state is not None:
00194 self._reset_graph_state(state[0], state[1])
00195
00196 self._lock.release()
00197
00198 def _get_data_from_message(self, message):
00199 return {value.key: value.value for value in message.status[0].values}
00200
00201 def _get_key(self, data):
00202 return data['name'].split('/')[1] + data['node_name']
00203
00204 def _redraw_graph_view(self):
00205
00206 self._current_graph.load()
00207 self._scene.clear()
00208
00209 for node_item in self._current_graph.nodes.itervalues():
00210 self._scene.addItem(node_item)
00211 for edge_items in self._current_graph.edges.itervalues():
00212 for edge_item in edge_items:
00213 edge_item.add_to_scene(self._scene)
00214
00215 self._scene.setSceneRect(self._scene.itemsBoundingRect())
00216
00217 def _fit_to_view(self):
00218 self.graphics_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio)