def _draw_playhead(self, painter):
"""
Draw a line and 2 triangles to denote the current position being viewed
:param painter: ,''QPainter''
"""
px = self.map_stamp_to_x(self.playhead.to_sec())
pw, ph = self._playhead_pointer_size
# Line
painter.setPen(QPen(self._playhead_color))
painter.setBrush(QBrush(self._playhead_color))
painter.drawLine(px, self._history_top - 1, px,
self._history_bottom + 2)
# Upper triangle
py = self._history_top - ph
painter.drawPolygon(
QPolygonF([
QPointF(px, py + ph),
QPointF(px + pw, py),
QPointF(px - pw, py)
]))
# Lower triangle
py = self._history_bottom + 1
painter.drawPolygon(
QPolygonF([
QPointF(px, py),
QPointF(px + pw, py + ph),
QPointF(px - pw, py + ph)
]))
painter.setBrush(self._default_brush)
painter.setPen(self._default_pen)
def c(msg):
if not (msg.header.frame_id == '/map' or msg.header.frame_id == ''):
try:
self._tf.waitForTransform(msg.header.frame_id, '/map', rospy.Time(), rospy.Duration(10))
points_stamped = []
for pt in msg.polygon.points:
ps = PointStamped()
ps.header.frame_id = msg.header.frame_id
ps.point.x = pt.x
ps.point.y = pt.y
points_stamped.append(ps)
trans_pts = [self._tf.transformPoint('/map', pt).point for pt in points_stamped]
except tf.Exception:
rospy.logerr("TF Error")
trans_pts = []
else:
trans_pts = [pt for pt in msg.polygon.points]
if len(trans_pts) > 0:
pts = [QPointF(pt.x / self.resolution, pt.y / self.resolution) for pt in trans_pts]
close = trans_pts[0]
pts.append(QPointF(close.x / self.resolution, close.y / self.resolution))
poly.path = QPolygonF(pts)
self.polygon_changed.emit(name)
def paint(self, painter, option, widget):
painter.setFont(self._time_font)
painter.fillRect(0, 0, SandtrayItem.length * SandtrayItem.scale,
SandtrayItem.width * SandtrayItem.scale,
painter.background())
for color, polys in self._zones.items():
painter.setPen(QPen(color, 2))
for poly in polys:
painter.drawPolygon(QPolygonF(poly))
painter.setBrush(QBrush(self._fg_color))
painter.setPen(QPen(self._border_color, 1))
painter.drawRect(0, 0, SandtrayItem.length * SandtrayItem.scale,
SandtrayItem.width * SandtrayItem.scale)
for label, pos in self._items.items():
x, y = pos[0] * SandtrayItem.scale, pos[1] * SandtrayItem.scale
painter.drawText(x - 10, y - 10, label)
if "cube" in label:
painter.setBrush(QBrush(self._cube_color))
painter.drawRect(x - 5, y - 5, 10, 10)
else:
painter.setBrush(QBrush(self._item_color))
painter.drawEllipse(QPointF(x, y), 10, 10)
def __init__(self, parent=None):
super(QMapAnnotation, self).__init__()
self._parent = parent
self.destroyed.connect(self.close)
self._load_ui()
self._scene = self.map_view._scene
self._viz_markers_pose = None
self._viz_marker_items = {}
self._viz_marker_items["annotations"] = []
self._viz_marker_items["new_annotations"] = []
self._viz_marker_polygon = QPolygonF(
[QPointF(-4, 4), QPointF(-4, -4),
QPointF(12, 0)])
self._callback = {}
self._callback['save_annotation'] = None
self._callback['load_world'] = None
self._callback['add_annotation'] = None
self._wc = None
self._init_events()
self._drawing_objects = {}
self._init_variables_for_drawing()
self._init_variableS_for_list()
def __init__(self, parent=None):
super(QSlamWidget, self).__init__()
self._parent = parent
self._slam_widget_interface = None
self._load_ui()
self._init_events()
self.destroyed.connect(self.close)
self._scene = self.map_view._scene
self._robot_pose = None
self._robot_pose_item = None
self._robot_polygon = QPolygonF(
[QPointF(-4, 4), QPointF(-4, -4),
QPointF(12, 0)])
self._scan = None
self._scan_items = []
self._callback = {}
self._callback['save_map'] = None
def paintEvent(self, event):
painter = QPainter(self)
#painter.begin(self)
# puts the arrow in the middle
painter.translate(self.width() / 2, self.height() / 2)
painter.rotate(self.angleRobo + self.angleVel)
line = QLineF(0, 0, self.width() / 2 - 3, 0)
headSize = min(self.width() / 20, 4)
points = QPolygonF()
points.append(QPointF(self.width() / 2 - headSize * 2, headSize))
points.append(QPointF(self.width() / 2 - headSize * 2, -headSize))
points.append(QPointF(self.width() / 2 - 3, 0))
pen = QPen(self.color, 2)
painter.setPen(pen)
brush = QBrush(self.color)
painter.setBrush(brush)
painter.drawLine(line)
painter.drawConvexPolygon(points)
def _create_vehicle_item(self, color, mav_id, vehicle_type):
if vehicle_type == MAV_TYPE_QUADROTOR:
# draw cross
item = QGraphicsPolygonItem(QPolygonF([
QPointF(0, 0),
QPointF(-3, -3),
QPointF(3, 3),
QPointF(0, 0),
QPointF(-3, 3),
QPointF(3, -3),
QPointF(0, 0),
]))
elif vehicle_type == MAV_TYPE_FIXED_WING:
# draw circle
item = QGraphicsEllipseItem(-3, -3, 6, 6)
else:
# draw square
item = QGraphicsRectItem(-3, -3, 6, 6)
item.setBrush(self._brushes[color])
item.setPen(self._pens[color])
item.setToolTip('%s #%d (%s)' % (color, mav_id, vehicle_type))
return item
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)
def __init__(self,
spline,
label,
label_center,
from_node,
to_node,
parent=None,
**kwargs):
super(EdgeItem, self).__init__(parent, **kwargs)
self._edge_pen_width = kwargs.get('edge_pen_width',
self.EDGE_PEN_WIDTH)
self.from_node = from_node
self.from_node.add_outgoing_edge(self)
self.to_node = to_node
self.to_node.add_incoming_edge(self)
self._brush = QBrush(self._color)
self._label_pen = QPen()
self._label_pen.setColor(self._color)
self._label_pen.setJoinStyle(Qt.RoundJoin)
self._label_pen.setWidthF(self._label_pen_width)
self._edge_pen = QPen()
self._edge_pen.setColor(self._color)
self._edge_pen.setWidthF(self._edge_pen_width)
self._sibling_edges = set()
self._label = None
if label is not None:
self._label = QGraphicsSimpleTextItem(label)
font = self._label.font()
font.setPointSize(8)
self._label.setFont(font)
label_rect = self._label.boundingRect()
label_rect.moveCenter(label_center)
self._label.setPos(label_rect.x(), label_rect.y())
# spline specification according to http://www.graphviz.org/doc/info/attrs.html#k:splineType
coordinates = spline.split(' ')
# extract optional end_point
end_point = None
if coordinates[0].startswith('e,'):
parts = coordinates.pop(0)[2:].split(',')
end_point = QPointF(float(parts[0]), -float(parts[1]))
# extract optional start_point
if coordinates[0].startswith('s,'):
parts = coordinates.pop(0).split(',')
# first point
parts = coordinates.pop(0).split(',')
point = QPointF(float(parts[0]), -float(parts[1]))
path = QPainterPath(point)
while len(coordinates) > 2:
# extract triple of points for a cubic spline
parts = coordinates.pop(0).split(',')
point1 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point2 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point3 = QPointF(float(parts[0]), -float(parts[1]))
path.cubicTo(point1, point2, point3)
self._arrow = None
if end_point is not None:
# draw arrow
self._arrow = QGraphicsPolygonItem()
polygon = QPolygonF()
polygon.append(point3)
offset = QPointF(end_point - point3)
corner1 = QPointF(-offset.y(), offset.x()) * 0.35
corner2 = QPointF(offset.y(), -offset.x()) * 0.35
polygon.append(point3 + corner1)
polygon.append(end_point)
polygon.append(point3 + corner2)
self._arrow.setPolygon(polygon)
self._path = QGraphicsPathItem()
self._path.setPath(path)
self.addToGroup(self._path)
self._brush.setColor(self._color)
self._edge_pen.setColor(self._color)
self._label_pen.setColor(self._color)
self._path.setPen(self._edge_pen)
if self._arrow is not None:
self._arrow.setBrush(self._brush)
self._arrow.setPen(self._edge_pen)
if self._label is not None:
self._label.setBrush(self._brush)
self._label.setPen(self._label_pen)
def __init__(self, highlight_level, spline, label_center, label, from_node, to_node, parent=None, penwidth=1, edge_color=None, style='solid'):
super(EdgeItem, self).__init__(highlight_level, parent)
self.from_node = from_node
self.from_node.add_outgoing_edge(self)
self.to_node = to_node
self.to_node.add_incoming_edge(self)
self._default_edge_color = self._COLOR_BLACK
if edge_color is not None:
self._default_edge_color = edge_color
self._default_text_color = self._COLOR_BLACK
self._default_color = self._COLOR_BLACK
self._text_brush = QBrush(self._default_color)
self._shape_brush = QBrush(self._default_color)
if style in ['dashed', 'dotted']:
self._shape_brush = QBrush(Qt.transparent)
self._label_pen = QPen()
self._label_pen.setColor(self._default_text_color)
self._label_pen.setJoinStyle(Qt.RoundJoin)
self._edge_pen = QPen(self._label_pen)
self._edge_pen.setWidth(penwidth)
self._edge_pen.setColor(self._default_edge_color)
self._edge_pen.setStyle(self._qt_pen_styles.get(style, Qt.SolidLine))
self._sibling_edges = set()
self._label = None
if label is not None:
self._label = QGraphicsSimpleTextItem(label)
label_rect = self._label.boundingRect()
label_rect.moveCenter(label_center)
self._label.setPos(label_rect.x(), label_rect.y())
self._label.hoverEnterEvent = self._handle_hoverEnterEvent
self._label.hoverLeaveEvent = self._handle_hoverLeaveEvent
self._label.setAcceptHoverEvents(True)
# spline specification according to http://www.graphviz.org/doc/info/attrs.html#k:splineType
coordinates = spline.split(' ')
# extract optional end_point
end_point = None
if (coordinates[0].startswith('e,')):
parts = coordinates.pop(0)[2:].split(',')
end_point = QPointF(float(parts[0]), -float(parts[1]))
# extract optional start_point
if (coordinates[0].startswith('s,')):
parts = coordinates.pop(0).split(',')
# first point
parts = coordinates.pop(0).split(',')
point = QPointF(float(parts[0]), -float(parts[1]))
path = QPainterPath(point)
while len(coordinates) > 2:
# extract triple of points for a cubic spline
parts = coordinates.pop(0).split(',')
point1 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point2 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point3 = QPointF(float(parts[0]), -float(parts[1]))
path.cubicTo(point1, point2, point3)
self._arrow = None
if end_point is not None:
# draw arrow
self._arrow = QGraphicsPolygonItem()
polygon = QPolygonF()
polygon.append(point3)
offset = QPointF(end_point - point3)
corner1 = QPointF(-offset.y(), offset.x()) * 0.35
corner2 = QPointF(offset.y(), -offset.x()) * 0.35
polygon.append(point3 + corner1)
polygon.append(end_point)
polygon.append(point3 + corner2)
self._arrow.setPolygon(polygon)
self._arrow.hoverEnterEvent = self._handle_hoverEnterEvent
self._arrow.hoverLeaveEvent = self._handle_hoverLeaveEvent
self._arrow.setAcceptHoverEvents(True)
self._path = QGraphicsPathItem()
self._path.setPath(path)
self.addToGroup(self._path)
self.set_node_color()
self.set_label_color()
def __init__(self,
highlight_level,
spline,
label_center,
label,
from_node,
to_node,
parent=None,
penwidth=1,
edge_color=None,
style='solid'):
super(EdgeItem, self).__init__(highlight_level, parent)
self.from_node = from_node
self.from_node.add_outgoing_edge(self)
self.to_node = to_node
self.to_node.add_incoming_edge(self)
self._default_edge_color = self._COLOR_BLACK
if edge_color is not None:
self._default_edge_color = edge_color
self._default_text_color = self._COLOR_BLACK
self._default_color = self._COLOR_BLACK
self._text_brush = QBrush(self._default_color)
self._shape_brush = QBrush(self._default_color)
if style in ['dashed', 'dotted']:
self._shape_brush = QBrush(Qt.transparent)
self._label_pen = QPen()
self._label_pen.setColor(self._default_text_color)
self._label_pen.setJoinStyle(Qt.RoundJoin)
self._edge_pen = QPen(self._label_pen)
self._edge_pen.setWidth(penwidth)
self._edge_pen.setColor(self._default_edge_color)
self._edge_pen.setStyle(self._qt_pen_styles.get(style, Qt.SolidLine))
self._sibling_edges = set()
self._label = None
if label is not None:
self._label = QGraphicsSimpleTextItem(label)
label_rect = self._label.boundingRect()
label_rect.moveCenter(label_center)
self._label.setPos(label_rect.x(), label_rect.y())
self._label.hoverEnterEvent = self._handle_hoverEnterEvent
self._label.hoverLeaveEvent = self._handle_hoverLeaveEvent
self._label.setAcceptHoverEvents(True)
# spline specification according to http://www.graphviz.org/doc/info/attrs.html#k:splineType
coordinates = spline.split(' ')
# extract optional end_point
end_point = None
if (coordinates[0].startswith('e,')):
parts = coordinates.pop(0)[2:].split(',')
end_point = QPointF(float(parts[0]), -float(parts[1]))
# extract optional start_point
if (coordinates[0].startswith('s,')):
parts = coordinates.pop(0).split(',')
# first point
parts = coordinates.pop(0).split(',')
point = QPointF(float(parts[0]), -float(parts[1]))
path = QPainterPath(point)
while len(coordinates) > 2:
# extract triple of points for a cubic spline
parts = coordinates.pop(0).split(',')
point1 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point2 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point3 = QPointF(float(parts[0]), -float(parts[1]))
path.cubicTo(point1, point2, point3)
self._arrow = None
if end_point is not None:
# draw arrow
self._arrow = QGraphicsPolygonItem()
polygon = QPolygonF()
polygon.append(point3)
offset = QPointF(end_point - point3)
corner1 = QPointF(-offset.y(), offset.x()) * 0.35
corner2 = QPointF(offset.y(), -offset.x()) * 0.35
polygon.append(point3 + corner1)
polygon.append(end_point)
polygon.append(point3 + corner2)
self._arrow.setPolygon(polygon)
self._arrow.hoverEnterEvent = self._handle_hoverEnterEvent
self._arrow.hoverLeaveEvent = self._handle_hoverLeaveEvent
self._arrow.setAcceptHoverEvents(True)
self._path = QGraphicsPathItem()
self._path.setPath(path)
self.addToGroup(self._path)
self.set_node_color()
self.set_label_color()
def create(shape, bounding_box):
ShapeFactory.message = None
graphics_item = None
if shape in ('box', 'rect', 'rectangle'):
graphics_item = QGraphicsRoundRectItem(bounding_box)
elif shape in ('ellipse', 'point'):
graphics_item = QGraphicsEllipseItem(bounding_box)
elif shape == 'diamond':
points = QPolygonF([QPointF(bounding_box.x(),
bounding_box.y() + bounding_box.height() / 2),
QPointF(bounding_box.x() + bounding_box.width() / 2,
bounding_box.y() + bounding_box.height()),
QPointF(bounding_box.x() + bounding_box.width(),
bounding_box.y() + bounding_box.height() / 2),
QPointF(bounding_box.x() + bounding_box.width() / 2,
bounding_box.y())])
graphics_item = QGraphicsPolygonItem(points)
elif shape == 'parallelogram':
points = QPolygonF([QPointF(bounding_box.x() + bounding_box.width() * 1/6,
bounding_box.y()),
QPointF(bounding_box.x() + bounding_box.width(),
bounding_box.y()),
QPointF(bounding_box.x() + bounding_box.width() * 5/6,
bounding_box.y() + bounding_box.height()),
QPointF(bounding_box.x(),
bounding_box.y() + bounding_box.height())])
graphics_item = QGraphicsPolygonItem(points)
elif shape == 'cds':
points = QPolygonF([QPointF(bounding_box.x(),
bounding_box.y()),
QPointF(bounding_box.x() + bounding_box.width() * 5/6,
bounding_box.y()),
QPointF(bounding_box.x() + bounding_box.width(),
bounding_box.y() + bounding_box.height() / 2),
QPointF(bounding_box.x() + bounding_box.width() * 5/6,
bounding_box.y() + bounding_box.height()),
QPointF(bounding_box.x(),
bounding_box.y() + bounding_box.height())])
graphics_item = QGraphicsPolygonItem(points)
elif shape == 'rarrow':
points = QPolygonF([QPointF(bounding_box.x(),
bounding_box.y()),
QPointF(bounding_box.x() + bounding_box.width() * 4/6,
bounding_box.y()),
QPointF(bounding_box.x() + bounding_box.width() * 4/6,
bounding_box.y() - bounding_box.height() * 2/6),
QPointF(bounding_box.x() + bounding_box.width(),
bounding_box.y() + bounding_box.height() / 2),
QPointF(bounding_box.x() + bounding_box.width() * 4/6,
bounding_box.y() + bounding_box.height() * 8/6),
QPointF(bounding_box.x() + bounding_box.width() * 4/6,
bounding_box.y() + bounding_box.height()),
QPointF(bounding_box.x(),
bounding_box.y() + bounding_box.height())])
graphics_item = QGraphicsPolygonItem(points)
elif shape == 'larrow':
points = QPolygonF([QPointF(bounding_box.x() + bounding_box.width() * 2/6,
bounding_box.y()),
QPointF(bounding_box.x() + bounding_box.width() * 2/6,
bounding_box.y() - bounding_box.height() * 2/6),
QPointF(bounding_box.x(),
bounding_box.y() + bounding_box.height() / 2),
QPointF(bounding_box.x() + bounding_box.width() * 2/6,
bounding_box.y() + bounding_box.height() * 8/6),
QPointF(bounding_box.x() + bounding_box.width() * 2/6,
bounding_box.y() + bounding_box.height()),
QPointF(bounding_box.x() + bounding_box.width(),
bounding_box.y() + bounding_box.height()),
QPointF(bounding_box.x() + bounding_box.width(),
bounding_box.y())])
graphics_item = QGraphicsPolygonItem(points)
elif shape == 'record':
graphics_item = QGraphicsRectItem(bounding_box)
elif shape == 'hexagon':
points = QPolygonF([QPointF(bounding_box.x() + bounding_box.width() * 1/5,
bounding_box.y()),
QPointF(bounding_box.x() + bounding_box.width() * 4/5,
bounding_box.y()),
QPointF(bounding_box.x() + bounding_box.width(),
bounding_box.y() + bounding_box.height() / 2),
QPointF(bounding_box.x() + bounding_box.width() * 4/5,
bounding_box.y() + bounding_box.height()),
QPointF(bounding_box.x() + bounding_box.width() * 1/5,
bounding_box.y() + bounding_box.height()),
QPointF(bounding_box.x(),
bounding_box.y() + bounding_box.height() / 2)])
graphics_item = QGraphicsPolygonItem(points)
elif shape == 'triangle':
points = QPolygonF([QPointF(bounding_box.x() + bounding_box.width() / 2,
bounding_box.y()),
QPointF(bounding_box.x() + bounding_box.width(),
bounding_box.y() + bounding_box.height() * 3/4),
QPointF(bounding_box.x(),
bounding_box.y() + bounding_box.height() * 3/4)])
graphics_item = QGraphicsPolygonItem(points)
elif shape == 'circle':
diameter = min(bounding_box.width(), bounding_box.height())
graphics_item = QGraphicsEllipseItem(bounding_box.x(), bounding_box.y(), diameter, diameter)
else:
graphics_item = QGraphicsRectItem(bounding_box)
ShapeFactory.message = "WARNING: %s is unknown shape, box used instead" % shape
return graphics_item
def addGraph(self, graph_str):
graph = graph_str.splitlines()
header = graph[0].split()
if header[0] != 'graph':
raise Exception('wrong graph format', 'header is: ' + graph[0])
self.scale_factor = 100.0
self.width = float(header[2])
self.height = float(header[3])
print "QGraphicsScene size:", self.width, self.height
self.scene = GraphScene(
QRectF(0, 0, self.scX(self.width), self.scY(self.height)))
for l in graph:
items = l.split()
if len(items) == 0:
continue
elif items[0] == 'stop':
break
elif items[0] == 'node':
#node CImp 16.472 5.25 0.86659 0.5 CImp filled ellipse lightblue lightblue
if len(items) != 11:
raise Exception('wrong number of items in line',
'line is: ' + l)
name = items[6]
if name == "\"\"":
name = ""
w = self.scX(items[4])
h = self.scY(items[5])
x = self.tfX(items[2])
y = self.tfY(items[3])
self.nodes[name] = self.scene.addEllipse(
x - w / 2, y - h / 2, w, h)
self.nodes[name].setData(0, name)
text_item = self.scene.addSimpleText(name)
br = text_item.boundingRect()
text_item.setPos(x - br.width() / 2, y - br.height() / 2)
elif items[0] == 'edge':
# without label:
# edge CImp Ts 4 16.068 5.159 15.143 4.9826 12.876 4.5503 11.87 4.3583 solid black
#
# with label:
# edge b_stSplit TorsoVelAggregate 7 7.5051 6.3954 7.7054 6.3043 7.9532 6.1899 8.1728 6.0833 8.4432 5.9522 8.7407 5.8012 8.9885 5.6735 aa 8.6798 5.9792 solid black
line_len = int(items[3])
label_text = None
label_pos = None
if (line_len * 2 + 6) == len(items):
# no label
pass
elif (line_len * 2 + 9) == len(items):
# edge with label
label_text = items[4 + line_len * 2]
label_pos = QPointF(self.tfX(items[4 + line_len * 2 + 1]),
self.tfY(items[4 + line_len * 2 + 2]))
else:
raise Exception(
'wrong number of items in line',
'should be: ' + str(line_len * 2 + 6) + " or " +
str(line_len * 2 + 9) + ', line is: ' + l)
line = []
for i in range(line_len):
line.append((self.tfX(items[4 + i * 2]),
self.tfY(items[5 + i * 2])))
control_points_idx = 1
path = QPainterPath(QPointF(line[0][0], line[0][1]))
while True:
q1 = line[control_points_idx]
q2 = line[control_points_idx + 1]
p2 = line[control_points_idx + 2]
path.cubicTo(q1[0], q1[1], q2[0], q2[1], p2[0], p2[1])
control_points_idx = control_points_idx + 3
if control_points_idx >= len(line):
break
edge = self.scene.addPath(path)
edge.setData(0, (items[1], items[2]))
self.edges.append(edge)
end_p = QPointF(line[-1][0], line[-1][1])
p0 = end_p - QPointF(line[-2][0], line[-2][1])
p0_norm = math.sqrt(p0.x() * p0.x() + p0.y() * p0.y())
p0 = p0 / p0_norm
p0 = p0 * self.scale_factor * 0.15
p1 = QPointF(p0.y(), -p0.x()) * 0.25
p2 = -p1
poly = QPolygonF()
poly.append(p0 + end_p)
poly.append(p1 + end_p)
poly.append(p2 + end_p)
poly.append(p0 + end_p)
# poly_path = QPainterPath()
# poly_path.addPolygon(poly)
# painter = QPainter()
self.scene.addPolygon(poly)
if label_text and label_pos:
if label_text[0] == "\"":
label_text = label_text[1:]
if label_text[-1] == "\"":
label_text = label_text[:-1]
label_text = label_text.replace("\\n", "\n")
label_item = self.scene.addSimpleText(label_text)
br = label_item.boundingRect()
label_item.setPos(label_pos.x() - br.width() / 2,
label_pos.y() - br.height() / 2)
def __init__(self, spline, label, label_center, from_node, to_node, parent=None, **kwargs):
super(EdgeItem, self).__init__(parent, **kwargs)
self._edge_pen_width = kwargs.get('edge_pen_width', self.EDGE_PEN_WIDTH)
self.from_node = from_node
self.from_node.add_outgoing_edge(self)
self.to_node = to_node
self.to_node.add_incoming_edge(self)
self._brush = QBrush(self._color)
self._label_pen = QPen()
self._label_pen.setColor(self._color)
self._label_pen.setJoinStyle(Qt.RoundJoin)
self._label_pen.setWidthF(self._label_pen_width)
self._edge_pen = QPen()
self._edge_pen.setColor(self._color)
self._edge_pen.setWidthF(self._edge_pen_width)
self._sibling_edges = set()
self._label = None
if label is not None:
self._label = QGraphicsSimpleTextItem(label)
font = self._label.font()
font.setPointSize(8)
self._label.setFont(font)
label_rect = self._label.boundingRect()
label_rect.moveCenter(label_center)
self._label.setPos(label_rect.x(), label_rect.y())
# spline specification according to http://www.graphviz.org/doc/info/attrs.html#k:splineType
coordinates = spline.split(' ')
# extract optional end_point
end_point = None
if coordinates[0].startswith('e,'):
parts = coordinates.pop(0)[2:].split(',')
end_point = QPointF(float(parts[0]), -float(parts[1]))
# extract optional start_point
if coordinates[0].startswith('s,'):
parts = coordinates.pop(0).split(',')
# first point
parts = coordinates.pop(0).split(',')
point = QPointF(float(parts[0]), -float(parts[1]))
path = QPainterPath(point)
while len(coordinates) > 2:
# extract triple of points for a cubic spline
parts = coordinates.pop(0).split(',')
point1 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point2 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point3 = QPointF(float(parts[0]), -float(parts[1]))
path.cubicTo(point1, point2, point3)
self._arrow = None
if end_point is not None:
# draw arrow
self._arrow = QGraphicsPolygonItem()
polygon = QPolygonF()
polygon.append(point3)
offset = QPointF(end_point - point3)
corner1 = QPointF(-offset.y(), offset.x()) * 0.35
corner2 = QPointF(offset.y(), -offset.x()) * 0.35
polygon.append(point3 + corner1)
polygon.append(end_point)
polygon.append(point3 + corner2)
self._arrow.setPolygon(polygon)
self._path = QGraphicsPathItem()
self._path.setPath(path)
self.addToGroup(self._path)
self._brush.setColor(self._color)
self._edge_pen.setColor(self._color)
self._label_pen.setColor(self._color)
self._path.setPen(self._edge_pen)
if self._arrow is not None:
self._arrow.setBrush(self._brush)
self._arrow.setPen(self._edge_pen)
if self._label is not None:
self._label.setBrush(self._brush)
self._label.setPen(self._label_pen)
def __init__(self, highlight_level, bounding_box, label, shape, color=None, parent=None, label_pos=None, tooltip=None):
super(NodeItem, self).__init__(highlight_level, parent)
self._default_color = self._COLOR_BLACK if color is None else color
self._brush = QBrush(self._default_color)
self._label_pen = QPen()
self._label_pen.setColor(self._default_color)
self._label_pen.setJoinStyle(Qt.RoundJoin)
self._ellipse_pen = QPen(self._label_pen)
self._ellipse_pen.setWidth(1)
self._incoming_edges = set()
self._outgoing_edges = set()
if shape == 'box':
self._graphics_item = QGraphicsRectItem(bounding_box)
# Since we don't have unique GraphicsItems other than Ellipse and Rect,
# Using Polygon to draw the following using bounding_box
elif shape == 'octagon':
rect = bounding_box.getRect()
octagon_polygon = QPolygonF([QPointF(rect[0], rect[1] + 3 * rect[3] / 10),
QPointF(rect[0], rect[1] + 7 * rect[3] / 10),
QPointF(rect[0] + 3 * rect[2] / 10, rect[1] + rect[3]),
QPointF(rect[0] + 7 * rect[2] / 10, rect[1] + rect[3]),
QPointF(rect[0] + rect[2], rect[1] + 7 * rect[3] / 10),
QPointF(rect[0] + rect[2], rect[1] + 3 * rect[3] / 10),
QPointF(rect[0] + 7 * rect[2] / 10, rect[1]),
QPointF(rect[0] + 3 * rect[2] / 10, rect[1])])
self._graphics_item = QGraphicsPolygonItem(octagon_polygon)
elif shape == 'doubleoctagon':
rect = bounding_box.getRect()
inner_fold = 3.0
octagon_polygon = QPolygonF([QPointF(rect[0], rect[1] + 3 * rect[3] / 10),
QPointF(rect[0], rect[1] + 7 * rect[3] / 10),
QPointF(rect[0] + 3 * rect[2] / 10, rect[1] + rect[3]),
QPointF(rect[0] + 7 * rect[2] / 10, rect[1] + rect[3]),
QPointF(rect[0] + rect[2], rect[1] + 7 * rect[3] / 10),
QPointF(rect[0] + rect[2], rect[1] + 3 * rect[3] / 10),
QPointF(rect[0] + 7 * rect[2] / 10, rect[1]),
QPointF(rect[0] + 3 * rect[2] / 10, rect[1]),
# inner
QPointF(rect[0], rect[1] + 3 * rect[3] / 10),
QPointF(rect[0] + inner_fold, rect[1] + 3 * rect[3] / 10 + inner_fold / 2),
QPointF(rect[0] + inner_fold, rect[1] + 7 * rect[3] / 10 - inner_fold / 2),
QPointF(rect[0] + 3 * rect[2] / 10, rect[1] + rect[3] - inner_fold),
QPointF(rect[0] + 7 * rect[2] / 10, rect[1] + rect[3] - inner_fold),
QPointF(rect[0] + rect[2] - inner_fold, rect[1] + 7 * rect[3] / 10 - inner_fold / 2),
QPointF(rect[0] + rect[2] - inner_fold, rect[1] + 3 * rect[3] / 10 + inner_fold / 2),
QPointF(rect[0] + 7 * rect[2] / 10, rect[1] + inner_fold),
QPointF(rect[0] + 3 * rect[2] / 10, rect[1] + inner_fold),
QPointF(rect[0] + inner_fold, rect[1] + 3 * rect[3] / 10 + inner_fold / 2)
])
self._graphics_item = QGraphicsPolygonItem(octagon_polygon)
elif shape == 'note':
rect = bounding_box.getRect()
note_polygon = QPolygonF([QPointF(rect[0] + 9 * rect[2] / 10, rect[1]),
QPointF(rect[0], rect[1]),
QPointF(rect[0], rect[1] + rect[3]),
QPointF(rect[0] + rect[2], rect[1] + rect[3]),
QPointF(rect[0] + rect[2], rect[1] + rect[3] / 5),
QPointF(rect[0] + 9 * rect[2] / 10, rect[1] + rect[3] / 5),
QPointF(rect[0] + 9 * rect[2] / 10, rect[1]),
QPointF(rect[0] + rect[2], rect[1] + rect[3] / 5),
QPointF(rect[0] + rect[2], rect[1] + rect[3] / 5)])
self._graphics_item = QGraphicsPolygonItem(note_polygon)
else:
self._graphics_item = QGraphicsEllipseItem(bounding_box)
self.addToGroup(self._graphics_item)
self._label = QGraphicsSimpleTextItem(label)
label_rect = self._label.boundingRect()
if label_pos is None:
label_rect.moveCenter(bounding_box.center())
else:
label_rect.moveCenter(label_pos)
self._label.setPos(label_rect.x(), label_rect.y())
self.addToGroup(self._label)
if tooltip is not None:
self.setToolTip(tooltip)
self.set_node_color()
self.setAcceptHoverEvents(True)
self.hovershape = None
def _draw_referee(self, painter):
# レフェリーの情報を描画する
PLACE_RADIUS = 0.15 # meters
AVOID_LENGTH = 0.5 # meter
if self._decoded_referee is None:
return
ball_pose = self._ball_info.pose
# ボールプレースメントの進入禁止エリアと設置位置を描画
if self._decoded_referee.referee_text == "OUR_BALL_PLACEMENT" \
or self._decoded_referee.referee_text == "THEIR_BALL_PLACEMENT":
replacement_pose = self._decoded_referee.placement_position
# 進入禁止エリアを描画
# Reference: Rule 8.2.3
angle_ball_to_target = tool.get_angle(ball_pose, replacement_pose)
dist_ball_to_target = tool.distance_2_poses(
ball_pose, replacement_pose)
trans_BtoT = tool.Trans(ball_pose, angle_ball_to_target)
# 進入禁止エリア長方形の角の座標を取得
avoid_upper_left = trans_BtoT.inverted_transform(
Pose2D(0, AVOID_LENGTH, 0))
avoid_lower_left = trans_BtoT.inverted_transform(
Pose2D(0, -AVOID_LENGTH, 0))
avoid_upper_right = trans_BtoT.inverted_transform(
Pose2D(dist_ball_to_target, AVOID_LENGTH, 0))
avoid_lower_right = trans_BtoT.inverted_transform(
Pose2D(dist_ball_to_target, -AVOID_LENGTH, 0))
# 各座標を描画座標に変換
upper_left_point = self._convert_to_view(avoid_upper_left.x,
avoid_upper_left.y)
lower_left_point = self._convert_to_view(avoid_lower_left.x,
avoid_lower_left.y)
upper_right_point = self._convert_to_view(avoid_upper_right.x,
avoid_upper_right.y)
lower_right_point = self._convert_to_view(avoid_lower_right.x,
avoid_lower_right.y)
# ポリゴンに追加
polygon = QPolygonF()
polygon.append(upper_left_point)
polygon.append(upper_right_point)
polygon.append(lower_right_point)
polygon.append(lower_left_point)
avoid_color = QColor(Qt.red)
avoid_color.setAlphaF(0.3)
painter.setPen(QPen(Qt.black, 1))
painter.setBrush(avoid_color)
painter.drawPolygon(polygon)
replace_point = self._convert_to_view(replacement_pose.x,
replacement_pose.y)
ball_point = self._convert_to_view(ball_pose.x, ball_pose.y)
size = AVOID_LENGTH * self._scale_field_to_view
painter.drawEllipse(replace_point, size, size)
painter.drawEllipse(ball_point, size, size)
# ボール設置位置を描画
size = PLACE_RADIUS * self._scale_field_to_view
place_color = QColor(Qt.white)
place_color.setAlphaF(0.6)
painter.setPen(QPen(Qt.black, 2))
painter.setBrush(place_color)
painter.drawEllipse(replace_point, size, size)
# ボール進入禁止エリアを描画
if self._decoded_referee.keep_out_radius_from_ball != -1:
point = self._convert_to_view(ball_pose.x, ball_pose.y)
size = self._decoded_referee.keep_out_radius_from_ball * self._scale_field_to_view
ball_color = copy.deepcopy(self._COLOR_BALL)
keepout_color = QColor(Qt.red)
keepout_color.setAlphaF(0.3)
painter.setPen(Qt.black)
painter.setBrush(keepout_color)
painter.drawEllipse(point, size, size)
# レフェリーテキストをカーソル周辺に表示する
if self._decoded_referee.referee_text:
# カーソル座標を取得
current_pos = self._convert_to_field(self._current_mouse_pos.x(),
self._current_mouse_pos.y())
# 他のテキストと被らないように位置を微調整
current_point = self._convert_to_view(current_pos.x() + 0.1,
current_pos.y() - 0.15)
text = self._decoded_referee.referee_text
painter.setPen(Qt.red)
painter.drawText(current_point, text)
