Esempio n. 1
0
    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()
Esempio n. 2
0
    def __init__(self, context):
        super(DataAcquisition, self).__init__(context)
        self.initialized = False
        self.setObjectName('DataAcquisition')

        self._graph = None
        self._current_dotcode = None

        self._widget = QWidget()

        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('data_acquisition_2d'), 'resource',
                               'data_acquisition.ui')
        loadUi(ui_file, self._widget,
               {'InteractiveGraphicsView': InteractiveGraphicsView})
        self._widget.setObjectName('DataAcquisitionUi')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() +
                                        (' (%d)' % context.serial_number()))

        # Fixed size
        width = 670
        height = 570
        # setting  the fixed size of window
        self._widget.setFixedSize(width, height)

        # Get a list of Image topic
        self._get_image_topics()

        # Robot mode : disable GUI when scanning
        self._in_scanning = False
        self._scan_time = 0
        self._image_topic = None

        # canvas : image will be load here
        self._scene = QGraphicsScene()
        self._scene.setBackgroundBrush(Qt.red)
        self._scene.setSceneRect(0, 0, 640, 480)
        self._widget.canvas.setScene(self._scene)
        self._set_image()

        # Button Event
        self._widget.setup_robot_push_button.pressed.connect(self._setup_robot)
        self._widget.save_ground_truth_push_button.pressed.connect(
            self._save_ground_truth)
        self._widget.create_bbox_push_button.pressed.connect(self._create_bbox)
        self._widget.start_scanning_push_button.pressed.connect(
            self._start_scanning)

        # Box event
        self._widget.scan_time_spin_box.valueChanged.connect(
            self._set_scan_time)
        self._widget.image_topic_combo_box.currentIndexChanged.connect(
            self._set_image_topic)

        context.add_widget(self._widget)
Esempio n. 3
0
    def __init__(self, context):
        super(Eyedrop, self).__init__(context)
        # Give QObjects reasonable names
        self.setObjectName('Eyedrop')
        rp = rospkg.RosPack()

        # Process standalone plugin command-line arguments
        from argparse import ArgumentParser
        parser = ArgumentParser()
        # Add argument(s) to the parser.
        parser.add_argument("-q",
                            "--quiet",
                            action="store_true",
                            dest="quiet",
                            help="Put plugin in silent mode")
        self.args, unknowns = parser.parse_known_args(context.argv())

        # Create QWidget
        self._widget = QWidget()
        # Get path to UI file which is a sibling of this file
        # in this example the .ui and .py file are in the same folder
        ui_file = os.path.join(rp.get_path('rqt_eyedropper'), 'resource',
                               'Eyedrop.ui')
        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self._widget)
        # Give QObjects reasonable names
        self._widget.setObjectName('EyedropUi')
        # Show _widget.windowTitle on left-top of each plugin (when
        # it's set in _widget). This is useful when you open multiple
        # plugins at once. Also if you open multiple instances of your
        # plugin at once, these lines add number to make it easy to
        # tell from pane to pane.
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() +
                                        (' (%d)' % context.serial_number()))
        # Add widget to the user interface
        context.add_widget(self._widget)

        self._widget.button_refresh_topics.clicked.connect(
            self.button_refresh_topics_pressed)
        self._widget.button_take_sample.clicked.connect(
            self.button_take_sample_pressed)

        self._widget.combo_box_topic_image.currentIndexChanged.connect(
            self.do_subscribe_image)
        self._widget.combo_box_topic_point.currentIndexChanged.connect(
            self.do_subscribe_point)

        self.colour_pane_scene = QGraphicsScene()
        self.colour_pane_scene.setBackgroundBrush(QBrush(QColor(0, 0, 0)))
        self._widget.graphics_view_colour_pane.setScene(self.colour_pane_scene)
        self._widget.graphics_view_colour_pane.show()

        self.bridge = CvBridge()

        self.do_refresh_topic_lists()
Esempio n. 4
0
    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)
Esempio n. 5
0
    def __init__(self, context):
        super(Ros2KnowledgeGraph, self).__init__(context)

        self._node = context.node
        self._logger = self._node.get_logger().get_child(
            'ros2_knowledge_graph_viewer.ros2_knowledge_graph.Ros2KnowledgeGraph'
        )
        self.setObjectName('Ros2KnowledgeGraph')

        self._ros2_knowledge_graph = Ros2KnowledgeGraphImpl()
        self._current_dotcode = None

        self._widget = QWidget()

        self.dotcode_factory = PydotFactory()
        self.dotcode_generator = Ros2KnowledgeGraphDotcodeGenerator()
        self.dot_to_qt = DotToQtGenerator()

        _, package_path = get_resource('packages',
                                       'ros2_knowledge_graph_viewer')
        ui_file = os.path.join(package_path, 'share',
                               'ros2_knowledge_graph_viewer', 'resource',
                               'Ros2KnowledgeGraph.ui')
        loadUi(ui_file, self._widget,
               {'InteractiveGraphicsView': InteractiveGraphicsView})
        self._widget.setObjectName('Ros2KnowledgeGraphUi')
        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.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_ros2_knowledge_graph()
        self._deferred_fit_in_view.connect(self._fit_in_view,
                                           Qt.QueuedConnection)
        self._deferred_fit_in_view.emit()

        context.add_widget(self._widget)

        self._updateTimer = QtCore.QTimer()
        self._updateTimer.timeout.connect(self.do_update)
        self._updateTimer.start(10)
Esempio n. 6
0
    def _init_plugin(self, context):
        self.setObjectName("DSD-Visualization")

        self._widget = QWidget()
        self._widget.setObjectName(self.objectName())

        # load qt ui definition from file
        rp = rospkg.RosPack()
        ui_file = os.path.join(
            rp.get_path("dynamic_stack_decider_visualization"), "resource",
            "StackmachineViz.ui")
        loadUi(ui_file, self._widget,
               {"InteractiveGraphicsView": InteractiveGraphicsView})

        # initialize qt scene
        self._scene = QGraphicsScene()
        self._scene.setBackgroundBrush(Qt.white)
        self._widget.graphics_view.setScene(self._scene)

        # Bind fit-in-view button
        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)

        # Fit-in-view on checkbox toggle
        self._widget.auto_fit_graph_check_box.toggled.connect(self.fit_in_view)

        # Freezing
        def toggle_freeze():
            self.freeze = not self.freeze
            self.refresh()

        self._widget.freeze_push_button.toggled.connect(toggle_freeze)

        # Exporting and importing
        self._widget.save_as_svg_push_button.pressed.connect(
            self.save_svg_to_file)

        # Fill choices for dsd_selector and bind on_select
        self._widget.dsd_selector_combo_box.addItem("Select DSD...")
        for choice in self.locations:
            self._widget.dsd_selector_combo_box.addItem(choice['display_name'])
        self._widget.dsd_selector_combo_box.currentTextChanged.connect(
            self.set_dsd)

        context.add_widget(self._widget)

        # Start a timer that calls back every 100 ms
        self._timer_id = self.startTimer(100)
    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()
Esempio n. 8
0
    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)
Esempio n. 9
0
    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)
