class QCameraView(QGraphicsView): """ Accepts an image of a teleop compressed image type and draws that in the scene/view. """ def __init__(self, parent=None): super(QCameraView, self).__init__(parent) self.scene = QGraphicsScene(self) self.setScene(self.scene) self._load_default_image() self.connect(self, SIGNAL("load_default_image"), self._load_default_image) def _load_default_image(self): joystick_icon = os.path.join( rospkg.RosPack().get_path('rocon_bubble_icons'), 'icons', 'joystick.png') pixmap = QPixmap(joystick_icon, format="png") if self.scene: self.scene.addPixmap(pixmap) self.scene.update() self.fitInView( QRectF(0, 0, self.scene.width(), self.scene.height()), Qt.KeepAspectRatio) def load_default_image(self): self.emit(SIGNAL("load_default_image")) @pyqtSlot(sensor_msgs.CompressedImage, name='image_received') def on_compressed_image_received(self, image): ''' :param sensor_msgs.CompressedImage image: convert and display this in the QGraphicsView. ''' if len(self.scene.items()) > 1: self.scene.removeItem(self.scene.items()[0]) pixmap = QPixmap() pixmap.loadFromData(image.data, format=image.format) self.scene.addPixmap(pixmap) self.scene.update() # setting fitInvView seems sensitive to here or prior to scene update self.fitInView(QRectF(0, 0, self.scene.width(), self.scene.height()), Qt.KeepAspectRatio) def resizeEvent(self, evt=None): self.fitInView(QRectF(0, 0, self.scene.width(), self.scene.height()), Qt.KeepAspectRatio)
class CameraView(QGraphicsView): image_changed = Signal() def __init__(self, camera_topic='/image_raw'): super(CameraView,self).__init__() self._scene = QGraphicsScene() self.bridge = CvBridge() self._map_item = 0 self.image_changed.connect(self._update) self._sub = rospy.Subscriber(camera_topic,Image,self.callback) self.setScene(self._scene) def callback(self, msg): self.w = msg.width self.h = msg.height a = self.bridge.imgmsg_to_cv(msg, "rgb8") a = numpy.array(a) image = QImage(a, self.w, self.h, QImage.Format_RGB888) self._map = image self._scene.setSceneRect(0,0,self.w,self.h) self.image_changed.emit() 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) self.centerOn(self._map_item) self.show() def _mirror(self, item): item.scale(-1, 1) item.translate(-self.w, 0) def close(self): if self._sub: self._sub.unregister() super(CameraView, self).close()
class CameraView(QGraphicsView): image_changed = Signal() def __init__(self, camera_topic='/image_raw'): super(CameraView, self).__init__() self._scene = QGraphicsScene() self.bridge = CvBridge() self._map_item = 0 self.image_changed.connect(self._update) self._sub = rospy.Subscriber(camera_topic, Image, self.callback) self.setScene(self._scene) def callback(self, msg): self.w = msg.width self.h = msg.height a = self.bridge.imgmsg_to_cv(msg, "rgb8") a = numpy.array(a) image = QImage(a, self.w, self.h, QImage.Format_RGB888) self._map = image self._scene.setSceneRect(0, 0, self.w, self.h) self.image_changed.emit() 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) self.centerOn(self._map_item) self.show() def _mirror(self, item): item.scale(-1, 1) item.translate(-self.w, 0) def close(self): if self._sub: self._sub.unregister() super(CameraView, self).close()
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) 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 QMapView(QGraphicsView): map_changed = Signal() def __init__(self, parent=None): super(QMapView, self).__init__() self._parent = parent self._scene = QGraphicsScene() self.map_changed.connect(self._update_map) self.destroyed.connect(self.close) # ScrollHandDrag self.setDragMode(QGraphicsView.ScrollHandDrag) self._map = None self.w = 0 self.h = 0 self._map_item = {} self._polygons = {} self._colors = [(238, 34, 116), (68, 134, 252), (236, 228, 46), (102, 224, 18), (242, 156, 6), (240, 64, 10), (196, 30, 250)] 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) @pyqtSlot(nav_msgs.OccupancyGrid, name='map_received') 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 _update_map(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 _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 RoboMap(QGraphicsView): map_change = Signal() def __init__(self, topic='/map', tf=None, parent=None): super(RoboMap, self).__init__() self.parent = parent self.tf = tf self.topic = topic self.map_change.connect(self.update) self.destroyed.connect(self.close) self.map = None self.mapitem = None self.w = 0 self.h = 0 self.resolution = None self.origin = None self.polygon = None self.point = None self.scene = QGraphicsScene() self.subscriber = rospy.Subscriber(topic, OccupancyGrid, self.callback) self.setScene(self.scene) self.timer = None def update(self): if self.mapitem: self.scene.removeItem(self.mapitem) qpix = QPixmap.fromImage(self.map) self.mapitem = self.scene.addPixmap(qpix) self.mirror(self.mapitem) self.show() def mirror(self, item): item.scale(-1, 1) item.translate(-self.w, 0) def callback(self, msg): self.w = msg.info.width self.h = msg.info.height self.resolution = msg.info.resolution self.origin = (msg.info.origin.position.x, msg.info.origin.position.y) arr = np.array(msg.data, dtype=np.uint8, copy=False, order='C') arr = arr.reshape((self.h, self.w)) img = QImage(arr.reshape((arr.shape[0] * arr.shape[1])), self.w, self.h, QImage.Format_Indexed8) #need to invert some colors :) for z in reversed(range(101)): img.setColor(100 - z, qRgb(z*2.55, z*2.55, z*2.55)) img.setColor(101, qRgb(255, 0, 0)) img.setColor(255, qRgb(100, 100, 100)) self.map = img self.setSceneRect(0, 0, self.w, self.h) self.map_change.emit() # Explorer laser callback self.parent.taskplanner.explorer.laser_callback(msg) def update_map(self, dead_pirates): tmp = [] for z in dead_pirates: x = z.pose.position.x y = z.pose.position.y #transform pose coordinates to map coordinates map_y = (y - self.origin[1])/self.resolution map_x = -((x - self.origin[0])/self.resolution) + self.w tmp.append(self.draw_point(map_x, map_y, color=Qt.red)) print 'Added dead' self.parent.dead_pirate_objects = tmp def draw_point(self, x, y, color=Qt.magenta, rad=1.0, message=None): ell = self.scene.addEllipse(x-rad, y-rad, rad*2.0, rad*2.0, color, QBrush(Qt.SolidPattern)) if message: self.parent.update_textbox(message, (str(x) + ' ' + str(y))) ell.setZValue(2000.0) return ell def mousePressEvent(self, e): point = self.mapToScene(e.x(), e.y()) if self.point: self.scene.removeItem(self.point) self.point = None self.point = self.draw_point(point.x(), point.y(), Qt.yellow, 1.0) def wheelEvent(self, e): e.ignore() if e.delta() > 0: self.scale(1.30, 1.30) else: self.scale(0.7, 0.7)
class TeleopApp(Plugin): _update_robot_list_signal = Signal() CAMERA_FPS = (1000 / 20) D2R = 3.141592 / 180 R2D = 180 / 3.141592 LINEAR_V = 1.5 ANGULAR_V = 60 * D2R def __init__(self, context): self._context = context super(TeleopApp, self).__init__(context) self.initialised = False self.setObjectName('Teleop App') self.is_setting_dlg_live = False self._widget = QWidget() rospack = rospkg.RosPack() ui_file = os.path.join(rospack.get_path('concert_teleop_app'), 'ui', 'teleop_app.ui') self._widget.setObjectName('TeleopAppUi') loadUi(ui_file, self._widget, {}) if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) #list item click event self._widget.robot_list_tree_widget.itemClicked.connect( self._select_robot_list_tree_item) self._widget.robot_list_tree_widget.itemDoubleClicked.connect( self._dbclick_robot_list_item) #button event connection self._widget.backward_btn.setAutoRepeat(True) self._widget.forward_btn.setAutoRepeat(True) self._widget.left_turn_btn.setAutoRepeat(True) self._widget.right_turn_btn.setAutoRepeat(True) self._widget.backward_btn.clicked.connect(self._backward) self._widget.forward_btn.clicked.connect(self._forward) self._widget.left_turn_btn.clicked.connect(self._left_turn) self._widget.right_turn_btn.clicked.connect(self._right_turn) self._widget.backward_btn.released.connect(self._stop) self._widget.forward_btn.released.connect(self._stop) self._widget.left_turn_btn.released.connect(self._stop) self._widget.right_turn_btn.released.connect(self._stop) self._widget.capture_teleop_btn.pressed.connect(self._capture_teleop) self._widget.release_teleop_btn.pressed.connect(self._release_teleop) #signal event connection self._widget.destroyed.connect(self._exit) self._update_robot_list_signal.connect(self._update_robot_list) self.connect(self, SIGNAL("capture"), self._show_capture_teleop_message) self.connect(self, SIGNAL("release"), self._show_release_teleop_message) self.connect(self, SIGNAL("error"), self._show_error_teleop_message) context.add_widget(self._widget) #init self.scene = QGraphicsScene() self._widget.camera_view.setScene(self.scene) self.timer = QTimer(self._widget) self.timer.timeout.connect(self._display_image) self.timer.start(self.CAMERA_FPS) self._widget.release_teleop_btn.setEnabled(False) self.teleop_app_info = TeleopAppInfo() self.teleop_app_info._reg_event_callback(self._refresh_robot_list) self.teleop_app_info._reg_capture_event_callback( self._capture_event_callback) self.teleop_app_info._reg_release_event_callback( self._release_event_callback) self.teleop_app_info._reg_error_event_callback( self._error_event_callback) self.robot_item_list = {} self.current_robot = None self.current_captured_robot = None def _exit(self): if self.current_captured_robot: self.teleop_app_info._release_teleop( self.current_captured_robot["rocon_uri"]) def _show_capture_teleop_message(self, rtn): if rtn: QMessageBox.warning(self._widget, 'SUCCESS', "CAPTURE!!!!", QMessageBox.Ok | QMessageBox.Ok) for k in self.robot_item_list.keys(): if self.robot_item_list[k] == self.current_robot: k.setBackground(0, QBrush(Qt.SolidPattern)) k.setBackgroundColor(0, QColor(0, 255, 0, 255)) robot_name = k.text(0) k.setText(0, str(robot_name) + " (captured)") else: k.setBackground(0, QBrush(Qt.NoBrush)) k.setBackgroundColor(0, QColor(0, 0, 0, 0)) robot_name = k.text(0) else: QMessageBox.warning(self._widget, 'FAIL', "FAIURE CAPTURE!!!!", QMessageBox.Ok | QMessageBox.Ok) self._widget.capture_teleop_btn.setEnabled(False) self._widget.release_teleop_btn.setEnabled(True) self._widget.setDisabled(False) self.current_captured_robot = self.current_robot def _show_release_teleop_message(self, rtn): if rtn: QMessageBox.warning(self._widget, 'SUCCESS', "RELEASE!!!!", QMessageBox.Ok | QMessageBox.Ok) for k in self.robot_item_list.keys(): if self.robot_item_list[k] == self.current_captured_robot: k.setBackground(0, QBrush(Qt.NoBrush)) k.setBackgroundColor(0, QColor(0, 0, 0, 0)) robot_name = k.text(0) k.setText(0, robot_name[:robot_name.find(" (captured)")]) else: QMessageBox.warning(self._widget, 'FAIL', "FAIURE RELEASE!!!!", QMessageBox.Ok | QMessageBox.Ok) self._widget.setDisabled(False) self._widget.capture_teleop_btn.setEnabled(True) self._widget.release_teleop_btn.setEnabled(False) self.current_captured_robot = None def _show_error_teleop_message(self, err): QMessageBox.warning(self._widget, 'ERROR', err, QMessageBox.Ok | QMessageBox.Ok) self._widget.setDisabled(False) self._widget.capture_teleop_btn.setEnabled(True) self._widget.release_teleop_btn.setEnabled(True) def _capture_event_callback(self, rtn): try: self.emit(SIGNAL("capture"), rtn) except: pass def _release_event_callback(self, rtn): try: self.emit(SIGNAL("release"), rtn) except: pass def _error_event_callback(self, err): try: self.emit(SIGNAL("error"), err) except: pass def _release_teleop(self): if self.current_robot != self.current_captured_robot: print "NO Capture robot (captured: %s)" % self.current_captured_robot[ 'name'].string return self.teleop_app_info._release_teleop(self.current_robot["rocon_uri"]) self._widget.setDisabled(True) def _capture_teleop(self): if self.current_robot == None: print "NO Select robot" return elif self.current_captured_robot: print "Already captured robot" return self.teleop_app_info._capture_teleop(self.current_robot["rocon_uri"]) self._widget.setDisabled(True) pass def _dbclick_robot_list_item(self): if self.current_captured_robot == None: self._capture_teleop() else: self._release_teleop() def _update_robot_list(self): self._widget.robot_list_tree_widget.clear() robot_list = self.teleop_app_info.robot_list for k in robot_list.values(): robot_item = QTreeWidgetItem(self._widget.robot_list_tree_widget) robot_item.setText(0, k["name"].string) self.robot_item_list[robot_item] = k def _backward(self): if self._widget.backward_btn.isDown(): self.teleop_app_info._request_teleop_cmd_vel(-self.LINEAR_V, 0) else: self._stop() def _forward(self): if self._widget.forward_btn.isDown(): self.teleop_app_info._request_teleop_cmd_vel(self.LINEAR_V, 0) else: self._stop() def _left_turn(self): if self._widget.left_turn_btn.isDown(): self.teleop_app_info._request_teleop_cmd_vel(0, self.ANGULAR_V) else: self._stop() def _right_turn(self): if self._widget.right_turn_btn.isDown(): self.teleop_app_info._request_teleop_cmd_vel(0, -self.ANGULAR_V) else: self._stop() def _stop(self): self.teleop_app_info._request_teleop_cmd_vel(0, 0) def _select_robot_list_tree_item(self, Item): if not Item in self.robot_item_list.keys(): print "HAS NO KEY" else: self.current_robot = self.robot_item_list[Item] if self.current_robot == self.current_captured_robot: self._widget.release_teleop_btn.setEnabled(True) else: self._widget.release_teleop_btn.setEnabled(False) def _refresh_robot_list(self): self._update_robot_list_signal.emit() pass def _display_image(self): image = self.teleop_app_info.image_data if image: if len(self.scene.items()) > 1: self.scene.removeItem(self.scene.items()[0]) image_data = self.teleop_app_info.image_data.data pixmap = QPixmap() pixmap.loadFromData( image_data, format="PNG", ) self._widget.camera_view.fitInView( QRectF(0, 0, pixmap.width(), pixmap.height()), Qt.KeepAspectRatio) self.scene.addPixmap(pixmap) self.scene.update() else: self.scene.clear()
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