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 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 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
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 SandtrayView(TopicMessageView): name = 'Sandtray' 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) # MessageView implementation def _resizeEvent(self, event): self._sandtray_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio) def message_viewed(self, bag, msg_details): """ render the sandtray """ TopicMessageView.message_viewed(self, bag, msg_details) topic, msg, t = msg_details[:3] if msg: if topic == "/zones": self._sandtray.update_zones(self._get_zones(msg)) else: for t in msg.transforms: if t.header.frame_id == "sandtray": self._items[t.child_frame_id] = t.transform.translation.x, -t.transform.translation.y self._sandtray.update(self._items) def message_cleared(self): TopicMessageView.message_cleared(self) self.set_image(None, None, None) # End MessageView implementation def _get_zones(self, msg): zones = {} for marker in msg.markers: polygon = [] color = QColor(255*marker.color.r, 255*marker.color.g, 255*marker.color.b, 255*marker.color.a) for p in marker.points: polygon.append(QPointF(p.x * SandtrayItem.scale, -p.y * SandtrayItem.scale)) zones.setdefault(color, []).append(polygon) return zones def put_image_into_scene(self): if self._image: QtImage = ImageQt(self._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, topics): super(ImageView, self).__init__(timeline, parent, topics[0]) 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) self._image_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio) parent.layout().addWidget(self._image_view) # MessageView implementation def _resizeEvent(self, event): self._image_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio) 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: QtImage = ImageQt(self._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()