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"])