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) 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']): super(NavView, self).__init__() 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() self._tf = tf.TransformListener() 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) self.setScene(self._scene) 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 range(101): image.setColor(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(0, 0, 150)) # 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): 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 (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 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() 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) 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)
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