capability_graph.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2013, Open Source Robotics Foundation, 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 Open Source Robotics Foundation, Inc. nor
00017 #    the names of its contributors may be used to endorse or promote
00018 #    products derived from this software without specific prior
00019 #    written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 # Author: William Woodall <william@osrfoundation.org>
00035 
00036 """This module implements the CapabilityGraph rqt plugin
00037 """
00038 
00039 from __future__ import print_function
00040 
00041 import os
00042 
00043 import rospkg
00044 import rospy
00045 
00046 from python_qt_binding import loadUi
00047 from python_qt_binding.QtCore import Qt
00048 from python_qt_binding.QtCore import Signal
00049 from python_qt_binding.QtGui import QIcon
00050 from python_qt_binding.QtGui import QGraphicsScene
00051 from python_qt_binding.QtGui import QWidget
00052 
00053 from qt_dotgraph.dot_to_qt import DotToQtGenerator
00054 
00055 from rqt_gui_py.plugin import Plugin
00056 
00057 from rqt_capabilities.graphics_view import CapabilitiesInteractiveGraphicsView
00058 
00059 from rqt_capabilities.dotcode import generate_dotcode_from_capability_info
00060 
00061 from capabilities.service_discovery import spec_index_from_service
00062 
00063 from capabilities.msg import CapabilityEvent
00064 
00065 from capabilities.srv import GetRunningCapabilities
00066 
00067 
00068 class CapabilityGraph(Plugin):
00069 
00070     __deferred_fit_in_view = Signal()
00071     __redraw_graph = Signal()
00072 
00073     def __init__(self, context):
00074         super(CapabilityGraph, self).__init__(context)
00075         self.setObjectName('CapabilityGraph')
00076 
00077         self.__current_dotcode = None
00078 
00079         self.__running_providers = []
00080         self.__spec_index = None
00081 
00082         self.__widget = QWidget()
00083 
00084         self.__dot_to_qt = DotToQtGenerator()
00085 
00086         rp = rospkg.RosPack()
00087         ui_file = os.path.join(rp.get_path('rqt_capabilities'), 'resources', 'CapabilityGraph.ui')
00088         loadUi(ui_file, self.__widget, {'CapabilitiesInteractiveGraphicsView': CapabilitiesInteractiveGraphicsView})
00089         self.__widget.setObjectName('CapabilityGraphUI')
00090         if context.serial_number() > 1:
00091             self.__widget.setWindowTitle(self.__widget.windowTitle() + (' (%d)' % context.serial_number()))
00092 
00093         self.__scene = QGraphicsScene()
00094         self.__scene.setBackgroundBrush(Qt.white)
00095         self.__widget.graphics_view.setScene(self.__scene)
00096 
00097         self.__widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
00098         self.__widget.refresh_graph_push_button.pressed.connect(self.__refresh_view)
00099 
00100         self.__refresh_view()
00101         self.__deferred_fit_in_view.connect(self.__fit_in_view, Qt.QueuedConnection)
00102         self.__deferred_fit_in_view.emit()
00103         self.__redraw_graph.connect(self.__update_capabilities_graph)
00104 
00105         # TODO: use user provided server node name
00106         rospy.Subscriber('/capability_server/events', CapabilityEvent, self.__handle_event)
00107 
00108         context.add_widget(self.__widget)
00109 
00110     def __handle_event(self, msg):
00111         if msg.type == CapabilityEvent.STOPPED:
00112             return
00113         if msg.type == CapabilityEvent.LAUNCHED and msg.provider not in self.__running_providers:
00114             self.__running_providers.append(msg.provider)
00115         if msg.type == CapabilityEvent.TERMINATED and msg.provider in self.__running_providers:
00116             self.__running_providers.remove(msg.provider)
00117         self.__redraw_graph.emit()
00118 
00119     def __get_specs(self):
00120         self.__spec_index, errors = spec_index_from_service()
00121         assert not errors
00122 
00123     def __get_running_providers(self):
00124         # TODO: replace 'capability_server' with user provided server name
00125         service_name = '/{0}/get_running_capabilities'.format('capability_server')
00126         rospy.wait_for_service(service_name)
00127         get_running_capabilities = rospy.ServiceProxy(service_name, GetRunningCapabilities)
00128         response = get_running_capabilities()
00129         self.__running_providers = []
00130         for cap in response.running_capabilities:
00131             self.__running_providers.append(cap.capability.provider)
00132 
00133     def __refresh_view(self):
00134         self.__get_specs()
00135         self.__get_running_providers()
00136         self.__update_capabilities_graph()
00137 
00138     def __update_capabilities_graph(self):
00139         self.__update_graph_view(self.__generate_dotcode())
00140 
00141     def __generate_dotcode(self):
00142         return generate_dotcode_from_capability_info(self.__spec_index, self.__running_providers)
00143 
00144     def __update_graph_view(self, dotcode):
00145         if dotcode == self.__current_dotcode:
00146             return
00147         self.__current_dotcode = dotcode
00148         self.__redraw_graph_view()
00149 
00150     def __fit_in_view(self):
00151         self.__widget.graphics_view.fitInView(self.__scene.itemsBoundingRect(), Qt.KeepAspectRatio)
00152 
00153     def __redraw_graph_view(self):
00154         self.__widget.graphics_view._running_providers = self.__running_providers
00155         self.__widget.graphics_view._spec_index = self.__spec_index
00156         self.__scene.clear()
00157 
00158         highlight_level = 1
00159 
00160         # layout graph and create qt items
00161         (nodes, edges) = self.__dot_to_qt.dotcode_to_qt_items(self.__current_dotcode,
00162                                                               highlight_level=highlight_level,
00163                                                               same_label_siblings=True)
00164 
00165         for node_item in nodes.itervalues():
00166             self.__scene.addItem(node_item)
00167         for edge_items in edges.itervalues():
00168             for edge_item in edge_items:
00169                 edge_item.add_to_scene(self.__scene)
00170 
00171         self.__scene.setSceneRect(self.__scene.itemsBoundingRect())
00172         self.__fit_in_view()


rqt_capabilities
Author(s): William Woodall
autogenerated on Sat Jun 8 2019 18:40:50