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)