Esempio n. 10
0
    def __init__(self, context):
        super(VehiclesViz, self).__init__(context)
        self.setObjectName('VehiclesViz')

        self.exposed_methods = {
            'vehicle': ['create', 'move', 'hide', 'show'],
            'rqt_vviz': ['resize', 'clear'],
            'road_marking': ['set_size', 'edit_first'],
            'draw': ['circle'],
            'remove': ['circle']
        }
        self._init_subs_and_channels()

        self.vehicles = {}

        self._widget = VehiclesVizWidget()
        ui_file = os.path.join(rospkg.RosPack().get_path('rqt_vviz'),
                               'resource', 'widget.ui')
        loadUi(ui_file, self._widget)

        self._widget.setObjectName('VehiclesViz')
        self._widget.setWindowTitle('VehiclesViz')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() +
                                        (' (%d)' % context.serial_number()))

        self._view = self._widget.findChild(QGraphicsView, 'graphicsView')
        self._view.setAlignment(QtCore.Qt.AlignLeft | QtCore.Qt.AlignTop)
        self._view.setHorizontalScrollBarPolicy(QtCore.Qt.ScrollBarAlwaysOff)
        self._view.setVerticalScrollBarPolicy(QtCore.Qt.ScrollBarAlwaysOff)

        self._view.resize(100, 100)

        self._scene = QGraphicsScene()
        self._scene.setSceneRect(0, 0,
                                 self._view.size().width(),
                                 self._view.size().height())
        self._view.setScene(self._scene)

        self._load_images()

        context.add_widget(self._widget)

        self.items_cache = {'circle': {}}
Esempio n. 11
0
    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
Esempio n. 12
0
    def __init__(self, parent=None):
        super(Widget, self).__init__(parent)
        self.setWindowTitle('U-CTF View')

        self._scene = QGraphicsScene()
        self._scene.setBackgroundBrush(Qt.white)

        # game cube grass area
        grass_color = QColor(0, 255, 0, 50)
        self._scene.addRect(
            0, 0, CUBE_LENGTH, CUBE_LENGTH,
            QPen(grass_color), QBrush(grass_color))
        # blue team area
        blue_color = QColor(0, 0, 255, 50)
        self._scene.addRect(
            0, -SUPPORT_AREA_DEPTH, CUBE_LENGTH, SUPPORT_AREA_DEPTH,
            QPen(blue_color), QBrush(blue_color))
        # gold team area
        gold_color = QColor(255, 215, 0, 50)
        self._scene.addRect(
            0, CUBE_LENGTH, CUBE_LENGTH, SUPPORT_AREA_DEPTH,
            QPen(gold_color), QBrush(gold_color))
        # penalty area
        orange_color = QColor(0, 200, 0, 50)
        self._scene.addRect(
            CUBE_LENGTH, 0, SUPPORT_AREA_DEPTH, CUBE_LENGTH,
            QPen(orange_color), QBrush(orange_color))

        # rotate view to match the coordinate system of the game cube
        self.rotate(180)
        self.setScene(self._scene)

        # pens and brushes for the vehicles
        self._pens = {
            'blue': QPen(Qt.blue, 2),
            'gold': QPen(QColor(191, 151, 0), 2),
        }
        self._brushes = {
            'blue': QBrush(Qt.blue),
            'gold': QBrush(QColor(191, 151, 0)),
        }

        self._vehicles = {}
Esempio n. 13
0
    def __init__(self, timeline, parent, topics):
        super(SandtrayView, self).__init__(timeline, parent, topics[0])

        self._items = {}

        self._image = None
        self._image_topic = None
        self._image_stamp = None
        self.quality = Image.NEAREST  # quality hint for scaling

        self._sandtray = SandtrayItem()

        self._sandtray_view = QGraphicsView(parent)
        self._sandtray_view.resizeEvent = self._resizeEvent
        self._scene = QGraphicsScene()
        self._scene.addItem(self._sandtray)

        self._sandtray_view.setScene(self._scene)
        self._sandtray_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio)

        parent.layout().addWidget(self._sandtray_view)
Esempio n. 14
0
    def __init__(self, parent=None):
        """Cannot take args other than parent due to loadUi limitation."""

        super(TimelineView, self).__init__(parent=parent)
        self._timeline_marker = QIcon.fromTheme('system-search')

        self._min = 0
        self._max = 0
        self._xpos_marker = -1

        self._timeline_marker_width = 15
        self._timeline_marker_height = 15
        self._last_marker_at = -1

        self.setUpdatesEnabled(True)
        self._scene = QGraphicsScene(self)
        self.setScene(self._scene)

        self._levels = None

        self.redraw.connect(self._signal_redraw)
Esempio n. 15
0
    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)
Esempio n. 16
0
    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)
Esempio n. 17
0
    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)
Esempio n. 18
0
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.values():
            self._scene.addItem(node_item)
        for edge_items in edges.values():
            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)
Esempio n. 19
0
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(max(len(self._timeline), 1))
        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)
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 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_scene(self):
        # remove items in order to not garbage nodes which will be continued to be used
        for item in self._scene.items():
            self._scene.removeItem(item)
        self._scene.clear()
        for node_item in self._nodes.values():
            self._scene.addItem(node_item)
        for edge_items in self._edges.values():
            for edge_item in edge_items:
                edge_item.add_to_scene(self._scene)

        self._scene.setSceneRect(self._scene.itemsBoundingRect())
        if self._options['auto_fit']:
            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.depth_combo_box.setEnabled(False)
        self._widget.directions_combo_box.setEnabled(False)
        self._widget.package_type_combo_box.setEnabled(False)
        self._widget.filter_line_edit.setEnabled(False)
        self._widget.with_stacks_check_box.setEnabled(False)
        self._widget.mark_check_box.setEnabled(False)
        self._widget.colorize_check_box.setEnabled(False)
        self._widget.hide_transitives_check_box.setEnabled(False)

        self._update_graph(dotcode)
        self._redraw_graph_scene()

    @Slot()
    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'), 'rospackgraph.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'), 'rospackgraph.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'), 'rospackgraph.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 _clear_filter(self):
        if not self._filtering_started:
            self._widget.filter_line_edit.setText('')
            self._filtering_started = True
