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)
