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 ImageView(TopicMessageView):
"""
Popup image viewer
"""
name = 'Image'
def __init__(self, timeline, parent):
super(ImageView, self).__init__(timeline, parent)
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, stamp)
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.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=['/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 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 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