Esempio n. 21
0
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()
Esempio n. 22
0
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()
Esempio n. 23
0
class DsdVizPlugin(Plugin):
    def __init__(self, context):
        super(DsdVizPlugin, self).__init__(context)

        self._initialized = False  # This gets set to true once the plugin hast completely finished loading

        # Ensure startup state
        self.freeze = False  # Controls whether the state should be updated from remote
        self.locations = parse_locations_yaml()
        self.dsd = None  # type: DsdSlave
        self._init_plugin(context)

        # Performance optimization variables
        self._prev_dotgraph = None
        self._prev_QItemModel = None

    def _init_plugin(self, context):
        self.setObjectName("DSD-Visualization")

        self._widget = QWidget()
        self._widget.setObjectName(self.objectName())

        # load qt ui definition from file
        rp = rospkg.RosPack()
        ui_file = os.path.join(
            rp.get_path("dynamic_stack_decider_visualization"), "resource",
            "StackmachineViz.ui")
        loadUi(ui_file, self._widget,
               {"InteractiveGraphicsView": InteractiveGraphicsView})

        # initialize qt scene
        self._scene = QGraphicsScene()
        self._scene.setBackgroundBrush(Qt.white)
        self._widget.graphics_view.setScene(self._scene)

        # Bind fit-in-view button
        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)

        # Fit-in-view on checkbox toggle
        self._widget.auto_fit_graph_check_box.toggled.connect(self.fit_in_view)

        # Freezing
        def toggle_freeze():
            self.freeze = not self.freeze
            self.refresh()

        self._widget.freeze_push_button.toggled.connect(toggle_freeze)

        # Exporting and importing
        self._widget.save_as_svg_push_button.pressed.connect(
            self.save_svg_to_file)

        # Fill choices for dsd_selector and bind on_select
        self._widget.dsd_selector_combo_box.addItem("Select DSD...")
        for choice in self.locations:
            self._widget.dsd_selector_combo_box.addItem(choice['display_name'])
        self._widget.dsd_selector_combo_box.currentTextChanged.connect(
            self.set_dsd)

        context.add_widget(self._widget)

        # Start a timer that calls back every 100 ms
        self._timer_id = self.startTimer(100)

    def save_settings(self, plugin_settings, instance_settings):
        super(DsdVizPlugin, self).save_settings(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):
        super(DsdVizPlugin, self).restore_settings(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()

    def save_svg_to_file(self):
        file_name, _ = QFileDialog.getSaveFileName(
            self._widget, self.tr("Save as SVG"), "stackmachine.svg",
            self.tr("Scalable Vector Graphic (*.svg)"))

        if file_name is not None and file_name != "":
            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 timerEvent(self, timer_event):
        """This gets called by QT whenever the timer ticks"""

        if not self.freeze:
            self.refresh()

        # Automatically fit in view if the checkbox is checked
        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)

    def refresh(self):
        """Refresh the complete drawn representation"""

        if not self._initialized:
            self._render_messages(
                "The plugin is not yet completely initialized. Please wait...")

        elif self.dsd is None:
            self._render_messages("No DSD selected")

        else:
            self._render_dotgraph(self.dsd.to_dotgraph())
            self._render_debug_data(self.dsd.to_QItemModel())

    def _render_messages(self, *messages):
        """Render simple messages on the canvas"""

        msg_dot = pydot.Dot()

        for msg in messages:
            msg_dot.add_node(pydot.Node(str(uuid.uuid4()), label=str(msg)))

        self._render_dotgraph(msg_dot)
        self._render_debug_data(QStandardItemModel(self._scene))

    def _render_dotgraph(self, dotgraph):
        """
        Render the specified dotgraph on canvas

        :type dotgraph: pydot.Dot
        """

        # Only redraw when the graph differs from the previous one
        if self._prev_dotgraph == dotgraph:
            return
        else:
            self._prev_dotgraph = dotgraph

        self._scene.clear()

        if self._widget.highlight_connections_check_box.isChecked():
            highlight_level = 3
        else:
            highlight_level = 1

        # Generate qt items from dotcode
        dotcode = PydotFactory().create_dot(dotgraph)
        nodes, edges = DotToQtGenerator().dotcode_to_qt_items(
            dotcode, highlight_level, same_label_siblings=False)

        # Add generated items to scene
        for node_item in nodes:
            self._scene.addItem(nodes.get(node_item))
        for edge_items in edges:
            for edge_item in edges.get(edge_items):
                edge_item.add_to_scene(self._scene)

        self._scene.setSceneRect(self._scene.itemsBoundingRect())

    def _render_debug_data(self, qitem_model):
        """Render debug data in the tree view on the right side of the scene"""

        # Only redraw when the item-model differs from the previous one
        if self._prev_QItemModel == qitem_model:
            return
        else:
            self._prev_QItemModel = qitem_model
            self._widget.stack_prop_tree_view.setModel(qitem_model)
            self._widget.stack_prop_tree_view.expandAll()

    def set_dsd(self, name):
        """
        Set the target dsd

        :param name: display_name of any dsd in the locations.yaml
        """
        # close debug connection of old dsd
        if self.dsd is not None:
            self.dsd.close()

        if name == 'Select DSD...':
            self.dsd = None
            return

        # Search for dsd_data in locations.yaml
        for i in self.locations:
            if i['display_name'] == name:
                dsd_data = i
                break
        else:
            raise ValueError('no dsd with name {} found'.format(name))

        # Figure out full paths with the help of rospkg
        rospack = rospkg.RosPack()
        dsd_path = rospack.get_path(dsd_data['package'])
        actions_path = os.path.join(dsd_path, dsd_data['relative_action_path'])
        decisions_path = os.path.join(dsd_path,
                                      dsd_data['relative_decision_path'])
        behaviour_path = os.path.join(dsd_path, dsd_data['relative_dsd_path'])

        # Initialize dsd instance
        dsd = DsdSlave(dsd_data['debug_topic'])
        dsd.register_actions(actions_path)
        dsd.register_decisions(decisions_path)
        dsd.load_behavior(behaviour_path)
        dsd.initialized = True

        self.dsd = dsd
