def _draw_control_target(self, painter, color, robot): # ロボットの制御目標値を描画する # 経路の線を描画するための変数 prev_point = None robot_id = robot.robot_id target = self._control_targets[color][robot_id] # 経路を描画 for pose in target.path: point = self._convert_to_view(pose.x, pose.y) size = self._ROBOT_RADIUS * self._scale_field_to_view target_color = QColor(Qt.magenta) target_color.setAlphaF(0.5) painter.setPen(Qt.black) painter.setBrush(target_color) painter.drawEllipse(point, size, size) # 角度 line_x = self._ROBOT_RADIUS * math.cos(pose.theta) line_y = self._ROBOT_RADIUS * math.sin(pose.theta) line_point = point + self._convert_to_view(line_x, line_y) painter.drawLine(point, line_point) # IDを添える text_point = point + self._convert_to_view(self._ID_POS[0], self._ID_POS[1]) painter.drawText(text_point, str(robot_id)) # 経路を描画 if prev_point is None: prev_point = point else: painter.setPen(QPen(QColor(0, 0, 255, 127), 4)) painter.drawLine(prev_point, point) prev_point = point # キック・ドリブルフラグを、ロボットの現在位置周辺に描画 text_flag = "" if target.kick_power > 0.0: text_flag += "K:" + str(target.kick_power) if target.chip_enable is True: text_flag += "\nC:ON" if target.dribble_power > 0.0: text_flag += "\nD:" + str(target.dribble_power) point = self._convert_to_view(robot.pose.x, robot.pose.y) text_point = point + self._convert_to_view(self._FLAG_POS[0], self._FLAG_POS[1]) # 複数行に分けてテキストを描画するため、widthとheightを設定する text_width = 50 text_height = 100 painter.setPen(QPen(Qt.red, 2)) painter.drawText(text_point.x(), text_point.y(), text_width, text_height, 0, text_flag)
def __init__(self, scene, time, color=None): y2 = len(scene._tracks) * TimelineTrack.height super(TimelineMarker, self).__init__(time, - TimelineTrack.height, time, y2, None, scene) self.setZValue(self._z) if color is None: color = QColor(self._default_color) color.setAlphaF(0.5) self.setPen(QPen(QBrush(color), 0.05))
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)