def _replaceRobotAngle(self, mouse_pos): real_pos = self.convertToRealWorld(mouse_pos.x(), mouse_pos.y()) robot_pos = QPointF() if self._replace_is_friend: robot_pos.setX(self.friendOdoms[self._replace_id].pose.pose.position.x) robot_pos.setY(self.friendOdoms[self._replace_id].pose.pose.position.y) else: robot_pos.setX(self.enemyOdoms[self._replace_id].pose.pose.position.x) robot_pos.setY(self.enemyOdoms[self._replace_id].pose.pose.position.y) is_yellow = True if self._replace_is_friend and self.friend_color == 'blue': is_yellow = False elif not self._replace_is_friend and self.friend_color == 'yellow': is_yellow = False replace = ReplaceRobot() replace.robot_id = self._replace_id replace.is_yellow = is_yellow replace.pos_x = robot_pos.x() replace.pos_y = robot_pos.y() replace.dir = math.degrees(self._to_angle(robot_pos, real_pos)) replace.turn_on = True self._pub_replace_robot.publish(replace) self._is_robotangle_replacement = False
def drawAngleReplacement(self, painter): robot_pos = QPointF() if self._replace_is_friend: robot_pos.setX( self.friendOdoms[self._replace_id].pose.pose.position.x) robot_pos.setY( self.friendOdoms[self._replace_id].pose.pose.position.y) else: robot_pos.setX( self.enemyOdoms[self._replace_id].pose.pose.position.x) robot_pos.setY( self.enemyOdoms[self._replace_id].pose.pose.position.y) currentPos = self.convertToRealWorld(self._current_mouse_pos.x(), self._current_mouse_pos.y()) angle = math.degrees(self._to_angle(robot_pos, currentPos)) robotPoint = self.convertToDrawWorld(robot_pos.x(), robot_pos.y()) currentPoint = self.convertToDrawWorld(currentPos.x(), currentPos.y()) painter.setPen(QPen(Qt.blue, 2)) painter.drawLine(robotPoint, currentPoint) text = "[" + str(round(angle, 2)) + "]" painter.setPen(Qt.black) painter.drawText(currentPoint, text)
def _draw_angle_replacement(self, painter): # ロボット角度のReplacementを描画する robot_pos = QPointF() if self._replace_is_yellow: robot_pos.setX(self._robot_info['yellow'][self._replace_id].pose.x) robot_pos.setY(self._robot_info['yellow'][self._replace_id].pose.y) else: robot_pos.setX(self._robot_info['blue'][self._replace_id].pose.x) robot_pos.setY(self._robot_info['blue'][self._replace_id].pose.y) robot_point = self._convert_to_view(robot_pos.x(), robot_pos.y()) 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(), current_pos.y()) painter.setPen(QPen(Qt.blue, 2)) painter.drawLine(robot_point, current_point) # 角度の数値を描画する angle = math.degrees(self._to_angle(robot_pos, current_pos)) text = "[" + str(round(angle, 2)) + "]" painter.setPen(Qt.black) painter.drawText(current_point, text)
class PaintWidget(QWidget): def __init__(self,parent=None): super(PaintWidget,self).__init__(parent) # geometry parameters self.geometry = Geometry() self.scaleOnField = 1.0 self.world_height = 0.0 self.world_width = 0.0 self.trans = QPointF(0.0,0.0) # 慢性的なトランス self.mouseTrans = QPointF(0.0, 0.0) # マウス操作で発生する一時的なトランス self.scale = QPointF(1.0,1.0) self.clickPoint = QPointF(0.0,0.0) self._current_mouse_pos = QPointF(0.0,0.0) # Colors self.friendDrawColor = Qt.cyan self.enemyDrawColor = Qt.yellow self.friend_color = rospy.get_param("friend_color", "blue") if self.friend_color != "blue": self.friendDrawColor = Qt.yellow self.enemyDrawColor = Qt.cyan self.targetPosDrawColor = QColor(102, 0, 255, 100) self.avoidingPointDrawColor = QColor(255, 0, 0, 100) # Replace self._CLICK_POS_THRESHOLD = 0.1 self._CLICK_VEL_ANGLE_THRESHOLD = self._CLICK_POS_THRESHOLD + 0.1 self._VEL_GAIN = 3.0 self._VEL_MAX = 8.0 self._replace_func = None self._replace_id = 0 self._replace_is_yellow = False self._replace_is_friend = False # Status self._should_rotate_world = False self._can_replace = False self._is_ballpos_replacement = False self._is_ballvel_replacement = False self._is_robotpos_replacement = False self._is_robotangle_replacement = False # Configs # This function enables mouse tracking without pressing mouse button self.setMouseTracking(True) # Subscribers self.field_geometry = GeometryFieldSize() self.sub_geometry = rospy.Subscriber("geometry_field_size", GeometryFieldSize, self.callbackGeometry) self.ballOdom = Odometry() self.sub_ballPosition = rospy.Subscriber("ball_observer/estimation", Odometry,self.callbackBallOdom) self.friendsIDArray = UIntArray() self.sub_friendsID = rospy.Subscriber("existing_friends_id", UIntArray, self.callbackFriendsID) self._ID_MAX = rospy.get_param('id_max', 12) self.friendOdoms = [Odometry()] * self._ID_MAX self.sub_friendOdoms = [] self.enemyIDArray = UIntArray() self.sub_enemiesID = rospy.Subscriber("existing_enemies_id", UIntArray, self.callbackEnemiesID) self.refereeBranch = String() self.sub_referee_branch = rospy.Subscriber("referee_branch", String, self.callbackRefereeBranch) self.passTargetPointFrameCount = 0 self.passTargetPoint = Point() self.sub_passTargetPoint = rospy.Subscriber("pass_target_point", Point, self.callbackPassTargetPoint) self.debugPoint = [Point()] * 10 rospy.Subscriber("debug_point_0", Point, self.callbackDebugPoint0) rospy.Subscriber("debug_point_1", Point, self.callbackDebugPoint1) rospy.Subscriber("debug_point_2", Point, self.callbackDebugPoint2) rospy.Subscriber("debug_point_3", Point, self.callbackDebugPoint3) rospy.Subscriber("debug_point_4", Point, self.callbackDebugPoint4) rospy.Subscriber("debug_point_5", Point, self.callbackDebugPoint5) rospy.Subscriber("debug_point_6", Point, self.callbackDebugPoint6) rospy.Subscriber("debug_point_7", Point, self.callbackDebugPoint7) rospy.Subscriber("debug_point_8", Point, self.callbackDebugPoint8) rospy.Subscriber("debug_point_9", Point, self.callbackDebugPoint9) self.enemyOdoms = [Odometry()] * self._ID_MAX self.sub_enemyOdoms = [] self.targetPositions = [PoseStamped()] * self._ID_MAX self.sub_targetPositions =[] self.targetVelocities = [TwistStamped()] * self._ID_MAX self.sub_targetVelocities = [] self.targetIsPosition = [False] * self._ID_MAX self.avoidingPoints = [Point()] * self._ID_MAX self.sub_avoidingPoints = [] for i in xrange(self._ID_MAX): strID = str(i) topicFriend = "robot_" + strID + "/odom" topicEnemy = "enemy_" + strID + "/odom" topicPosition = "robot_" + strID + "/move_base_simple/goal" topicVelocity = "robot_" + strID + "/move_base_simple/target_velocity" topicAvoidingPoint = "robot_" + strID + "/avoiding_point" self.sub_friendOdoms.append( rospy.Subscriber(topicFriend, Odometry, self.callbackFriendOdom, callback_args=i)) self.sub_enemyOdoms.append( rospy.Subscriber(topicEnemy, Odometry, self.callbackEnemiesOdom, callback_args=i)) self.sub_targetPositions.append( rospy.Subscriber(topicPosition, PoseStamped, self.callbackTargetPosition, callback_args=i)) self.sub_targetVelocities.append( rospy.Subscriber(topicVelocity, TwistStamped, self.callbackTargetVelocity, callback_args=i)) self.sub_avoidingPoints.append( rospy.Subscriber(topicAvoidingPoint, Point, self.callbackAvoidingPoint, callback_args=i)) # Publishers self._pub_replace_ball = rospy.Publisher( 'replacement_ball', ReplaceBall, queue_size=10) self._pub_replace_robot = rospy.Publisher( 'replacement_robot', ReplaceRobot, queue_size=10) self.resizeDrawWorld() def callbackGeometry(self, msg): self.field_geometry = msg self.geometry.set_field( self.field_geometry.field_length, self.field_geometry.field_width) def callbackBallOdom(self, msg): self.ballOdom = msg # self.update() def callbackFriendsID(self, msg): self.friendsIDArray = msg # self.update() def callbackFriendOdom(self, msg, robot_id): self.friendOdoms[robot_id] = msg def callbackEnemiesID(self, msg): self.enemyIDArray = msg def callbackRefereeBranch(self, msg): self.refereeBranch = msg def callbackPassTargetPoint(self, msg): self.passTargetPoint = msg # 10フレーム以内に更新があれば表示するためのカウンタ self.passTargetPointFrameCount = 0 def callbackDebugPoint0(self, msg): self.debugPoint[0] = msg def callbackDebugPoint1(self, msg): self.debugPoint[1] = msg def callbackDebugPoint2(self, msg): self.debugPoint[2] = msg def callbackDebugPoint3(self, msg): self.debugPoint[3] = msg def callbackDebugPoint4(self, msg): self.debugPoint[4] = msg def callbackDebugPoint5(self, msg): self.debugPoint[5] = msg def callbackDebugPoint6(self, msg): self.debugPoint[6] = msg def callbackDebugPoint7(self, msg): self.debugPoint[7] = msg def callbackDebugPoint8(self, msg): self.debugPoint[8] = msg def callbackDebugPoint9(self, msg): self.debugPoint[9] = msg def callbackEnemiesOdom(self, msg, robot_id): self.enemyOdoms[robot_id] = msg def callbackTargetPosition(self, msg, robot_id): self.targetPositions[robot_id] = msg self.targetIsPosition[robot_id] = True def callbackTargetVelocity(self, msg, robot_id): self.targetVelocities[robot_id] = msg self.targetIsPosition[robot_id] = False def callbackAvoidingPoint(self, msg, robot_id): self.avoidingPoints[robot_id] = msg @mouseevent_wrapper def mousePressEvent(self, event): if event.buttons() == Qt.LeftButton: self.clickPoint = event.posF() self._can_replace = self._isReplacementClick(self.clickPoint) elif event.buttons() == Qt.RightButton: self.resetPainterState() self.update() @mouseevent_wrapper def mouseMoveEvent(self, event): self._current_mouse_pos = event.posF() if self._can_replace : pass elif event.buttons() == Qt.LeftButton: pos = event.posF() self.mouseTrans = (pos - self.clickPoint) / self.scale.x() self.update() @mouseevent_wrapper def mouseReleaseEvent(self, event): if self._can_replace: self._can_replace = False self._replace_func(event.posF()) else: self.trans += self.mouseTrans self.mouseTrans = QPointF(0.0, 0.0) self.update() @wheelevent_wrapper def wheelEvent(self, event): # マウスのホイール操作でスケールを変える if event.delta() > 0: s = self.scale.x() self.scale.setX(s + 0.1) self.scale.setY(s + 0.1) else: s = self.scale.x() if s > 0.2 : self.scale.setX(s - 0.1) self.scale.setY(s - 0.1) self.update() def resizeEvent(self, event): # widgetのサイズ変更によるイベント self.resizeDrawWorld() def paintEvent(self, event): painter = QPainter(self) # 描画の中心をWidgetの中心に持ってくる cx = float(self.width()) * 0.5 cy = float(self.height()) * 0.5 painter.translate(cx,cy) # これ以降にトランスとスケール操作を持ってくる painter.scale(self.scale.x(), self.scale.y()) painter.translate(self.trans + self.mouseTrans) if self._should_rotate_world == True: painter.rotate(-90) # これ以降に描きたいものを重ねていく self.drawField(painter) self.drawAvoidingPoints(painter) self.drawAvoidingPaths(painter) self.drawFriends(painter) self.drawEnemis(painter) self.drawBallVelocity(painter) self.drawBall(painter) self.drawPassTargetPoint(painter) self.drawTargets(painter) if self._is_ballpos_replacement or self._is_robotpos_replacement: self.drawPosReplacement(painter) self.drawCoordinateText(painter) elif self._is_ballvel_replacement: self.drawVelReplacement(painter) elif self._is_robotangle_replacement: self.drawAngleReplacement(painter) else: self.drawCoordinateText(painter) def resetPainterState(self): self.trans = QPointF(0.0,0.0) self.mouseTrans = QPointF(0.0, 0.0) self.scale = QPointF(1.0, 1.0) def resizeDrawWorld(self): # Widgetのサイズに合わせて、描くフィールドのサイズを変える # 描画の回転判断もしてくれるすぐれもの widgetHeight = float(self.height()) widgetWidth = float(self.width()) w_per_h = widgetWidth/widgetHeight if w_per_h >= self.geometry.WORLD_W_PER_H: # Widgetが横長のとき self.world_height = widgetHeight self.world_width = widgetHeight * self.geometry.WORLD_W_PER_H self._should_rotate_world = False elif w_per_h <= self.geometry.WORLD_H_PER_W: # Widgetが縦長のとき self.world_height = widgetWidth self.world_width = widgetWidth * self.geometry.WORLD_W_PER_H self._should_rotate_world = True else: # 描画回転にヒステリシス性をもたせる if self._should_rotate_world == True: self.world_height = widgetHeight * self.geometry.WORLD_H_PER_W self.world_width = widgetHeight else: self.world_height = widgetWidth * self.geometry.WORLD_H_PER_W self.world_width = widgetWidth self.scaleOnField = self.world_width / self.geometry.WORLD_WIDTH def convertToDrawWorld(self, x, y): drawX = x * self.scaleOnField drawY = -y * self.scaleOnField point = QPointF(drawX, drawY) return point def convertToRealWorld(self, x, y): x /= self.scale.x() y /= self.scale.y() x -= (self.trans.x() + self.mouseTrans.x()) y -= (self.trans.y() + self.mouseTrans.y()) x -= self.width() * 0.5 / self.scale.x() y -= self.height() * 0.5 / self.scale.y() if self._should_rotate_world: x, y = -y, x real_x = x / self.scaleOnField real_y = -y / self.scaleOnField point = QPointF(real_x, real_y) return point def _isReplacementClick(self, mouse_pos): real_pos = self.convertToRealWorld(mouse_pos.x(), mouse_pos.y()) is_clicked = True result = self._isBallClicked(real_pos) if result == 'pos': self._is_ballpos_replacement = True self._replace_func = self._replaceBallPos elif result == 'vel_angle': self._is_ballvel_replacement = True self._replace_func = self._replaceBallVel else: result, robot_id, is_friend = self._isRobotClicked(real_pos) self._replace_id = robot_id self._replace_is_friend = is_friend if result == 'pos': self._is_robotpos_replacement = True self._replace_func = self._replaceRobotPos elif result == 'vel_angle': self._is_robotangle_replacement = True self._replace_func = self._replaceRobotAngle else: is_clicked = False return is_clicked def _isBallClicked(self, real_pos): posX = self.ballOdom.pose.pose.position.x posY = self.ballOdom.pose.pose.position.y ball_pos = QPointF(posX, posY) return self._isClicked(real_pos, ball_pos) def _isRobotClicked(self, real_pos): is_clicked = False replace_id = 0 is_friend = False for robot_id in self.friendsIDArray.data: posX = self.friendOdoms[robot_id].pose.pose.position.x posY = self.friendOdoms[robot_id].pose.pose.position.y robot_pos = QPointF(posX, posY) is_clicked = self._isClicked(real_pos, robot_pos) if is_clicked: is_friend = True return is_clicked, robot_id, is_friend for robot_id in self.enemyIDArray.data: posX = self.enemyOdoms[robot_id].pose.pose.position.x posY = self.enemyOdoms[robot_id].pose.pose.position.y robot_pos = QPointF(posX, posY) is_clicked = self._isClicked(real_pos, robot_pos) if is_clicked: is_friend = False return is_clicked, robot_id, is_friend return is_clicked, replace_id, is_friend def _isClicked(self, real_pos1, real_pos2): diff = real_pos1 - real_pos2 diff_size = math.hypot(diff.x(), diff.y()) if diff_size < self._CLICK_POS_THRESHOLD: return "pos" elif diff_size < self._CLICK_VEL_ANGLE_THRESHOLD: return "vel_angle" return False def _replaceBallPos(self, mouse_pos): real_pos = self.convertToRealWorld(mouse_pos.x(), mouse_pos.y()) replace = ReplaceBall() replace.pos_x = real_pos.x() replace.pos_y = real_pos.y() self._pub_replace_ball.publish(replace) self._is_ballpos_replacement = False def _replaceBallVel(self, mouse_pos): ballPos = self.ballOdom.pose.pose.position ballPoint = QPointF(ballPos.x, ballPos.y) currentPos = self.convertToRealWorld( mouse_pos.x(), mouse_pos.y()) velocity = self._calcuReplaceVelocity(ballPoint, currentPos) replace = ReplaceBall() replace.pos_x = ballPoint.x() replace.pos_y = ballPoint.y() replace.vel_x = velocity.x() replace.vel_y = velocity.y() self._pub_replace_ball.publish(replace) self._is_ballvel_replacement = False def _replaceRobotPos(self, mouse_pos): real_pos = self.convertToRealWorld(mouse_pos.x(), mouse_pos.y()) # euler <- (roll, pitch, yaw) orientation = None if self._replace_is_friend: orientation = self.friendOdoms[self._replace_id].pose.pose.orientation else: orientation = self.enemyOdoms[self._replace_id].pose.pose.orientation euler = self._to_euler(orientation) is_yellow = True if self._replace_is_friend and self.friend_color == 'blue': is_yellow = False elif not self._replace_is_friend and self.friend_color == 'yellow': is_yellow = False replace = ReplaceRobot() replace.robot_id = self._replace_id replace.is_yellow = is_yellow replace.pos_x = real_pos.x() replace.pos_y = real_pos.y() replace.dir = math.degrees(euler[2]) replace.turn_on = True self._pub_replace_robot.publish(replace) self._is_robotpos_replacement = False def _replaceRobotAngle(self, mouse_pos): real_pos = self.convertToRealWorld(mouse_pos.x(), mouse_pos.y()) robot_pos = QPointF() if self._replace_is_friend: robot_pos.setX(self.friendOdoms[self._replace_id].pose.pose.position.x) robot_pos.setY(self.friendOdoms[self._replace_id].pose.pose.position.y) else: robot_pos.setX(self.enemyOdoms[self._replace_id].pose.pose.position.x) robot_pos.setY(self.enemyOdoms[self._replace_id].pose.pose.position.y) is_yellow = True if self._replace_is_friend and self.friend_color == 'blue': is_yellow = False elif not self._replace_is_friend and self.friend_color == 'yellow': is_yellow = False replace = ReplaceRobot() replace.robot_id = self._replace_id replace.is_yellow = is_yellow replace.pos_x = robot_pos.x() replace.pos_y = robot_pos.y() replace.dir = math.degrees(self._to_angle(robot_pos, real_pos)) replace.turn_on = True self._pub_replace_robot.publish(replace) self._is_robotangle_replacement = False def _calcuReplaceVelocity(self, from_pos, to_pos): diff = to_pos - from_pos diff_size = math.hypot(diff.x(), diff.y()) velocity_size = diff_size * self._VEL_GAIN if velocity_size > self._VEL_MAX: velocity_size = self._VEL_MAX angle = math.atan2(diff.y(), diff.x()) velocity = QPointF( velocity_size * math.cos(angle), velocity_size * math.sin(angle)) return velocity def drawField(self, painter): # draw green surface rectangle painter.setPen(Qt.black) painter.setBrush(Qt.green) rx = -self.world_width * 0.5 ry = -self.world_height * 0.5 rect = QRectF(rx, ry, self.world_width, self.world_height) painter.drawRect(rect) # draw white lines painter.setPen(QPen(Qt.white,2)) for line in self.field_geometry.field_lines: p1 = self.convertToDrawWorld(line.p1_x, line.p1_y) p2 = self.convertToDrawWorld(line.p2_x, line.p2_y) painter.drawLine(p1, p2) for arc in self.field_geometry.field_arcs: top_left = self.convertToDrawWorld( arc.center_x - arc.radius, arc.center_y + arc.radius) size = arc.radius * 2.0 * self.scaleOnField start_angle = math.degrees(arc.a1) end_angle = math.degrees(arc.a2) span_angle = end_angle - start_angle # angle must be 1/16 degrees order start_angle *= 16 span_angle *= 16 painter.drawArc(top_left.x(), top_left.y(), size, size, start_angle, span_angle) def drawBall(self, painter): posX = self.ballOdom.pose.pose.position.x posY = self.ballOdom.pose.pose.position.y point = self.convertToDrawWorld(posX,posY) size = self.geometry.BALL_RADIUS * self.scaleOnField painter.setPen(Qt.black) painter.setBrush(Qt.red) painter.drawEllipse(point, size, size) def drawPassTargetPoint(self, painter): posX = self.passTargetPoint.x posY = self.passTargetPoint.y point = self.convertToDrawWorld(posX,posY) size = self.geometry.BALL_RADIUS * self.scaleOnField painter.setPen(Qt.black) painter.setBrush(Qt.blue) # 10フレーム以内に更新があれば表示 if self.passTargetPointFrameCount <= 10: painter.drawEllipse(point, size, size) self.passTargetPointFrameCount = self.passTargetPointFrameCount + 1 def drawBallVelocity(self, painter): ballPos = self.ballOdom.pose.pose.position ballVel = self.ballOdom.twist.twist.linear if math.hypot(ballVel.x, ballVel.y) < 1.0: return angleOfSpeed = math.atan2(ballVel.y, ballVel.x) paintDist = 10.0 velPosX = paintDist * math.cos(angleOfSpeed) + ballPos.x velPosY = paintDist * math.sin(angleOfSpeed) + ballPos.y ballPosPoint = self.convertToDrawWorld(ballPos.x, ballPos.y) velPosPoint = self.convertToDrawWorld(velPosX, velPosY) painter.setPen(QPen(QColor(102,0,255),2)) painter.drawLine(ballPosPoint, velPosPoint) def drawFriends(self, painter): for robot_id in self.friendsIDArray.data: self.drawRobot(painter, robot_id, self.friendOdoms[robot_id], self.friendDrawColor) def drawEnemis(self, painter): for robot_id in self.enemyIDArray.data: self.drawRobot(painter, robot_id, self.enemyOdoms[robot_id], self.enemyDrawColor) def drawRobot(self, painter,robot_id, odom, color): # draw robot body on its position posX = odom.pose.pose.position.x posY = odom.pose.pose.position.y point = self.convertToDrawWorld(posX, posY) size = self.geometry.ROBOT_RADIUS * self.scaleOnField painter.setPen(Qt.black) painter.setBrush(color) painter.drawEllipse(point, size, size) # draw robot angle on its body orientation = odom.pose.pose.orientation euler = self._to_euler(orientation) # euler <- (roll, pitch, yaw) linePosX = self.geometry.ROBOT_RADIUS * math.cos(euler[2]) linePosY = self.geometry.ROBOT_RADIUS * math.sin(euler[2]) linePoint = point + self.convertToDrawWorld(linePosX, linePosY) painter.drawLine(point, linePoint) # draw robot_id on its head textPosX = 0.15 textPosY = 0.15 textPoint = point + self.convertToDrawWorld(textPosY, textPosY) painter.drawText(textPoint, str(robot_id)) def drawTargets(self, painter): for robot_id in self.friendsIDArray.data: if self.targetIsPosition[robot_id] == True: self.drawTargetPosition(painter, robot_id, self.targetPositions[robot_id]) else: self.drawTargetVelocity(painter, robot_id, self.targetVelocities[robot_id]) def drawTargetPosition(self, painter, robot_id, positionStamped): posX = positionStamped.pose.position.x posY = positionStamped.pose.position.y point = self.convertToDrawWorld(posX, posY) size = self.geometry.ROBOT_RADIUS * self.scaleOnField painter.setPen(Qt.black) painter.setBrush(self.targetPosDrawColor) painter.drawEllipse(point, size, size) orientation = positionStamped.pose.orientation # euler_from_quaternion does not support geometry_msgs:Quaternion euler = tf.transformations.euler_from_quaternion( (orientation.x, orientation.y, orientation.z, orientation.w)) # euler <- (roll, pitch, yaw) linePosX = self.geometry.ROBOT_RADIUS * math.cos(euler[2]) linePosY = self.geometry.ROBOT_RADIUS * math.sin(euler[2]) linePoint = point + self.convertToDrawWorld(linePosX, linePosY) painter.drawLine(point, linePoint) # draw robot_id on its head textPosX = 0.15 textPosY = 0.15 textPoint = point + self.convertToDrawWorld(textPosY, textPosY) painter.drawText(textPoint, str(robot_id)) def drawTargetVelocity(self, painter, robot_id, twistStamped): odom = self.friendOdoms[robot_id] posX = odom.pose.pose.position.x posY = odom.pose.pose.position.y point = self.convertToDrawWorld(posX, posY) # draw robot_id on its head textPosX = 0.15 textPosY = -0.15 textPoint = point + self.convertToDrawWorld(textPosY, textPosY) velX = twistStamped.twist.linear.x velY = twistStamped.twist.linear.y velZ = twistStamped.twist.angular.z text = "(" + str(velX) + ", " + str(velY) + ", " + str(velZ) + ")" painter.setPen(Qt.red) painter.drawText(textPoint, text) def drawAvoidingPaths(self, painter): for robot_id in self.friendsIDArray.data: current_point = self.friendOdoms[robot_id].pose.pose.position avoiding_point = self.avoidingPoints[robot_id] target_point = self.targetPositions[robot_id].pose.position current_point = self.convertToDrawWorld(current_point.x, current_point.y) avoiding_point = self.convertToDrawWorld(avoiding_point.x, avoiding_point.y) target_point = self.convertToDrawWorld(target_point.x, target_point.y) painter.setPen(QPen(Qt.yellow,2)) painter.drawLine(current_point, avoiding_point) painter.drawLine(avoiding_point, target_point) def drawAvoidingPoints(self, painter): for robot_id in self.friendsIDArray.data: self.drawAvoidingPoint(painter, robot_id, self.avoidingPoints[robot_id]) def drawAvoidingPoint(self, painter, robot_id, point): drawPoint = self.convertToDrawWorld(point.x, point.y) size = self.geometry.ROBOT_RADIUS * self.scaleOnField painter.setPen(Qt.black) painter.setBrush(self.avoidingPointDrawColor) painter.drawEllipse(drawPoint, size, size) # draw robot_id on its head textPosX = 0.15 textPosY = 0.15 textPoint = drawPoint + self.convertToDrawWorld(textPosY, textPosY) painter.drawText(textPoint, str(robot_id)) def drawCoordinateText(self, painter): mouse = self._current_mouse_pos mouse = self.convertToRealWorld(mouse.x(), mouse.y()) text = "(" + str(round(mouse.x(),2)) + ", " + str(round(mouse.y(),2)) + ")" draw_pos = self.convertToDrawWorld(mouse.x(), mouse.y()) painter.setPen(Qt.black) painter.drawText(draw_pos, text) mouse = self._current_mouse_pos mouse = self.convertToRealWorld(mouse.x(), mouse.y() + 20) draw_pos = self.convertToDrawWorld(mouse.x(), mouse.y()) painter.setPen(Qt.black) painter.drawText(draw_pos, self.refereeBranch.data) def drawPosReplacement(self, painter): startPos = self.convertToRealWorld( self.clickPoint.x(), self.clickPoint.y()) currentPos = self.convertToRealWorld( self._current_mouse_pos.x(), self._current_mouse_pos.y()) startPoint = self.convertToDrawWorld(startPos.x(), startPos.y()) currentPoint = self.convertToDrawWorld(currentPos.x(), currentPos.y()) painter.setPen(QPen(Qt.red,2)) painter.drawLine(startPoint, currentPoint) def drawVelReplacement(self, painter): ball_odom = self.ballOdom.pose.pose.position ball_pos = QPointF(ball_odom.x, ball_odom.y) currentPos = self.convertToRealWorld( self._current_mouse_pos.x(), self._current_mouse_pos.y()) velocity = self._calcuReplaceVelocity(ball_pos, currentPos) ballPoint = self.convertToDrawWorld(ball_pos.x(), ball_pos.y()) currentPoint = self.convertToDrawWorld(currentPos.x(), currentPos.y()) painter.setPen(QPen(Qt.blue,2)) painter.drawLine(ballPoint, currentPoint) text = "[" + str(round(velocity.x(),2)) + ", " + str(round(velocity.y(),2)) + "]" painter.setPen(Qt.black) painter.drawText(currentPoint, text) def drawAngleReplacement(self, painter): robot_pos = QPointF() if self._replace_is_friend: robot_pos.setX(self.friendOdoms[self._replace_id].pose.pose.position.x) robot_pos.setY(self.friendOdoms[self._replace_id].pose.pose.position.y) else: robot_pos.setX(self.enemyOdoms[self._replace_id].pose.pose.position.x) robot_pos.setY(self.enemyOdoms[self._replace_id].pose.pose.position.y) currentPos = self.convertToRealWorld( self._current_mouse_pos.x(), self._current_mouse_pos.y()) angle = math.degrees(self._to_angle(robot_pos, currentPos)) robotPoint = self.convertToDrawWorld(robot_pos.x(), robot_pos.y()) currentPoint = self.convertToDrawWorld(currentPos.x(), currentPos.y()) painter.setPen(QPen(Qt.blue,2)) painter.drawLine(robotPoint, currentPoint) text = "[" + str(round(angle,2)) + "]" painter.setPen(Qt.black) painter.drawText(currentPoint, text) def _to_euler(self, orientation): # euler_from_quaternion does not support geometry_msgs:Quaternion euler = tf.transformations.euler_from_quaternion( (orientation.x, orientation.y, orientation.z, orientation.w)) return euler def _to_angle(self, from_pos, to_pos): diff_pos = to_pos - from_pos return math.atan2(diff_pos.y(), diff_pos.x())
class PaintWidget(QWidget): def __init__(self, parent=None): super(PaintWidget, self).__init__(parent) self.CW = ConstWorld() self.scaleOnField = 1.0 self.fieldHeight = 0.0 self.fieldWidth = 0.0 self.rotatingWorld = False self.trans = QPointF(0.0, 0.0) # 慢性的なトランス self.mouseTrans = QPointF(0.0, 0.0) # マウス操作で発生する一時的なトランス self.scale = QPointF(1.0, 1.0) self.clickPoint = QPointF(0.0, 0.0) self.friendDrawColor = Qt.cyan self.enemyDrawColor = Qt.yellow self.friend_color = rospy.get_param("/friend_color", "blue") if self.friend_color != "blue": self.friendDrawColor = Qt.yellow self.enemyDrawColor = Qt.cyan self.targetPosDrawColor = QColor(102, 0, 255, 100) self.ballOdom = Odometry() self.sub_ballPosition = rospy.Subscriber("/ball_observer/estimation", Odometry, self.callbackBallOdom) self.friendsIDArray = UIntArray() self.sub_friendsID = rospy.Subscriber("/existing_friends_id", UIntArray, self.callbackFriendsID) self.friendOdoms = [Odometry()] * 12 self.sub_friendOdoms = [] self.enemyIDArray = UIntArray() self.sub_enemiesID = rospy.Subscriber("/existing_enemies_id", UIntArray, self.callbackEnemiesID) self.enemyOdoms = [Odometry()] * 12 self.sub_enemyOdoms = [] self.targetPositions = [PoseStamped()] * 12 self.sub_targetPositions = [] self.targetVelocities = [TwistStamped()] * 12 self.sub_targetVelocities = [] self.targetIsPosition = [False] * 12 self.avoidPoints = [Point()] * 12 self.sub_avoidPoints = [] for i in xrange(12): strID = str(i) topicFriend = "/robot_" + strID + "/odom" topicEnemy = "/enemy_" + strID + "/odom" topicPosition = "/robot_" + strID + "/move_base_simple/goal" topicVelocity = "/robot_" + strID + "/move_base_simple/target_velocity" topicAvoidPoint = "/robot_" + strID + "/avoid_point" self.sub_friendOdoms.append( rospy.Subscriber(topicFriend, Odometry, self.callbackFriendOdom, callback_args=i)) self.sub_enemyOdoms.append( rospy.Subscriber(topicEnemy, Odometry, self.callbackEnemiesOdom, callback_args=i)) self.sub_targetPositions.append( rospy.Subscriber(topicPosition, PoseStamped, self.callbackTargetPosition, callback_args=i)) self.sub_targetVelocities.append( rospy.Subscriber(topicVelocity, TwistStamped, self.callbackTargetVelocity, callback_args=i)) self.sub_avoidPoints.append( rospy.Subscriber(topicAvoidPoint, Point, self.callbackAvoidPoint, callback_args=i)) def callbackBallOdom(self, msg): self.ballOdom = msg # self.update() def callbackFriendsID(self, msg): self.friendsIDArray = msg # self.update() def callbackFriendOdom(self, msg, robot_id): self.friendOdoms[robot_id] = msg def callbackEnemiesID(self, msg): self.enemyIDArray = msg def callbackEnemiesOdom(self, msg, robot_id): self.enemyOdoms[robot_id] = msg def callbackTargetPosition(self, msg, robot_id): self.targetPositions[robot_id] = msg self.targetIsPosition[robot_id] = True def callbackTargetVelocity(self, msg, robot_id): self.targetVelocities[robot_id] = msg self.targetIsPosition[robot_id] = False def callbackAvoidPoint(self, msg, robot_id): self.avoidPoints[robot_id] = msg def mousePressEvent(self, event): if event.buttons() == Qt.LeftButton: self.clickPoint = event.posF() elif event.buttons() == Qt.RightButton: self.resetPainterState() self.update() def mouseMoveEvent(self, event): if event.buttons() == Qt.LeftButton: pos = event.posF() self.mouseTrans = (pos - self.clickPoint) / self.scale.x() self.update() def mouseReleaseEvent(self, event): self.trans += self.mouseTrans self.mouseTrans = QPointF(0.0, 0.0) self.update() def wheelEvent(self, event): # マウスのホイール操作でスケールを変える if event.delta() > 0: s = self.scale.x() self.scale.setX(s + 0.1) self.scale.setY(s + 0.1) else: s = self.scale.x() if s > 0.2: self.scale.setX(s - 0.1) self.scale.setY(s - 0.1) self.update() def resizeEvent(self, event): # widgetのサイズ変更によるイベント self.updateDrawState() def paintEvent(self, event): painter = QPainter(self) # 描画の中心をWidgetの中心に持ってくる cx = float(self.width()) * 0.5 cy = float(self.height()) * 0.5 painter.translate(cx, cy) # これ以降にトランスとスケール操作を持ってくる painter.scale(self.scale.x(), self.scale.y()) painter.translate(self.trans + self.mouseTrans) if self.rotatingWorld == True: painter.rotate(-90) # これ以降に描きたいものを重ねていく self.drawField(painter) self.drawTargets(painter) self.drawAvoidPoints(painter) self.drawFriends(painter) self.drawEnemis(painter) self.drawBallVelocity(painter) self.drawBall(painter) def resetPainterState(self): self.trans = QPointF(1.0, 1.0) self.mouseTrans = QPointF(0.0, 0.0) self.scale = QPointF(1.0, 1.0) def updateDrawState(self): # Widgetのサイズに合わせて、描くフィールドのサイズを変える # 描画の回転判断もしてくれるすぐれもの widgetHeight = float(self.height()) widgetWidth = float(self.width()) w_per_h = widgetWidth / widgetHeight if w_per_h >= self.CW.FIELD_W_PER_H: # Widgetが横長のとき self.fieldHeight = widgetHeight self.fieldWidth = widgetHeight * self.CW.FIELD_W_PER_H self.rotatingWorld = False elif w_per_h <= self.CW.FIELD_H_PER_W: # Widgetが縦長のとき self.fieldHeight = widgetWidth self.fieldWidth = widgetWidth * self.CW.FIELD_W_PER_H self.rotatingWorld = True else: # 描画回転にヒステリシス性をもたせる if self.rotatingWorld == True: self.fieldHeight = widgetHeight * self.CW.FIELD_H_PER_W self.fieldWidth = widgetHeight else: self.fieldHeight = widgetWidth * self.CW.FIELD_H_PER_W self.fieldWidth = widgetWidth self.scaleOnField = self.fieldWidth / self.CW.FIELD_WIDTH def convertToDrawWorld(self, x, y): drawX = x * self.scaleOnField drawY = -y * self.scaleOnField point = QPointF(drawX, drawY) return point def drawField(self, painter): # draw green surface rectangle painter.setPen(Qt.black) painter.setBrush(Qt.green) rx = -self.fieldWidth * 0.5 ry = -self.fieldHeight * 0.5 rect = QRectF(rx, ry, self.fieldWidth, self.fieldHeight) painter.drawRect(rect) # draw wall rectangle painter.setPen(QPen(Qt.black, 3)) painter.setBrush(Qt.NoBrush) sizeX = self.CW.WALL_WIDTH * self.scaleOnField sizeY = self.CW.WALL_HEIGHT * self.scaleOnField rx = -sizeX * 0.5 ry = -sizeY * 0.5 rect = QRectF(rx, ry, sizeX, sizeY) painter.drawRect(rect) # draw center circle painter.setPen(QPen(Qt.white, 2)) point = self.convertToDrawWorld(0.0, 0.0) size = self.CW.FIELD_CENTER_RADIUS * self.scaleOnField painter.drawEllipse(point, size, size) # draw play field rectangle sizeX = self.CW.PLAY_FIELD_WIDTH * self.scaleOnField sizeY = self.CW.PLAY_FIELD_HEIGHT * self.scaleOnField rx = -sizeX * 0.5 ry = -sizeY * 0.5 rect = QRectF(rx, ry, sizeX, sizeY) painter.drawRect(rect) # draw mid-line point1 = self.convertToDrawWorld(-self.CW.PLAY_FIELD_WIDTH * 0.5, 0) point2 = self.convertToDrawWorld(self.CW.PLAY_FIELD_WIDTH * 0.5, 0) painter.drawLine(point1, point2) # draw center line point1 = self.convertToDrawWorld(0, self.CW.PLAY_FIELD_HEIGHT * 0.5) point2 = self.convertToDrawWorld(0, -self.CW.PLAY_FIELD_HEIGHT * 0.5) painter.drawLine(point1, point2) # draw streach line x = self.CW.PLAY_FIELD_WIDTH * 0.5 - self.CW.FIELD_DEFENCE_RADIUS point1 = self.convertToDrawWorld(x, self.CW.FIELD_STREACH * 0.5) point2 = self.convertToDrawWorld(x, -self.CW.FIELD_STREACH * 0.5) painter.drawLine(point1, point2) x *= -1.0 point1 = self.convertToDrawWorld(x, self.CW.FIELD_STREACH * 0.5) point2 = self.convertToDrawWorld(x, -self.CW.FIELD_STREACH * 0.5) painter.drawLine(point1, point2) # draw defence arc sizeX = self.CW.FIELD_DEFENCE_RADIUS * 2.0 * self.scaleOnField sizeY = self.CW.FIELD_DEFENCE_RADIUS * 2.0 * self.scaleOnField rx = self.CW.PLAY_FIELD_WIDTH * 0.5 - self.CW.FIELD_DEFENCE_RADIUS ry = self.CW.FIELD_STREACH * 0.5 + self.CW.FIELD_DEFENCE_RADIUS rx *= self.scaleOnField ry *= self.scaleOnField ry *= -1.0 rect = QRectF(rx, ry, sizeX, sizeY) startAngle = 90 * 16 spanAngle = 90 * 16 painter.drawArc(rect, startAngle, spanAngle) ry = self.CW.FIELD_STREACH * 0.5 - self.CW.FIELD_DEFENCE_RADIUS ry *= self.scaleOnField rect = QRectF(rx, ry, sizeX, sizeY) startAngle = 180 * 16 spanAngle = 90 * 16 painter.drawArc(rect, startAngle, spanAngle) rx = -self.CW.FIELD_WIDTH - self.CW.FIELD_DEFENCE_RADIUS ry = self.CW.FIELD_STREACH * 0.5 + self.CW.FIELD_DEFENCE_RADIUS rx *= self.scaleOnField ry *= self.scaleOnField rect = QRectF(rx, ry, sizeX, sizeY) startAngle = 180 * 16 spanAngle = 90 * 16 painter.drawArc(rect, startAngle, spanAngle) def drawBall(self, painter): posX = self.ballOdom.pose.pose.position.x posY = self.ballOdom.pose.pose.position.y point = self.convertToDrawWorld(posX, posY) size = self.CW.BALL_RADIUS * self.scaleOnField painter.setPen(Qt.black) painter.setBrush(Qt.red) painter.drawEllipse(point, size, size) def drawBallVelocity(self, painter): ballPos = self.ballOdom.pose.pose.position ballVel = self.ballOdom.twist.twist.linear if math.hypot(ballVel.x, ballVel.y) < 1.0: return angleOfSpeed = math.atan2(ballVel.y, ballVel.x) paintDist = 10.0 velPosX = paintDist * math.cos(angleOfSpeed) + ballPos.x velPosY = paintDist * math.sin(angleOfSpeed) + ballPos.y ballPosPoint = self.convertToDrawWorld(ballPos.x, ballPos.y) velPosPoint = self.convertToDrawWorld(velPosX, velPosY) painter.setPen(QPen(QColor(102, 0, 255), 2)) painter.drawLine(ballPosPoint, velPosPoint) def drawFriends(self, painter): for robot_id in self.friendsIDArray.data: self.drawRobot(painter, robot_id, self.friendOdoms[robot_id], self.friendDrawColor) def drawEnemis(self, painter): for robot_id in self.enemyIDArray.data: self.drawRobot(painter, robot_id, self.enemyOdoms[robot_id], self.enemyDrawColor) def drawRobot(self, painter, robot_id, odom, color): # draw robot body on its position posX = odom.pose.pose.position.x posY = odom.pose.pose.position.y point = self.convertToDrawWorld(posX, posY) size = self.CW.ROBOT_RADIUS * self.scaleOnField painter.setPen(Qt.black) painter.setBrush(color) painter.drawEllipse(point, size, size) # draw robot angle on its body orientation = odom.pose.pose.orientation # euler_from_quaternion does not support geometry_msgs:Quaternion euler = tf.transformations.euler_from_quaternion( (orientation.x, orientation.y, orientation.z, orientation.w)) # euler <- (roll, pitch, yaw) linePosX = self.CW.ROBOT_RADIUS * math.cos(euler[2]) linePosY = self.CW.ROBOT_RADIUS * math.sin(euler[2]) linePoint = point + self.convertToDrawWorld(linePosX, linePosY) painter.drawLine(point, linePoint) # draw robot_id on its head textPosX = 0.15 textPosY = 0.15 textPoint = point + self.convertToDrawWorld(textPosY, textPosY) painter.drawText(textPoint, str(robot_id)) def drawTargets(self, painter): for robot_id in self.friendsIDArray.data: if self.targetIsPosition[robot_id] == True: self.drawTargetPosition(painter, robot_id, self.targetPositions[robot_id]) else: self.drawTargetVelocity(painter, robot_id, self.targetVelocities[robot_id]) def drawTargetPosition(self, painter, robot_id, positionStamped): posX = positionStamped.pose.position.x posY = positionStamped.pose.position.y point = self.convertToDrawWorld(posX, posY) size = self.CW.ROBOT_RADIUS * self.scaleOnField painter.setPen(Qt.black) painter.setBrush(self.targetPosDrawColor) painter.drawEllipse(point, size, size) orientation = positionStamped.pose.orientation # euler_from_quaternion does not support geometry_msgs:Quaternion euler = tf.transformations.euler_from_quaternion( (orientation.x, orientation.y, orientation.z, orientation.w)) # euler <- (roll, pitch, yaw) linePosX = self.CW.ROBOT_RADIUS * math.cos(euler[2]) linePosY = self.CW.ROBOT_RADIUS * math.sin(euler[2]) linePoint = point + self.convertToDrawWorld(linePosX, linePosY) painter.drawLine(point, linePoint) # draw robot_id on its head textPosX = 0.15 textPosY = 0.15 textPoint = point + self.convertToDrawWorld(textPosY, textPosY) painter.drawText(textPoint, str(robot_id)) def drawTargetVelocity(self, painter, robot_id, twistStamped): odom = self.friendOdoms[robot_id] posX = odom.pose.pose.position.x posY = odom.pose.pose.position.y point = self.convertToDrawWorld(posX, posY) # draw robot_id on its head textPosX = 0.15 textPosY = -0.15 textPoint = point + self.convertToDrawWorld(textPosY, textPosY) velX = twistStamped.twist.linear.x velY = twistStamped.twist.linear.y velZ = twistStamped.twist.angular.z text = "(" + str(velX) + ", " + str(velY) + ", " + str(velZ) + ")" painter.setPen(Qt.red) painter.drawText(textPoint, text) def drawAvoidPoints(self, painter): for robot_id in self.friendsIDArray.data: self.drawAvoidPoint(painter, robot_id, self.avoidPoints[robot_id]) def drawAvoidPoint(self, painter, robot_id, point): drawPoint = self.convertToDrawWorld(point.x, point.y) size = self.CW.ROBOT_RADIUS * self.scaleOnField painter.setPen(Qt.black) painter.setBrush(Qt.red) painter.drawEllipse(drawPoint, size, size) # draw robot_id on its head textPosX = 0.15 textPosY = 0.15 textPoint = drawPoint + self.convertToDrawWorld(textPosY, textPosY) painter.drawText(textPoint, str(robot_id))
class PaintWidget(QWidget): def __init__(self, parent=None): super(PaintWidget, self).__init__(parent) self._WHITE_LINE_THICKNESS = 2 # 白線の太さ self._ZOOM_RATE = 0.1 # 拡大縮小率 self._SCALE_LIMIT = 0.2 # 縮小率の限界値 self._ALPHA_DETECTED = 255 # 検出できたロボット・ボールの透明度 self._ALPHA_NOT_DETECTED = 127 # 検出できなかったロボット・ボールの透明度 self._COLOR_BALL = QColor(Qt.red) self._COLOR_ROBOT = { 'blue': QColor(Qt.cyan), 'yellow': QColor(Qt.yellow) } self._ID_POS = (0.15, 0.15) # IDを描画するロボット中心からの位置 self._FLAG_POS = (0.15, 0) # control_targetのフラグを描画するロボット中心からの位置 # Replace self._REPLACE_CLICK_POS_THRESHOLD = 0.1 self._REPLACE_CLICK_VEL_ANGLE_THRESHOLD = self._REPLACE_CLICK_POS_THRESHOLD + 0.1 self._REPLACE_BALL_VELOCITY_GAIN = 3.0 self._REPLACE_MAX_BALL_VELOCITY = 8.0 self._BALL_RADIUS = rospy.get_param('consai2_description/ball_radius', 0.0215) self._ROBOT_RADIUS = rospy.get_param( 'consai2_description/robot_radius', 0.09) self._MAX_ID = rospy.get_param('consai2_description/max_id', 15) self._SIDE = rospy.get_param('consai2_description/our_side', 'left') # チームサイドの反転 self._invert_side = False if self._SIDE != 'left': self._invert_side = True # GUIパラメータ self._trans = QPointF(0.0, 0.0) # x, y方向の移動 self._mouse_trans = QPointF(0.0, 0.0) # マウス操作による移動 self._scale = QPointF(1.0, 1.0) # 拡大, 縮小 self._do_rotate_view = False # 90度回転判定 self._view_height = self.height() # 描画サイズ(縦) self._view_width = self.width() # 描画サイズ(横) self._scale_field_to_view = 1.0 # フィールドから描画領域に縮小するスケール self._click_point = QPointF(0.0, 0.0) # マウスでクリックした描画座標 self._current_mouse_pos = QPointF(0.0, 0.0) # マウスカーソル位置 self._replace_func = None self._replace_id = 0 self._replace_is_yellow = False self._do_replacement = False self._replacement_target = { 'ball_pos': False, 'ball_vel': False, 'robot_pos': False, 'robot_angle': False } # フィールド形状 # raw_vision_geometryの値で更新される self._field_length = 9.0 self._field_width = 6.0 self._field_goal_width = 1.0 self._field_goal_depth = 0.2 self._field_boundary_width = 0.3 self._field_lines = [] self._field_arcs = [] # ジョイスティック情報 self._joy_target = ControlTarget() # ロボット・ボール情報 self._ball_info = BallInfo() self._robot_info = {'blue': [], 'yellow': []} # レフェリー情報 self._decoded_referee = None # Publisher self._pub_replace = rospy.Publisher('sim_sender/replacements', Replacements, queue_size=1) # Subscribers self._sub_decoded_referee = rospy.Subscriber( 'referee_wrapper/decoded_referee', DecodedReferee, self._callback_referee, queue_size=1) self._sub_geometry = rospy.Subscriber( 'vision_receiver/raw_vision_geometry', VisionGeometry, self._callback_geometry, queue_size=1) self._sub_ball_info = rospy.Subscriber('vision_wrapper/ball_info', BallInfo, self._callback_ball_info, queue_size=1) self._sub_joy_target = rospy.Subscriber('consai2_examples/joy_target', ControlTarget, self._callback_joy_target, queue_size=1) self._subs_robot_info = {'blue': [], 'yellow': []} self._control_targets = {'blue': [], 'yellow': []} self._subs_control_target = {'blue': [], 'yellow': []} for robot_id in range(self._MAX_ID + 1): self._robot_info['blue'].append(RobotInfo()) self._robot_info['yellow'].append(RobotInfo()) self._control_targets['blue'].append(ControlTarget()) self._control_targets['yellow'].append(ControlTarget()) # 末尾に16進数の文字列をつける topic_id = hex(robot_id)[2:] topic_name = 'vision_wrapper/robot_info_blue_' + topic_id self._subs_robot_info['blue'].append( rospy.Subscriber(topic_name, RobotInfo, self._callback_blue_info, callback_args=robot_id)) topic_name = 'vision_wrapper/robot_info_yellow_' + topic_id self._subs_robot_info['yellow'].append( rospy.Subscriber(topic_name, RobotInfo, self._callback_yellow_info, callback_args=robot_id)) topic_name = 'consai2_game/control_target_blue_' + topic_id self._subs_control_target['blue'].append( rospy.Subscriber(topic_name, ControlTarget, self._callback_blue_target, callback_args=robot_id)) topic_name = 'consai2_game/control_target_blue_' + topic_id self._subs_control_target['blue'].append( rospy.Subscriber(topic_name, ControlTarget, self._callback_blue_target, callback_args=robot_id)) topic_name = 'consai2_game/control_target_yellow_' + topic_id self._subs_control_target['yellow'].append( rospy.Subscriber(topic_name, ControlTarget, self._callback_yellow_target, callback_args=robot_id)) # Configs # This function enables mouse tracking without pressing mouse button self.setMouseTracking(True) def _callback_geometry(self, msg): # フィールド形状を更新 if msg.field_length: self._field_length = msg.field_length if msg.field_width: self._field_width = msg.field_width if msg.goal_width: self._field_goal_width = msg.goal_width if msg.goal_depth: self._field_goal_depth = msg.goal_depth if msg.boundary_width: self._field_boundary_width = msg.boundary_width if msg.field_lines: self._field_lines = [] for line in msg.field_lines: self._field_lines.append({ "name": line.name, "p1_x": line.p1_x, "p1_y": line.p1_y, "p2_x": line.p2_x, "p2_y": line.p2_y, "thickness": line.thickness }) if msg.field_arcs: self._field_arcs = [] for arc in msg.field_arcs: self._field_arcs.append({ "name": arc.name, "center_x": arc.center_x, "center_y": arc.center_y, "radius": arc.radius, "a1": arc.a1, "a2": arc.a2, "thickness": arc.thickness }) self._resize_draw_world() def _callback_referee(self, msg): self._decoded_referee = msg def _callback_ball_info(self, msg): self._ball_info = msg def _callback_blue_info(self, msg, robot_id): self._robot_info['blue'][robot_id] = msg def _callback_yellow_info(self, msg, robot_id): self._robot_info['yellow'][robot_id] = msg def _callback_blue_target(self, msg, robot_id): self._control_targets['blue'][robot_id] = msg def _callback_yellow_target(self, msg, robot_id): self._control_targets['yellow'][robot_id] = msg def _callback_joy_target(self, msg): self._joy_target = msg def mousePressEvent(self, event): # マウスのドラッグ操作で描画領域を移動する # 右クリックで移動と拡大縮小をリセットする if event.buttons() == Qt.LeftButton: self._click_point = event.localPos() self._do_replacement = self._is_replacement_click( self._click_point) elif event.buttons() == Qt.RightButton: self._reset_painter_status() self.update() def mouseMoveEvent(self, event): # マウスのドラッグ操作で描画領域を移動する # Replacementを行うときは描画領域を移動しない self._current_mouse_pos = event.localPos() if self._do_replacement: pass elif event.buttons() == Qt.LeftButton: self._mouse_trans = (self._current_mouse_pos - self._click_point) / self._scale.x() self.update() def mouseReleaseEvent(self, event): # マウスのドラッグ操作で描画領域を移動する # Replacementを行うときは描画領域を移動しない if self._do_replacement: self._do_replacement = False self._replace_func(event.localPos()) else: self._trans += self._mouse_trans self._mouse_trans = QPointF(0.0, 0.0) self.update() def wheelEvent(self, event): # マウスのホイール操作で描画領域を拡大縮小する s = self._scale.x() if event.angleDelta().y() > 0: self._scale.setX(s + self._ZOOM_RATE) self._scale.setY(s + self._ZOOM_RATE) else: if s > self._SCALE_LIMIT: self._scale.setX(s - self._ZOOM_RATE) self._scale.setY(s - self._ZOOM_RATE) self.update() def paintEvent(self, event): painter = QPainter(self) # 描画の中心をWidgetの中心に持ってくる cx = float(self.width()) * 0.5 cy = float(self.height()) * 0.5 painter.translate(cx, cy) painter.scale(self._scale.x(), self._scale.y()) painter.translate(self._trans + self._mouse_trans) if self._do_rotate_view is True: painter.rotate(-90) # これ以降に書きたいものを重ねる self._draw_field(painter) # Referee情報 self._draw_referee(painter) self._draw_ball(painter) self._draw_ball_velocity(painter) # JoyStick関連 if len(self._joy_target.path) > 0: self._draw_joy_target(painter) self._draw_robots(painter) # grSim Replacement関連 if self._replacement_target['ball_pos'] or self._replacement_target[ 'robot_pos']: self._draw_pos_replacement(painter) self._draw_cursor_coordinate(painter) elif self._replacement_target['ball_vel']: self._draw_vel_replacement(painter) elif self._replacement_target['robot_angle']: self._draw_angle_replacement(painter) else: self._draw_cursor_coordinate(painter) def resizeEvent(self, event): self._resize_draw_world() def _resize_draw_world(self): # Widgetのサイズに合わせて、描くフィールドのサイズを変える # 描画の回転判断もしてくれるすぐれもの # Widgetの縦横比を算出 widget_height = float(self.height()) widget_width = float(self.width()) widget_w_per_h = widget_width / widget_height # Fieldの縦横比を算出 field_width = self._field_length + self._field_boundary_width * 2.0 field_height = self._field_width + self._field_boundary_width * 2.0 field_w_per_h = field_width / field_height field_h_per_w = 1.0 / field_w_per_h if widget_w_per_h >= field_w_per_h: # Widgetが横長のとき self._view_height = widget_height self._view_width = widget_height * field_w_per_h self._do_rotate_view = False elif widget_w_per_h <= field_h_per_w: # Widgetが縦長のとき self._view_height = widget_width self._view_width = widget_width * field_w_per_h self._do_rotate_view = True else: # 描画回転にヒステリシスをもたせる if self._do_rotate_view is True: self._view_height = widget_height * field_h_per_w self._view_width = widget_height else: self._view_height = widget_width * field_h_per_w self._view_width = widget_width self._scale_field_to_view = self._view_width / field_width def _convert_to_view(self, x, y): # フィールド座標系を描画座標系に変換する view_x = x * self._scale_field_to_view view_y = -y * self._scale_field_to_view point = QPointF(view_x, view_y) return point def _convert_to_field(self, x, y): # 描画座標系をフィールド座標系に変換する x /= self._scale.x() y /= self._scale.y() x -= (self._trans.x() + self._mouse_trans.x()) y -= (self._trans.y() + self._mouse_trans.y()) x -= self.width() * 0.5 / self._scale.x() y -= self.height() * 0.5 / self._scale.y() if self._do_rotate_view: x, y = -y, x field_x = x / self._scale_field_to_view field_y = -y / self._scale_field_to_view point = QPointF(field_x, field_y) return point def _reset_painter_status(self): # 描画領域の移動と拡大縮小を初期化する self._trans = QPointF(0.0, 0.0) self._mouse_trans = QPointF(0.0, 0.0) self._scale = QPointF(1.0, 1.0) def _is_replacement_click(self, mouse_pos): # クリックした描画位置にオブジェクトがあればReplacementと判定する # ボールとロボットが近い場合、ボールのReplacementを優先する field_point = self._convert_to_field(mouse_pos.x(), mouse_pos.y()) is_clicked = True result = self._is_ball_clicked(field_point) if result == 'pos': self._replacement_target['ball_pos'] = True self._replace_func = self._replace_ball_pos elif result == 'vel_angle': self._replacement_target['ball_vel'] = True self._replace_func = self._replace_ball_vel else: result, robot_id, is_yellow = self._is_robot_clicked(field_point) self._replace_id = robot_id self._replace_is_yellow = is_yellow if result == 'pos': self._replacement_target['robot_pos'] = True self._replace_func = self._replace_robot_pos elif result == 'vel_angle': self._replacement_target['robot_angle'] = True self._replace_func = self._replace_robot_angle else: is_clicked = False return is_clicked def _is_clicked(self, field_point1, field_point2): # フィールド上のオブジェクトをクリックしたかどうか判定する diff_point = field_point1 - field_point2 diff_norm = math.hypot(diff_point.x(), diff_point.y()) if diff_norm < self._REPLACE_CLICK_POS_THRESHOLD: return 'pos' elif diff_norm < self._REPLACE_CLICK_VEL_ANGLE_THRESHOLD: return 'vel_angle' return False def _is_ball_clicked(self, field_point): # ボールをクリックしたか判定する # ボールが消えていれば判定しない if self._ball_info.disappeared: return False pos_x = self._ball_info.pose.x pos_y = self._ball_info.pose.y ball_pos = QPointF(pos_x, pos_y) return self._is_clicked(field_point, ball_pos) def _is_robot_clicked(self, field_point): # ロボットをクリックしたか判定する # 消えたロボットは対照外 is_clicked = False replace_id = 0 is_yellow = False for robot in self._robot_info['blue']: if robot.disappeared: continue robot_point = QPointF(robot.pose.x, robot.pose.y) is_clicked = self._is_clicked(field_point, robot_point) if is_clicked: is_yellow = False return is_clicked, robot.robot_id, is_yellow for robot in self._robot_info['yellow']: if robot.disappeared: continue robot_point = QPointF(robot.pose.x, robot.pose.y) is_clicked = self._is_clicked(field_point, robot_point) if is_clicked: is_yellow = True return is_clicked, robot.robot_id, is_yellow return is_clicked, replace_id, is_yellow def _replace_ball_pos(self, mouse_pos): # ボール位置のReplacement field_point = self._convert_to_field(mouse_pos.x(), mouse_pos.y()) ball = ReplaceBall() ball.x = field_point.x() ball.y = field_point.y() ball.is_enabled = True if self._invert_side: ball.x *= -1.0 ball.y *= -1.0 replacements = Replacements() replacements.ball = ball self._pub_replace.publish(replacements) self._replacement_target['ball_pos'] = False def _replace_ball_vel(self, mouse_pos): # ボール速度のReplacement ball_point = QPointF(self._ball_info.pose.x, self._ball_info.pose.y) field_point = self._convert_to_field(mouse_pos.x(), mouse_pos.y()) velocity = self._replacement_velocity(ball_point, field_point) ball = ReplaceBall() ball.x = ball_point.x() ball.y = ball_point.y() ball.vx = velocity.x() ball.vy = velocity.y() ball.is_enabled = True if self._invert_side: ball.x *= -1.0 ball.y *= -1.0 ball.vx *= -1.0 ball.vy *= -1.0 replacements = Replacements() replacements.ball = ball self._pub_replace.publish(replacements) self._replacement_target['ball_vel'] = False def _replacement_velocity(self, from_point, to_point): # Replacementのボール速度を計算する diff_point = to_point - from_point diff_norm = math.hypot(diff_point.x(), diff_point.y()) velocity_norm = diff_norm * self._REPLACE_BALL_VELOCITY_GAIN if velocity_norm > self._REPLACE_MAX_BALL_VELOCITY: velocity_norm = self._REPLACE_MAX_BALL_VELOCITY angle = math.atan2(diff_point.y(), diff_point.x()) velocity = QPointF(velocity_norm * math.cos(angle), velocity_norm * math.sin(angle)) return velocity def _replace_robot_pos(self, mouse_pos): # ロボット位置のReplacement field_point = self._convert_to_field(mouse_pos.x(), mouse_pos.y()) # ロボット角度をradiansからdegreesに変換する direction = 0 if self._replace_is_yellow: direction = math.degrees( self._robot_info['yellow'][self._replace_id].pose.theta) else: direction = math.degrees( self._robot_info['blue'][self._replace_id].pose.theta) robot = ReplaceRobot() robot.x = field_point.x() robot.y = field_point.y() robot.dir = direction robot.id = self._replace_id robot.yellowteam = self._replace_is_yellow robot.turnon = True if self._invert_side: robot.x *= -1.0 robot.y *= -1.0 robot.dir += 180 replacements = Replacements() replacements.robots.append(robot) self._pub_replace.publish(replacements) self._replacement_target['robot_pos'] = False def _replace_robot_angle(self, mouse_pos): # ロボット角度のReplacement field_point = self._convert_to_field(mouse_pos.x(), mouse_pos.y()) # ロボット角度をradiansからdegreesに変換する robot_point = QPointF() if self._replace_is_yellow: robot_point = QPointF( self._robot_info['yellow'][self._replace_id].pose.x, self._robot_info['yellow'][self._replace_id].pose.y) else: robot_point = QPointF( self._robot_info['blue'][self._replace_id].pose.x, self._robot_info['blue'][self._replace_id].pose.y) robot = ReplaceRobot() robot.x = robot_point.x() robot.y = robot_point.y() robot.dir = math.degrees(self._to_angle(robot_point, field_point)) robot.id = self._replace_id robot.yellowteam = self._replace_is_yellow robot.turnon = True if self._invert_side: robot.x *= -1.0 robot.y *= -1.0 robot.dir += 180 replacements = Replacements() replacements.robots.append(robot) self._pub_replace.publish(replacements) self._replacement_target['robot_angle'] = False def _draw_field(self, painter): # フィールドの緑と白線を描画する # グリーンカーペットを描画 painter.setPen(Qt.black) painter.setBrush(Qt.green) rect = QRectF(-self._view_width * 0.5, -self._view_height * 0.5, self._view_width, self._view_height) painter.drawRect(rect) # 白線を描画 painter.setPen(QPen(Qt.white, self._WHITE_LINE_THICKNESS)) for line in self._field_lines: p1 = self._convert_to_view(line["p1_x"], line["p1_y"]) p2 = self._convert_to_view(line["p2_x"], line["p2_y"]) painter.drawLine(p1, p2) for arc in self._field_arcs: top_left = self._convert_to_view(arc["center_x"] - arc["radius"], arc["center_y"] + arc["radius"]) size = arc["radius"] * 2.0 * self._scale_field_to_view # angle must be 1/16 degrees order start_angle = math.degrees(arc["a1"]) * 16 end_angle = math.degrees(arc["a2"]) * 16 span_angle = end_angle - start_angle painter.drawArc(top_left.x(), top_left.y(), size, size, start_angle, span_angle) def _draw_ball(self, painter): # ボールを描画する if self._ball_info.disappeared is False: point = self._convert_to_view(self._ball_info.pose.x, self._ball_info.pose.y) size = self._BALL_RADIUS * self._scale_field_to_view ball_color = copy.deepcopy(self._COLOR_BALL) if self._ball_info.detected is False: # ボールを検出してないときは透明度を変える ball_color.setAlpha(self._ALPHA_NOT_DETECTED) painter.setPen(Qt.black) painter.setBrush(ball_color) painter.drawEllipse(point, size, size) def _draw_ball_velocity(self, painter): # ボールの速度方向を描画する VELOCITY_THRESH = 1.0 PAINT_DIST = 10.0 # meters ball_pos = self._ball_info.pose ball_vel = self._ball_info.velocity # 速度が小さければ描画しない if math.hypot(ball_vel.x, ball_vel.y) < VELOCITY_THRESH: return direction = math.atan2(ball_vel.y, ball_vel.x) vel_pos_x = PAINT_DIST * math.cos(direction) + ball_pos.x vel_pos_y = PAINT_DIST * math.sin(direction) + ball_pos.y point1 = self._convert_to_view(ball_pos.x, ball_pos.y) point2 = self._convert_to_view(vel_pos_x, vel_pos_y) painter.setPen(QPen(QColor(102, 0, 255), 2)) painter.drawLine(point1, point2) def _draw_robots(self, painter): # 全てのロボットを描画する for blue in self._robot_info['blue']: self._draw_robot(painter, blue, 'blue') for yellow in self._robot_info['yellow']: self._draw_robot(painter, yellow, 'yellow') def _draw_robot(self, painter, robot, color): # ロボット1台を描画する if robot.disappeared is False: point = self._convert_to_view(robot.pose.x, robot.pose.y) size = self._ROBOT_RADIUS * self._scale_field_to_view robot_color = copy.deepcopy(self._COLOR_ROBOT[color]) if robot.detected is False: # ロボットを検出してないときは透明度を変える robot_color.setAlpha(self._ALPHA_NOT_DETECTED) painter.setPen(Qt.black) painter.setBrush(robot_color) painter.drawEllipse(point, size, size) # ロボット角度 line_x = self._ROBOT_RADIUS * math.cos(robot.pose.theta) line_y = self._ROBOT_RADIUS * math.sin(robot.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.robot_id)) # ControlTarget self._draw_control_target(painter, color, robot) def _draw_cursor_coordinate(self, painter): # マウスカーソルのフィールド座標を描画する 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(), current_pos.y()) text = "(" + str(round(current_pos.x(), 2)) + ", " + str( round(current_pos.y(), 2)) + ")" painter.setPen(Qt.black) painter.drawText(current_point, text) def _draw_pos_replacement(self, painter): # 位置のReplacementを描画する start_pos = self._convert_to_field(self._click_point.x(), self._click_point.y()) current_pos = self._convert_to_field(self._current_mouse_pos.x(), self._current_mouse_pos.y()) start_point = self._convert_to_view(start_pos.x(), start_pos.y()) current_point = self._convert_to_view(current_pos.x(), current_pos.y()) painter.setPen(QPen(Qt.red, 2)) painter.drawLine(start_point, current_point) def _draw_vel_replacement(self, painter): # ボール速度のReplacementを描画する 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(), current_pos.y()) ball_pos = QPointF(self._ball_info.pose.x, self._ball_info.pose.y) ball_point = self._convert_to_view(ball_pos.x(), ball_pos.y()) painter.setPen(QPen(Qt.blue, 2)) painter.drawLine(ball_point, current_point) # 速度の数値を描画する velocity = self._replacement_velocity(ball_pos, current_pos) text = "[" + str(round(velocity.x(), 2)) + ", " + str( round(velocity.y(), 2)) + "]" painter.setPen(Qt.black) painter.drawText(current_point, text) def _draw_angle_replacement(self, painter): # ロボット角度のReplacementを描画する robot_pos = QPointF() if self._replace_is_yellow: robot_pos.setX(self._robot_info['yellow'][self._replace_id].pose.x) robot_pos.setY(self._robot_info['yellow'][self._replace_id].pose.y) else: robot_pos.setX(self._robot_info['blue'][self._replace_id].pose.x) robot_pos.setY(self._robot_info['blue'][self._replace_id].pose.y) robot_point = self._convert_to_view(robot_pos.x(), robot_pos.y()) 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(), current_pos.y()) painter.setPen(QPen(Qt.blue, 2)) painter.drawLine(robot_point, current_point) # 角度の数値を描画する angle = math.degrees(self._to_angle(robot_pos, current_pos)) text = "[" + str(round(angle, 2)) + "]" painter.setPen(Qt.black) painter.drawText(current_point, text) 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 _to_angle(self, from_point, to_point): diff_point = to_point - from_point return math.atan2(diff_point.y(), diff_point.x()) def _draw_joy_target(self, painter): # JoyStickのControlTargetを描画する # 経路の線を描画するための変数 prev_point = None for i, pose in enumerate(self._joy_target.path): point = self._convert_to_view(pose.x, pose.y) size = self._ROBOT_RADIUS * self._scale_field_to_view joy_color = QColor(Qt.magenta) painter.setPen(Qt.black) painter.setBrush(joy_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) # インデックス text_point = point + self._convert_to_view(self._ID_POS[0], self._ID_POS[1]) painter.drawText(text_point, str(i)) # 経路を描画 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 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)