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 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 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 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):
"""
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()
