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