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 __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 __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 __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 __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 _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()
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)
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)
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__(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 __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 __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)
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 __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 __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)
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)
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)
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
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()
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()
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
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()
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
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)
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)
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
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
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]
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)
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()))
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
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)
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)
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
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 __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)
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
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)
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
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
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)