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()
Exemple #2
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)
Exemple #3
0
    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()
Exemple #4
0
    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()
Exemple #5
0
    def _update_map(self):
        """
        Method used in the rqt_nav_view plugin to read the image from the map server.
        """
        a = numpy.array(self.map.data,
                        dtype=numpy.uint8,
                        copy=False,
                        order='C')
        a = a.reshape((self.map.info.height, self.map.info.width))
        if self.map.info.width % 4:
            e = numpy.empty(
                (self.map.info.height, 4 - self.map.info.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.info.width,
                       self.map.info.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.graphics_view.setSceneRect(0, 0, self.map.info.width,
                                        self.map.info.height)

        pixmap = QPixmap.fromImage(self._map)
        self._map_item = self._scene.addPixmap(pixmap)

        # Everything must be mirrored
        self._map_item.scale(-1, 1)
        self._map_item.translate(-1 * self.map.info.width, 0)

        # Add drag and drop functionality
        # self.add_dragdrop(self._map_item)

        self.graphics_view.centerOn(self._map_item)
        self.graphics_view.show()