Esempio n. 24
0
class Eyedrop(Plugin):
    def __init__(self, context):
        super(Eyedrop, self).__init__(context)
        # Give QObjects reasonable names
        self.setObjectName('Eyedrop')
        rp = rospkg.RosPack()

        # Process standalone plugin command-line arguments
        from argparse import ArgumentParser
        parser = ArgumentParser()
        # Add argument(s) to the parser.
        parser.add_argument("-q",
                            "--quiet",
                            action="store_true",
                            dest="quiet",
                            help="Put plugin in silent mode")
        self.args, unknowns = parser.parse_known_args(context.argv())

        # Create QWidget
        self._widget = QWidget()
        # Get path to UI file which is a sibling of this file
        # in this example the .ui and .py file are in the same folder
        ui_file = os.path.join(rp.get_path('rqt_eyedropper'), 'resource',
                               'Eyedrop.ui')
        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self._widget)
        # Give QObjects reasonable names
        self._widget.setObjectName('EyedropUi')
        # Show _widget.windowTitle on left-top of each plugin (when
        # it's set in _widget). This is useful when you open multiple
        # plugins at once. Also if you open multiple instances of your
        # plugin at once, these lines add number to make it easy to
        # tell from pane to pane.
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() +
                                        (' (%d)' % context.serial_number()))
        # Add widget to the user interface
        context.add_widget(self._widget)

        self._widget.button_refresh_topics.clicked.connect(
            self.button_refresh_topics_pressed)
        self._widget.button_take_sample.clicked.connect(
            self.button_take_sample_pressed)

        self._widget.combo_box_topic_image.currentIndexChanged.connect(
            self.do_subscribe_image)
        self._widget.combo_box_topic_point.currentIndexChanged.connect(
            self.do_subscribe_point)

        self.colour_pane_scene = QGraphicsScene()
        self.colour_pane_scene.setBackgroundBrush(QBrush(QColor(0, 0, 0)))
        self._widget.graphics_view_colour_pane.setScene(self.colour_pane_scene)
        self._widget.graphics_view_colour_pane.show()

        self.bridge = CvBridge()

        self.do_refresh_topic_lists()

    def shutdown_plugin(self):
        try:
            self.image_sub.unregister()
        except:
            pass

        try:
            self.point_sub.unregister()
        except:
            pass

    def save_settings(self, plugin_settings, instance_settings):
        # TODO save intrinsic configuration, usually using:
        # instance_settings.set_value(k, v)
        pass

    def restore_settings(self, plugin_settings, instance_settings):
        # TODO restore intrinsic configuration, usually using:
        # v = instance_settings.value(k)
        pass

    #def trigger_configuration(self):
    # Comment in to signal that the plugin has a way to configure
    # This will enable a setting button (gear icon) in each dock widget title bar
    # Usually used to open a modal configuration dialog

    def do_get_topic_list(self, msg_type):
        topics = rospy.get_published_topics()

        match_topics = []

        for t in topics:
            if t[1] == msg_type:
                match_topics.append(t[0])

        return match_topics

    def do_refresh_topic_lists(self):
        self._widget.combo_box_topic_image.clear()
        self._widget.combo_box_topic_point.clear()
        self._widget.combo_box_topic_image.addItem('')
        self._widget.combo_box_topic_point.addItem('')

        for it in self.do_get_topic_list('sensor_msgs/Image'):
            self._widget.combo_box_topic_image.addItem(it)

        for pt in self.do_get_topic_list('geometry_msgs/Point'):
            self._widget.combo_box_topic_point.addItem(pt)

    def button_refresh_topics_pressed(self):
        self.do_refresh_topic_lists()

    def button_take_sample_pressed(self):
        self.do_take_sample()

    def do_subscribe_image(self):
        topic_name = self._widget.combo_box_topic_image.currentText()

        rospy.loginfo("image topic: %s" % topic_name)

        if topic_name:
            self.image_sub = rospy.Subscriber(topic_name, Image,
                                              self.image_callback)
        else:
            try:
                self.image_sub.unregister()
            except:
                pass

    def do_subscribe_point(self):
        topic_name = self._widget.combo_box_topic_point.currentText()

        rospy.loginfo("point topic: %s" % topic_name)

        if topic_name:
            self.point_sub = rospy.Subscriber(topic_name, Point,
                                              self.point_callback)
        else:
            try:
                self.point_sub.unregister()
            except:
                pass

    def do_take_sample(self):
        try:
            px_x = int(self._widget.spinbox_pixel_location_x.value())
            px_y = int(self._widget.spinbox_pixel_location_y.value())

            b = self.cv_image.item(px_y, px_x, 0)
            g = self.cv_image.item(px_y, px_x, 1)
            r = self.cv_image.item(px_y, px_x, 2)

            px_bgr = np.zeros((1, 1, 3), np.uint8)
            px_bgr[:] = tuple((b, g, r))
            px_hsv = cv2.cvtColor(px_bgr, cv2.COLOR_BGR2HSV)

            h = px_hsv.item(0, 0, 0)
            s = px_hsv.item(0, 0, 1)
            v = px_hsv.item(0, 0, 2)

            self._widget.line_edit_pixel_b.setText(str(b))
            self._widget.line_edit_pixel_g.setText(str(g))
            self._widget.line_edit_pixel_r.setText(str(r))

            self._widget.line_edit_pixel_h.setText(str(h))
            self._widget.line_edit_pixel_s.setText(str(s))
            self._widget.line_edit_pixel_v.setText(str(v))

            self.colour_pane_scene.setForegroundBrush(QBrush(QColor(r, g, b)))
            self._widget.graphics_view_colour_pane.show()

            if not self.args.quiet:
                rospy.loginfo("----------------")
                rospy.loginfo("XY:  [%d, %d]" % (px_x, px_y))
                rospy.loginfo("RGB: [%d, %d, %d]" % (r, g, b))
                rospy.loginfo("HSV: [%d, %d, %d]" % (h, s, v))

        except Exception as e:
            rospy.loginfo(e)

    def point_callback(self, data):
        self._widget.spinbox_pixel_location_x.setValue(int(data.x))
        self._widget.spinbox_pixel_location_y.setValue(int(data.y))

        self.do_take_sample()

    def image_callback(self, data):
        self._widget.spinbox_pixel_location_x.setMaximum(data.width - 1)
        self._widget.spinbox_pixel_location_y.setMaximum(data.height - 1)

        self._widget.spinbox_pixel_location_x.setEnabled(True)
        self._widget.spinbox_pixel_location_y.setEnabled(True)
        self._widget.button_take_sample.setEnabled(True)

        try:
            self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            rospy.loginfo(e)

        if self._widget.check_box_auto_update.isChecked():
            self.do_take_sample()
Esempio n. 25
0
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))

        self.graph_changed(self._current_graph)

    # 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.graph_changed(self._current_graph)

        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_key, edge_items in self._current_graph.edges.iteritems(
        ):
            for edge_item in self._current_graph.edges[edge_items_key]:
                edge_item.add_to_scene(self._scene)

                if (edge_item._label is not None):
                    edge_item._label.mouseDoubleClickEvent = partial(
                        self._mouse_double_click_callback, edge_items_key)
                if (edge_item._arrow is not None):
                    edge_item._arrow.mouseDoubleClickEvent = partial(
                        self._mouse_double_click_callback, edge_items_key)

        self._scene.setSceneRect(self._scene.itemsBoundingRect())

    def _fit_to_view(self):
        self.graphics_view.fitInView(self._scene.itemsBoundingRect(),
                                     Qt.KeepAspectRatio)

    def _mouse_double_click_callback(self, edge_item, qt_event=None):
        if (qt_event == None):
            return
        self.mouseDoubleClickOnEvent(self._current_graph, edge_item)

    def mouseDoubleClickOnEvent(self, graph, event_name):
        print(graph, event_name)
        pass

    def graph_changed(self, graph):
        pass
Esempio n. 26
0
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.
    """

    paused = Signal(bool)
    position_changed = Signal(int)
    redraw = Signal()

    def __init__(self, parent=None):
        """Cannot take args other than parent due to loadUi limitation."""

        super(TimelineView, self).__init__(parent=parent)
        self._timeline_marker = QIcon.fromTheme('system-search')

        self._min = 0
        self._max = 0
        self._xpos_marker = -1

        self._timeline_marker_width = 15
        self._timeline_marker_height = 15
        self._last_marker_at = -1

        self.setUpdatesEnabled(True)
        self._scene = QGraphicsScene(self)
        self.setScene(self._scene)

        self._levels = None

        self.redraw.connect(self._signal_redraw)

    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
        """
        # Pause the timeline
        self.paused.emit(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(max(len(self._levels), 1))
        position = int(floor(x / width_cell))
        if position == len(self._levels) - 1:
            return -1
        return position

    @Slot(int)
    def set_marker_pos(self, xpos):
        """
        Set marker position from index

        :param xpos: Marker index
        """
        if self._levels is None:
            rospy.logwarn('Called set_marker_pos before set_levels')
            return

        if xpos == -1:
            # stick to the latest when position is -1
            self._xpos_marker = -1
            # check if we chose latest item
            if self._last_marker_at != self._xpos_marker:
                # update variable to check for change during next round
                self._last_marker_at = self._xpos_marker
                # emit change to all timeline_panes
                self.position_changed.emit(self._xpos_marker)
            self.redraw.emit()
            return

        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._levels):
            # 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.position_changed.emit(self._xpos_marker)
        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(list)
    def set_levels(self, levels):
        self._levels = levels
        self.redraw.emit()

    @Slot()
    def _signal_redraw(self):
        """
        Gets called either when new msg comes in or when marker is moved by
        user.
        """
        if self._levels is None:
            return

        # update the limits
        self._min = 0
        self._max = len(self._levels) - 1

        self._scene.clear()

        qsize = self.size()
        width_tl = qsize.width()

        w = width_tl / float(max(len(self._levels), 1))
        is_enabled = self.isEnabled()

        for i, level in enumerate(self._levels):
            h = self.viewport().height()

            # Figure out each cell's color.
            qcolor = QColor('grey')
            if is_enabled and level is not None:
                if level > DiagnosticStatus.ERROR:
                    # Stale items should be reported as errors unless all stale
                    level = DiagnosticStatus.ERROR
                qcolor = util.level_to_color(level)
