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 NavView(QGraphicsView):
map_changed = Signal()
path_changed = Signal(str)
polygon_changed = Signal(str)
def __init__(self,
map_topic='/map',
paths=None,
polygons=None,
tf_listener=None,
parent=None):
super(NavView, self).__init__()
if paths is None:
paths = [
'/move_base/SBPLLatticePlanner/plan',
'/move_base/TrajectoryPlannerROS/local_plan'
]
if polygons is None:
polygons = ['/move_base/local_costmap/robot_footprint']
if tf_listener is None:
tf_listener = tf.TransformListener()
self._parent = parent
self._pose_mode = False
self._goal_mode = False
self.last_path = None
self.drag_start = None
self.map_changed.connect(self._update)
self.destroyed.connect(self.close)
# ScrollHandDrag
self.setDragMode(QGraphicsView.ScrollHandDrag)
self._map = None
self._map_hash = None
self._map_item = None
self.map_width = 0
self.map_height = 0
self.map_resolution = 0
self.map_origin = None
self.frame_id = ""
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_listener
self.map_sub = rospy.Subscriber(map_topic, 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()
try:
delta = event.angleDelta().y()
except AttributeError:
delta = event.delta()
if delta > 0:
self.scale(1.15, 1.15)
else:
self.scale(0.85, 0.85)
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 add_path(self, name):
path = PathInfo(name)
def cb(msg):
if not self._map:
return
pp = QPainterPath()
# Transform everything in to the map frame
if not (msg.header.frame_id == self.frame_id
or msg.header.frame_id == ''):
try:
self._tf.waitForTransform(msg.header.frame_id,
self.frame_id, rospy.Time(),
rospy.Duration(10))
data = [
self._tf.transformPose(self.frame_id, 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(*self.point_map_to_qt((start.x, start.y)))
for pose in data:
pt = pose.pose.position
pp.lineTo(*self.point_map_to_qt((pt.x, pt.y)))
path.path = pp
self.path_changed.emit(name)
path.color = random.choice(self._colors)
self._colors.remove(path.color)
path.cb = cb
path.sub = rospy.Subscriber(path.name, Path, path.cb)
self._paths[name] = path
def add_polygon(self, name):
poly = PathInfo(name)
def cb(msg):
if not self._map:
return
if not (msg.header.frame_id == self.frame_id
or msg.header.frame_id == ''):
try:
self._tf.waitForTransform(msg.header.frame_id,
self.frame_id, 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 = []
for pt in points_stamped:
point = self._tf.transformPoint(self.frame_id,
pt).point
trans_pts.append((point.x, point.y))
except tf.Exception:
rospy.logerr("TF Error")
trans_pts = []
else:
trans_pts = [(pt.x, pt.y) for pt in msg.polygon.points]
if len(trans_pts) > 0:
trans_pts.append(trans_pts[0])
pts = [QPointF(*self.point_map_to_qt(pt)) for pt in trans_pts]
poly.path = QPolygonF(pts)
self.polygon_changed.emit(name)
poly.color = random.choice(self._colors)
self._colors.remove(poly.color)
poly.cb = cb
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):
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))
v = (v[0] / mag, v[1] / mag) # Normalize diff vector
u = (-v[1], v[0]) # Project diff vector to mirrored map
if self.last_path:
self._scene.removeItem(self.last_path)
self.last_path = None
res = (v[0] * 25, v[1] * 25)
if self._pose_mode:
pen = QPen(QColor("red"))
elif self._goal_mode:
pen = QPen(QColor("green"))
self.last_path = self._scene.addLine(self.drag_start[0],
self.drag_start[1],
self.drag_start[0] + res[0],
self.drag_start[1] + res[1], pen)
map_p = self.point_qt_to_map(self.drag_start)
angle = atan2(u[0], u[1])
quat = quaternion_from_euler(0, 0, angle)
self.drag_start = None
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:
map_p, quat = self.draw_position(e)
self.goal_mode(
) # Disable goal_mode and enable dragging/scrolling again
msg = PoseStamped()
msg.header.frame_id = self.frame_id
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = map_p[0]
msg.pose.position.y = map_p[1]
msg.pose.orientation.z = quat[2]
msg.pose.orientation.w = quat[3]
self._goal_pub.publish(msg)
elif self._pose_mode:
map_p, quat = self.draw_position(e)
self.pose_mode(
) # Disable pose_mode and enable dragging/scrolling again
msg = PoseWithCovarianceStamped()
msg.header.frame_id = self.frame_id
msg.header.stamp = rospy.Time.now()
# ToDo: Is it ok to just ignore the covariance matrix here?
msg.pose.pose.orientation.z = quat[2]
msg.pose.pose.orientation.w = quat[3]
msg.pose.pose.position.x = map_p[0]
msg.pose.pose.position.y = map_p[1]
self._pose_pub.publish(msg)
def close(self):
if self.map_sub:
self.map_sub.unregister()
for p in self._paths.values():
if p.sub:
p.sub.unregister()
for p in self._polygons.values():
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)))
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)))
if old_item:
self._scene.removeItem(old_item)
def _mirror(self, item):
"""
Mirror any QItem to have correct orientation
:param item:
:return:
"""
item.setTransform(QTransform().scale(1, -1).translate(
0, -self.map_height))
def point_qt_to_map(self, point):
"""
Convert point from Qt to map coordinates
:param point: tuple or list
:return: map point
"""
# Mirror point over y axis
x = point[0]
y = self.map_height - point[1]
# Orientation might need to be taken into account
return [
x * self.map_resolution + self.map_origin.position.x,
y * self.map_resolution + self.map_origin.position.y
]
def point_map_to_qt(self, point):
"""
Convert point from map to qt coordinates
:param point: tuple or list
:return: map point
"""
# Orientation might need to be taken into account
x = (point[0] - self.map_origin.position.x) / self.map_resolution
y = (point[1] - self.map_origin.position.y) / self.map_resolution
# Mirror point over y axis
return [x, self.map_height - y]
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 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
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()
try:
delta = event.angleDelta().y()
except AttributeError:
delta = event.delta()
if 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 DataAcquisition(Plugin):
_deferred_fit_in_view = Signal()
def __init__(self, context):
super(DataAcquisition, self).__init__(context)
self.initialized = False
self.setObjectName('DataAcquisition')
self._graph = None
self._current_dotcode = None
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('data_acquisition_2d'), 'resource',
'data_acquisition.ui')
loadUi(ui_file, self._widget,
{'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('DataAcquisitionUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Fixed size
width = 670
height = 570
# setting the fixed size of window
self._widget.setFixedSize(width, height)
# Get a list of Image topic
self._get_image_topics()
# Robot mode : disable GUI when scanning
self._in_scanning = False
self._scan_time = 0
self._image_topic = None
# canvas : image will be load here
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.red)
self._scene.setSceneRect(0, 0, 640, 480)
self._widget.canvas.setScene(self._scene)
self._set_image()
# Button Event
self._widget.setup_robot_push_button.pressed.connect(self._setup_robot)
self._widget.save_ground_truth_push_button.pressed.connect(
self._save_ground_truth)
self._widget.create_bbox_push_button.pressed.connect(self._create_bbox)
self._widget.start_scanning_push_button.pressed.connect(
self._start_scanning)
# Box event
self._widget.scan_time_spin_box.valueChanged.connect(
self._set_scan_time)
self._widget.image_topic_combo_box.currentIndexChanged.connect(
self._set_image_topic)
context.add_widget(self._widget)
def _setup_robot(self):
print("_setup_robot")
def _save_ground_truth(self):
print("_save_ground_truth")
def _create_bbox(self):
print("_create_bbox for image from topic : {} ".format(
self._widget.image_topic_combo_box.itemData(
self._widget.image_topic_combo_box.currentIndex())))
def _start_scanning(self):
print("_start_scanning")
def _set_scan_time(self):
if not self._in_scanning:
self._scan_time = self._widget.scan_time_spin_box.value()
print("_set_scan_time : {}".format(
self._widget.scan_time_spin_box.value()))
else:
print("Is scanning. Can't set the value")
def _get_image_topics(self):
topic_list = rospy.get_published_topics()
index = 0
for topic in topic_list:
# Remove topic which doesn't contain Image from list
try:
msg = rospy.wait_for_message(topic, Image, timeout=0.1)
self._widget.image_topic_combo_box.insertItem(
index, self.tr(topic), topic)
index = index + 1
except Exception as err:
continue
self._widget.image_topic_combo_box.insertItem(0, self.tr("aaaaa"),
"aaaaa")
self._widget.image_topic_combo_box.insertItem(1, self.tr("bbbbb"),
"bbbbb")
self._widget.image_topic_combo_box.setCurrentIndex(0)
def _set_image_topic(self):
print(self._widget.image_topic_combo_box.currentIndex())
print(
self._widget.image_topic_combo_box.itemData(
self._widget.image_topic_combo_box.
curre_current_dotcodentIndex()))
print("_set_image_topic : {}".format(self._image_topic))
self._set_image()
def _set_image(self):
print("_set_image")
self._scene.clear()
img = cv2.imread("/home/gachiemchiep/Pictures/aaa.png", 1)
img_rz = cv2.resize(img, (640, 480))
# https://stackoverflow.com/questions/34232632/convert-python-opencv-image-numpy-array-to-pyqt-qpixmap-image
height, width, channel = img_rz.shape
print(img_rz.shape)
bytesPerLine = 3 * width
img_q = QImage(img_rz.data, width, height, bytesPerLine,
QImage.Format_RGB888).rgbSwapped()
# painter : this does not work
# painter = QPainter(img_q)
# painter.setRenderHint(QPainter.Antialiasing)
# self._scene.render(painter)
# painter.end()
# pixmap : this work
pixmap = QPixmap.fromImage(img_q)
self._scene.addPixmap(pixmap)
self._scene.setSceneRect(0, 0, 640, 480)
def _draw_bbox_start(self, event):
print("_draw_bbox_start: {}".format(event.pos()))
def _draw_bbox_end(self, event):
print("_draw_bbox_end: {}".format(event.pos()))
class SandtrayView(TopicMessageView):
name = 'Sandtray'
def __init__(self, timeline, parent, topics):
super(SandtrayView, self).__init__(timeline, parent, topics[0])
self._items = {}
self._image = None
self._image_topic = None
self._image_stamp = None
self.quality = Image.NEAREST # quality hint for scaling
self._sandtray = SandtrayItem()
self._sandtray_view = QGraphicsView(parent)
self._sandtray_view.resizeEvent = self._resizeEvent
self._scene = QGraphicsScene()
self._scene.addItem(self._sandtray)
self._sandtray_view.setScene(self._scene)
self._sandtray_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio)
parent.layout().addWidget(self._sandtray_view)
# MessageView implementation
def _resizeEvent(self, event):
self._sandtray_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio)
def message_viewed(self, bag, msg_details):
"""
render the sandtray
"""
TopicMessageView.message_viewed(self, bag, msg_details)
topic, msg, t = msg_details[:3]
if msg:
if topic == "/zones":
self._sandtray.update_zones(self._get_zones(msg))
else:
for t in msg.transforms:
if t.header.frame_id == "sandtray":
self._items[t.child_frame_id] = t.transform.translation.x, -t.transform.translation.y
self._sandtray.update(self._items)
def message_cleared(self):
TopicMessageView.message_cleared(self)
self.set_image(None, None, None)
# End MessageView implementation
def _get_zones(self, msg):
zones = {}
for marker in msg.markers:
polygon = []
color = QColor(255*marker.color.r, 255*marker.color.g, 255*marker.color.b, 255*marker.color.a)
for p in marker.points:
polygon.append(QPointF(p.x * SandtrayItem.scale, -p.y * SandtrayItem.scale))
zones.setdefault(color, []).append(polygon)
return zones
def put_image_into_scene(self):
if self._image:
QtImage = ImageQt(self._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 ImageView(TopicMessageView):
"""
Popup image viewer
"""
name = 'Image'
def __init__(self, timeline, parent, topics):
super(ImageView, self).__init__(timeline, parent, topics[0])
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)
self._image_view.fitInView(self._scene.itemsBoundingRect(),
Qt.KeepAspectRatio)
parent.layout().addWidget(self._image_view)
# MessageView implementation
def _resizeEvent(self, event):
self._image_view.fitInView(self._scene.itemsBoundingRect(),
Qt.KeepAspectRatio)
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:
QtImage = ImageQt(self._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()
