def __init__(self, scene, x, y, caption, answers, done_cb, parent=None):
self.w = 0
self.h = 0
super(DialogItem, self).__init__(scene, x, y, parent=parent)
self.sp = self.m2pix(0.01)
self.done_cb = done_cb
self.fixed = False
self.desc = DescItem(scene, 0, 0, self)
self.desc.set_content(caption)
self.items = []
self.w = (len(answers) + 1) * self.sp
for answer in answers:
btn = ButtonItem(self.scene(), 0, 0, answer, self, self.answer_cb)
self.items.append(btn)
self.w += btn._width()
y = self.sp
self._place_childs_horizontally(y, self.sp, [self.desc])
y += self.desc._height() + self.sp
self._place_childs_horizontally(y, self.sp, self.items)
y += self.items[0]._height()
y += self.sp
self.h = y
self.update()
self.setZValue(300)
def __init__(self, scene, object_id, object_type, x,
y, z, quaternion=(0, 0, 0, 1), sel_cb=None, selected=False, parent=None, horizontal=False):
self.object_id = object_id
self.selected = selected
self.sel_cb = sel_cb
self.horizontal = horizontal
# TODO check bbox type and use rectangle (used now) / ellipse, consider
# other angles
self.object_type = object_type
self.inflate = 0.01
self.def_color = QtCore.Qt.gray
self.lx = 0
self.ly = 0
self.desc = None
self.quaternion = (0, 0, 0, 1)
self.on_table = False
super(ObjectItem, self).__init__(scene, x, y, z, parent=parent)
self.desc = DescItem(scene, 0, 0, parent=self)
self.desc.setFlag(QtGui.QGraphicsItem.ItemIgnoresTransformations)
self.update_text()
self.set_orientation(quaternion)
if selected:
self.set_selected()
self._update_desc_pos()
self.setZValue(50)
def __init__(self, scene, object_id, object_type, x,
y, yaw, sel_cb=None, selected=False):
self.object_id = object_id
self.selected = selected
self.sel_cb = sel_cb
# TODO check bbox type and use rectangle (used now) / ellipse, consider
# other angles
self.object_type = object_type
self.inflate = 1.2
self.hover_ratio = 1.1
self.def_color = QtCore.Qt.gray
self.desc = None
super(ObjectItem, self).__init__(scene, x, y)
self.desc = DescItem(scene, 0, 0, self)
self.desc.setFlag(QtGui.QGraphicsItem.ItemIgnoresTransformations)
self.update_text()
self.setRotation(yaw)
if selected:
self.set_selected()
self._update_desc_pos()
self.setZValue(50)
class DialogItem(Item):
def __init__(self, scene, x, y, caption, answers, done_cb, parent=None):
self.w = 0
self.h = 0
super(DialogItem, self).__init__(scene, x, y, parent=parent)
self.sp = self.m2pix(0.01)
self.done_cb = done_cb
self.fixed = False
self.desc = DescItem(scene, 0, 0, self)
self.desc.set_content(caption)
self.items = []
self.w = (len(answers) + 1) * self.sp
for answer in answers:
btn = ButtonItem(self.scene(), 0, 0, answer, self, self.answer_cb)
self.items.append(btn)
self.w += btn._width()
y = self.sp
self._place_childs_horizontally(y, self.sp, [self.desc])
y += self.desc._height() + self.sp
self._place_childs_horizontally(y, self.sp, self.items)
y += self.items[0]._height()
y += self.sp
self.h = y
self.update()
self.setZValue(300)
def answer_cb(self, btn):
self.done_cb(self.items.index(btn))
def paint(self, painter, option, widget):
if not self.scene():
return
painter.setClipRect(option.exposedRect)
painter.setRenderHint(QtGui.QPainter.Antialiasing)
pen = QtGui.QPen()
pen.setStyle(QtCore.Qt.NoPen)
painter.setPen(pen)
painter.setBrush(QtCore.Qt.gray)
painter.setOpacity(0.5)
painter.drawRoundedRect(QtCore.QRect(0, 0, self.w, self.h), 5.0, 5.0)
def boundingRect(self):
return QtCore.QRectF(0, 0, self.w, self.h)
def __init__(self, scene, x, y, caption, answers, done_cb, parent=None):
self.w = 0
self.h = 0
super(DialogItem, self).__init__(scene, x, y, parent=parent)
self.sp = self.m2pix(0.01)
self.done_cb = done_cb
self.fixed = False
self.desc = DescItem(scene, 0, 0, self)
self.desc.set_content(caption, color=QtCore.Qt.white)
self.items = []
for answer in answers:
btn = ButtonItem(self.scene(), 0, 0, answer, self, self.answer_cb)
self.items.append(btn)
self.w = max(self.desc.boundingRect().width(),
sum(btn.boundingRect().width() for btn in self.items)) + 2 * self.sp
y = self.sp
self._place_childs_horizontally(y, self.sp, [self.desc])
y += self.desc._height() + self.sp
self._place_childs_horizontally(y, self.sp, self.items)
y += self.items[0]._height()
y += self.sp
self.h = y
self.setFlag(QtGui.QGraphicsItem.ItemIsMovable, True)
self.setFlag(QtGui.QGraphicsItem.ItemIsSelectable, True)
self.update()
self.setZValue(300)
def __init__(self, scene, rpm, caption, x, y, place_pose_changed=None, outline_diameter=0.1, selected=False, fixed=False):
self.outline_diameter = outline_diameter
self.caption = caption
self.in_collision = False
super(PlaceItem, self).__init__(scene, rpm, x, y)
self.desc = DescItem(scene, rpm, -self.outline_diameter * 1.3 / 2.0, self.outline_diameter * 1.3 / 2 + 0.01, self)
self.update_text()
self.fixed = fixed
self.place_pose_changed = place_pose_changed
def __init__(self, scene, rpm, object_id, object_type, x, y, sel_cb=None, outline_diameter=0.1, selected=False):
self.object_id = object_id
self.object_type = object_type
self.outline_diameter = outline_diameter
self.selected = selected
self.sel_cb = sel_cb
super(ObjectItem, self).__init__(scene, rpm, x, y)
self.desc = DescItem(scene, rpm, -self.outline_diameter * 1.3 / 2.0, self.outline_diameter * 1.3 / 2 + 0.01, self)
self.update_text()
if selected:
self.set_selected()
def __init__(self,
scene,
caption,
obj_coords=[],
poly_points=[],
polygon_changed=None,
fixed=False):
self.caption = caption
self.polygon_changed = polygon_changed
self.poly = QtGui.QPolygon()
self.desc = None
super(PolygonItem, self).__init__(scene, 0.5,
0.5) # TODO what should be here?
self.fixed = fixed
self.convex = True
if not self.fixed:
self.setFlag(QtGui.QGraphicsItem.ItemIsMovable, True)
self.setFlag(QtGui.QGraphicsItem.ItemIsSelectable, True)
self.pts = []
if len(obj_coords) > 0:
min = [obj_coords[0][0], obj_coords[0][1]]
max = [obj_coords[0][0], obj_coords[0][1]]
for pt in obj_coords:
pt = [pt[0], pt[1]]
if pt[0] < min[0]:
min[0] = pt[0]
if pt[1] < min[1]:
min[1] = pt[1]
if pt[0] > max[0]:
max[0] = pt[0]
if pt[1] > max[1]:
max[1] = pt[1]
pad = 0.02
min[0] -= pad
min[1] -= pad
max[0] += pad
max[1] += pad
self.pts.append(
PointItem(scene, min[0], min[1], self, self.point_changed))
self.pts.append(
PointItem(scene, max[0], min[1], self, self.point_changed))
self.pts.append(
PointItem(scene, max[0], max[1], self, self.point_changed))
self.pts.append(
PointItem(scene, min[0], max[1], self, self.point_changed))
if self.polygon_changed is not None:
self.polygon_changed(self.get_poly_points())
elif len(poly_points) > 0:
for pt in poly_points:
self.pts.append(
PointItem(scene, pt[0], pt[1], self, self.point_changed,
fixed))
else:
pass # TODO chyba
for pt in self.pts:
self.poly.append(pt.pos().toPoint())
self.desc = DescItem(scene, 0, 0, self)
self.desc.setFlag(QtGui.QGraphicsItem.ItemIgnoresTransformations)
self.__update_desc_pos()
self.__update_text()
self.update()
class ObjectItem(Item):
"""The class to visualize (detected) object.
It currently supports only rotation around z-axis.
"""
def __init__(self, scene, object_id, object_type, x,
y, yaw, sel_cb=None, selected=False):
self.object_id = object_id
self.selected = selected
self.sel_cb = sel_cb
# TODO check bbox type and use rectangle (used now) / ellipse, consider
# other angles
self.object_type = object_type
self.inflate = 1.2
self.hover_ratio = 1.1
self.def_color = QtCore.Qt.gray
self.desc = None
super(ObjectItem, self).__init__(scene, x, y)
self.desc = DescItem(scene, 0, 0, self)
self.desc.setFlag(QtGui.QGraphicsItem.ItemIgnoresTransformations)
self.update_text()
self.setRotation(yaw)
if selected:
self.set_selected()
self._update_desc_pos()
self.setZValue(50)
def set_color(self, color=QtCore.Qt.gray):
self.def_color = color
self.update()
def _update_desc_pos(self):
if self.desc is not None:
# make upper left corner of description aligned with left extent of
# the (possibly rotated) object bounding box (highlight area)
self.desc.setPos(
self.mapFromScene(
self.x() -
self.sceneBoundingRect().width() /
2,
self.y() +
self.sceneBoundingRect().height() /
2 +
self.m2pix(0.01)))
def set_pos(self, x, y, parent_coords=False, yaw=None):
super(ObjectItem, self).set_pos(x, y, parent_coords, yaw)
self._update_desc_pos()
self.update_text()
def update_text(self):
if self.desc is None:
return
desc = ""
desc += translate("ObjectItem", "ID: ") + self.object_id
if self.hover:
desc += "\n" + translate("ObjectItem",
"TYPE: ") + self.object_type.name
desc += "\n" + self.get_pos_str()
self.desc.set_content(desc)
def hover_changed(self):
self.update_text()
self.update()
def boundingRect(self):
if not self.scene():
return QtCore.QRectF()
lx = self.hover_ratio * self.inflate * \
self.m2pix(self.object_type.bbox.dimensions[0])
ly = self.hover_ratio * self.inflate * \
self.m2pix(self.object_type.bbox.dimensions[1])
p = 1.0
return QtCore.QRectF(-lx / 2 - p, -ly / 2 - p, lx + 2 * p, ly + 2 * p)
def paint(self, painter, option, widget):
if not self.scene():
return
painter.setClipRect(option.exposedRect)
painter.setRenderHint(QtGui.QPainter.Antialiasing)
lx = self.inflate * self.m2pix(self.object_type.bbox.dimensions[0])
ly = self.inflate * self.m2pix(self.object_type.bbox.dimensions[1])
rr = 10
if self.selected:
painter.setBrush(QtCore.Qt.green)
painter.setPen(QtCore.Qt.green)
painter.drawRoundedRect(-lx / 2 * self.hover_ratio, -ly / 2 * self.hover_ratio,
lx * self.hover_ratio, ly * self.hover_ratio, rr, rr, QtCore.Qt.RelativeSize)
elif self.hover:
painter.setBrush(QtCore.Qt.gray)
painter.setPen(QtCore.Qt.gray)
painter.drawRoundedRect(-lx / 2 * self.hover_ratio, -ly / 2 * self.hover_ratio,
lx * self.hover_ratio, ly * self.hover_ratio, rr, rr, QtCore.Qt.RelativeSize)
painter.setBrush(self.def_color)
painter.setPen(self.def_color)
painter.drawRoundedRect(-lx / 2, -ly / 2, lx,
ly, rr, rr, QtCore.Qt.RelativeSize)
fr = 1.0 - (self.hover_ratio - 1.0) # fill ratio
painter.setBrush(QtCore.Qt.black)
painter.setPen(QtCore.Qt.black)
painter.drawRoundedRect(-lx / 2 * fr, -ly / 2 * fr,
lx * fr, ly * fr, rr, rr, QtCore.Qt.RelativeSize)
def cursor_press(self):
# TODO call base class method
if self.sel_cb is not None:
# callback should handle object selection
self.sel_cb(self.object_id, self.selected)
else:
# no callback - object will handle its selection
if not self.selected:
self.set_selected()
else:
self.set_selected(False)
def set_selected(self, selected=True):
if selected:
self.selected = True
# rospy.logdebug('Object ID ' + self.object_id + ' selected')
else:
self.selected = False
# rospy.logdebug('Object ID ' + self.object_id + ' unselected')
self.update()
def __init__(
self,
scene,
caption,
min_x,
min_y,
square_width,
square_height,
object_type,
poses,
grid_points,
scene_items,
square_changed=None,
fixed=False):
self.scn = scene
self.caption = caption
self.object_type = object_type
self.scene_items = scene_items
self.square_changed = square_changed
self.space = 0.05 # is added to bbox of an object
self.object_side_length_x = self.object_type.bbox.dimensions[0] + self.space
self.object_side_length_y = self.object_type.bbox.dimensions[1] + self.space
self.poses = poses
self.square = QtGui.QPolygon()
self.previous_width = 0
self.previous_height = 0
self.items = []
self.last_corner = "BR"
if len(grid_points) == 0:
self.min = [min_x, min_y]
self.max = [min_x + square_width, min_y + square_height]
self.pom_min = self.min # to save original value because y is changed in update_bound
self.pom_max = self.max
else:
self.min = list(min(grid_points))
self.max = list(max(grid_points))
self.orig_x = []
self.orig_y = []
self.desc = None
self.dialog = None
self.horizontal = False
super(SquareItem, self).__init__(scene, min_x, min_y)
self.fixed = fixed
self.desc = DescItem(scene, self.min[0] - 0.01, self.min[1] - 0.015, self)
self.desc.setFlag(QtGui.QGraphicsItem.ItemIgnoresTransformations)
self.update_text()
self.plus_btn = ButtonItem(scene, self.min[0] - 0.01, self.min[1] - 0.04, "+", self, self.plus_clicked, 1.25,
QtCore.Qt.darkCyan, width=0.04)
self.minus_btn = ButtonItem(scene, self.min[0] + 0.035, self.min[1] - 0.04, "-", self, self.minus_clicked, 1.25,
QtCore.Qt.darkCyan, width=0.04)
self.plus_btn.setEnabled(False)
self.minus_btn.setEnabled(False)
self.pts = []
self.pts.append(SquarePointItem(scene, self.min[0], self.min[1], self, "BL", self.fixed)) # bottom-left corner
self.pts.append(SquarePointItem(scene, self.max[0], self.min[1], self, "BR", self.fixed)) # bottom-right corner
self.pts.append(SquarePointItem(scene, self.max[0], self.max[1], self, "TR", self.fixed)) # top-right corner
self.pts.append(SquarePointItem(scene, self.min[0], self.max[1], self, "TL", self.fixed)) # top-left corner
if len(poses) > 0 and self.fixed:
# depicts fixed objects
for pose in poses:
it = PlaceItem(
self.scn,
"Object",
pose.pose.position.x,
pose.pose.position.y,
pose.pose.position.z,
conversions.q2a(pose.pose.orientation),
self.object_type,
None,
place_pose_changed=None,
fixed=True,
txt=False,
parent=self,
horizontal=self.horizontal
)
self.items.append(it)
else:
# depicts editable objects
for i, pose in enumerate(poses):
rot_point = None # to save xy coordinates for rotation point (for rotating objects in a grid)
if i == 0:
rot = True
pom = self.find_corner("TL").get_pos()
rot_point = [pom[0] - 0.025, pom[1] + 0.025]
else:
rot = False
it = PlaceItem(
self.scn,
"Object",
pose.pose.position.x,
pose.pose.position.y,
pose.pose.position.z,
conversions.q2a(pose.pose.orientation),
self.object_type,
None,
place_pose_changed=None,
fixed=False,
txt=False,
rot=rot,
rot_point=rot_point,
rotation_changed=self.items_rotation_changed,
parent=self,
horizontal=self.horizontal
)
it.update_point()
self.items.append(it)
if self.items:
# rotation of all objects in a grid (list of all objects is handed over to
# the first object. When the first object is rotated, all objects are
# rotated)
self.items[0].set_other_items(self.items[1:])
self.plus_btn.setEnabled(True)
self.minus_btn.setEnabled(True)
self.dialog = DialogItem(self.scene(),
self.pix2m(self.scene().width() / 2),
0.1,
translate(
"Place item",
"Object place pose options"),
[
translate(
"Place item", "Rotate |"),
translate(
"Place item", "Rotate --")
],
self.dialog_cb)
if self.square_changed is not None:
# to save grid points and objects into the ProgramItem message
self.square_changed(self.get_square_points(), self.items)
self.update_bound()
self.update()
class SquareItem(Item):
# Class constructor
def __init__(
self,
scene,
caption,
min_x,
min_y,
square_width,
square_height,
object_type,
poses,
grid_points,
scene_items,
square_changed=None,
fixed=False):
self.scn = scene
self.caption = caption
self.object_type = object_type
self.scene_items = scene_items
self.square_changed = square_changed
self.space = 0.05 # is added to bbox of an object
self.object_side_length_x = self.object_type.bbox.dimensions[0] + self.space
self.object_side_length_y = self.object_type.bbox.dimensions[1] + self.space
self.poses = poses
self.square = QtGui.QPolygon()
self.previous_width = 0
self.previous_height = 0
self.items = []
self.last_corner = "BR"
if len(grid_points) == 0:
self.min = [min_x, min_y]
self.max = [min_x + square_width, min_y + square_height]
self.pom_min = self.min # to save original value because y is changed in update_bound
self.pom_max = self.max
else:
self.min = list(min(grid_points))
self.max = list(max(grid_points))
self.orig_x = []
self.orig_y = []
self.desc = None
self.dialog = None
self.horizontal = False
super(SquareItem, self).__init__(scene, min_x, min_y)
self.fixed = fixed
self.desc = DescItem(scene, self.min[0] - 0.01, self.min[1] - 0.015, self)
self.desc.setFlag(QtGui.QGraphicsItem.ItemIgnoresTransformations)
self.update_text()
self.plus_btn = ButtonItem(scene, self.min[0] - 0.01, self.min[1] - 0.04, "+", self, self.plus_clicked, 1.25,
QtCore.Qt.darkCyan, width=0.04)
self.minus_btn = ButtonItem(scene, self.min[0] + 0.035, self.min[1] - 0.04, "-", self, self.minus_clicked, 1.25,
QtCore.Qt.darkCyan, width=0.04)
self.plus_btn.setEnabled(False)
self.minus_btn.setEnabled(False)
self.pts = []
self.pts.append(SquarePointItem(scene, self.min[0], self.min[1], self, "BL", self.fixed)) # bottom-left corner
self.pts.append(SquarePointItem(scene, self.max[0], self.min[1], self, "BR", self.fixed)) # bottom-right corner
self.pts.append(SquarePointItem(scene, self.max[0], self.max[1], self, "TR", self.fixed)) # top-right corner
self.pts.append(SquarePointItem(scene, self.min[0], self.max[1], self, "TL", self.fixed)) # top-left corner
if len(poses) > 0 and self.fixed:
# depicts fixed objects
for pose in poses:
it = PlaceItem(
self.scn,
"Object",
pose.pose.position.x,
pose.pose.position.y,
pose.pose.position.z,
conversions.q2a(pose.pose.orientation),
self.object_type,
None,
place_pose_changed=None,
fixed=True,
txt=False,
parent=self,
horizontal=self.horizontal
)
self.items.append(it)
else:
# depicts editable objects
for i, pose in enumerate(poses):
rot_point = None # to save xy coordinates for rotation point (for rotating objects in a grid)
if i == 0:
rot = True
pom = self.find_corner("TL").get_pos()
rot_point = [pom[0] - 0.025, pom[1] + 0.025]
else:
rot = False
it = PlaceItem(
self.scn,
"Object",
pose.pose.position.x,
pose.pose.position.y,
pose.pose.position.z,
conversions.q2a(pose.pose.orientation),
self.object_type,
None,
place_pose_changed=None,
fixed=False,
txt=False,
rot=rot,
rot_point=rot_point,
rotation_changed=self.items_rotation_changed,
parent=self,
horizontal=self.horizontal
)
it.update_point()
self.items.append(it)
if self.items:
# rotation of all objects in a grid (list of all objects is handed over to
# the first object. When the first object is rotated, all objects are
# rotated)
self.items[0].set_other_items(self.items[1:])
self.plus_btn.setEnabled(True)
self.minus_btn.setEnabled(True)
self.dialog = DialogItem(self.scene(),
self.pix2m(self.scene().width() / 2),
0.1,
translate(
"Place item",
"Object place pose options"),
[
translate(
"Place item", "Rotate |"),
translate(
"Place item", "Rotate --")
],
self.dialog_cb)
if self.square_changed is not None:
# to save grid points and objects into the ProgramItem message
self.square_changed(self.get_square_points(), self.items)
self.update_bound()
self.update()
'''
Method updates text below the grid.
'''
def update_text(self):
if self.desc is None:
return
desc = self.caption
self.desc.set_content(desc)
'''
Method is called when "+" button is pushed. It increase spacing between objects and walls of the box,
and between objects.
'''
def plus_clicked(self, btn):
self.space += 0.01
self.point_changed(True, self.last_corner)
'''
Method is called when "-" button is pushed. It decrease spacing between objects and walls of the box,
and between objects.
'''
def minus_clicked(self, btn):
if self.space > 0.02:
self.space -= 0.01
self.point_changed(True, self.last_corner)
'''
Method updates the bounding rectangle.
'''
def update_bound(self):
self.min[0] = self.pix2m(self.pts[0].x())
self.min[1] = self.pix2m(self.pts[0].y())
self.max[0] = self.pix2m(self.pts[0].x())
self.max[1] = self.pix2m(self.pts[0].y())
self.orig_x = []
self.orig_y = []
for pt in self.pts:
p = (self.pix2m(pt.x()), self.pix2m(pt.y()))
if p[0] < self.min[0]:
self.min[0] = p[0]
if p[1] < self.min[1]:
self.min[1] = p[1]
if p[0] > self.max[0]:
self.max[0] = p[0]
if p[1] > self.max[1]:
self.max[1] = p[1]
point = pt.get_pos()
self.orig_x.append(point[0])
self.orig_y.append(point[1])
self.pom_min = [min(self.orig_x), min(self.orig_y)]
self.pom_max = [max(self.orig_x), max(self.orig_y)]
'''
Method returns a required corner.
'''
def find_corner(self, corner):
for pt in self.pts:
if pt.get_corner() == corner:
return pt
return None
'''
Metoda pre vykreslovanie gridu a objektov v nom. Je volana vzdy, ked sa pohne s niektorym rohom.
Rovnomerne rozmiestnuje objekty v gride, kontroluje ci nie su v kolizii.
Zaistuje ukladanie poloh bodov a gridu do spravy ProgramItem.
Method depics the gird and objects in it. It is called, when some corner is moved.
It secures even distribution of objects in the grid, checks collisions.
Also secures saving grid points and positions of objects into the ProgramItem message.
'''
def point_changed(self, finished=False, corner=""):
if self.fixed:
return
self.prepareGeometryChange()
corner = corner
# update of bounding rect
self.update_bound()
for pt in self.pts:
if (pt.get_corner() == "BR") and pt.get_changed():
self.find_corner("TR").setPos(pt.x(), self.find_corner("TR").y())
self.find_corner("BL").setPos(self.find_corner("BL").x(), pt.y())
self.desc.setPos(self.m2pix(self.min[0] - 0.01), self.m2pix(self.max[1] + 0.015))
corner = "BR"
pt.set_changed(False)
elif (pt.get_corner() == "BL") and pt.get_changed():
self.find_corner("TL").setPos(pt.x(), self.find_corner("TL").y())
self.find_corner("BR").setPos(self.find_corner("BR").x(), pt.y())
self.desc.setPos(self.m2pix(self.min[0] - 0.01), self.m2pix(self.max[1] + 0.015))
corner = "BL"
pt.set_changed(False)
elif (pt.get_corner() == "TL") and pt.get_changed():
self.find_corner("BL").setPos(pt.x(), self.find_corner("BL").y())
self.find_corner("TR").setPos(self.find_corner("TR").x(), pt.y())
self.desc.setPos(self.m2pix(self.min[0] - 0.01), self.m2pix(self.max[1] + 0.015))
corner = "TL"
pt.set_changed(False)
elif (pt.get_corner() == "TR") and pt.get_changed():
self.find_corner("BR").setPos(pt.x(), self.find_corner("BR").y())
self.find_corner("TL").setPos(self.find_corner("TL").x(), pt.y())
self.desc.setPos(self.m2pix(self.min[0] - 0.01), self.m2pix(self.max[1] + 0.015))
corner = "TR"
pt.set_changed(False)
self.plus_btn.setPos(self.m2pix(self.min[0] - 0.01), self.m2pix(self.max[1] + 0.04))
self.minus_btn.setPos(self.m2pix(self.min[0] + 0.035), self.m2pix(self.max[1] + 0.04))
if corner != "":
self.object_side_length_x = self.object_type.bbox.dimensions[0] + self.space
if not self.horizontal:
self.object_side_length_y = self.object_type.bbox.dimensions[1] + self.space
else:
self.object_side_length_y = self.object_type.bbox.dimensions[2] + self.space
width_count = int(
modf(
round(
(((self.max[0] - self.min[0]) - self.space) / self.object_side_length_x),
5))[1])
height_count = int(
modf(
round(
(((self.max[1] - self.min[1]) - self.space) / self.object_side_length_y),
5))[1])
if self.previous_width != width_count or self.previous_height != height_count:
ps = PoseStamped()
if corner == "BR" or corner == "TR":
ps.pose.position.x = self.pom_min[0] + self.space / 2 + self.object_side_length_x / 2
else:
ps.pose.position.x = self.pom_max[0] - self.space / 2 - self.object_side_length_x / 2
if corner == "BR" or corner == "BL":
ps.pose.position.y = self.pom_max[1] - self.space / 2 - self.object_side_length_y / 2
else:
ps.pose.position.y = self.pom_min[1] + self.space / 2 + self.object_side_length_y / 2
ps.pose.orientation.w = 1.0
if self.items:
rotation = self.items[0].rotation()
else:
rotation = 0.0
for it in self.items:
self.scn.removeItem(it)
del self.items[:]
for i in range(0, height_count):
for j in range(0, width_count):
rot_point = None # to save xy coordinates for rotation point (for rotating objects in a grid)
if corner == "BR" and i == 0 and j == 0:
rot = True
pom = self.find_corner("TL").get_pos()
rot_point = [pom[0] - 0.025, pom[1] + 0.025]
elif corner == "BL" and i == 0 and j == 0:
rot = True
pom = self.find_corner("TR").get_pos()
rot_point = [pom[0] + 0.025, pom[1] + 0.025]
elif corner == "TR" and i == 0 and j == 0:
rot = True
pom = self.find_corner("BL").get_pos()
rot_point = [pom[0] - 0.025, pom[1] - 0.025]
elif corner == "TL" and i == 0 and j == 0:
rot = True
pom = self.find_corner("BR").get_pos()
rot_point = [pom[0] + 0.025, pom[1] - 0.025]
else:
rot = False
it = PlaceItem(
self.scn,
"Object",
ps.pose.position.x,
ps.pose.position.y,
ps.pose.position.z,
conversions.q2a(ps.pose.orientation),
self.object_type,
None,
place_pose_changed=None,
fixed=False,
txt=False,
rot=rot,
rot_point=rot_point,
rotation_changed=self.items_rotation_changed,
parent=self,
horizontal=self.horizontal
)
it.setRotation(rotation)
it.update_point()
self.items.append(it)
if corner == "BR" or corner == "TR":
ps.pose.position.x += self.object_side_length_x # BR TR
else:
ps.pose.position.x -= self.object_side_length_x # TL BL
if corner == "BR" or corner == "TR":
ps.pose.position.x = self.pom_min[0] + self.space / 2 + self.object_side_length_x / 2 # BR a TR
else:
ps.pose.position.x = self.pom_max[0] - self.space / 2 - self.object_side_length_x / 2 # TL BL
if corner == "BR" or corner == "BL":
ps.pose.position.y -= self.object_side_length_y + self.space / 2 # BR BL
else:
ps.pose.position.y += self.object_side_length_y + self.space / 2 # TL TR
self.previous_width = width_count
self.previous_height = height_count
self.last_corner = corner
# rotation of all objects in a grid (list of all objects is handed over to
# the first object. When the first object is rotated, all objects are
# rotated)
if self.items:
self.items[0].set_other_items(self.items[1:])
if finished and self.square_changed is not None:
# even distribution of objects in the grid
if self.items:
new_object_length_x = ((self.pom_max[0] - self.pom_min[0]) - self.space) / self.previous_width
new_object_length_y = ((self.pom_max[1] - self.pom_min[1]) - self.space) / self.previous_height
for i, it in enumerate(self.items):
if self.last_corner == "BR" or self.last_corner == "TR":
new_x = self.pom_min[0] + self.space / 2 + new_object_length_x / \
2 + new_object_length_x * (i % self.previous_width)
else:
new_x = self.pom_max[0] - self.space / 2 - new_object_length_x / \
2 - new_object_length_x * (i % self.previous_width)
if self.last_corner == "BR" or self.last_corner == "BL":
new_y = self.pom_max[1] - self.space / 2 - new_object_length_y / 2 - new_object_length_y * (
i / self.previous_width)
else:
new_y = self.pom_min[1] + self.space / 2 + new_object_length_y / 2 + new_object_length_y * (
i / self.previous_width)
it.set_pos(new_x, new_y)
it.update_point()
for it in self.items: # to check if there are still some collisions
it.item_moved()
self.plus_btn.setEnabled(True)
self.minus_btn.setEnabled(True)
if self.last_corner == "BR" or self.last_corner == "BL": # TODO: skontrolovat ci nema byt TR a TL!!
self.items.reverse() # we want robot always to place object from furthest line
in_collision = False
for it in self.items: # to check collisions
if it.in_collision:
in_collision = True
break
if in_collision:
# to save only grid points into the ProgramItem message
self.square_changed(self.get_square_points(), [])
else:
# to save grid points and objects into the ProgramItem message
self.square_changed(self.get_square_points(), self.items)
self.update()
def dialog_cb(self, idx):
if idx == 0:
self.horizontal = False
self.point_changed(True, "BR")
else:
self.horizontal = True
self.point_changed(True, "BR")
'''
Method returns grid points.
'''
def get_square_points(self):
pts = []
for pt in self.pts:
pts.append(pt.get_pos())
return pts
'''
Method which is called after releasing the grid.
It saves new positions of grid points and objects into the ProgramItem message.
It checks collisions between grid objects and scene objects.
'''
def cursor_release(self):
if self.fixed:
return
for it in self.items:
it.item_moved()
if self.square_changed is not None:
in_collision = False
for it in self.items: # to check collisions
if it.in_collision:
in_collision = True
break
if in_collision:
# to save only grid points into the ProgramItem message
self.square_changed(self.get_square_points(), [])
else:
# to save grid points and objects into the ProgramItem message
self.square_changed(self.get_square_points(), self.items)
def cursor_press(self):
pass
'''
Method saves new rotations after rotating objects.
'''
def items_rotation_changed(self, items):
# to save grid points and objects into the ProgramItem message
self.square_changed(self.get_square_points(), items)
'''
Method returns the shape of this grid as a QPainterPath in local coordinates.
'''
def shape(self):
path = QtGui.QPainterPath()
path.addPolygon(QtGui.QPolygonF(self.square))
return path
'''
Method defines the outer bounds of the item as a rectangle.
'''
def boundingRect(self):
return QtCore.QRectF(
self.m2pix(
self.min[0]) -
2.5,
self.m2pix(
self.min[1]) -
2.5,
self.m2pix(
self.max[0] -
self.min[0]) +
5,
self.m2pix(
self.max[1] -
self.min[1]) +
5)
'''
Method paints the grid.
'''
def paint(self, painter, option, widget):
painter.setClipRect(option.exposedRect)
painter.setRenderHint(QtGui.QPainter.Antialiasing)
pen = QtGui.QPen()
pen.setStyle(QtCore.Qt.DotLine)
pen.setWidth(5)
pen.setBrush(QtCore.Qt.white)
pen.setCapStyle(QtCore.Qt.RoundCap)
pen.setJoinStyle(QtCore.Qt.RoundJoin)
painter.setPen(pen)
self.square = QtGui.QPolygon()
for i in range(0, len(self.pts)):
self.square.append(self.pts[i].pos().toPoint())
painter.drawPolygon(self.square)
def __init__(
self,
scene,
x,
y,
program_headers,
learned_dict,
selected_program_id=None,
program_selected_cb=None,
program_selection_changed_cb=None):
self.w = 100
self.h = 100
self.program_headers = program_headers
self.learned_dict = learned_dict
self.program_selected_cb = program_selected_cb
self.program_selection_changed_cb = program_selection_changed_cb
super(ProgramListItem, self).__init__(scene, x, y)
self.w = self.m2pix(0.2)
self.h = self.m2pix(0.25)
self.fixed = False
self.setZValue(100)
title = DescItem(self.scene(), 0, 0, self)
title.set_content(translate("ProgramListItem", "Program list"), 1.2)
title.setPos(QtCore.QPointF(self.m2pix(0.01), self.m2pix(0.01))) # TODO it should take coords given to __init__
data = []
self.map_from_idx_to_program_id = {}
self.map_from_program_id_to_idx = {}
self.program_headers.sort(key=lambda p: p.id)
for ph in self.program_headers:
data.append("Program " + str(ph.id) + "\n" + ph.name)
idx = len(data) - 1
self.map_from_idx_to_program_id[idx] = ph.id
self.map_from_program_id_to_idx[ph.id] = idx
self.list = ListItem(scene, 0, 0, 0.2 - 2 * 0.005, data, self.item_selected_cb, parent=self)
for idx in range(0, len(data)):
if not self.learned_dict[self.map_from_idx_to_program_id[idx]]:
self.list.items[idx].set_background_color(QtCore.Qt.red)
rospack = rospkg.RosPack()
icons_path = rospack.get_path('art_projected_gui') + '/icons/'
self.run_btn = ButtonItem(scene, 0, 0, "BTN", self, self.run_btn_cb, image_path=icons_path + "run.svg")
self.edit_btn = ButtonItem(scene, 0, 0, "BTN", self, self.edit_btn_cb, image_path=icons_path + "edit.svg")
self.template_btn = ButtonItem(scene, 0, 0, "BTN", self, self.template_btn_cb,
image_path=icons_path + "template.svg")
self.visualize_btn = ButtonItem(scene, 0, 0, "BTN", self, self.visualize_btn_cb,
image_path=icons_path + "visualize.svg")
self.run_btn.set_enabled(False)
self.edit_btn.set_enabled(False)
self.template_btn.set_enabled(False)
self.visualize_btn.set_enabled(False)
if selected_program_id is not None:
self.list.set_current_idx(self.map_from_program_id_to_idx[selected_program_id])
sp = self.m2pix(0.005)
# h = title.mapToParent(title.boundingRect().bottomLeft()).y() + sp
h = 0
self.list.setPos(sp, h)
h += self.list._height()
h += 2 * sp
btns = (self.run_btn, self.edit_btn, self.template_btn, self.visualize_btn)
self._place_childs_horizontally(h, sp, btns)
h += self.run_btn._height()
h += 3 * sp
self.h = h
self.setFlag(QtGui.QGraphicsItem.ItemIsMovable, True)
self.setFlag(QtGui.QGraphicsItem.ItemIsSelectable, True)
self.update()
class ObjectItem(Item):
"""The class to visualize (detected) object.
It currently supports only rotation around z-axis.
"""
def __init__(self, scene, object_id, object_type, x,
y, z, quaternion=(0, 0, 0, 1), sel_cb=None, selected=False, parent=None, horizontal=False):
self.object_id = object_id
self.selected = selected
self.sel_cb = sel_cb
self.horizontal = horizontal
# TODO check bbox type and use rectangle (used now) / ellipse, consider
# other angles
self.object_type = object_type
self.inflate = 0.01
self.def_color = QtCore.Qt.gray
self.lx = 0
self.ly = 0
self.desc = None
self.quaternion = (0, 0, 0, 1)
self.on_table = False
super(ObjectItem, self).__init__(scene, x, y, z, parent=parent)
self.desc = DescItem(scene, 0, 0, parent=self)
self.desc.setFlag(QtGui.QGraphicsItem.ItemIgnoresTransformations)
self.update_text()
self.set_orientation(quaternion)
if selected:
self.set_selected()
self._update_desc_pos()
self.setZValue(50)
def set_color(self, color=QtCore.Qt.gray):
self.def_color = color
self.update()
def _update_desc_pos(self):
if self.desc is not None:
# make upper left corner of description aligned with left extent of
# the (possibly rotated) object bounding box (highlight area)
self.desc.setPos(
self.mapFromScene(
self.x() -
self.sceneBoundingRect().width() /
2,
self.y() +
self.sceneBoundingRect().height() /
2 +
self.m2pix(0.01)))
def set_pos(self, x, y, z=None, parent_coords=False):
super(ObjectItem, self).set_pos(x, y, z, parent_coords)
self._update_desc_pos()
self.update_text()
def get_yaw_axis(self):
ax = ((1, 0, 0), (0, 1, 0), (0, 0, 1))
for idx in range(0, len(ax)):
res = conversions.qv_mult(self.quaternion, ax[idx])
# TODO euclid dist
if conversions.is_close(res[0], 0, abs_tol=0.1) and conversions.is_close(res[1], 0, abs_tol=0.1):
return idx
return -1
def set_orientation(self, q):
self.quaternion = q
ax = self.get_yaw_axis()
if ax == ObjectItem.Z:
self.lx = self.m2pix(self.inflate + self.object_type.bbox.dimensions[0])
self.ly = self.m2pix(self.inflate + self.object_type.bbox.dimensions[1])
sres = conversions.qv_mult(self.quaternion, (1, 0, 0))
angle = math.atan2(sres[1], sres[0])
self.on_table = self.position[2] < self.object_type.bbox.dimensions[2] + 0.05
elif ax in [ObjectItem.X, ObjectItem.Y]:
res = conversions.qv_mult(self.quaternion, (0, 0, 1))
self.lx = self.m2pix(self.inflate + self.object_type.bbox.dimensions[2])
# TODO use correct dimension (x/y) - now let's assume that x and y dimensions are same
self.ly = self.m2pix(self.inflate + self.object_type.bbox.dimensions[1])
angle = math.atan2(res[1], res[0])
self.on_table = self.position[2] < self.object_type.bbox.dimensions[0] + 0.05
else:
self.set_enabled(False, True)
return
self.setRotation(-angle / (math.pi * 2) * 360)
# TODO if not on table - display somewhere list of detected objects or what?
self.set_enabled(self.on_table, True)
self.update()
def update_text(self):
if self.desc is None:
return
desc = ""
desc += translate("ObjectItem", "ID: ") + self.object_id
if self.hover:
desc += "\n" + translate("ObjectItem",
"TYPE: ") + self.object_type.name
desc += "\n" + self.get_pos_str()
self.desc.set_content(desc)
def hover_changed(self):
self.update_text()
self.update()
def boundingRect(self):
if not self.scene():
return QtCore.QRectF()
self.lx = self.hover_ratio * self.inflate * \
self.m2pix(self.object_type.bbox.dimensions[0])
if not self.horizontal:
self.ly = self.hover_ratio * self.inflate * \
self.m2pix(self.object_type.bbox.dimensions[1])
else:
self.ly = self.hover_ratio * self.inflate * \
self.m2pix(self.object_type.bbox.dimensions[2])
p = 10.0
return QtCore.QRectF(-self.lx / 2 - p, -self.ly / 2 - p, self.lx + 2 * p, self.ly + 2 * p)
def paint(self, painter, option, widget):
if not self.scene():
return
if not self.on_table:
return
painter.setClipRect(option.exposedRect)
painter.setRenderHint(QtGui.QPainter.Antialiasing)
# if not self.horizontal:
# self.ly = self.m2pix(self.inflate + self.object_type.bbox.dimensions[1])
# else:
# self.ly = self.m2pix(self.inflate + self.object_type.bbox.dimensions[2])
rr = 10
painter.setBrush(QtCore.Qt.NoBrush)
pen = QtGui.QPen(self.def_color, 5, QtCore.Qt.SolidLine, QtCore.Qt.RoundCap)
if self.selected:
pen.setColor(QtCore.Qt.green)
pen.setWidth(10)
elif self.hover:
pen.setWidth(10)
painter.setPen(pen)
painter.drawRoundedRect(-self.lx / 2, -self.ly / 2, self.lx,
self.ly, rr, rr, QtCore.Qt.RelativeSize)
def cursor_press(self):
# TODO call base class method
if self.sel_cb is not None:
# callback should handle object selection
self.sel_cb(self.object_id, self.selected)
else:
# no callback - object will handle its selection
if not self.selected:
self.set_selected()
else:
self.set_selected(False)
def set_selected(self, selected=True):
if selected:
self.selected = True
# rospy.logdebug('Object ID ' + self.object_id + ' selected')
else:
self.selected = False
# rospy.logdebug('Object ID ' + self.object_id + ' unselected')
self.update()
class ProgramItem(Item):
def __init__(self,
scene,
x,
y,
program_helper,
instruction,
ih,
done_cb=None,
item_switched_cb=None,
learning_request_cb=None,
pause_cb=None,
cancel_cb=None,
stopped=False,
visualize=False,
v_visualize_cb=None,
v_back_cb=None,
vis_pause_cb=None,
vis_stop_cb=None,
vis_replay_cb=None,
vis_back_to_blocks_cb=None):
self.w = 100
self.h = 100
self.instruction = instruction
self.ih = ih
self.done_cb = done_cb
self.item_switched_cb = item_switched_cb
self.learning_request_cb = learning_request_cb
self.pause_cb = pause_cb
self.cancel_cb = cancel_cb
self.readonly = False
self.stopped = stopped
# variables for HoloLens visualization
self.visualize = visualize
self.visualization_paused = False
# callbacks for visualization buttons
self.v_visualize_cb = v_visualize_cb
self.v_back_cb = v_back_cb
self.vis_pause_cb = vis_pause_cb
self.vis_stop_cb = vis_stop_cb
self.vis_replay_cb = vis_replay_cb
self.vis_back_to_blocks_cb = vis_back_to_blocks_cb
super(ProgramItem, self).__init__(scene, x, y)
self.title = DescItem(self.scene(), 0, 0, self)
# TODO it should take coords given to __init__
self.title.setPos(QtCore.QPointF(self.m2pix(0.01), self.m2pix(0.01)))
self.w = self.m2pix(0.2)
self.h = self.m2pix(0.25)
self.sp = self.m2pix(0.005)
self.ph = program_helper
self.block_id = None
self.item_id = None
self.block_learned = False
self.program_learned = False
# block "view"
self.block_finished_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.block_finished_btn_cb,
image_path=icons_path +
"back.svg")
self.block_edit_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.block_edit_btn_cb,
image_path=icons_path + "edit.svg")
self.block_on_success_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.block_on_success_btn_cb,
image_path=icons_path +
"success.svg")
self.block_on_failure_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.block_on_failure_btn_cb,
image_path=icons_path +
"failure.svg")
# block "view" when in visualization
self.program_visualize_btn = ButtonItem(
self.scene(), 0, 0, translate("ProgramItem", "Visualize Program"),
self, self.program_visualize_btn_cb)
self.block_visualize_btn = ButtonItem(
self.scene(), 0, 0, translate("ProgramItem", "Visualize Block"),
self, self.block_visualize_btn_cb)
self.block_back_btn = ButtonItem(self.scene(), 0, 0,
translate("ProgramItem", "Back"),
self, self.block_back_btn_cb)
bdata = []
self.blocks_map = {} # map from indexes (key) to block_id (value)
self.blocks_map_rev = {}
for i in range(len(self.ph.get_program().blocks)):
bmsg = self.ph.get_program().blocks[i]
bdata.append(
translate("ProgramItem",
"Block %1\n%2\nSuccess: %3, failure: %4").arg(
bmsg.id).arg(bmsg.name).arg(bmsg.on_success).arg(
bmsg.on_failure))
idx = len(bdata) - 1
self.blocks_map[idx] = bmsg.id
self.blocks_map_rev[bmsg.id] = idx
self.blocks_list = ListItem(self.scene(),
0,
0,
0.2 - 2 * 0.005,
bdata,
self.block_selected_cb,
parent=self)
for k, v in self.blocks_map.iteritems():
self._update_block(v)
y = self.title.mapToParent(self.title.boundingRect().bottomLeft()).y()
self.blocks_list.setPos(self.sp, y)
y += self.blocks_list._height() + self.sp
if visualize:
self._place_childs_horizontally(y, self.sp, [
self.program_visualize_btn, self.block_visualize_btn,
self.block_back_btn
])
y += self.block_visualize_btn._height() + self.sp
self.block_back_btn.set_enabled(True)
self.block_visualize_btn.set_enabled(False)
self.program_visualize_btn.set_enabled(True)
# hide edit block buttons
group_visible(
(self.block_finished_btn, self.block_edit_btn,
self.block_on_failure_btn, self.block_on_success_btn), False)
else:
self._place_childs_horizontally(y, self.sp, [
self.block_edit_btn, self.block_on_success_btn,
self.block_on_failure_btn, self.block_finished_btn
])
y += self.block_finished_btn._height() + self.sp
group_enable((self.block_edit_btn, self.block_on_failure_btn,
self.block_on_success_btn), False)
# hide visualization block buttons
group_visible((self.block_visualize_btn,
self.program_visualize_btn, self.block_back_btn),
False)
self.h = y
# items "view"
self.item_edit_btn = ButtonItem(self.scene(),
0,
0,
translate("ProgramItem", "Ed"),
self,
self.item_edit_btn_cb,
image_path=icons_path + "edit.svg")
self.item_run_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.item_run_btn_cb,
image_path=icons_path + "run.svg")
self.item_on_success_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.item_on_success_btn_cb,
image_path=icons_path +
"success.svg")
self.item_on_failure_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.item_on_failure_btn_cb,
image_path=icons_path +
"failure.svg")
self.item_finished_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.item_finished_btn_cb,
image_path=icons_path + "back.svg")
self.items_list = None
group_visible((self.item_finished_btn, self.item_run_btn,
self.item_on_success_btn, self.item_on_failure_btn,
self.item_edit_btn), False)
# readonly (program running) "view"
self.pr_pause_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.pr_pause_btn_cb,
image_path=icons_path + "pause.svg")
if self.stopped:
self.pr_pause_btn.set_image(icons_path + "run.svg")
self.pr_cancel_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.pr_cancel_btn_cb,
image_path=icons_path + "stop.svg")
group_visible((self.pr_pause_btn, self.pr_cancel_btn), False)
# buttons for HoloLens visualization
self.vis_pause_btn = ButtonItem(self.scene(), 0, 0,
translate("ProgramItem", "Resume"),
self, self.vis_pause_btn_cb)
# quick hack .. init button with 'Resume' caption and switch back to
# 'Pause' to keep the button large enough for text switching
if not self.visualization_paused:
self.vis_pause_btn.set_caption(translate("ProgramItem", "Pause"))
self.vis_stop_btn = ButtonItem(self.scene(), 0, 0,
translate("ProgramItem", "Stop"), self,
self.vis_stop_btn_cb)
self.vis_replay_btn = ButtonItem(self.scene(), 0, 0,
translate("ProgramItem", "Replay"),
self, self.vis_replay_btn_cb)
self.vis_back_btn = ButtonItem(
self.scene(), 0, 0, translate("ProgramItem", "Back to blocks"),
self, self.vis_back_btn_cb)
group_visible((self.vis_pause_btn, self.vis_stop_btn,
self.vis_replay_btn, self.vis_back_btn), False)
self.fixed = False
self.editing_item = False
self.edit_request = False
self.run_request = False
self.setFlag(QtGui.QGraphicsItem.ItemIsMovable, True)
self.setFlag(QtGui.QGraphicsItem.ItemIsSelectable, True)
self.setZValue(100)
self._update_learned()
self.update()
if self.item_switched_cb:
self.item_switched_cb(None, None, blocks=True)
def _update_title(self):
color = QtCore.Qt.white
if self.items_list is not None:
if not self.block_learned and not self.readonly:
color = QtCore.Qt.red
self.title.set_content(translate(
"ProgramItem", "Program %1, block %2").arg(
self.ph.get_program_id()).arg(self.block_id),
scale=1.2,
color=color)
else:
if not self.program_learned and not self.readonly:
color = QtCore.Qt.red
self.title.set_content(translate("ProgramItem", "Program %1").arg(
self.ph.get_program_id()),
scale=1.2,
color=color)
def pr_pause_btn_cb(self, btn):
if self.pause_cb is not None:
ret = self.pause_cb()
if ret:
# set disabled and wait for state update
self.set_enabled(False)
def pr_cancel_btn_cb(self, btn):
if self.cancel_cb is not None:
ret = self.cancel_cb()
if ret:
# set disabled and wait for state update
self.set_enabled(False)
def vis_pause_btn_cb(self, btn):
# callback which notifies HoloLens that pause/resume button was hit
if self.vis_pause_cb is not None:
self.vis_pause_cb(self.visualization_paused)
# if visualization is paused .. then resume it - e.g. hit RESUME button
if self.visualization_paused:
self.visualization_paused = False
self.vis_pause_btn.set_caption(translate("ProgramItem", "Pause"))
# or visualization is running .. then pause it - e.g. hit PAUSE button
else:
self.visualization_paused = True
self.vis_pause_btn.set_caption(translate("ProgramItem", "Resume"))
def vis_stop_btn_cb(self, btn):
# callback which notifies HoloLens that stop button was hit
if self.vis_stop_cb is not None:
self.vis_stop_cb()
# make sure that visualization is not paused and handle it's button caption properly
if self.visualization_paused:
self.visualization_paused = False
self.vis_pause_btn.set_caption(translate("ProgramItem", "Pause"))
group_enable((self.vis_stop_btn, self.vis_pause_btn), False)
group_enable((self.vis_replay_btn, self.vis_back_btn), True)
def vis_replay_btn_cb(self, btn):
# callback which notifies HoloLens that replay button was hit
if self.vis_replay_cb is not None:
self.vis_replay_cb()
group_enable((self.vis_stop_btn, self.vis_pause_btn), True)
group_enable((self.vis_replay_btn, self.vis_back_btn), False)
def vis_back_btn_cb(self, btn):
# callback which notifies HoloLens that visualization ended
if self.vis_back_to_blocks_cb is not None:
self.vis_back_to_blocks_cb()
# go back to blocks view from visualization
group_visible((self.block_visualize_btn, self.program_visualize_btn,
self.block_back_btn, self.blocks_list), True)
self.block_back_btn.set_enabled(True)
self.program_visualize_btn.set_enabled(True)
self.show_visualization_buttons(False)
self.block_selected_cb(
) # TODO extract method to set buttons to proper state
self.blocks_list.setEnabled(True)
self.scene().removeItem(self.items_list)
self.items_list = None
self.item_id = None
if self.item_switched_cb is not None:
self.item_switched_cb(*self.cid)
self.update()
def _update_learned(self):
if self.block_id is not None:
self.block_learned = self.ph.block_learned(self.block_id)
self.program_learned = self.ph.program_learned()
self._update_title()
def set_readonly(self, readonly):
self.readonly = readonly
if self.readonly:
if self.items_list is not None:
self.items_list.setVisible(True)
self.items_list.setEnabled(False)
self.blocks_list.set_enabled(False, True)
group_visible(
(self.block_finished_btn, self.block_edit_btn,
self.block_on_failure_btn, self.block_on_success_btn), False)
group_enable((self.pr_pause_btn, self.pr_cancel_btn), True)
else:
# TODO
pass
self.update()
def set_program_btns_enabled(self, state):
group_enable((self.pr_pause_btn, self.pr_cancel_btn), state)
def set_active(self, block_id, item_id):
# print "set_active", block_id, item_id
old_block_id = self.block_id
self.block_id = block_id
self.item_id = item_id
if old_block_id != self.block_id and item_id is not None:
# remove old list first
self.scene().removeItem(self.items_list)
self.items_list = None
# init new one
self._init_items_list()
if self.item_id is not None:
self.items_list.set_current_idx(self.items_map_rev[self.item_id],
select=True)
self._handle_item_btns()
group_visible(
(self.block_finished_btn, self.block_edit_btn,
self.block_on_failure_btn, self.block_on_success_btn,
self.block_visualize_btn, self.program_visualize_btn,
self.block_back_btn, self.blocks_list), False)
else:
self.blocks_list.set_current_idx(
self.blocks_map_rev[self.block_id], select=True)
self._update_title()
def get_text_for_item(self, block_id, item_id):
item = self.ph.get_item_msg(block_id, item_id)
text = str(item.id)
text += " | "
# TODO deal with long strings
if item.name:
text += item.name
else:
text += self.ih[item.type].gui.learn.NAME
if len(item.ref_id) > 0:
if self.ph.item_has_nothing_to_set(block_id, item_id):
text += translate("ProgramItem",
" (copy of %1)").arg(item.ref_id[0])
# else:
# text += translate("ProgramItem", " (refers to %1)").arg(', '.join(str(x) for x in item.ref_id))
if item.type in self.ih.properties.using_object:
(obj, ref_id) = self.ph.get_object(block_id, item_id)
text += "\n"
if self.ph.is_object_set(block_id, item_id):
obj_txt = obj[0]
else:
obj_txt = "??"
text += translate("ProgramItem",
" Object type: %1").arg(obj_txt)
if ref_id != item_id:
text += translate("ProgramItem",
" (same as in %1)").arg(ref_id)
# instruction-specific additional text
# TODO it should use different class when running?
text += self.ih[item.type].gui.learn.get_text(self.ph, block_id,
item_id)
text += "\n"
text += translate("ProgramItem", " Success: %1, failure: %2").arg(
item.on_success).arg(item.on_failure)
return text
def show_visualization_buttons(self, buttons_visible):
"""Shows or hides buttons for visualization mode for HoloLens"""
group_visible((self.vis_pause_btn, self.vis_stop_btn,
self.vis_replay_btn, self.vis_back_btn),
buttons_visible)
def _init_items_list(self):
idata = []
self.items_map = {} # map from indexes (key) to item_id (value)
self.items_map_rev = {}
bmsg = self.ph.get_block_msg(self.block_id)
for i in range(len(bmsg.items)):
item_id = bmsg.items[i].id
idata.append(self.get_text_for_item(self.block_id, item_id))
self.items_map[i] = item_id
self.items_map_rev[item_id] = i
self.items_list = ListItem(self.scene(),
0,
0,
0.2 - 2 * 0.005,
idata,
self.item_selected_cb,
parent=self)
for k, v in self.items_map.iteritems():
if self.ph.get_item_msg(
self.block_id,
v).type in self.ih.properties.runnable_during_learning:
self._update_item(self.block_id, v)
else:
self.items_list.items[k].set_enabled(False)
y = self.title.mapToParent(
self.title.boundingRect().bottomLeft()).y() + self.sp
self.items_list.setPos(self.sp, y)
y += self.items_list._height() + self.sp
# in running state
if self.readonly:
self.items_list.setEnabled(False)
self._place_childs_horizontally(
y, self.sp, [self.pr_pause_btn, self.pr_cancel_btn])
y += self.pr_pause_btn._height() + 3 * self.sp
pr = (self.pr_pause_btn, self.pr_cancel_btn)
group_enable(pr, True)
group_visible((self.item_finished_btn, self.item_run_btn,
self.item_on_success_btn, self.item_on_failure_btn,
self.item_edit_btn), False)
self.show_visualization_buttons(False)
# going to HoloLens visualization
elif self.visualize:
self.items_list.setEnabled(False)
self._place_childs_horizontally(
y, self.sp,
[self.vis_pause_btn, self.vis_stop_btn, self.vis_replay_btn])
y += self.vis_back_btn._height() + self.sp
self._place_childs_horizontally(y, self.sp, [self.vis_back_btn])
y += self.vis_back_btn._height() + 3 * self.sp
self.show_visualization_buttons(True)
group_enable((self.vis_pause_btn, self.vis_stop_btn), True)
self.vis_back_btn.set_enabled(False)
group_visible((self.pr_pause_btn, self.pr_cancel_btn), False)
group_visible((self.item_run_btn, self.item_on_success_btn,
self.item_on_failure_btn, self.item_edit_btn),
False)
# in learning state
else:
btns = (self.item_edit_btn, self.item_run_btn,
self.item_on_success_btn, self.item_on_failure_btn,
self.item_finished_btn)
self._place_childs_horizontally(y, self.sp, btns)
y += max(btn._height() for btn in btns)
y += self.sp
group_visible((self.item_finished_btn, self.item_run_btn,
self.item_on_success_btn, self.item_on_failure_btn,
self.item_edit_btn), True)
self.item_finished_btn.setEnabled(True)
group_enable((self.item_run_btn, self.item_on_failure_btn,
self.item_on_success_btn, self.item_on_failure_btn),
False)
group_visible((self.pr_pause_btn, self.pr_cancel_btn), False)
self.show_visualization_buttons(False)
self.h = y
self._update_title()
self.update()
if self.item_switched_cb:
self.item_switched_cb(self.block_id, self.item_id, blocks=False)
def block_edit_btn_cb(self, btn):
group_visible((self.block_finished_btn, self.block_edit_btn,
self.item_on_success_btn, self.block_on_failure_btn,
self.block_on_success_btn, self.blocks_list), False)
self._init_items_list()
def block_visualize_btn_cb(self, btn):
group_visible((self.block_visualize_btn, self.program_visualize_btn,
self.block_back_btn, self.blocks_list), False)
# callback which notifies HoloLens that visualization started
if self.v_visualize_cb is not None:
self.v_visualize_cb(visualize_whole_program=False)
self._init_items_list()
def program_visualize_btn_cb(self, btn):
group_visible((self.block_visualize_btn, self.program_visualize_btn,
self.block_back_btn, self.blocks_list), False)
# callback which notifies HoloLens that visualization started
if self.v_visualize_cb is not None:
self.v_visualize_cb(visualize_whole_program=True)
self.block_id = self.ph.get_first_block_id()
self._init_items_list()
# go back from block view visualization into main menu
def block_back_btn_cb(self, btn):
group_visible((self.block_visualize_btn, self.program_visualize_btn,
self.block_back_btn), False)
# callback which notifies HoloLens that visualization ended
if self.v_back_cb is not None:
self.v_back_cb()
def block_selected_cb(self):
if self.blocks_list.selected_item_idx is not None:
self.block_id = self.blocks_map[self.blocks_list.selected_item_idx]
self.block_on_failure_btn.set_enabled(
self.ph.get_block_on_failure(self.block_id) != 0)
self.block_on_success_btn.set_enabled(
self.ph.get_block_on_success(self.block_id) != 0)
self.block_edit_btn.set_enabled(True)
self.block_visualize_btn.set_enabled(True)
if self.item_switched_cb:
self.item_switched_cb(self.block_id, None, blocks=True)
self._update_learned()
else:
self.block_id = None
self.item_id = None
self.block_edit_btn.set_enabled(False)
self.block_on_failure_btn.set_enabled(False)
self.block_on_success_btn.set_enabled(False)
self.block_visualize_btn.set_enabled(False)
if self.item_switched_cb is not None:
self.item_switched_cb(self.block_id, None, blocks=True)
@property
def cid(self):
"""Shortcut for accessing program item"""
return self.block_id, self.item_id
def _handle_item_btns(self):
# print ("_handle_item_btns, self.editing_item: " + str(self.editing_item))
if not self.editing_item:
of = self.ph.get_id_on_failure(*self.cid)
os = self.ph.get_id_on_success(*self.cid)
self.item_on_failure_btn.set_enabled(
of[0] != 0
and not (of[0] == self.block_id and of[1] == self.item_id))
self.item_on_success_btn.set_enabled(
os[0] != 0
and not (os[0] == self.block_id and os[1] == self.item_id))
self.item_run_btn.set_enabled(self._item_runnable())
self.item_edit_btn.set_enabled(self._item_editable())
else:
self.item_edit_btn.set_enabled(True)
self.item_edit_btn.set_image(icons_path + "save.svg")
group_enable((self.item_finished_btn, self.items_list), False)
self.item_run_btn.set_enabled(False)
group_visible((self.pr_cancel_btn, self.pr_pause_btn), False)
def _item_runnable(self):
if self.ph.item_requires_learning(*self.cid):
return self.ph.item_learned(*self.cid)
return self.ph.get_item_msg(
*self.cid).type in self.ih.properties.runnable_during_learning
def _item_editable(self):
if not self.ph.item_requires_learning(*self.cid):
return False
if self.ph.item_takes_params_from_ref(
*self.cid) and not self.ph.ref_params_learned(*self.cid):
return False
if self.ph.get_item_type(*self.cid) in self.ih.properties.place | self.ih.properties.ref_to_pick and not \
self.ph.ref_pick_learned(*self.cid)[0]:
return False
return True
def item_selected_cb(self):
# print ("self.items_list.selected_item_idx", self.items_list.selected_item_idx)
if self.items_list.selected_item_idx is not None:
self.item_id = self.items_map[self.items_list.selected_item_idx]
self._handle_item_btns()
self._update_learned()
else:
self.item_id = None
group_enable((self.item_run_btn, self.item_on_success_btn,
self.item_on_failure_btn, self.item_edit_btn), False)
if self.item_switched_cb is not None:
self.item_switched_cb(*self.cid)
def block_on_failure_btn_cb(self, btn):
self.set_active(self.ph.get_block_on_failure(self.block_id), None)
def block_on_success_btn_cb(self, btn):
self.set_active(self.ph.get_block_on_success(self.block_id), None)
def block_finished_btn_cb(self, btn):
if self.done_cb is not None:
self.done_cb()
def item_finished_btn_cb(self, btn):
# go back to blocks view
group_visible((self.block_finished_btn, self.block_edit_btn,
self.block_on_failure_btn, self.block_on_success_btn,
self.blocks_list), True)
group_visible((self.item_finished_btn, self.item_run_btn,
self.item_on_success_btn, self.item_on_failure_btn,
self.item_edit_btn), False)
self.block_selected_cb(
) # TODO extract method to set buttons to proper state
self.blocks_list.setEnabled(True)
self.block_finished_btn.setEnabled(True)
self.scene().removeItem(self.items_list)
self.items_list = None
self.item_id = None
if self.item_switched_cb is not None:
self.item_switched_cb(*self.cid)
self._update_title()
self.update()
def item_on_failure_btn_cb(self, btn):
of = self.ph.get_id_on_failure(*self.cid)
self.set_active(*of)
if self.item_switched_cb is not None:
self.item_switched_cb(*of)
def item_on_success_btn_cb(self, btn):
of = self.ph.get_id_on_success(*self.cid)
self.set_active(*of)
if self.item_switched_cb is not None:
self.item_switched_cb(*of)
def item_run_btn_cb(self, btn):
self.run_request = True
self.set_enabled(False)
self.learning_request_cb(LearningRequestGoal.EXECUTE_ITEM)
def item_edit_btn_cb(self, btn):
self.edit_request = True
# call action / disable all, wait for result (callback), enable editing
if not self.editing_item:
self.learning_request_cb(LearningRequestGoal.GET_READY)
else:
self.learning_request_cb(LearningRequestGoal.DONE)
self.set_enabled(False)
def learning_request_result(self, success):
self.set_enabled(True)
# TODO no success, no editing
if self.edit_request:
self.edit_request = False
if not self.editing_item:
self.editing_item = True
self.item_edit_btn.set_image(icons_path + "save.svg")
group_enable(
(self.item_finished_btn, self.items_list,
self.item_on_failure_btn, self.item_on_success_btn),
False)
else:
self.editing_item = False
self.item_edit_btn.set_image(icons_path + "edit.svg")
group_enable((self.item_finished_btn, self.items_list), True)
self._update_learned()
self._handle_item_btns()
elif self.run_request:
self.run_request = False
if self.item_switched_cb is not None:
self.item_switched_cb(self.block_id, self.item_id,
not self.editing_item)
def boundingRect(self):
return QtCore.QRectF(0, 0, self.w, self.h)
def set_place_pose(self, place):
msg = self.get_current_item()
assert len(msg.pose) > 0
msg.pose[0].pose.position.x = place.position[0]
msg.pose[0].pose.position.y = place.position[1]
msg.pose[0].pose.position.z = place.position[2]
msg.pose[0].pose.orientation = conversions.a2q(place.quaternion)
self._update_item()
'''
Method which saves place poses of all objects (in the grid) into the ProgramItem message.
'''
def set_place_poses(self, poses):
msg = self.get_current_item()
poses_count = len(poses)
msg_poses_count = len(msg.pose)
# TODO nemel by byt pocet objektu v gridu spis fixni (dany strukturou programu)?
if poses_count > msg_poses_count:
for i in range(poses_count - msg_poses_count):
ps = PoseStamped()
msg.pose.append(ps)
elif poses_count < msg_poses_count:
for i in range(msg_poses_count - poses_count):
msg.pose.pop()
for i, pose in enumerate(poses):
pos = pose.get_pos()
msg.pose[i].pose.position.x = pos[0]
msg.pose[i].pose.position.y = pos[1]
msg.pose[i].pose.orientation = conversions.yaw2quaternion(
pose.rotation())
self._update_item()
def set_pose(self, ps):
msg = self.get_current_item()
assert len(msg.pose) > 0
msg.pose[0] = ps
self._update_item()
def update_pose(self, ps, idx):
msg = self.get_current_item()
msg.pose[idx] = ps
self._update_item()
def clear_poses(self):
for ps in self.get_current_item().pose:
ps.pose = Pose()
self._update_item()
def get_poses_count(self):
msg = self.get_current_item()
return len(msg.pose)
def set_object(self, obj):
msg = self.get_current_item()
assert len(msg.object) > 0
msg.object[0] = obj
self._update_item()
def set_polygon(self, pts):
msg = self.get_current_item()
assert len(msg.polygon) > 0
del msg.polygon[0].polygon.points[:]
for pt in pts:
msg.polygon[0].polygon.points.append(Point32(pt[0], pt[1], 0))
self._update_item()
'''
Method which saves 4 points forming a grid into the ProgramItem message.
'''
def set_place_grid(self, pts):
msg = self.get_current_item()
assert len(msg.polygon) > 0
del msg.polygon[0].polygon.points[:]
for pt in pts:
msg.polygon[0].polygon.points.append(Point32(pt[0], pt[1], 0))
self._update_item()
def _update_block(self, block_id):
idx = self.blocks_map_rev[block_id]
if self.ph.block_learned(block_id):
self.blocks_list.items[idx].set_background_color()
else:
self.blocks_list.items[idx].set_background_color(QtCore.Qt.red)
def _update_item(self, block_id=None, item_id=None):
if block_id is None:
block_id = self.block_id
# need to update all items in block as there might be various dependencies (ref_id)
for idx, item_id in self.items_map.iteritems():
if self.ph.item_learned(block_id, item_id) or \
(self.ph.get_item_msg(block_id, item_id).type in self.ih.properties.runnable_during_learning and
not self.ih.requires_learning(self.ph.get_item_msg(block_id, item_id).type)):
self.items_list.items[idx].set_background_color()
else:
self.items_list.items[idx].set_background_color(QtCore.Qt.red)
self.items_list.items[idx].set_caption(
self.get_text_for_item(block_id, item_id))
self._update_block(block_id)
def get_current_item(self):
if self.block_id is not None and self.item_id is not None:
return self.ph.get_item_msg(*self.cid)
return None
def paint(self, painter, option, widget):
if not self.scene():
return
painter.setClipRect(option.exposedRect)
painter.setRenderHint(QtGui.QPainter.Antialiasing)
pen = QtGui.QPen()
pen.setStyle(QtCore.Qt.NoPen)
painter.setPen(pen)
painter.setBrush(QtCore.Qt.gray)
painter.setOpacity(0.5)
painter.drawRoundedRect(QtCore.QRect(0, 0, self.w, self.h), 5.0, 5.0)
def __init__(self,
scene,
x,
y,
program_helper,
instruction,
ih,
done_cb=None,
item_switched_cb=None,
learning_request_cb=None,
pause_cb=None,
cancel_cb=None,
stopped=False,
visualize=False,
v_visualize_cb=None,
v_back_cb=None,
vis_pause_cb=None,
vis_stop_cb=None,
vis_replay_cb=None,
vis_back_to_blocks_cb=None):
self.w = 100
self.h = 100
self.instruction = instruction
self.ih = ih
self.done_cb = done_cb
self.item_switched_cb = item_switched_cb
self.learning_request_cb = learning_request_cb
self.pause_cb = pause_cb
self.cancel_cb = cancel_cb
self.readonly = False
self.stopped = stopped
# variables for HoloLens visualization
self.visualize = visualize
self.visualization_paused = False
# callbacks for visualization buttons
self.v_visualize_cb = v_visualize_cb
self.v_back_cb = v_back_cb
self.vis_pause_cb = vis_pause_cb
self.vis_stop_cb = vis_stop_cb
self.vis_replay_cb = vis_replay_cb
self.vis_back_to_blocks_cb = vis_back_to_blocks_cb
super(ProgramItem, self).__init__(scene, x, y)
self.title = DescItem(self.scene(), 0, 0, self)
# TODO it should take coords given to __init__
self.title.setPos(QtCore.QPointF(self.m2pix(0.01), self.m2pix(0.01)))
self.w = self.m2pix(0.2)
self.h = self.m2pix(0.25)
self.sp = self.m2pix(0.005)
self.ph = program_helper
self.block_id = None
self.item_id = None
self.block_learned = False
self.program_learned = False
# block "view"
self.block_finished_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.block_finished_btn_cb,
image_path=icons_path +
"back.svg")
self.block_edit_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.block_edit_btn_cb,
image_path=icons_path + "edit.svg")
self.block_on_success_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.block_on_success_btn_cb,
image_path=icons_path +
"success.svg")
self.block_on_failure_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.block_on_failure_btn_cb,
image_path=icons_path +
"failure.svg")
# block "view" when in visualization
self.program_visualize_btn = ButtonItem(
self.scene(), 0, 0, translate("ProgramItem", "Visualize Program"),
self, self.program_visualize_btn_cb)
self.block_visualize_btn = ButtonItem(
self.scene(), 0, 0, translate("ProgramItem", "Visualize Block"),
self, self.block_visualize_btn_cb)
self.block_back_btn = ButtonItem(self.scene(), 0, 0,
translate("ProgramItem", "Back"),
self, self.block_back_btn_cb)
bdata = []
self.blocks_map = {} # map from indexes (key) to block_id (value)
self.blocks_map_rev = {}
for i in range(len(self.ph.get_program().blocks)):
bmsg = self.ph.get_program().blocks[i]
bdata.append(
translate("ProgramItem",
"Block %1\n%2\nSuccess: %3, failure: %4").arg(
bmsg.id).arg(bmsg.name).arg(bmsg.on_success).arg(
bmsg.on_failure))
idx = len(bdata) - 1
self.blocks_map[idx] = bmsg.id
self.blocks_map_rev[bmsg.id] = idx
self.blocks_list = ListItem(self.scene(),
0,
0,
0.2 - 2 * 0.005,
bdata,
self.block_selected_cb,
parent=self)
for k, v in self.blocks_map.iteritems():
self._update_block(v)
y = self.title.mapToParent(self.title.boundingRect().bottomLeft()).y()
self.blocks_list.setPos(self.sp, y)
y += self.blocks_list._height() + self.sp
if visualize:
self._place_childs_horizontally(y, self.sp, [
self.program_visualize_btn, self.block_visualize_btn,
self.block_back_btn
])
y += self.block_visualize_btn._height() + self.sp
self.block_back_btn.set_enabled(True)
self.block_visualize_btn.set_enabled(False)
self.program_visualize_btn.set_enabled(True)
# hide edit block buttons
group_visible(
(self.block_finished_btn, self.block_edit_btn,
self.block_on_failure_btn, self.block_on_success_btn), False)
else:
self._place_childs_horizontally(y, self.sp, [
self.block_edit_btn, self.block_on_success_btn,
self.block_on_failure_btn, self.block_finished_btn
])
y += self.block_finished_btn._height() + self.sp
group_enable((self.block_edit_btn, self.block_on_failure_btn,
self.block_on_success_btn), False)
# hide visualization block buttons
group_visible((self.block_visualize_btn,
self.program_visualize_btn, self.block_back_btn),
False)
self.h = y
# items "view"
self.item_edit_btn = ButtonItem(self.scene(),
0,
0,
translate("ProgramItem", "Ed"),
self,
self.item_edit_btn_cb,
image_path=icons_path + "edit.svg")
self.item_run_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.item_run_btn_cb,
image_path=icons_path + "run.svg")
self.item_on_success_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.item_on_success_btn_cb,
image_path=icons_path +
"success.svg")
self.item_on_failure_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.item_on_failure_btn_cb,
image_path=icons_path +
"failure.svg")
self.item_finished_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.item_finished_btn_cb,
image_path=icons_path + "back.svg")
self.items_list = None
group_visible((self.item_finished_btn, self.item_run_btn,
self.item_on_success_btn, self.item_on_failure_btn,
self.item_edit_btn), False)
# readonly (program running) "view"
self.pr_pause_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.pr_pause_btn_cb,
image_path=icons_path + "pause.svg")
if self.stopped:
self.pr_pause_btn.set_image(icons_path + "run.svg")
self.pr_cancel_btn = ButtonItem(self.scene(),
0,
0,
"BTN",
self,
self.pr_cancel_btn_cb,
image_path=icons_path + "stop.svg")
group_visible((self.pr_pause_btn, self.pr_cancel_btn), False)
# buttons for HoloLens visualization
self.vis_pause_btn = ButtonItem(self.scene(), 0, 0,
translate("ProgramItem", "Resume"),
self, self.vis_pause_btn_cb)
# quick hack .. init button with 'Resume' caption and switch back to
# 'Pause' to keep the button large enough for text switching
if not self.visualization_paused:
self.vis_pause_btn.set_caption(translate("ProgramItem", "Pause"))
self.vis_stop_btn = ButtonItem(self.scene(), 0, 0,
translate("ProgramItem", "Stop"), self,
self.vis_stop_btn_cb)
self.vis_replay_btn = ButtonItem(self.scene(), 0, 0,
translate("ProgramItem", "Replay"),
self, self.vis_replay_btn_cb)
self.vis_back_btn = ButtonItem(
self.scene(), 0, 0, translate("ProgramItem", "Back to blocks"),
self, self.vis_back_btn_cb)
group_visible((self.vis_pause_btn, self.vis_stop_btn,
self.vis_replay_btn, self.vis_back_btn), False)
self.fixed = False
self.editing_item = False
self.edit_request = False
self.run_request = False
self.setFlag(QtGui.QGraphicsItem.ItemIsMovable, True)
self.setFlag(QtGui.QGraphicsItem.ItemIsSelectable, True)
self.setZValue(100)
self._update_learned()
self.update()
if self.item_switched_cb:
self.item_switched_cb(None, None, blocks=True)
class PolygonItem(Item):
def __init__(self,
scene,
caption,
obj_coords=[],
poly_points=[],
polygon_changed=None,
fixed=False):
self.caption = caption
self.polygon_changed = polygon_changed
self.poly = QtGui.QPolygon()
self.desc = None
super(PolygonItem, self).__init__(scene, 0.5,
0.5) # TODO what should be here?
self.fixed = fixed
self.convex = True
if not self.fixed:
self.setFlag(QtGui.QGraphicsItem.ItemIsMovable, True)
self.setFlag(QtGui.QGraphicsItem.ItemIsSelectable, True)
self.pts = []
if len(obj_coords) > 0:
min = [obj_coords[0][0], obj_coords[0][1]]
max = [obj_coords[0][0], obj_coords[0][1]]
for pt in obj_coords:
pt = [pt[0], pt[1]]
if pt[0] < min[0]:
min[0] = pt[0]
if pt[1] < min[1]:
min[1] = pt[1]
if pt[0] > max[0]:
max[0] = pt[0]
if pt[1] > max[1]:
max[1] = pt[1]
pad = 0.02
min[0] -= pad
min[1] -= pad
max[0] += pad
max[1] += pad
self.pts.append(
PointItem(scene, min[0], min[1], self, self.point_changed))
self.pts.append(
PointItem(scene, max[0], min[1], self, self.point_changed))
self.pts.append(
PointItem(scene, max[0], max[1], self, self.point_changed))
self.pts.append(
PointItem(scene, min[0], max[1], self, self.point_changed))
if self.polygon_changed is not None:
self.polygon_changed(self.get_poly_points())
elif len(poly_points) > 0:
for pt in poly_points:
self.pts.append(
PointItem(scene, pt[0], pt[1], self, self.point_changed,
fixed))
else:
pass # TODO chyba
for pt in self.pts:
self.poly.append(pt.pos().toPoint())
self.desc = DescItem(scene, 0, 0, self)
self.desc.setFlag(QtGui.QGraphicsItem.ItemIgnoresTransformations)
self.__update_desc_pos()
self.__update_text()
self.update()
def __update_desc_pos(self):
if self.desc is not None:
p = self.poly.boundingRect().bottomLeft() + QtCore.QPointF(
0, self.m2pix(0.02))
self.desc.setPos(p)
def __update_text(self):
if self.desc is None:
return
desc = self.caption
# TODO number of objects in polygon?
self.desc.set_content(desc)
def point_changed(self, pt, finished):
self.poly.setPoint(self.pts.index(pt), pt.pos().toPoint())
ps = self.poly.size()
# TODO what to do with shuffled points? should be fixed...
for i in range(2, ps + 2):
line1 = QtCore.QLineF(self.poly.point(i - 2),
self.poly.point((i - 1) % ps))
line2 = QtCore.QLineF(self.poly.point((i - 1) % ps),
self.poly.point(i % ps))
a1 = line1.angle()
a2 = line2.angle()
d = a2 - a1
if d < 0:
d = 360 + d
if d < 315 and d > 180:
self.convex = False
break
else:
self.convex = True
self.__update_desc_pos()
self.update()
if finished and self.convex and self.polygon_changed is not None:
self.polygon_changed(self.get_poly_points())
def mouseReleaseEvent(self, event):
self.cursor_release()
super(Item, self).mouseReleaseEvent(event)
def cursor_release(self):
# TODO call base class method
if self.convex and self.polygon_changed is not None:
self.polygon_changed(self.get_poly_points())
def get_poly_points(self):
pts = []
try:
for pt in self.pts:
pts.append(pt.get_pos())
except AttributeError:
return None
return pts
def shape(self):
path = QtGui.QPainterPath()
path.addPolygon(QtGui.QPolygonF(self.poly))
return path
def boundingRect(self):
return QtCore.QRectF(self.poly.boundingRect())
def paint(self, painter, option, widget):
# TODO detekovat ze je polygon "divny" (prekrouceny) a zcervenat
# TODO vypsat kolik obsahuje objektu
if not self.scene():
return
painter.setClipRect(option.exposedRect)
painter.setRenderHint(QtGui.QPainter.Antialiasing)
# painter.setPen(QtCore.Qt.white)
# painter.drawRect(self.boundingRect())
pen = QtGui.QPen()
pen.setStyle(QtCore.Qt.DotLine)
pen.setWidth(5)
if self.convex:
pen.setBrush(QtCore.Qt.white)
else:
pen.setBrush(QtCore.Qt.red)
pen.setCapStyle(QtCore.Qt.RoundCap)
pen.setJoinStyle(QtCore.Qt.RoundJoin)
painter.setPen(pen)
painter.drawPolygon(self.poly)
class PlaceItem(Item):
def __init__(self, scene, rpm, caption, x, y, place_pose_changed=None, outline_diameter=0.1, selected=False, fixed=False):
self.outline_diameter = outline_diameter
self.caption = caption
self.in_collision = False
super(PlaceItem, self).__init__(scene, rpm, x, y)
self.desc = DescItem(scene, rpm, -self.outline_diameter * 1.3 / 2.0, self.outline_diameter * 1.3 / 2 + 0.01, self)
self.update_text()
self.fixed = fixed
self.place_pose_changed = place_pose_changed
def hover_changed(self):
self.update_text()
self.update()
def update_text(self):
desc = []
desc.append(self.caption)
if self.hover:
desc.append(self.get_pos_str())
self.desc.set_content(desc)
def cursor_release(self):
if self.place_pose_changed is not None:
self.place_pose_changed(self.get_pos())
def boundingRect(self):
es = self.m2pix(self.outline_diameter * 1.3)
return QtCore.QRectF(-es / 2, -es / 2, es, es + 40)
def shape(self):
path = QtGui.QPainterPath()
es = self.m2pix(self.outline_diameter)
path.addEllipse(QtCore.QPoint(0, 0), es / 2, es / 2)
return path
def item_moved(self):
# TODO testovat kolize jen s PlaceItem?
for it in self.collidingItems():
if isinstance(it, PlaceItem) or isinstance(it, ObjectItem):
self.in_collision = True
break
else:
self.in_collision = False
def paint(self, painter, option, widget):
painter.setClipRect(option.exposedRect)
painter.setRenderHint(QtGui.QPainter.Antialiasing)
es = self.m2pix(self.outline_diameter)
if self.hover and not self.fixed:
painter.setBrush(QtCore.Qt.gray)
painter.setPen(QtCore.Qt.gray)
painter.drawEllipse(QtCore.QPoint(0, 0), es / 2 * 1.3, es / 2 * 1.3)
if self.fixed:
painter.setBrush(QtCore.Qt.gray)
elif not self.in_collision:
painter.setBrush(QtCore.Qt.cyan)
else:
painter.setBrush(QtCore.Qt.red)
painter.drawEllipse(QtCore.QPoint(0, 0), es / 2, es / 2)
class ObjectItem(Item):
def __init__(self, scene, rpm, object_id, object_type, x, y, sel_cb=None, outline_diameter=0.1, selected=False):
self.object_id = object_id
self.object_type = object_type
self.outline_diameter = outline_diameter
self.selected = selected
self.sel_cb = sel_cb
super(ObjectItem, self).__init__(scene, rpm, x, y)
self.desc = DescItem(scene, rpm, -self.outline_diameter * 1.3 / 2.0, self.outline_diameter * 1.3 / 2 + 0.01, self)
self.update_text()
if selected:
self.set_selected()
def update_text(self):
desc = []
desc.append(translate("ObjectItem", "ID: ") + self.object_id)
if self.hover:
desc.append(translate("ObjectItem", "TYPE: ") + self.object_type)
desc.append(self.get_pos_str())
self.desc.set_content(desc)
def hover_changed(self):
self.update_text()
self.update()
def boundingRect(self):
es = self.m2pix(self.outline_diameter * 1.3)
p = 1.0
return QtCore.QRectF(-es / 2 - p, -es / 2 - p, es + 2 * p, es + 2 * p)
def shape(self):
path = QtGui.QPainterPath()
es = self.m2pix(self.outline_diameter)
path.addEllipse(QtCore.QPoint(0, 0), es / 2, es / 2)
return path
def paint(self, painter, option, widget):
painter.setClipRect(option.exposedRect)
painter.setRenderHint(QtGui.QPainter.Antialiasing)
eso = self.m2pix(self.outline_diameter * 1.3)
es = self.m2pix(self.outline_diameter)
if self.selected:
painter.setBrush(QtCore.Qt.green)
painter.setPen(QtCore.Qt.green)
painter.drawEllipse(QtCore.QPoint(0, 0), eso / 2, eso / 2)
elif self.hover:
painter.setBrush(QtCore.Qt.gray)
painter.setPen(QtCore.Qt.gray)
painter.drawEllipse(QtCore.QPoint(0, 0), eso / 2, eso / 2)
# TODO disp add info
painter.setBrush(QtCore.Qt.white)
painter.setPen(QtCore.Qt.white)
painter.drawEllipse(QtCore.QPoint(0, 0), es / 2, es / 2)
def cursor_press(self): # TODO cursor_click??
if self.sel_cb is not None:
# callback should handle object selection
self.sel_cb(self.object_id, self.selected)
else:
# no callback - object will handle its selection
if not self.selected:
self.set_selected()
else:
self.set_selected(False)
def set_selected(self, selected=True):
if selected:
self.selected = True
# rospy.logdebug('Object ID ' + self.object_id + ' selected')
else:
self.selected = False
# rospy.logdebug('Object ID ' + self.object_id + ' unselected')
self.update()