#  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)

        # Getting marker index.
        xpos_marker = self._xpos_marker

        # If marker is -1 for latest use (number_of_cells -1)
        if xpos_marker == -1:
            xpos_marker = len(self._levels) - 1

        # Convert get horizontal pixel value of selected cell's center
        xpos_marker_in_pixel = (xpos_marker * w + (w / 2.0) -
                                (self._timeline_marker_width / 2.0))
        pos_marker = QPointF(xpos_marker_in_pixel, 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)
Esempio n. 27
0
class Ros2KnowledgeGraph(Plugin):

    _deferred_fit_in_view = Signal()

    def __init__(self, context):
        super(Ros2KnowledgeGraph, self).__init__(context)

        self._node = context.node
        self._logger = self._node.get_logger().get_child(
            'ros2_knowledge_graph_viewer.ros2_knowledge_graph.Ros2KnowledgeGraph'
        )
        self.setObjectName('Ros2KnowledgeGraph')

        self._ros2_knowledge_graph = Ros2KnowledgeGraphImpl()
        self._current_dotcode = None

        self._widget = QWidget()

        self.dotcode_factory = PydotFactory()
        self.dotcode_generator = Ros2KnowledgeGraphDotcodeGenerator()
        self.dot_to_qt = DotToQtGenerator()

        _, package_path = get_resource('packages',
                                       'ros2_knowledge_graph_viewer')
        ui_file = os.path.join(package_path, 'share',
                               'ros2_knowledge_graph_viewer', 'resource',
                               'Ros2KnowledgeGraph.ui')
        loadUi(ui_file, self._widget,
               {'InteractiveGraphicsView': InteractiveGraphicsView})
        self._widget.setObjectName('Ros2KnowledgeGraphUi')
        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.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_ros2_knowledge_graph()
        self._deferred_fit_in_view.connect(self._fit_in_view,
                                           Qt.QueuedConnection)
        self._deferred_fit_in_view.emit()

        context.add_widget(self._widget)

        self._updateTimer = QtCore.QTimer()
        self._updateTimer.timeout.connect(self.do_update)
        self._updateTimer.start(10)

    def do_update(self):
        # print("Spinnning")
        rclpy.spin_once(self._ros2_knowledge_graph, timeout_sec=0.01)
        # print("Spinned")

        self._update_ros2_knowledge_graph()

        self._updateTimer = QtCore.QTimer()
        self._updateTimer.timeout.connect(self.do_update)
        self._updateTimer.start(10)

    def _update_ros2_knowledge_graph(self):
        self._refresh_ros2_knowledge_graph()

    def _refresh_ros2_knowledge_graph(self):
        # print("_refresh_ros2_knowledge_graph")
        self._update_graph_view(self._generate_dotcode())

    def _generate_dotcode(self):
        return self.dotcode_generator.generate_dotcode(
            ros2_knowledge_graphinst=self._ros2_knowledge_graph,
            dotcode_factory=self.dotcode_factory)

    def _update_graph_view(self, dotcode):
        # print("_update_graph_view")
        if dotcode == self._current_dotcode:
            return
        self._current_dotcode = dotcode
        self._redraw_graph_view()

    def _redraw_graph_view(self):
        # print("_redraw_graph_view")
        self._scene.clear()

        # layout graph and create qt items
        (nodes,
         edges) = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode,
                                                     highlight_level=3,
                                                     same_label_siblings=True,
                                                     scene=self._scene)

        self._scene.setSceneRect(self._scene.itemsBoundingRect())
        self._fit_in_view()

    def _fit_in_view(self):
        self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(),
                                             Qt.KeepAspectRatio)

    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)
Esempio n. 28
0
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)
    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
Esempio n. 30
0
    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
Esempio n. 31
0
class NavView(QGraphicsView):
    map_changed = Signal()
    path_changed = Signal(str)
    polygon_changed = Signal(str)

    def __init__(self,
                 map_topic='/map',
                 paths=None,
                 polygons=None,
                 tf_listener=None,
                 parent=None):
        super(NavView, self).__init__()
        if paths is None:
            paths = [
                '/move_base/SBPLLatticePlanner/plan',
                '/move_base/TrajectoryPlannerROS/local_plan'
            ]
        if polygons is None:
            polygons = ['/move_base/local_costmap/robot_footprint']
        if tf_listener is None:
            tf_listener = tf.TransformListener()

        self._parent = parent

        self._pose_mode = False
        self._goal_mode = False
        self.last_path = None
        self.drag_start = None

        self.map_changed.connect(self._update)
        self.destroyed.connect(self.close)

        # ScrollHandDrag
        self.setDragMode(QGraphicsView.ScrollHandDrag)

        self._map = None
        self._map_hash = None
        self._map_item = None

        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = None
        self.frame_id = ""

        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_listener
        self.map_sub = rospy.Subscriber(map_topic, 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()
        try:
            delta = event.angleDelta().y()
        except AttributeError:
            delta = event.delta()
        if delta > 0:
            self.scale(1.15, 1.15)
        else:
            self.scale(0.85, 0.85)

    def map_cb(self, msg):
        map_hash = hash(msg.data)
        if map_hash == self._map_hash:
            rospy.logdebug("Skipping map cb, because the map is the same")
            return

        self._map_hash = map_hash

        self.map_resolution = msg.info.resolution
        self.map_width = msg.info.width
        self.map_height = msg.info.height
        self.map_origin = msg.info.origin
        self.frame_id = msg.header.frame_id

        a = numpy.array(msg.data, dtype=numpy.uint8, copy=False, order='C')
        a = a.reshape((self.map_height, self.map_width))
        if self.map_width % 4:
            e = numpy.empty((self.map_height, 4 - self.map_width % 4),
                            dtype=a.dtype,
                            order='C')
            a = numpy.append(a, e, axis=1)
        image = QImage(a.reshape((a.shape[0] * a.shape[1])), self.map_width,
                       self.map_height, 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.map_width, self.map_height)
        self.map_changed.emit()

    def add_path(self, name):
        path = PathInfo(name)

        def cb(msg):
            if not self._map:
                return

            pp = QPainterPath()

            # Transform everything in to the map frame
            if not (msg.header.frame_id == self.frame_id
                    or msg.header.frame_id == ''):
                try:
                    self._tf.waitForTransform(msg.header.frame_id,
                                              self.frame_id, rospy.Time(),
                                              rospy.Duration(10))
                    data = [
                        self._tf.transformPose(self.frame_id, 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(*self.point_map_to_qt((start.x, start.y)))

                for pose in data:
                    pt = pose.pose.position
                    pp.lineTo(*self.point_map_to_qt((pt.x, pt.y)))

                path.path = pp
                self.path_changed.emit(name)

        path.color = random.choice(self._colors)
        self._colors.remove(path.color)

        path.cb = cb
        path.sub = rospy.Subscriber(path.name, Path, path.cb)

        self._paths[name] = path

    def add_polygon(self, name):
        poly = PathInfo(name)

        def cb(msg):
            if not self._map:
                return

            if not (msg.header.frame_id == self.frame_id
                    or msg.header.frame_id == ''):
                try:
                    self._tf.waitForTransform(msg.header.frame_id,
                                              self.frame_id, 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 = []
                    for pt in points_stamped:
                        point = self._tf.transformPoint(self.frame_id,
                                                        pt).point
                        trans_pts.append((point.x, point.y))
                except tf.Exception:
                    rospy.logerr("TF Error")
                    trans_pts = []
            else:
                trans_pts = [(pt.x, pt.y) for pt in msg.polygon.points]

            if len(trans_pts) > 0:
                trans_pts.append(trans_pts[0])
                pts = [QPointF(*self.point_map_to_qt(pt)) for pt in trans_pts]
                poly.path = QPolygonF(pts)

                self.polygon_changed.emit(name)

        poly.color = random.choice(self._colors)
        self._colors.remove(poly.color)

        poly.cb = cb
        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):
        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))
        v = (v[0] / mag, v[1] / mag)  # Normalize diff vector
        u = (-v[1], v[0])  # Project diff vector to mirrored map

        if self.last_path:
            self._scene.removeItem(self.last_path)
            self.last_path = None

        res = (v[0] * 25, v[1] * 25)

        if self._pose_mode:
            pen = QPen(QColor("red"))
        elif self._goal_mode:
            pen = QPen(QColor("green"))
        self.last_path = self._scene.addLine(self.drag_start[0],
                                             self.drag_start[1],
                                             self.drag_start[0] + res[0],
                                             self.drag_start[1] + res[1], pen)

        map_p = self.point_qt_to_map(self.drag_start)

        angle = atan2(u[0], u[1])
        quat = quaternion_from_euler(0, 0, angle)

        self.drag_start = None

        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:
            map_p, quat = self.draw_position(e)
            self.goal_mode(
            )  # Disable goal_mode and enable dragging/scrolling again

            msg = PoseStamped()
            msg.header.frame_id = self.frame_id
            msg.header.stamp = rospy.Time.now()

            msg.pose.position.x = map_p[0]
            msg.pose.position.y = map_p[1]
            msg.pose.orientation.z = quat[2]
            msg.pose.orientation.w = quat[3]

            self._goal_pub.publish(msg)

        elif self._pose_mode:
            map_p, quat = self.draw_position(e)
            self.pose_mode(
            )  # Disable pose_mode and enable dragging/scrolling again

            msg = PoseWithCovarianceStamped()
            msg.header.frame_id = self.frame_id
            msg.header.stamp = rospy.Time.now()

            # ToDo: Is it ok to just ignore the covariance matrix here?
            msg.pose.pose.orientation.z = quat[2]
            msg.pose.pose.orientation.w = quat[3]
            msg.pose.pose.position.x = map_p[0]
            msg.pose.pose.position.y = map_p[1]

            self._pose_pub.publish(msg)

    def close(self):
        if self.map_sub:
            self.map_sub.unregister()
        for p in self._paths.values():
            if p.sub:
                p.sub.unregister()

        for p in self._polygons.values():
            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)))

        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)))

        if old_item:
            self._scene.removeItem(old_item)

    def _mirror(self, item):
        """
        Mirror any QItem to have correct orientation
        :param item:
        :return:
        """
        item.setTransform(QTransform().scale(1, -1).translate(
            0, -self.map_height))

    def point_qt_to_map(self, point):
        """
        Convert point from Qt to map coordinates

        :param point: tuple or list
        :return: map point
        """
        # Mirror point over y axis
        x = point[0]
        y = self.map_height - point[1]

        # Orientation might need to be taken into account
        return [
            x * self.map_resolution + self.map_origin.position.x,
            y * self.map_resolution + self.map_origin.position.y
        ]

    def point_map_to_qt(self, point):
        """
        Convert point from map to qt coordinates

        :param point: tuple or list
        :return: map point
        """
        # Orientation might need to be taken into account
        x = (point[0] - self.map_origin.position.x) / self.map_resolution
        y = (point[1] - self.map_origin.position.y) / self.map_resolution

        # Mirror point over y axis
        return [x, self.map_height - y]
