def paint(self, painter: Painter):
if self.robots is None:
return
painter.setPen(qtg.QPen(qtc.Qt.black, 0))
painter.setBrush(qtc.Qt.blue)
for robot in self.robots:
self.drawRobot(painter, robot)
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):
self.update_image()
painter.scale(1, -1)
rect = qtc.QRectF(
-1.0 / 2.0, # xpos
-1.0 / 2.0, # ypos
1, # width
1 # height
)
painter.drawImage(rect, self.image)
def paint(self, painter: Painter):
painter.transformByPose(self.transformation)
painter.setPen(qtc.Qt.black, 0)
painter.setBrush(qtg.QColor(self.config["position"]["color"]))
if self.position is not None:
painter.drawEllipse(
qtc.QPointF(self.position[0], self.position[1]),
self.config["position"]["circleDiameter"] / 2,
self.config["position"]["circleDiameter"] / 2)
def paintEvent(self, event):
if self.isFirstPaint:
logger.debug(__name__ + ": First painting of map")
mapPaintTime = time.time()
self.calc_max_pixel_viewport()
self.meter_viewport = qtc.QRectF(
0, 0, self.parent.model["config"]["viewport"][0],
self.parent.model["config"]["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:
if self.isFirstPaint:
logger.debug(__name__ + ": First painting of " +
layer_painter.name + " layer")
layerPaintTime = time.time()
try:
painter = Painter(pixels_per_meter)
painter.begin(self)
painter.setTransform(transformation)
# Translate coordinate system to field center.
painter.translate(layer_painter.config["center_x"],
layer_painter.config["center_y"])
layer_painter.paint(painter)
painter.end()
except Exception as e:
if self.isFirstPaint:
logger.error(__name__ + ": Exception when painting " +
layer_painter.name + " layer: ")
logger.error(__name__ + ": " + str(e))
if self.isFirstPaint:
logger.debug(__name__ + ": First painting of " +
layer_painter.name + " layer took: " +
logger.timerLogStr(layerPaintTime))
if self.isFirstPaint:
logger.debug(__name__ + ": First painting of map took: " +
logger.timerLogStr(mapPaintTime))
self.isFirstPaint = False
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_model["config"]["lines"]["lineColor"]),
self.layer_model["config"]["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.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):
if self.pathObstacles:
painter.setBrush(qtc.Qt.NoBrush)
painter.setPen(qtg.QPen(qtc.Qt.blue, 0.03))
for obstacle in self.pathObstacles:
painter.drawEllipse(
qtc.QPointF(obstacle["circlePosition"][0],
obstacle["circlePosition"][1]),
obstacle["radius"], obstacle["radius"])
if self.pathLines:
painter.setBrush(qtc.Qt.NoBrush)
painter.setPen(qtg.QPen(qtc.Qt.green, 0.1))
for line in self.pathLines:
painter.drawLineF(line[0][0], line[0][1], line[1][0],
line[1][1])
if self.arcLines:
painter.setBrush(qtc.Qt.NoBrush)
painter.setPen(qtg.QPen(qtc.Qt.green, 0.1))
for arc in self.arcLines:
x, y, diameter, start, angleDiff = arc
painter.drawArc(qtc.QRectF(x, y, diameter, diameter),
start * 16, angleDiff * 16)
if self.blockedArcLines:
painter.setBrush(qtc.Qt.NoBrush)
painter.setPen(qtg.QPen(qtc.Qt.red, 0.1))
for arc in self.blockedArcLines:
x, y, diameter, start, angleDiff = arc
painter.drawArc(qtc.QRectF(x, y, diameter, diameter),
start * 16, angleDiff * 16)
if self.nodes:
painter.setBrush(qtc.Qt.NoBrush)
painter.setPen(qtg.QPen(qtc.Qt.yellow, 0.04))
for node in self.nodes:
painter.drawPoint(qtc.QPointF(node[0], node[1]))
if self.start_nodes:
painter.setBrush(qtc.Qt.NoBrush)
painter.setPen(qtg.QPen(qtc.Qt.black, 0.12))
for node in self.start_nodes:
painter.drawPoint(qtc.QPointF(node[0], node[1]))
if self.end_nodes:
painter.setBrush(qtc.Qt.NoBrush)
painter.setPen(qtg.QPen(qtc.Qt.red, 0.06))
for node in self.end_nodes:
painter.drawPoint(qtc.QPointF(node[0], node[1]))
# Draw special nodes
if self.nodes:
painter.setBrush(qtc.Qt.NoBrush)
for node in self.nodes:
if node[2] == 0:
continue
if node[2] == 1:
painter.setPen(qtg.QPen(qtc.Qt.blue, 0.15))
else:
painter.setPen(qtg.QPen(qtc.Qt.red, 0.15))
painter.drawPoint(qtc.QPointF(node[0], node[1]))
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_model["config"]["motionPlan"]["targetColor"]),
0)
painter.drawTarget(
self.motionPlan["walkTarget"], self.layer_model["config"]
["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):
pen = qtg.QPen(qtc.Qt.white)
pen.setWidthF(self.config["field"]["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.config["field"]["length"] / 2 -
self.config["field"]["borderStripWidth"],
-self.config["field"]["width"] / 2 -
self.config["field"]["borderStripWidth"],
self.config["field"]["length"] +
self.config["field"]["borderStripWidth"] * 2,
self.config["field"]["width"] +
self.config["field"]["borderStripWidth"] * 2)
# white pen
painter.setPen(pen)
painter.setBrush(qtc.Qt.NoBrush)
# field border
painter.drawRectF(-self.config["field"]["length"] / 2,
-self.config["field"]["width"] / 2,
self.config["field"]["length"],
self.config["field"]["width"])
# penalty area left
painter.drawRectF(-self.config["field"]["length"] / 2,
-self.config["field"]["penaltyAreaWidth"] / 2,
self.config["field"]["penaltyAreaLength"],
self.config["field"]["penaltyAreaWidth"])
# penalty area right
painter.drawRectF(self.config["field"]["length"] / 2,
-self.config["field"]["penaltyAreaWidth"] / 2,
-self.config["field"]["penaltyAreaLength"],
self.config["field"]["penaltyAreaWidth"])
# center line
painter.drawLineF(0, -self.config["field"]["width"] / 2, 0,
self.config["field"]["width"] / 2)
# center circle
painter.drawEllipse(qtc.QPointF(0, 0),
self.config["field"]["centerCircleDiameter"] / 2,
self.config["field"]["centerCircleDiameter"] / 2)
# penalty mark left
painter.setBrush(qtc.Qt.white)
painter.drawEllipse(
qtc.QPointF(
-self.config["field"]["length"] / 2 +
self.config["field"]["penaltyMarkerDistance"], 0),
self.config["field"]["penaltyMarkerSize"] / 2,
self.config["field"]["penaltyMarkerSize"] / 2)
# penalty mark right
painter.drawEllipse(
qtc.QPointF(
self.config["field"]["length"] / 2 -
self.config["field"]["penaltyMarkerDistance"], 0),
self.config["field"]["penaltyMarkerSize"] / 2,
self.config["field"]["penaltyMarkerSize"] / 2)
# kick off pointEllipse
painter.drawEllipse(qtc.QPointF(0, 0),
self.config["field"]["penaltyMarkerSize"] / 2,
self.config["field"]["penaltyMarkerSize"] / 2)
def paint(self, painter: Painter):
# Draw background
bgColor = qtg.QColor(
self.config["coordinateSystem"]["backgroundColor"])
bgColor.setAlpha(self.config["coordinateSystem"]["backgroundAlpha"])
painter.setBrush(bgColor)
painter.setPen(qtc.Qt.NoPen)
painter.drawRectF(-self.config["coordinateSystem"]["width"] / 2,
-self.config["coordinateSystem"]["height"] / 2,
self.config["coordinateSystem"]["width"],
self.config["coordinateSystem"]["height"])
# Draw Axis
lineColor = qtg.QColor(self.config["coordinateSystem"]["lineColor"])
pen = qtg.QPen(lineColor)
pen.setWidthF(self.config["coordinateSystem"]["lineWidth"])
painter.setPen(pen)
painter.setBrush(qtc.Qt.NoBrush)
painter.drawLineF(0, -self.config["coordinateSystem"]["height"] / 2, 0,
self.config["coordinateSystem"]["height"] / 2)
painter.drawLineF(-self.config["coordinateSystem"]["width"] / 2, 0,
self.config["coordinateSystem"]["width"] / 2, 0)
if self.config["coordinateSystem"]["polar"]:
# Draw Circles
r = 0.0
while (r + self.config["coordinateSystem"]["radialStepSize"] <=
max(self.config["coordinateSystem"]["height"] / 2,
self.config["coordinateSystem"]["width"] / 2)):
r += self.config["coordinateSystem"]["radialStepSize"]
painter.drawEllipse(qtc.QPointF(0, 0), r, r)
# Draw Angles
l = max(self.config["coordinateSystem"]["height"] / 2,
self.config["coordinateSystem"]["width"] / 2)
deg = 90.0
while deg + self.config["coordinateSystem"][
"polarAngleStepSize"] < 270.0:
deg += self.config["coordinateSystem"]["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.config["coordinateSystem"]["stepSizeY"] <=
self.config["coordinateSystem"]["height"] / 2):
offset += self.config["coordinateSystem"]["stepSizeY"]
painter.drawLineF(
-self.config["coordinateSystem"]["width"] / 2, offset,
self.config["coordinateSystem"]["width"] / 2, offset)
painter.drawLineF(
-self.config["coordinateSystem"]["width"] / 2, -offset,
self.config["coordinateSystem"]["width"] / 2, -offset)
# vertical lines
offset = 0.0
while (offset + self.config["coordinateSystem"]["stepSizeX"] <=
self.config["coordinateSystem"]["width"] / 2):
offset += self.config["coordinateSystem"]["stepSizeX"]
painter.drawLineF(
offset, -self.config["coordinateSystem"]["height"] / 2,
offset, self.config["coordinateSystem"]["height"] / 2)
painter.drawLineF(
-offset, -self.config["coordinateSystem"]["height"] / 2,
-offset, self.config["coordinateSystem"]["height"] / 2)
def paintEvent(self, event):
import math
if self.isFirstPaint:
logger.debug(__name__ + ": First painting of map")
mapPaintTime = time.time()
self.calc_max_pixel_viewport()
self.meter_viewport = qtc.QRectF(
0,
0,
self.parent.model["config"]["viewport"][0],
self.parent.model["config"]["viewport"][1])
self.counter += 1
# Create transformation matrix to automatically rescale all field
# coordinates (meters) to pixel coordinates and to flip the y-axis.
pixels_per_meter = (self.pixel_viewport.width() /
self.meter_viewport.width())
w = self.parent.model["config"]["viewport"][0]
h = self.parent.model["config"]["viewport"][1]
polyon = qtg.QPolygonF([
qtc.QPointF(0.0, 0.0),
qtc.QPointF(w * pixels_per_meter, 0.0),
qtc.QPointF(w * pixels_per_meter, h * pixels_per_meter),
qtc.QPointF(0.0, h * pixels_per_meter),
])
polyon2 = qtg.QPolygonF([
qtc.QPointF(
p[0] * w * pixels_per_meter,
p[1] * h * pixels_per_meter
) for p in self.points
])
perspective = qtg.QTransform()
qtg.QTransform.quadToQuad(polyon, polyon2, perspective)
# From here on, directly use the field coordinates (in meters).
fieldScale = qtg.QTransform()
for layer_painter in self.layer_painter:
if self.isFirstPaint:
logger.debug(__name__ + ": First painting of " +
layer_painter.name + " layer")
layerPaintTime = time.time()
try:
transformation = qtg.QTransform()
transformation.scale(pixels_per_meter, -pixels_per_meter)
layerFieldScale = getattr(layer_painter, "fieldScale", None)
if layerFieldScale is not None:
fieldScale = qtg.QTransform()
fieldScale.scale(layerFieldScale[0], layerFieldScale[1])
painter = Painter(pixels_per_meter)
painter.begin(self)
painter.resetTransform()
if self.parent.model["enable_perspective"] and not layer_painter.ignore_perspective:
painter.setTransform(perspective)
painter.setTransform(transformation, True)
# Translate coordinate system to field center.
painter.translate(w / 2.0 + layer_painter.config["center_x"],
-h / 2.0 + layer_painter.config["center_y"])
if layer_painter.ignore_scale:
painter.scale(w, h)
if self.parent.model["enable_perspective"] and not layer_painter.ignore_perspective:
painter.setTransform(fieldScale, True)
# These are swapped because in SPL cooredinates, the Y axis is the long side of the field
if self.parent.model["flip_x"]:
painter.scale(1.0, -1.0)
if self.parent.model["flip_y"]:
painter.scale(-1.0, 1.0)
layer_painter.paint(painter)
painter.end()
except Exception as e:
if self.isFirstPaint:
logger.error(__name__ + ": Exception when painting " +
layer_painter.name + " layer: ")
logger.error(__name__ + ": " + str(e))
import traceback
print(traceback.format_exc())
if self.isFirstPaint:
logger.debug(__name__ + ": First painting of " +
layer_painter.name + " layer took: " +
logger.timerLogStr(layerPaintTime))
if self.isFirstPaint:
logger.debug(__name__ + ": First painting of map took: " +
logger.timerLogStr(mapPaintTime))
self.isFirstPaint = False
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_model["config"]["teamPlayers"][
Roles(robot["currentlyPerformingRole"]).name + "Color"])
# fov
if self.layer_model["config"]["teamPlayers"]["showFOV"]:
# fov
painter.setBrush(qtc.Qt.NoBrush)
painter.setPen(qtg.QPen(qtc.Qt.yellow, 0))
painter.drawFOV(
robot["pose"], robot["headYaw"],
self.layer_model["config"]["teamPlayers"]
["maxDistance"], self.layer_model["config"]
["teamPlayers"]["cameraOpeningAngle"])
if self.layer_model["config"]["teamPlayers"]["showTarget"]:
# Walking-target
painter.setBrush(qtc.Qt.NoBrush)
painter.setPen(color, 0)
painter.drawTarget(
robot["walkingTo"], self.layer_model["config"]
["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_model["config"]["teamPlayers"]["showPlayer"]:
# Pose
painter.setBrush(color)
painter.setPen(qtg.QPen(qtc.Qt.black, 0))
painter.drawPose(
robot["pose"], self.layer_model["config"]
["teamPlayers"]["poseCircleDiameter"],
self.layer_model["config"]["teamPlayers"]
["poseCircleDiameter"])
# PlayerNumber
painter.setPen(
qtg.QPen(qtg.QColor(ui_utils.ideal_text_color(color)),
0))
diameter = self.layer_model["config"]["teamPlayers"][
"poseCircleDiameter"]
painter.drawText(
qtc.QPointF(robot["pose"][0][0] - (diameter * 0.2),
robot["pose"][0][1] - (diameter * 0.25)),
str(robot["playerNumber"]), diameter * 0.61)
if self.layer_model["config"]["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_model["config"]["teamPlayers"]
["searchPositionDiameter"] / 2,
self.layer_model["config"]["teamPlayers"]
["searchPositionDiameter"] / 2)
def paint(self, painter: Painter):
painter.transformByPose(self.transformation)
if self.filteredSonar is not None:
sensors = [[
-self.layer_model["config"]["sonar"]["zAngle"],
self.layer_model["config"]["sonar"]["yOffset"] / 100,
self.filteredSonar["filteredValues"][0],
self.filteredSonar["valid"][0]
],
[
self.layer_model["config"]["sonar"]["zAngle"],
-self.layer_model["config"]["sonar"]["yOffset"] /
100, self.filteredSonar["filteredValues"][1],
self.filteredSonar["valid"][1]
]]
openAngle = self.layer_model["config"]["sonar"]["openingAngle"]
color = qtg.QColor(self.layer_model["config"]["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_model["config"]["sonar"]["zAngle"],
self.layer_model["config"]["sonar"]["yOffset"] / 100,
self.rawSonar["SONAR_LEFT_SENSOR_0"],
self.rawSonar["valid_SONAR_LEFT_SENSOR_0"]
],
[
self.layer_model["config"]["sonar"]["zAngle"],
-self.layer_model["config"]["sonar"]["yOffset"] /
100, self.rawSonar["SONAR_RIGHT_SENSOR_0"],
self.rawSonar["valid_SONAR_RIGHT_SENSOR_0"]
]]
openAngle = self.layer_model["config"]["sonar"]["openingAngle"]
color = qtg.QColor(self.layer_model["config"]["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_model["config"]["search"]["showNumericProbability"]:
paintText = True
elif self.layer_model["config"]["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_model["config"]["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_model["config"]["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_model["config"]["search"][
"showNumericProbability"]:
painter.drawText(
qtc.QPointF(cell[2] - 0.15, cell[3] + 0.03),
"%.2f" % (cell[0] * 100.0), 0.1)
# Age in seconds
if self.layer_model["config"]["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.1)
# Show voronoi seeds: opaque when searching, transparent when not
if self.voronoiSeeds and (self.explorerCount > 0):
if self.layer_model["config"]["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 drawRobot(self, painter: Painter, robot):
painter.setBrush(qtg.QColor(255, 255, 0, 120))
cov = np.array(robot["covariance"])
w, v = np.linalg.eig(cov)
v1 = v[:, 0]
phi_inferred = np.rad2deg(np.arctan2(v1[1], v1[0]))
std_x = math.sqrt(w[0])
std_y = math.sqrt(w[1])
painter.save()
painter.translate(robot["state"][0], robot["state"][1])
painter.rotate(-phi_inferred)
painter.drawEllipse(qtc.QPointF(0, 0), std_x, std_y)
painter.restore()
painter.setBrush(qtc.Qt.red)
painter.drawEllipse(qtc.QPointF(robot["state"][0], robot["state"][1]),
0.1, 0.1)
def paint(self, painter: Painter):
painter.transformByPose(self.transformation)
painter.setBrush(qtc.Qt.NoBrush)
if self.targetPosition is not None:
painter.setPen(
qtg.QColor(self.layer_model["config"]["targetPosition"]
["targetColor"]), 0)
painter.drawTarget([self.targetPosition, 0],
self.layer_model["config"]["targetPosition"]
["targetCircleDiameter"])
if self.displacementVector is not None and self.displacementVector[
0] != 0 and self.displacementVector[1] != 0:
painter.setPen(
qtg.QPen(
qtg.QColor(self.layer_model["config"]["displacementVector"]
["lineColor"]), self.layer_model["config"]
["displacementVector"]["lineWidth"], qtc.Qt.DashDotLine))
painter.drawLineF(0.0, 0.0, self.displacementVector[0],
self.displacementVector[1])
if self.targetPosition is not None and self.displacementVector is not None:
painter.setPen(qtc.Qt.yellow, 0)
painter.drawTarget([[
self.targetPosition[0] - self.displacementVector[0],
self.targetPosition[1] - self.displacementVector[1]
], 0], self.layer_model["config"]["targetPosition"]
["targetCircleDiameter"])
def paint(self, painter: Painter):
if self.pose is not None:
color = qtg.QColor(
self.layer_model["config"]["pose"]["fixedColor"])
if not self.layer_model["config"]["pose"]["useFixedColor"]:
if self.role is not None:
color = qtg.QColor(self.layer_model["config"]["pose"][
Roles(self.role).name + "Color"])
if self.searchPosition is not None and self.pose is not None:
if self.layer_model["config"]["ballSearch"]["drawSearchTarget"]:
# Search position
painter.setPen(qtg.QPen(qtc.Qt.black, 0))
painter.setBrush(color)
painter.drawEllipse(
qtc.QPointF(self.searchPosition[0],
self.searchPosition[1]),
self.layer_model["config"]["ballSearch"]
["searchCircleDiameter"] / 2, self.layer_model["config"]
["ballSearch"]["searchCircleDiameter"] / 2)
if self.pose is not None:
# Transform to local coords
painter.transformByPose(self.pose)
if self.jointSensorData is not None and self.pose is not None:
if self.layer_model["config"]["fov"]["drawFOV"]:
# FOV
painter.setBrush(qtc.Qt.NoBrush)
painter.setPen(qtg.QPen(qtc.Qt.yellow, 0))
painter.drawFOV(
[[0.0, 0.0], 0.0], self.jointSensorData[0],
self.layer_model["config"]["fov"]["maxDistance"],
self.layer_model["config"]["fov"]["cameraOpeningAngle"])
if self.motionPlan is not None and self.pose is not None:
if self.layer_model["config"]["motionPlan"]["drawMotionPlan"]:
# Walking-target
painter.setBrush(qtc.Qt.NoBrush)
painter.setPen(color, 0)
painter.drawTarget(
self.motionPlan["walkTarget"], self.layer_model["config"]
["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])
if self.layer_model["config"]["motionPlan"]["drawTranslation"]:
# Translation
tl = self.motionPlan["translation"]
painter.setPen(
qtg.QColor(self.layer_model["config"]["motionPlan"]
["translationColor"]), 0)
orthoOffset = [tl[1] * 0.05, -tl[0] * 0.05]
painter.drawLineF(0.0, 0.0, tl[0], tl[1])
painter.drawLineF(tl[0] * 0.9 - orthoOffset[0],
tl[1] * 0.9 - orthoOffset[1], tl[0], tl[1])
painter.drawLineF(tl[0] * 0.9 + orthoOffset[0],
tl[1] * 0.9 + orthoOffset[1], tl[0], tl[1])
if self.pose is not None:
if self.layer_model["config"]["pose"]["drawPose"]:
# Pose
painter.setPen(qtg.QPen(qtc.Qt.black, 0))
painter.setBrush(color)
painter.drawPose([[0.0, 0.0], 0.0], self.layer_model["config"]
["pose"]["positionCircleDiameter"],
self.layer_model["config"]["pose"]
["orientationLineLength"])