def paintEvent(self, event):
self.calc_max_pixel_viewport()
self.meter_viewport = qtc.QRectF(
0,
0,
self.map_model.viewport[0],
self.map_model.viewport[1])
# Create transformation matrix to automatically rescale all field
# coordinates (meters) to pixel coordinates and to flip the y-axis.
transformation = qtg.QTransform()
pixels_per_meter = (self.pixel_viewport.width() /
self.meter_viewport.width())
transformation.scale(pixels_per_meter, -pixels_per_meter)
# From here on, directly use the field coordinates (in meters).
for layer_painter in self.layer_painter:
painter = Painter(pixels_per_meter)
painter.begin(self)
painter.setTransform(transformation)
# Translate coordinate system to field center.
painter.translate(layer_painter.layer["settings"]["center_x"],
layer_painter.layer["settings"]["center_y"])
layer_painter.paint(painter)
painter.end()
def drawHypothesis(self, painter: Painter, hypothesis):
painter.setBrush(qtc.Qt.yellow)
for sigmaPoint in hypothesis["sigmaPoints"]:
painter.drawPose(painter.getPoseFromVector3(sigmaPoint), 0.15,
0.15)
painter.setBrush(qtc.Qt.red)
painter.drawPose(painter.getPoseFromVector3(hypothesis["stateMean"]),
0.2, 0.2,
"{:.2f}".format(hypothesis["meanEvalError"]))
def paint(self, painter: Painter):
painter.transformByPose(self.transformation)
painter.setPen(qtc.Qt.black, 0)
painter.setBrush(qtg.QColor(
self.layer["settings"]["position"]["color"]))
if self.position is not None:
painter.drawEllipse(
qtc.QPointF(self.position[0], self.position[1]),
self.layer["settings"]["position"]
["circleDiameter"] / 2,
self.layer["settings"]["position"]
["circleDiameter"] / 2
)
def paint(self, painter: Painter):
painter.transformByPose(self.transformation)
if self.obstacles is not None:
# Obstacles
painter.setBrush(qtc.Qt.NoBrush)
for obstacle in self.obstacles:
pen = self.penForObstacleType(obstacle["type"])
painter.setPen(pen)
painter.drawEllipse(
qtc.QPointF(obstacle["relativePosition"][0],
obstacle["relativePosition"][1]),
obstacle["radius"],
obstacle["radius"]
)
def paint(self, painter: Painter):
painter.transformByPose(self.transformation)
painter.setPen(
qtg.QColor(self.layer["settings"]["lines"]["lineColor"]),
self.layer["settings"]["lines"]["lineWidth"])
if self.lines is not None:
for line in self.lines:
painter.drawLineF(line[0][0], line[0][1], line[1][0],
line[1][1])
def paint(self, painter: Painter):
if not (self.poseHypotheses and self.publishedPose):
return
painter.setPen(qtg.QPen(qtc.Qt.black, 0))
painter.setBrush(qtc.Qt.blue)
painter.drawPose(self.publishedPose, 0.2, 0.2)
for hypothesis in self.poseHypotheses:
self.drawHypothesis(painter, hypothesis)
def paint(self, painter: Painter):
painter.transformByPose(self.transformation)
if self.filteredSonar is not None:
sensors = [[
-self.layer["settings"]["sonar"]["zAngle"],
self.layer["settings"]["sonar"]["yOffset"] / 100,
self.filteredSonar["filteredValues"][0],
self.filteredSonar["valid"][0]
],
[
self.layer["settings"]["sonar"]["zAngle"],
-self.layer["settings"]["sonar"]["yOffset"] / 100,
self.filteredSonar["filteredValues"][1],
self.filteredSonar["valid"][1]
]]
openAngle = self.layer["settings"]["sonar"]["openingAngle"]
color = qtg.QColor(self.layer["settings"]["sonar"]["color"])
for [zAngle, yOffset, filtered, fValid] in sensors:
if fValid:
painter.setPen(color, 0.02)
rect = qtc.QRectF(0.0, 0.0, filtered * 2, filtered * 2)
rect.moveCenter(qtc.QPointF(0.0, yOffset))
painter.drawArc(rect, (-(openAngle / 2) + zAngle) * 16,
openAngle * 16)
else:
painter.setPen(qtc.Qt.red, 0.02)
rect = qtc.QRectF(0.0, 0.0, 0.1, 0.1)
rect.moveCenter(qtc.QPointF(0.0, yOffset))
painter.drawPie(rect, (-(openAngle / 2) + zAngle) * 16,
openAngle * 16)
if self.rawSonar is not None:
sensors = [[
-self.layer["settings"]["sonar"]["zAngle"],
self.layer["settings"]["sonar"]["yOffset"] / 100,
self.rawSonar["SONAR_LEFT_SENSOR_0"],
self.rawSonar["valid_SONAR_LEFT_SENSOR_0"]
],
[
self.layer["settings"]["sonar"]["zAngle"],
-self.layer["settings"]["sonar"]["yOffset"] / 100,
self.rawSonar["SONAR_RIGHT_SENSOR_0"],
self.rawSonar["valid_SONAR_RIGHT_SENSOR_0"]
]]
openAngle = self.layer["settings"]["sonar"]["openingAngle"]
color = qtg.QColor(self.layer["settings"]["sonar"]["color"])
for [zAngle, yOffset, raw, rValid] in sensors:
if rValid:
painter.setPen(color, 0)
rect = qtc.QRectF(0.0, 0.0, raw * 2, raw * 2)
rect.moveCenter(qtc.QPointF(0.0, yOffset))
painter.drawArc(rect, (-(openAngle / 2) + zAngle) * 16,
openAngle * 16)
else:
painter.setPen(qtc.Qt.red, 0)
rect = qtc.QRectF(0.0, 0.0, 0.1, 0.1)
rect.moveCenter(qtc.QPointF(0.0, yOffset))
painter.drawPie(rect, (-(openAngle / 2) + zAngle) * 16,
openAngle * 16)
def paint(self, painter: Painter):
if self.probabilityMap:
paintText = False
if self.layer["settings"]["search"]["showNumericProbability"]:
paintText = True
elif self.layer["settings"]["search"]["showNumericAge"]:
paintText = True
for row in self.probabilityMap:
for cell in row:
painter.setPen(qtc.Qt.black, 0.0)
# to help debugging
if (cell[0] < 0.0):
print("cell probability < 0 p:", cell[0])
if (cell[0] > 1.0):
print("cell probability > 1 p:", cell[0])
if (cell[1] < 0):
print("cell age < 0 age:", cell[1])
# Age in redscale as outline, saturated at 50 seconds
if self.layer["settings"]["search"]["showAge"]:
painter.setPen(
qtg.QColor(
round(255 * ((min(cell[1], 5000)) / 5000)), 0,
0, 255), 0.02)
# Probability in greyscale at center,
# scaled by fourth root (^0.25) for visibility
scaledProbability = 0
if self.layer["settings"]["search"]["showProbability"]:
scaledProbability = round(255 * (cell[0]**0.25))
scaledProbability = max(0, min(255, scaledProbability))
painter.setBrush(
qtg.QColor(scaledProbability, scaledProbability,
scaledProbability, 255))
painter.drawRect(
qtc.QRectF(cell[2] - 0.18, cell[3] - 0.18, 0.36, 0.36))
if paintText:
# Showing numeric values
painter.setPen(qtc.Qt.white, 0)
# Probability in %
if self.layer["settings"]["search"][
"showNumericProbability"]:
painter.drawText(
qtc.QPointF(cell[2] - 0.15, cell[3] + 0.03),
"%.2f" % (cell[0] * 100.0), 0.01)
# Age in seconds
if self.layer["settings"]["search"]["showNumericAge"]:
ageToShow = math.floor(cell[1] * 0.016667)
logOffset = 0
if ageToShow > 0:
logOffset = math.floor(math.log10(ageToShow))
painter.drawText(
qtc.QPointF(
cell[2] - 0.025 - (0.05 * logOffset),
cell[3] - 0.15), str(ageToShow), 0.01)
# Show voronoi seeds: opaque when searching, transparent when not
if self.voronoiSeeds and (self.explorerCount > 0):
if self.layer["settings"]["search"]["showVoronoiSeeds"]:
# To check if search is active,
# we need walkTarget in absolute coordinates and
# compare it to ballSearchPose.
# This is a very ugly method,
# but very useful information for testing.
searching = 128 # not searching actively
if self.ballSearchPose:
if self.walkTarget:
if self.pose:
walkTargetAbsolute = [[
self.pose[0][0] + (math.cos(self.pose[1]) *
self.walkTarget[0][0]) -
(math.sin(self.pose[1]) *
self.walkTarget[0][1]),
self.pose[0][1] + (math.sin(self.pose[1]) *
self.walkTarget[0][0]) +
(math.cos(self.pose[1]) *
self.walkTarget[0][1])
], (self.pose[1] + self.walkTarget[1]) %
(math.pi * 2.0)]
if walkTargetAbsolute[1] > math.pi:
walkTargetAbsolute[1] -= math.pi * 2.0
elif walkTargetAbsolute[1] < -math.pi:
walkTargetAbsolute[1] += math.pi * 2.0
# For the comparison a high threshold is being
# used to avoid flickering,
# as data might be somewhat out of sync
if abs(walkTargetAbsolute[1] -
self.ballSearchPose[1]) < 0.1:
if abs(walkTargetAbsolute[0][0] -
self.ballSearchPose[0][0]) < 0.1:
if abs(walkTargetAbsolute[0][1] -
self.ballSearchPose[0][1]) < 0.1:
searching = 255 # actively searching
# paint voronoi seeds
painter.setPen(qtc.Qt.black, 0)
painter.setBrush(qtg.QColor(255, 255, 0, searching))
for seed in self.voronoiSeeds:
painter.drawEllipse(qtc.QPointF(seed[0], seed[1]), 0.15,
0.15)
def paint(self, painter: Painter):
pen = qtg.QPen(qtc.Qt.white)
pen.setWidthF(self.field_settings["lineWidth"])
# This creates sharp edges where lines join instead of rounded corners
pen.setJoinStyle(qtc.Qt.MiterJoin)
# green background
painter.setBrush(qtc.Qt.darkGreen)
painter.setPen(qtc.Qt.NoPen)
painter.drawRectF(
-self.field_settings["length"] / 2 -
self.field_settings["borderStripWidth"],
-self.field_settings["width"] / 2 -
self.field_settings["borderStripWidth"],
self.field_settings["length"] +
self.field_settings["borderStripWidth"] * 2,
self.field_settings["width"] +
self.field_settings["borderStripWidth"] * 2)
# white pen
painter.setPen(pen)
painter.setBrush(qtc.Qt.NoBrush)
# field border
painter.drawRectF(-self.field_settings["length"] / 2,
-self.field_settings["width"] / 2,
self.field_settings["length"],
self.field_settings["width"])
# penalty area left
painter.drawRectF(-self.field_settings["length"] / 2,
-self.field_settings["penaltyAreaWidth"] / 2,
self.field_settings["penaltyAreaLength"],
self.field_settings["penaltyAreaWidth"])
# penalty area right
painter.drawRectF(self.field_settings["length"] / 2,
-self.field_settings["penaltyAreaWidth"] / 2,
-self.field_settings["penaltyAreaLength"],
self.field_settings["penaltyAreaWidth"])
# center line
painter.drawLineF(0, -self.field_settings["width"] / 2, 0,
self.field_settings["width"] / 2)
# center circle
painter.drawEllipse(qtc.QPointF(0, 0),
self.field_settings["centerCircleDiameter"] / 2,
self.field_settings["centerCircleDiameter"] / 2)
# penalty mark left
painter.setBrush(qtc.Qt.white)
painter.drawEllipse(
qtc.QPointF(
-self.field_settings["length"] / 2 +
self.field_settings["penaltyMarkerDistance"], 0),
self.field_settings["penaltyMarkerSize"] / 2,
self.field_settings["penaltyMarkerSize"] / 2)
# penalty mark right
painter.drawEllipse(
qtc.QPointF(
self.field_settings["length"] / 2 -
self.field_settings["penaltyMarkerDistance"], 0),
self.field_settings["penaltyMarkerSize"] / 2,
self.field_settings["penaltyMarkerSize"] / 2)
# kick off pointEllipse
painter.drawEllipse(qtc.QPointF(0, 0),
self.field_settings["penaltyMarkerSize"] / 2,
self.field_settings["penaltyMarkerSize"] / 2)
def paint(self, painter: Painter):
if self.teamPlayers is not None:
# Draw for each team player
for robot in self.teamPlayers:
color = qtg.QColor(
self.layer["settings"]["teamPlayers"][Roles(
robot["currentlyPerformingRole"]).name + "Color"])
# fov
if self.layer["settings"]["teamPlayers"]["showFOV"]:
# fov
painter.setBrush(qtc.Qt.NoBrush)
painter.setPen(qtg.QPen(qtc.Qt.yellow, 0))
painter.drawFOV(robot["pose"],
robot["headYaw"],
self.layer["settings"]["teamPlayers"]
["maxDistance"],
self.layer["settings"]["teamPlayers"]
["cameraOpeningAngle"])
if self.layer["settings"]["teamPlayers"]["showTarget"]:
# Walking-target
painter.setBrush(qtc.Qt.NoBrush)
painter.setPen(color, 0)
painter.drawTarget(robot["walkingTo"],
self.layer["settings"]["teamPlayers"]
["targetCircleDiameter"])
# Dotted line
dotted_pen = qtg.QPen(qtc.Qt.yellow,
0,
qtc.Qt.DashDotLine)
painter.setPen(dotted_pen)
painter.drawLineF(robot["pose"][0][0],
robot["pose"][0][1],
robot["walkingTo"][0][0],
robot["walkingTo"][0][1])
if self.layer["settings"]["teamPlayers"]["showPlayer"]:
# Pose
painter.setBrush(color)
painter.setPen(qtg.QPen(qtc.Qt.black, 0))
painter.drawPose(robot["pose"],
self.layer["settings"]["teamPlayers"]
["poseCircleDiameter"],
self.layer["settings"]["teamPlayers"]
["poseCircleDiameter"])
# PlayerNumber
painter.setPen(
qtg.QPen(qtg.QColor(ui_utils.ideal_text_color(color)),
0))
painter.drawText(qtc.QPointF(robot["pose"][0][0] -
(self.layer["settings"]
["teamPlayers"]
["poseCircleDiameter"] * 0.2),
robot["pose"][0][1] -
(self.layer["settings"]
["teamPlayers"]
["poseCircleDiameter"] * 0.25)),
str(robot["playerNumber"]),
self.layer["settings"]
["teamPlayers"]["poseCircleDiameter"] * 0.61)
if self.layer["settings"]["teamPlayers"]["showSearchPosition"]:
# Show Search Position
painter.setBrush(color)
painter.setPen(qtg.QPen(qtc.Qt.black, 0))
painter.drawEllipse(
qtc.QPointF(
robot["currentSearchPosition"][0],
robot["currentSearchPosition"][1]),
self.layer["settings"]["teamPlayers"]["searchPositionDiameter"] / 2,
self.layer["settings"]["teamPlayers"]["searchPositionDiameter"] / 2)
def paint(self, painter: Painter):
painter.setPen(qtg.QPen(qtc.Qt.black, 0))
painter.drawPose([self.teamBallPosition, 0.0], 0, 0, str(self.rateKick), 0.15, [0.1, 0.1])
painter.setBrush(qtg.QColor("#000000"))
if self.firstShadowPoint is not None and self.secondShadowPoint is not None:
painter.drawPose([self.firstShadowPoint, 0.0], 0.1, 0, "first", 0.1)
painter.drawPose([self.secondShadowPoint, 0.0], 0.1, 0, "second", 0.1)
if self.firstShadowPointAfter is not None and self.secondShadowPointAfter is not None:
painter.drawPose([self.firstShadowPointAfter, 0.0], 0.1, 0, "firstAfter", 0.1)
painter.drawPose([self.secondShadowPointAfter, 0.0], 0.1, 0, "secondAfter", 0.1)
for index, chunk in enumerate(self.kickRatingChunks):
if chunk:
color = qtg.QColor("#00ff00")
else:
color = qtg.QColor("#ff0000")
painter.setBrush(color)
if self.hitPoints is not None:
painter.drawPose([self.hitPoints[index], 0.0], 0.1, 0, "{0:.2f}".format(self.kickRatingChunkWeights[index]), 0.1, [0.1, 0])
def paint(self, painter: Painter):
painter.transformByPose(self.transformation)
if self.motionPlan is not None:
# Walking-target
painter.setBrush(qtc.Qt.NoBrush)
painter.setPen(qtg.QColor(self.layer["settings"
]["motionPlan"
]["targetColor"]),
0)
painter.drawTarget(self.motionPlan["walkTarget"],
self.layer["settings"
]["motionPlan"
]["targetCircleDiameter"])
# Dotted line to walking-target
dotted_pen = qtg.QPen(qtc.Qt.yellow,
0,
qtc.Qt.DashDotLine)
painter.setPen(dotted_pen)
painter.drawLineF(0.0,
0.0,
self.motionPlan["walkTarget"][0][0],
self.motionPlan["walkTarget"][0][1])
# Translation
painter.setPen(qtg.QColor("#ff0000"), 0)
painter.drawLineF(0.0,
0.0,
self.motionPlan["translation"][0],
self.motionPlan["translation"][1])
def paint(self, painter: Painter):
# Draw background
bgColor = qtg.QColor(self.settings["backgroundColor"])
bgColor.setAlpha(self.settings["backgroundAlpha"])
painter.setBrush(bgColor)
painter.setPen(qtc.Qt.NoPen)
painter.drawRectF(-self.settings["width"] / 2,
-self.settings["height"] / 2, self.settings["width"],
self.settings["height"])
# Draw Axis
lineColor = qtg.QColor(self.settings["lineColor"])
pen = qtg.QPen(lineColor)
pen.setWidthF(self.settings["lineWidth"])
painter.setPen(pen)
painter.setBrush(qtc.Qt.NoBrush)
painter.drawLineF(0, -self.settings["height"] / 2, 0,
self.settings["height"] / 2)
painter.drawLineF(-self.settings["width"] / 2, 0,
self.settings["width"] / 2, 0)
if self.settings["polar"]:
# Draw Circles
r = 0.0
while (r + self.settings["radialStepSize"] <= max(
self.settings["height"] / 2, self.settings["width"] / 2)):
r += self.settings["radialStepSize"]
painter.drawEllipse(qtc.QPointF(0, 0), r, r)
# Draw Angles
l = max(self.settings["height"] / 2, self.settings["width"] / 2)
deg = 90.0
while (deg + self.settings["polarAngleStepSize"] < 270.0):
deg += self.settings["polarAngleStepSize"]
v = [
math.cos(math.radians(deg)) * l,
math.sin(math.radians(deg)) * l
]
painter.drawLineF(0, 0, v[0], v[1])
painter.drawLineF(0, 0, -v[0], v[1])
else:
# draw grid
# horizontal lines
offset = 0.0
while (offset + self.settings["stepSizeY"] <=
self.settings["height"] / 2):
offset += self.settings["stepSizeY"]
painter.drawLineF(-self.settings["width"] / 2, offset,
self.settings["width"] / 2, offset)
painter.drawLineF(-self.settings["width"] / 2, -offset,
self.settings["width"] / 2, -offset)
# vertical lines
offset = 0.0
while (offset + self.settings["stepSizeX"] <=
self.settings["width"] / 2):
offset += self.settings["stepSizeX"]
painter.drawLineF(offset, -self.settings["height"] / 2, offset,
self.settings["height"] / 2)
painter.drawLineF(-offset, -self.settings["height"] / 2,
-offset, self.settings["height"] / 2)