Esempio n. 32
0
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)
Esempio n. 33
0
class DataAcquisition(Plugin):

    _deferred_fit_in_view = Signal()

    def __init__(self, context):
        super(DataAcquisition, self).__init__(context)
        self.initialized = False
        self.setObjectName('DataAcquisition')

        self._graph = None
        self._current_dotcode = None

        self._widget = QWidget()

        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('data_acquisition_2d'), 'resource',
                               'data_acquisition.ui')
        loadUi(ui_file, self._widget,
               {'InteractiveGraphicsView': InteractiveGraphicsView})
        self._widget.setObjectName('DataAcquisitionUi')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() +
                                        (' (%d)' % context.serial_number()))

        # Fixed size
        width = 670
        height = 570
        # setting  the fixed size of window
        self._widget.setFixedSize(width, height)

        # Get a list of Image topic
        self._get_image_topics()

        # Robot mode : disable GUI when scanning
        self._in_scanning = False
        self._scan_time = 0
        self._image_topic = None

        # canvas : image will be load here
        self._scene = QGraphicsScene()
        self._scene.setBackgroundBrush(Qt.red)
        self._scene.setSceneRect(0, 0, 640, 480)
        self._widget.canvas.setScene(self._scene)
        self._set_image()

        # Button Event
        self._widget.setup_robot_push_button.pressed.connect(self._setup_robot)
        self._widget.save_ground_truth_push_button.pressed.connect(
            self._save_ground_truth)
        self._widget.create_bbox_push_button.pressed.connect(self._create_bbox)
        self._widget.start_scanning_push_button.pressed.connect(
            self._start_scanning)

        # Box event
        self._widget.scan_time_spin_box.valueChanged.connect(
            self._set_scan_time)
        self._widget.image_topic_combo_box.currentIndexChanged.connect(
            self._set_image_topic)

        context.add_widget(self._widget)

    def _setup_robot(self):
        print("_setup_robot")

    def _save_ground_truth(self):
        print("_save_ground_truth")

    def _create_bbox(self):
        print("_create_bbox for image from topic : {} ".format(
            self._widget.image_topic_combo_box.itemData(
                self._widget.image_topic_combo_box.currentIndex())))

    def _start_scanning(self):
        print("_start_scanning")

    def _set_scan_time(self):
        if not self._in_scanning:
            self._scan_time = self._widget.scan_time_spin_box.value()
            print("_set_scan_time : {}".format(
                self._widget.scan_time_spin_box.value()))
        else:
            print("Is scanning. Can't set the value")

    def _get_image_topics(self):

        topic_list = rospy.get_published_topics()
        index = 0
        for topic in topic_list:
            # Remove topic which doesn't contain Image from list
            try:
                msg = rospy.wait_for_message(topic, Image, timeout=0.1)
                self._widget.image_topic_combo_box.insertItem(
                    index, self.tr(topic), topic)
                index = index + 1
            except Exception as err:
                continue

        self._widget.image_topic_combo_box.insertItem(0, self.tr("aaaaa"),
                                                      "aaaaa")
        self._widget.image_topic_combo_box.insertItem(1, self.tr("bbbbb"),
                                                      "bbbbb")
        self._widget.image_topic_combo_box.setCurrentIndex(0)

    def _set_image_topic(self):
        print(self._widget.image_topic_combo_box.currentIndex())
        print(
            self._widget.image_topic_combo_box.itemData(
                self._widget.image_topic_combo_box.
                curre_current_dotcodentIndex()))
        print("_set_image_topic : {}".format(self._image_topic))

        self._set_image()

    def _set_image(self):
        print("_set_image")
        self._scene.clear()
        img = cv2.imread("/home/gachiemchiep/Pictures/aaa.png", 1)
        img_rz = cv2.resize(img, (640, 480))

        # https://stackoverflow.com/questions/34232632/convert-python-opencv-image-numpy-array-to-pyqt-qpixmap-image
        height, width, channel = img_rz.shape
        print(img_rz.shape)
        bytesPerLine = 3 * width
        img_q = QImage(img_rz.data, width, height, bytesPerLine,
                       QImage.Format_RGB888).rgbSwapped()

        # painter : this does not work
        # painter = QPainter(img_q)
        # painter.setRenderHint(QPainter.Antialiasing)
        # self._scene.render(painter)
        # painter.end()

        # pixmap : this work
        pixmap = QPixmap.fromImage(img_q)
        self._scene.addPixmap(pixmap)

        self._scene.setSceneRect(0, 0, 640, 480)

    def _draw_bbox_start(self, event):
        print("_draw_bbox_start: {}".format(event.pos()))

    def _draw_bbox_end(self, event):
        print("_draw_bbox_end: {}".format(event.pos()))
