00001
00002
00003
00004 from __future__ import division
00005 import os
00006
00007
00008 PKG = 'rqt_tf_tree'
00009
00010 import roslib
00011 roslib.load_manifest(PKG)
00012 roslib.load_manifest('tf2_tools')
00013
00014 import rospy
00015
00016 from tf2_msgs.srv import FrameGraph
00017 import tf2_ros
00018
00019 from python_qt_binding import loadUi
00020 from python_qt_binding.QtCore import QFile, QIODevice, QObject, Qt, Signal
00021 from python_qt_binding.QtGui import QFileDialog, QGraphicsScene, QIcon, QImage, QPainter, QWidget
00022 from python_qt_binding.QtSvg import QSvgGenerator
00023
00024
00025 from .dotcode_tf import RosTfTreeDotcodeGenerator
00026 from qt_dotgraph.pydotfactory import PydotFactory
00027
00028 from qt_dotgraph.dot_to_qt import DotToQtGenerator
00029
00030 from rqt_graph.interactive_graphics_view import InteractiveGraphicsView
00031
00032
00033 class RosTfTree(QObject):
00034
00035 _deferred_fit_in_view = Signal()
00036
00037 def __init__(self, context):
00038 super(RosTfTree, self).__init__(context)
00039 self.initialized = False
00040
00041 self.setObjectName('RosTfTree')
00042
00043 self._current_dotcode = None
00044
00045 self._widget = QWidget()
00046
00047
00048 self.dotcode_factory = PydotFactory()
00049
00050
00051 self.dotcode_generator = RosTfTreeDotcodeGenerator()
00052 self.tf2_buffer_ = tf2_ros.Buffer()
00053 self.tf2_listener_ = tf2_ros.TransformListener(self.tf2_buffer_)
00054
00055
00056 self.dot_to_qt = DotToQtGenerator()
00057
00058 ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'RosTfTree.ui')
00059 loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
00060 self._widget.setObjectName('RosTfTreeUi')
00061 if context.serial_number() > 1:
00062 self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
00063
00064 self._scene = QGraphicsScene()
00065 self._scene.setBackgroundBrush(Qt.white)
00066 self._widget.graphics_view.setScene(self._scene)
00067
00068 self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
00069 self._widget.refresh_graph_push_button.pressed.connect(self._update_tf_graph)
00070
00071 self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view)
00072 self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view)
00073 self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
00074 self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
00075
00076 self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
00077 self._widget.load_dot_push_button.pressed.connect(self._load_dot)
00078 self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
00079 self._widget.save_dot_push_button.pressed.connect(self._save_dot)
00080 self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
00081 self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
00082 self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
00083 self._widget.save_as_image_push_button.pressed.connect(self._save_image)
00084
00085 self._deferred_fit_in_view.connect(self._fit_in_view,
00086 Qt.QueuedConnection)
00087 self._deferred_fit_in_view.emit()
00088
00089 context.add_widget(self._widget)
00090
00091 self._force_refresh = False
00092
00093 def save_settings(self, plugin_settings, instance_settings):
00094 instance_settings.set_value('auto_fit_graph_check_box_state',
00095 self._widget.auto_fit_graph_check_box.isChecked())
00096 instance_settings.set_value('highlight_connections_check_box_state',
00097 self._widget.highlight_connections_check_box.isChecked())
00098
00099 def restore_settings(self, plugin_settings, instance_settings):
00100 self._widget.auto_fit_graph_check_box.setChecked(
00101 instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true'])
00102 self._widget.highlight_connections_check_box.setChecked(
00103 instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true'])
00104 self.initialized = True
00105 self._refresh_tf_graph()
00106
00107 def _update_tf_graph(self):
00108 self._force_refresh = True
00109 self._refresh_tf_graph()
00110
00111 def _refresh_tf_graph(self):
00112 if not self.initialized:
00113 return
00114 self._update_graph_view(self._generate_dotcode())
00115
00116 def _generate_dotcode(self):
00117 force_refresh = self._force_refresh
00118 self._force_refresh = False
00119 rospy.wait_for_service('~tf2_frames')
00120 tf2_frame_srv = rospy.ServiceProxy('~tf2_frames', FrameGraph)
00121 return self.dotcode_generator.generate_dotcode(dotcode_factory=self.dotcode_factory,
00122 tf2_frame_srv=tf2_frame_srv,
00123 force_refresh=force_refresh)
00124
00125 def _update_graph_view(self, dotcode):
00126 if dotcode == self._current_dotcode:
00127 return
00128 self._current_dotcode = dotcode
00129 self._redraw_graph_view()
00130
00131 def _generate_tool_tip(self, url):
00132 return url
00133
00134 def _redraw_graph_view(self):
00135 self._scene.clear()
00136
00137 if self._widget.highlight_connections_check_box.isChecked():
00138 highlight_level = 3
00139 else:
00140 highlight_level = 1
00141
00142 (nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode,
00143 highlight_level)
00144
00145 for node_item in nodes.itervalues():
00146 self._scene.addItem(node_item)
00147 for edge_items in edges.itervalues():
00148 for edge_item in edge_items:
00149 edge_item.add_to_scene(self._scene)
00150
00151 self._scene.setSceneRect(self._scene.itemsBoundingRect())
00152 if self._widget.auto_fit_graph_check_box.isChecked():
00153 self._fit_in_view()
00154
00155 def _load_dot(self, file_name=None):
00156 if file_name is None:
00157 file_name, _ = QFileDialog.getOpenFileName(
00158 self._widget,
00159 self.tr('Open graph from file'),
00160 None,
00161 self.tr('DOT graph (*.dot)'))
00162 if file_name is None or file_name == '':
00163 return
00164
00165 try:
00166 fhandle = open(file_name, 'rb')
00167 dotcode = fhandle.read()
00168 fhandle.close()
00169 except IOError:
00170 return
00171
00172 self._update_graph_view(dotcode)
00173
00174 def _fit_in_view(self):
00175 self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(),
00176 Qt.KeepAspectRatio)
00177
00178 def _save_dot(self):
00179 file_name, _ = QFileDialog.getSaveFileName(self._widget,
00180 self.tr('Save as DOT'),
00181 'frames.dot',
00182 self.tr('DOT graph (*.dot)'))
00183 if file_name is None or file_name == '':
00184 return
00185
00186 file = QFile(file_name)
00187 if not file.open(QIODevice.WriteOnly | QIODevice.Text):
00188 return
00189
00190 file.write(self._current_dotcode)
00191 file.close()
00192
00193 def _save_svg(self):
00194 file_name, _ = QFileDialog.getSaveFileName(
00195 self._widget,
00196 self.tr('Save as SVG'),
00197 'frames.svg',
00198 self.tr('Scalable Vector Graphic (*.svg)'))
00199 if file_name is None or file_name == '':
00200 return
00201
00202 generator = QSvgGenerator()
00203 generator.setFileName(file_name)
00204 generator.setSize((self._scene.sceneRect().size() * 2.0).toSize())
00205
00206 painter = QPainter(generator)
00207 painter.setRenderHint(QPainter.Antialiasing)
00208 self._scene.render(painter)
00209 painter.end()
00210
00211 def _save_image(self):
00212 file_name, _ = QFileDialog.getSaveFileName(
00213 self._widget,
00214 self.tr('Save as image'),
00215 'frames.png',
00216 self.tr('Image (*.bmp *.jpg *.png *.tiff)'))
00217 if file_name is None or file_name == '':
00218 return
00219
00220 img = QImage((self._scene.sceneRect().size() * 2.0).toSize(),
00221 QImage.Format_ARGB32_Premultiplied)
00222 painter = QPainter(img)
00223 painter.setRenderHint(QPainter.Antialiasing)
00224 self._scene.render(painter)
00225 painter.end()
00226 img.save(file_name)