tf_tree.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2011, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Willow Garage, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 
00033 from __future__ import division
00034 import os
00035 
00036 
00037 import rospy
00038 import rospkg
00039 
00040 from tf2_msgs.srv import FrameGraph
00041 import tf2_ros
00042 
00043 from python_qt_binding import loadUi
00044 from python_qt_binding.QtCore import QFile, QIODevice, QObject, Qt, Signal
00045 from python_qt_binding.QtGui import QFileDialog, QGraphicsScene, QIcon, QImage, QPainter, QWidget
00046 from python_qt_binding.QtSvg import QSvgGenerator
00047 from qt_dotgraph.pydotfactory import PydotFactory
00048 # from qt_dotgraph.pygraphvizfactory import PygraphvizFactory
00049 from qt_dotgraph.dot_to_qt import DotToQtGenerator
00050 from rqt_graph.interactive_graphics_view import InteractiveGraphicsView
00051 
00052 from .dotcode_tf import RosTfTreeDotcodeGenerator
00053 
00054 
00055 class RosTfTree(QObject):
00056 
00057     _deferred_fit_in_view = Signal()
00058 
00059     def __init__(self, context):
00060         super(RosTfTree, self).__init__(context)
00061         self.initialized = False
00062 
00063         self.setObjectName('RosTfTree')
00064 
00065         self._current_dotcode = None
00066 
00067         self._widget = QWidget()
00068 
00069         # factory builds generic dotcode items
00070         self.dotcode_factory = PydotFactory()
00071         # self.dotcode_factory = PygraphvizFactory()
00072         # generator builds rosgraph
00073         self.dotcode_generator = RosTfTreeDotcodeGenerator()
00074         self.tf2_buffer_ = tf2_ros.Buffer()
00075         self.tf2_listener_ = tf2_ros.TransformListener(self.tf2_buffer_)
00076 
00077         # dot_to_qt transforms into Qt elements using dot layout
00078         self.dot_to_qt = DotToQtGenerator()
00079 
00080         rp = rospkg.RosPack()
00081         ui_file = os.path.join(rp.get_path('rqt_tf_tree'), 'resource', 'RosTfTree.ui')
00082         loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
00083         self._widget.setObjectName('RosTfTreeUi')
00084         if context.serial_number() > 1:
00085             self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
00086 
00087         self._scene = QGraphicsScene()
00088         self._scene.setBackgroundBrush(Qt.white)
00089         self._widget.graphics_view.setScene(self._scene)
00090 
00091         self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
00092         self._widget.refresh_graph_push_button.pressed.connect(self._update_tf_graph)
00093 
00094         self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view)
00095         self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view)
00096         self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
00097         self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
00098 
00099         self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
00100         self._widget.load_dot_push_button.pressed.connect(self._load_dot)
00101         self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
00102         self._widget.save_dot_push_button.pressed.connect(self._save_dot)
00103         self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
00104         self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
00105         self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
00106         self._widget.save_as_image_push_button.pressed.connect(self._save_image)
00107 
00108         self._deferred_fit_in_view.connect(self._fit_in_view,
00109                                            Qt.QueuedConnection)
00110         self._deferred_fit_in_view.emit()
00111 
00112         context.add_widget(self._widget)
00113 
00114         self._force_refresh = False
00115 
00116     def save_settings(self, plugin_settings, instance_settings):
00117         instance_settings.set_value('auto_fit_graph_check_box_state',
00118                                     self._widget.auto_fit_graph_check_box.isChecked())
00119         instance_settings.set_value('highlight_connections_check_box_state',
00120                                     self._widget.highlight_connections_check_box.isChecked())
00121 
00122     def restore_settings(self, plugin_settings, instance_settings):
00123         self._widget.auto_fit_graph_check_box.setChecked(
00124             instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true'])
00125         self._widget.highlight_connections_check_box.setChecked(
00126             instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true'])
00127         self.initialized = True
00128         self._refresh_tf_graph()
00129 
00130     def _update_tf_graph(self):
00131         self._force_refresh = True
00132         self._refresh_tf_graph()
00133 
00134     def _refresh_tf_graph(self):
00135         if not self.initialized:
00136             return
00137         self._update_graph_view(self._generate_dotcode())
00138 
00139     def _generate_dotcode(self):
00140         force_refresh = self._force_refresh
00141         self._force_refresh = False
00142         rospy.wait_for_service('~tf2_frames')
00143         tf2_frame_srv = rospy.ServiceProxy('~tf2_frames', FrameGraph)
00144         return self.dotcode_generator.generate_dotcode(dotcode_factory=self.dotcode_factory,
00145                                                        tf2_frame_srv=tf2_frame_srv,
00146                                                        force_refresh=force_refresh)
00147 
00148     def _update_graph_view(self, dotcode):
00149         if dotcode == self._current_dotcode:
00150             return
00151         self._current_dotcode = dotcode
00152         self._redraw_graph_view()
00153 
00154     def _generate_tool_tip(self, url):
00155         return url
00156 
00157     def _redraw_graph_view(self):
00158         self._scene.clear()
00159 
00160         if self._widget.highlight_connections_check_box.isChecked():
00161             highlight_level = 3
00162         else:
00163             highlight_level = 1
00164 
00165         (nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode,
00166                                                             highlight_level)
00167 
00168         for node_item in nodes.itervalues():
00169             self._scene.addItem(node_item)
00170         for edge_items in edges.itervalues():
00171             for edge_item in edge_items:
00172                 edge_item.add_to_scene(self._scene)
00173 
00174         self._scene.setSceneRect(self._scene.itemsBoundingRect())
00175         if self._widget.auto_fit_graph_check_box.isChecked():
00176             self._fit_in_view()
00177 
00178     def _load_dot(self, file_name=None):
00179         if file_name is None:
00180             file_name, _ = QFileDialog.getOpenFileName(
00181                 self._widget,
00182                 self.tr('Open graph from file'),
00183                 None,
00184                 self.tr('DOT graph (*.dot)'))
00185             if file_name is None or file_name == '':
00186                 return
00187 
00188         try:
00189             fhandle = open(file_name, 'rb')
00190             dotcode = fhandle.read()
00191             fhandle.close()
00192         except IOError:
00193             return
00194 
00195         self._update_graph_view(dotcode)
00196 
00197     def _fit_in_view(self):
00198         self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(),
00199                                              Qt.KeepAspectRatio)
00200 
00201     def _save_dot(self):
00202         file_name, _ = QFileDialog.getSaveFileName(self._widget,
00203                                                    self.tr('Save as DOT'),
00204                                                    'frames.dot',
00205                                                    self.tr('DOT graph (*.dot)'))
00206         if file_name is None or file_name == '':
00207             return
00208 
00209         file = QFile(file_name)
00210         if not file.open(QIODevice.WriteOnly | QIODevice.Text):
00211             return
00212 
00213         file.write(self._current_dotcode)
00214         file.close()
00215 
00216     def _save_svg(self):
00217         file_name, _ = QFileDialog.getSaveFileName(
00218             self._widget,
00219             self.tr('Save as SVG'),
00220             'frames.svg',
00221             self.tr('Scalable Vector Graphic (*.svg)'))
00222         if file_name is None or file_name == '':
00223             return
00224 
00225         generator = QSvgGenerator()
00226         generator.setFileName(file_name)
00227         generator.setSize((self._scene.sceneRect().size() * 2.0).toSize())
00228 
00229         painter = QPainter(generator)
00230         painter.setRenderHint(QPainter.Antialiasing)
00231         self._scene.render(painter)
00232         painter.end()
00233 
00234     def _save_image(self):
00235         file_name, _ = QFileDialog.getSaveFileName(
00236             self._widget,
00237             self.tr('Save as image'),
00238             'frames.png',
00239             self.tr('Image (*.bmp *.jpg *.png *.tiff)'))
00240         if file_name is None or file_name == '':
00241             return
00242 
00243         img = QImage((self._scene.sceneRect().size() * 2.0).toSize(),
00244                      QImage.Format_ARGB32_Premultiplied)
00245         painter = QPainter(img)
00246         painter.setRenderHint(QPainter.Antialiasing)
00247         self._scene.render(painter)
00248         painter.end()
00249         img.save(file_name)


rqt_tf_tree
Author(s): Thibault Kruse
autogenerated on Fri Aug 28 2015 12:50:42