Esempio n. 34
0
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
Esempio n. 35
0
    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)
Esempio n. 36
0
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)
Esempio n. 37
0
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.show_system_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(
            'show_system_state',
            self._widget.show_system_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.show_system_check_box.setChecked(
            instance_settings.value('show_system_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._widget.show_system_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(
            )
        self._options[
            'show_system'] = self._widget.show_system_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'],
            show_system=self._options['show_system'],
            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 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_scene(self):
        # remove items in order to not garbage nodes which will be continued to be used
        for item in self._scene.items():
            self._scene.removeItem(item)
        self._scene.clear()
        for node_item in self._nodes.values():
            self._scene.addItem(node_item)
        for edge_items in self._edges.values():
            for edge_item in edge_items:
                edge_item.add_to_scene(self._scene)

        self._scene.setSceneRect(self._scene.itemsBoundingRect())
        if self._options['auto_fit']:
            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.depth_combo_box.setEnabled(False)
        self._widget.directions_combo_box.setEnabled(False)
        self._widget.package_type_combo_box.setEnabled(False)
        self._widget.filter_line_edit.setEnabled(False)
        self._widget.with_stacks_check_box.setEnabled(False)
        self._widget.mark_check_box.setEnabled(False)
        self._widget.colorize_check_box.setEnabled(False)
        self._widget.hide_transitives_check_box.setEnabled(False)
        self._widget.show_system_check_box.setEnabled(False)

        self._update_graph(dotcode)
        self._redraw_graph_scene()

    @Slot()
    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'), 'rospackgraph.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'), 'rospackgraph.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'), 'rospackgraph.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 _clear_filter(self):
        if not self._filtering_started:
            self._widget.filter_line_edit.setText('')
            self._filtering_started = True
Esempio n. 38
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.show_system_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
Esempio n. 39
0
    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('data_acquisition_2d'), '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_spin_box.valueChanged.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.unreachable_check_box.clicked.connect(
            self._refresh_rosgraph)
        self._widget.group_tf_check_box.clicked.connect(self._refresh_rosgraph)
        self._widget.hide_tf_nodes_check_box.clicked.connect(
            self._refresh_rosgraph)
        self._widget.group_image_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)
Esempio n. 40
0
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()
        try:
            delta = event.angleDelta().y()
        except AttributeError:
            delta = event.delta()
        if 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
Esempio n. 41
0
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('data_acquisition_2d'), '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_spin_box.valueChanged.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.unreachable_check_box.clicked.connect(
            self._refresh_rosgraph)
        self._widget.group_tf_check_box.clicked.connect(self._refresh_rosgraph)
        self._widget.hide_tf_nodes_check_box.clicked.connect(
            self._refresh_rosgraph)
        self._widget.group_image_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_spin_box_value',
            self._widget.namespace_cluster_spin_box.value())
        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(
            'unreachable_check_box_state',
            self._widget.unreachable_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())
        instance_settings.set_value(
            'group_tf_check_box_state',
            self._widget.group_tf_check_box.isChecked())
        instance_settings.set_value(
            'hide_tf_nodes_check_box_state',
            self._widget.hide_tf_nodes_check_box.isChecked())
        instance_settings.set_value(
            'group_image_check_box_state',
            self._widget.group_image_check_box.isChecked())
        instance_settings.set_value(
            'hide_dynamic_reconfigure_check_box_state',
            self._widget.hide_dynamic_reconfigure_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_spin_box.setValue(
            int(instance_settings.value('namespace_cluster_spin_box_value',
                                        2)))
        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.unreachable_check_box.setChecked(
            instance_settings.value('unreachable_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._widget.hide_tf_nodes_check_box.setChecked(
            instance_settings.value('hide_tf_nodes_check_box_state', False) in
            [True, 'true'])
        self._widget.group_tf_check_box.setChecked(
            instance_settings.value('group_tf_check_box_state', True) in
            [True, 'true'])
        self._widget.group_image_check_box.setChecked(
            instance_settings.value('group_image_check_box_state', True) in
            [True, 'true'])
        self._widget.hide_dynamic_reconfigure_check_box.setChecked(
            instance_settings.value('hide_dynamic_reconfigure_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_spin_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._widget.unreachable_check_box.setEnabled(True)
        self._widget.group_tf_check_box.setEnabled(True)
        self._widget.hide_tf_nodes_check_box.setEnabled(True)
        self._widget.group_image_check_box.setEnabled(True)
        self._widget.hide_dynamic_reconfigure_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'
        namespace_cluster = self._widget.namespace_cluster_spin_box.value()
        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()
        unreachable = self._widget.unreachable_check_box.isChecked()
        group_tf_nodes = self._widget.group_tf_check_box.isChecked()
        hide_tf_nodes = self._widget.hide_tf_nodes_check_box.isChecked()
        group_image_nodes = self._widget.group_image_check_box.isChecked()
        hide_dynamic_reconfigure = self._widget.hide_dynamic_reconfigure_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,
            unreachable=unreachable,
            group_tf_nodes=group_tf_nodes,
            hide_tf_nodes=hide_tf_nodes,
            group_image_nodes=group_image_nodes,
            hide_dynamic_reconfigure=hide_dynamic_reconfigure)

    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,
            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_spin_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._widget.unreachable_check_box.setEnabled(False)
        self._widget.group_tf_check_box.setEnabled(False)
        self._widget.hide_tf_nodes_check_box.setEnabled(False)
        self._widget.group_image_check_box.setEnabled(False)
        self._widget.hide_dynamic_reconfigure_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)
Esempio n. 42
0
class Widget(QGraphicsView):

    def __init__(self, parent=None):
        super(Widget, self).__init__(parent)
        self.setWindowTitle('U-CTF View')

        self._scene = QGraphicsScene()
        self._scene.setBackgroundBrush(Qt.white)

        # game cube grass area
        grass_color = QColor(0, 255, 0, 50)
        self._scene.addRect(
            0, 0, CUBE_LENGTH, CUBE_LENGTH,
            QPen(grass_color), QBrush(grass_color))
        # blue team area
        blue_color = QColor(0, 0, 255, 50)
        self._scene.addRect(
            0, -SUPPORT_AREA_DEPTH, CUBE_LENGTH, SUPPORT_AREA_DEPTH,
            QPen(blue_color), QBrush(blue_color))
        # gold team area
        gold_color = QColor(255, 215, 0, 50)
        self._scene.addRect(
            0, CUBE_LENGTH, CUBE_LENGTH, SUPPORT_AREA_DEPTH,
            QPen(gold_color), QBrush(gold_color))
        # penalty area
        orange_color = QColor(0, 200, 0, 50)
        self._scene.addRect(
            CUBE_LENGTH, 0, SUPPORT_AREA_DEPTH, CUBE_LENGTH,
            QPen(orange_color), QBrush(orange_color))

        # rotate view to match the coordinate system of the game cube
        self.rotate(180)
        self.setScene(self._scene)

        # pens and brushes for the vehicles
        self._pens = {
            'blue': QPen(Qt.blue, 2),
            'gold': QPen(QColor(191, 151, 0), 2),
        }
        self._brushes = {
            'blue': QBrush(Qt.blue),
            'gold': QBrush(QColor(191, 151, 0)),
        }

        self._vehicles = {}

    def update_vehicle(self, color, mav_id, vehicle_type, global_pos):
        if mav_id not in self._vehicles:
            item = self._create_vehicle_item(color, mav_id, vehicle_type)
            self._scene.addItem(item)
            self._vehicles[mav_id] = item
        else:
            item = self._vehicles[mav_id]
        cube_pos = global_to_cube(global_pos['lat'], global_pos['lon'])
        item.setPos(*cube_pos)

        # set visible area
        padding = 10
        self.fitInView(
            -SUPPORT_AREA_DEPTH - padding, -SUPPORT_AREA_DEPTH - padding,
            CUBE_LENGTH + 2.0 * (SUPPORT_AREA_DEPTH + 2),
            CUBE_LENGTH + 2.0 * (SUPPORT_AREA_DEPTH + padding),
            Qt.KeepAspectRatio)

    def _create_vehicle_item(self, color, mav_id, vehicle_type):
        if vehicle_type == MAV_TYPE_QUADROTOR:
            # draw cross
            item = QGraphicsPolygonItem(QPolygonF([
                QPointF(0, 0),
                QPointF(-3, -3),
                QPointF(3, 3),
                QPointF(0, 0),
                QPointF(-3, 3),
                QPointF(3, -3),
                QPointF(0, 0),
            ]))
        elif vehicle_type == MAV_TYPE_FIXED_WING:
            # draw circle
            item = QGraphicsEllipseItem(-3, -3, 6, 6)
        else:
            # draw square
            item = QGraphicsRectItem(-3, -3, 6, 6)
        item.setBrush(self._brushes[color])
        item.setPen(self._pens[color])
        item.setToolTip('%s #%d (%s)' % (color, mav_id, vehicle_type))
        return item
Esempio n. 43
0
class VehiclesViz(Plugin):
    def __init__(self, context):
        super(VehiclesViz, self).__init__(context)
        self.setObjectName('VehiclesViz')

        self.exposed_methods = {
            'vehicle': ['create', 'move', 'hide', 'show'],
            'rqt_vviz': ['resize', 'clear'],
            'road_marking': ['set_size', 'edit_first'],
            'draw': ['circle'],
            'remove': ['circle']
        }
        self._init_subs_and_channels()

        self.vehicles = {}

        self._widget = VehiclesVizWidget()
        ui_file = os.path.join(rospkg.RosPack().get_path('rqt_vviz'),
                               'resource', 'widget.ui')
        loadUi(ui_file, self._widget)

        self._widget.setObjectName('VehiclesViz')
        self._widget.setWindowTitle('VehiclesViz')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() +
                                        (' (%d)' % context.serial_number()))

        self._view = self._widget.findChild(QGraphicsView, 'graphicsView')
        self._view.setAlignment(QtCore.Qt.AlignLeft | QtCore.Qt.AlignTop)
        self._view.setHorizontalScrollBarPolicy(QtCore.Qt.ScrollBarAlwaysOff)
        self._view.setVerticalScrollBarPolicy(QtCore.Qt.ScrollBarAlwaysOff)

        self._view.resize(100, 100)

        self._scene = QGraphicsScene()
        self._scene.setSceneRect(0, 0,
                                 self._view.size().width(),
                                 self._view.size().height())
        self._view.setScene(self._scene)

        self._load_images()

        context.add_widget(self._widget)

        self.items_cache = {'circle': {}}

    def _init_subs_and_channels(self):
        self.channels = Channels()
        self.subs_handler = SubscriptionsHandler(self.channels)
        for group, methods in self.exposed_methods.items():
            for method in methods:
                self.subs_handler.expose(group, method)
                getattr(self.channels, group + '_' + method).connect(
                    getattr(self, 'scene_' + group + '_' + method))

    def _load_images(self):
        images_path = os.path.join(rospkg.RosPack().get_path('rqt_vviz'),
                                   'resource', 'img')
        self.images = {}
        for img_file in os.listdir(images_path):
            self.images[img_file.split('.')[0]] = QImage(
                os.path.join(images_path, img_file))

    ############# scene update methods #######################

    def scene_vehicle_create(self, name, vtype, coords, width_height):
        img = self.images[vtype]
        if width_height[0] is None:
            img = img.scaledToHeight(width_height[1])
        elif width_height[1] is None:
            img = img.scaledToWidth(width_height[0])
        else:
            img = img.scaled(width_height[0], width_height[1])
        vv = players.Vehicle(name, img, coords)
        self.vehicles[name] = vv
        self._scene.addItem(vv.pixmap_item)

    def scene_vehicle_remove(self, name):
        if name in self.vehicles:
            self._scene.removeItem(self.vehicles[name].pixmap_item)
            del self.vehicles[name]

    def scene_vehicle_move(self, name, coords):
        self.vehicles[name].moveto(coords['x'], coords['y'])
        self.vehicles[name].rotate(coords['theta'])

    def scene_vehicle_hide(self, name):
        self.vehicles[name].hide()

    def scene_vehicle_show(self, name):
        self.vehicles[name].show()

    def scene_rqt_vviz_resize(self, dim):
        self._view.resize(*dim)
        self._scene.setSceneRect(0, 0,
                                 self._view.size().width(),
                                 self._view.size().height())

    def scene_rqt_vviz_clear(self):
        self.scene_road_marking_remove()
        for vname, vv in self.vehicles.items():
            self.scene_vehicle_remove(vname)
        for circle_id, cc in self.items_cache['circle'].items():
            self.scene_remove_circle(circle_id)

    def scene_road_marking_set_size(self, dim, n):
        self.road_markings = []
        for i in range(1, n):
            self.road_markings.append(
                players.RoadMarking(dim,
                                    (self._view.size().width(),
                                     (self._view.size().height() * i) / n)))
            self.road_markings[-1].add_to_scene(self._scene)

    def scene_road_marking_edit_first(self, color, percentage):
        for marking in self.road_markings:
            marking.edit_first(color, percentage)

    def scene_road_marking_remove(self):
        if hasattr(self, 'road_markings'):
            for road_marking in self.road_markings:
                road_marking.remove_from_scene(self._scene)
            self.road_markings = []

    def scene_draw_circle(self, id, pos, radius, color):
        ellipse = QGraphicsEllipseItem(pos['x'] - radius, pos['y'] - radius,
                                       radius * 2, radius * 2)
        ellipse.setBrush(QBrush(getattr(QtCore.Qt, color.lower())))
        self._scene.addItem(ellipse)
        self.items_cache['circle'][id] = ellipse

    def scene_remove_circle(self, id):
        if id in self.items_cache['circle']:
            self._scene.removeItem(self.items_cache['circle'][id])
            del self.items_cache['circle'][id]

    ###########################################################

    def shutdown_plugin(self):
        # TODO unregister all publishers here
        pass

    def save_settings(self, plugin_settings, instance_settings):
        # TODO save intrinsic configuration, usually using:
        # instance_settings.set_value(k, v)
        pass

    def restore_settings(self, plugin_settings, instance_settings):
        # TODO restore intrinsic configuration, usually using:
        # v = instance_settings.value(k)
        pass
Esempio n. 44
0
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)