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 _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 _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 _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 _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 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 _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 _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())
def __init__(self, highlight_level, spline, label_center, label, from_node, to_node, parent=None, penwidth=1, edge_color=None, style='solid'):
super(EdgeItem, self).__init__(highlight_level, parent)
self.from_node = from_node
self.from_node.add_outgoing_edge(self)
self.to_node = to_node
self.to_node.add_incoming_edge(self)
self._default_edge_color = self._COLOR_BLACK
if edge_color is not None:
self._default_edge_color = edge_color
self._default_text_color = self._COLOR_BLACK
self._default_color = self._COLOR_BLACK
self._text_brush = QBrush(self._default_color)
self._shape_brush = QBrush(self._default_color)
if style in ['dashed', 'dotted']:
self._shape_brush = QBrush(Qt.transparent)
self._label_pen = QPen()
self._label_pen.setColor(self._default_text_color)
self._label_pen.setJoinStyle(Qt.RoundJoin)
self._edge_pen = QPen(self._label_pen)
self._edge_pen.setWidth(penwidth)
self._edge_pen.setColor(self._default_edge_color)
self._edge_pen.setStyle(self._qt_pen_styles.get(style, Qt.SolidLine))
self._sibling_edges = set()
self._label = None
if label is not None:
self._label = QGraphicsSimpleTextItem(label)
label_rect = self._label.boundingRect()
label_rect.moveCenter(label_center)
self._label.setPos(label_rect.x(), label_rect.y())
self._label.hoverEnterEvent = self._handle_hoverEnterEvent
self._label.hoverLeaveEvent = self._handle_hoverLeaveEvent
self._label.setAcceptHoverEvents(True)
# spline specification according to http://www.graphviz.org/doc/info/attrs.html#k:splineType
coordinates = spline.split(' ')
# extract optional end_point
end_point = None
if (coordinates[0].startswith('e,')):
parts = coordinates.pop(0)[2:].split(',')
end_point = QPointF(float(parts[0]), -float(parts[1]))
# extract optional start_point
if (coordinates[0].startswith('s,')):
parts = coordinates.pop(0).split(',')
# first point
parts = coordinates.pop(0).split(',')
point = QPointF(float(parts[0]), -float(parts[1]))
path = QPainterPath(point)
while len(coordinates) > 2:
# extract triple of points for a cubic spline
parts = coordinates.pop(0).split(',')
point1 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point2 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point3 = QPointF(float(parts[0]), -float(parts[1]))
path.cubicTo(point1, point2, point3)
self._arrow = None
if end_point is not None:
# draw arrow
self._arrow = QGraphicsPolygonItem()
polygon = QPolygonF()
polygon.append(point3)
offset = QPointF(end_point - point3)
corner1 = QPointF(-offset.y(), offset.x()) * 0.35
corner2 = QPointF(offset.y(), -offset.x()) * 0.35
polygon.append(point3 + corner1)
polygon.append(end_point)
polygon.append(point3 + corner2)
self._arrow.setPolygon(polygon)
self._arrow.hoverEnterEvent = self._handle_hoverEnterEvent
self._arrow.hoverLeaveEvent = self._handle_hoverLeaveEvent
self._arrow.setAcceptHoverEvents(True)
self._path = QGraphicsPathItem()
self._path.setPath(path)
self.addToGroup(self._path)
self.set_node_color()
self.set_label_color()
class QwtDataPlot(Qwt.QwtPlot):
mouseCoordinatesChanged = Signal(QPointF)
_num_value_saved = 1000
_num_values_ploted = 1000
limits_changed = Signal()
def __init__(self, *args):
super(QwtDataPlot, self).__init__(*args)
self.setCanvasBackground(Qt.white)
self.insertLegend(Qwt.QwtLegend(), Qwt.QwtPlot.BottomLegend)
self._curves = {}
# TODO: rejigger these internal data structures so that they're easier
# to work with, and easier to use with set_xlim and set_ylim
# this may also entail rejiggering the _time_axis so that it's
# actually time axis data, or just isn't required any more
# new data structure
self._x_limits = [0.0, 10.0]
self._y_limits = [0.0, 10.0]
# old data structures
self._last_canvas_x = 0
self._last_canvas_y = 0
self._pressed_canvas_y = 0
self._pressed_canvas_x = 0
self._last_click_coordinates = None
marker_axis_y = Qwt.QwtPlotMarker()
marker_axis_y.setLabelAlignment(Qt.AlignRight | Qt.AlignTop)
marker_axis_y.setLineStyle(Qwt.QwtPlotMarker.HLine)
marker_axis_y.setYValue(0.0)
marker_axis_y.attach(self)
self._picker = Qwt.QwtPlotPicker(
Qwt.QwtPlot.xBottom, Qwt.QwtPlot.yLeft, Qwt.QwtPicker.PolygonSelection,
Qwt.QwtPlotPicker.PolygonRubberBand, Qwt.QwtPicker.AlwaysOn, self.canvas()
)
self._picker.setRubberBandPen(QPen(Qt.blue))
self._picker.setTrackerPen(QPen(Qt.blue))
# Initialize data
self.rescale()
#self.move_canvas(0, 0)
self.canvas().setMouseTracking(True)
self.canvas().installEventFilter(self)
def eventFilter(self, _, event):
if event.type() == QEvent.MouseButtonRelease:
x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x())
y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y())
self._last_click_coordinates = QPointF(x, y)
self.limits_changed.emit()
elif event.type() == QEvent.MouseMove:
x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x())
y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y())
coords = QPointF(x, y)
if self._picker.isActive() and self._last_click_coordinates is not None:
toolTip = 'origin x: %.5f, y: %.5f' % (self._last_click_coordinates.x(), self._last_click_coordinates.y())
delta = coords - self._last_click_coordinates
toolTip += '\ndelta x: %.5f, y: %.5f\nlength: %.5f' % (delta.x(), delta.y(), QVector2D(delta).length())
else:
toolTip = 'buttons\nleft: measure\nmiddle: move\nright: zoom x/y\nwheel: zoom y'
self.setToolTip(toolTip)
self.mouseCoordinatesChanged.emit(coords)
return False
def log(self, level, message):
# self.emit(SIGNAL('logMessage'), level, message)
pass
def resizeEvent(self, event):
Qwt.QwtPlot.resizeEvent(self, event)
self.rescale()
def add_curve(self, curve_id, curve_name, curve_color=QColor(Qt.blue), markers_on=False):
curve_id = str(curve_id)
if curve_id in self._curves:
return
curve_object = Qwt.QwtPlotCurve(curve_name)
curve_object.attach(self)
curve_object.setPen(curve_color)
if markers_on:
curve_object.setSymbol(Qwt.QwtSymbol(Qwt.QwtSymbol.Ellipse, QBrush(curve_color), QPen(Qt.black), QSize(4,4)))
self._curves[curve_id] = curve_object
def remove_curve(self, curve_id):
curve_id = str(curve_id)
if curve_id in self._curves:
self._curves[curve_id].hide()
self._curves[curve_id].attach(None)
del self._curves[curve_id]
def set_values(self, curve_id, data_x, data_y):
curve = self._curves[curve_id]
curve.setData(data_x, data_y)
def redraw(self):
self.replot()
# ----------------------------------------------
# begin qwtplot internal methods
# ----------------------------------------------
def rescale(self):
self.setAxisScale(Qwt.QwtPlot.yLeft,
self._y_limits[0],
self._y_limits[1])
self.setAxisScale(Qwt.QwtPlot.xBottom,
self._x_limits[0],
self._x_limits[1])
self._canvas_display_height = self._y_limits[1] - self._y_limits[0]
self._canvas_display_width = self._x_limits[1] - self._x_limits[0]
self.redraw()
def rescale_axis_x(self, delta__x):
"""
add delta_x to the length of the x axis
"""
x_width = self._x_limits[1] - self._x_limits[0]
x_width += delta__x
self._x_limits[1] = x_width + self._x_limits[0]
self.rescale()
def scale_axis_y(self, max_value):
"""
set the y axis height to max_value, about the current center
"""
canvas_display_height = max_value
canvas_offset_y = (self._y_limits[1] + self._y_limits[0])/2.0
y_lower_limit = canvas_offset_y - (canvas_display_height / 2)
y_upper_limit = canvas_offset_y + (canvas_display_height / 2)
self._y_limits = [y_lower_limit, y_upper_limit]
self.rescale()
def move_canvas(self, delta_x, delta_y):
"""
move the canvas by delta_x and delta_y in SCREEN COORDINATES
"""
canvas_offset_x = delta_x * self._canvas_display_width / self.canvas().width()
canvas_offset_y = delta_y * self._canvas_display_height / self.canvas().height()
self._x_limits = [ l + canvas_offset_x for l in self._x_limits]
self._y_limits = [ l + canvas_offset_y for l in self._y_limits]
self.rescale()
def mousePressEvent(self, event):
self._last_canvas_x = event.x() - self.canvas().x()
self._last_canvas_y = event.y() - self.canvas().y()
self._pressed_canvas_y = event.y() - self.canvas().y()
def mouseMoveEvent(self, event):
canvas_x = event.x() - self.canvas().x()
canvas_y = event.y() - self.canvas().y()
if event.buttons() & Qt.MiddleButton: # middle button moves the canvas
delta_x = self._last_canvas_x - canvas_x
delta_y = canvas_y - self._last_canvas_y
self.move_canvas(delta_x, delta_y)
elif event.buttons() & Qt.RightButton: # right button zooms
zoom_factor = max(-0.6, min(0.6, (self._last_canvas_y - canvas_y) / 20.0 / 2.0))
delta_y = (self.canvas().height() / 2.0) - self._pressed_canvas_y
self.move_canvas(0, zoom_factor * delta_y * 1.0225)
self.scale_axis_y(max(0.005, self._canvas_display_height - (zoom_factor * self._canvas_display_height)))
self.rescale_axis_x(self._last_canvas_x - canvas_x)
self._last_canvas_x = canvas_x
self._last_canvas_y = canvas_y
def wheelEvent(self, event): # mouse wheel zooms the y-axis
# y position of pointer in graph coordinates
canvas_y = event.y() - self.canvas().y()
try:
delta = event.angleDelta().y()
except AttributeError:
delta = event.delta()
zoom_factor = max(-0.6, min(0.6, (delta / 120) / 6.0))
delta_y = (self.canvas().height() / 2.0) - canvas_y
self.move_canvas(0, zoom_factor * delta_y * 1.0225)
self.scale_axis_y(max(0.0005, self._canvas_display_height - zoom_factor * self._canvas_display_height))
self.limits_changed.emit()
def vline(self, x, color):
qWarning("QwtDataPlot.vline is not implemented yet")
def set_xlim(self, limits):
self.setAxisScale(Qwt.QwtPlot.xBottom, limits[0], limits[1])
self._x_limits = limits
def set_ylim(self, limits):
self.setAxisScale(Qwt.QwtPlot.yLeft, limits[0], limits[1])
self._y_limits = limits
def get_xlim(self):
return self._x_limits
def get_ylim(self):
return self._y_limits
def __init__(self,
highlight_level,
spline,
label_center,
label,
from_node,
to_node,
parent=None,
penwidth=1,
edge_color=None,
style='solid'):
super(EdgeItem, self).__init__(highlight_level, parent)
self.from_node = from_node
self.from_node.add_outgoing_edge(self)
self.to_node = to_node
self.to_node.add_incoming_edge(self)
self._default_edge_color = self._COLOR_BLACK
if edge_color is not None:
self._default_edge_color = edge_color
self._default_text_color = self._COLOR_BLACK
self._default_color = self._COLOR_BLACK
self._text_brush = QBrush(self._default_color)
self._shape_brush = QBrush(self._default_color)
if style in ['dashed', 'dotted']:
self._shape_brush = QBrush(Qt.transparent)
self._label_pen = QPen()
self._label_pen.setColor(self._default_text_color)
self._label_pen.setJoinStyle(Qt.RoundJoin)
self._edge_pen = QPen(self._label_pen)
self._edge_pen.setWidth(penwidth)
self._edge_pen.setColor(self._default_edge_color)
self._edge_pen.setStyle(self._qt_pen_styles.get(style, Qt.SolidLine))
self._sibling_edges = set()
self._label = None
if label is not None:
self._label = QGraphicsSimpleTextItem(label)
label_rect = self._label.boundingRect()
label_rect.moveCenter(label_center)
self._label.setPos(label_rect.x(), label_rect.y())
self._label.hoverEnterEvent = self._handle_hoverEnterEvent
self._label.hoverLeaveEvent = self._handle_hoverLeaveEvent
self._label.setAcceptHoverEvents(True)
# spline specification according to http://www.graphviz.org/doc/info/attrs.html#k:splineType
coordinates = spline.split(' ')
# extract optional end_point
end_point = None
if (coordinates[0].startswith('e,')):
parts = coordinates.pop(0)[2:].split(',')
end_point = QPointF(float(parts[0]), -float(parts[1]))
# extract optional start_point
if (coordinates[0].startswith('s,')):
parts = coordinates.pop(0).split(',')
# first point
parts = coordinates.pop(0).split(',')
point = QPointF(float(parts[0]), -float(parts[1]))
path = QPainterPath(point)
while len(coordinates) > 2:
# extract triple of points for a cubic spline
parts = coordinates.pop(0).split(',')
point1 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point2 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point3 = QPointF(float(parts[0]), -float(parts[1]))
path.cubicTo(point1, point2, point3)
self._arrow = None
if end_point is not None:
# draw arrow
self._arrow = QGraphicsPolygonItem()
polygon = QPolygonF()
polygon.append(point3)
offset = QPointF(end_point - point3)
corner1 = QPointF(-offset.y(), offset.x()) * 0.35
corner2 = QPointF(offset.y(), -offset.x()) * 0.35
polygon.append(point3 + corner1)
polygon.append(end_point)
polygon.append(point3 + corner2)
self._arrow.setPolygon(polygon)
self._arrow.hoverEnterEvent = self._handle_hoverEnterEvent
self._arrow.hoverLeaveEvent = self._handle_hoverLeaveEvent
self._arrow.setAcceptHoverEvents(True)
self._path = QGraphicsPathItem()
self._path.setPath(path)
self.addToGroup(self._path)
self.set_node_color()
self.set_label_color()
class QwtDataPlot(Qwt.QwtPlot):
mouseCoordinatesChanged = Signal(QPointF)
_colors = [
Qt.red, Qt.blue, Qt.magenta, Qt.cyan, Qt.green, Qt.darkYellow,
Qt.black, Qt.darkRed, Qt.gray, Qt.darkCyan
]
_num_value_saved = 1000
_num_values_ploted = 1000
def __init__(self, *args):
super(QwtDataPlot, self).__init__(*args)
self.setCanvasBackground(Qt.white)
self.insertLegend(Qwt.QwtLegend(), Qwt.QwtPlot.BottomLegend)
self._curves = {}
self._data_offset_x = 0
self._canvas_offset_x = 0
self._canvas_offset_y = 0
self._last_canvas_x = 0
self._last_canvas_y = 0
self._pressed_canvas_y = 0
self._last_click_coordinates = None
self._color_index = 0
marker_axis_y = Qwt.QwtPlotMarker()
marker_axis_y.setLabelAlignment(Qt.AlignRight | Qt.AlignTop)
marker_axis_y.setLineStyle(Qwt.QwtPlotMarker.HLine)
marker_axis_y.setYValue(0.0)
marker_axis_y.attach(self)
self._picker = Qwt.QwtPlotPicker(Qwt.QwtPlot.xBottom,
Qwt.QwtPlot.yLeft,
Qwt.QwtPicker.PolygonSelection,
Qwt.QwtPlotPicker.PolygonRubberBand,
Qwt.QwtPicker.AlwaysOn, self.canvas())
self._picker.setRubberBandPen(QPen(self._colors[-1]))
self._picker.setTrackerPen(QPen(self._colors[-1]))
# Initialize data
self._time_axis = arange(self._num_values_ploted)
self._canvas_display_height = 1000
self._canvas_display_width = self.canvas().width()
self._data_offset_x = self._num_value_saved - len(self._time_axis)
self.redraw()
self.move_canvas(0, 0)
self.canvas().setMouseTracking(True)
self.canvas().installEventFilter(self)
def eventFilter(self, _, event):
if event.type() == QEvent.MouseButtonRelease:
x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x())
y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y())
self._last_click_coordinates = QPointF(x, y)
elif event.type() == QEvent.MouseMove:
x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x())
y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y())
coords = QPointF(x, y)
if self._picker.isActive(
) and self._last_click_coordinates is not None:
toolTip = 'origin x: %.5f, y: %.5f' % (
self._last_click_coordinates.x(),
self._last_click_coordinates.y())
delta = coords - self._last_click_coordinates
toolTip += '\ndelta x: %.5f, y: %.5f\nlength: %.5f' % (
delta.x(), delta.y(), QVector2D(delta).length())
else:
toolTip = 'buttons\nleft: measure\nmiddle: move\nright: zoom x/y\nwheel: zoom y'
self.setToolTip(toolTip)
self.mouseCoordinatesChanged.emit(coords)
return False
def log(self, level, message):
self.emit(SIGNAL('logMessage'), level, message)
def resizeEvent(self, event):
#super(QwtDataPlot, self).resizeEvent(event)
Qwt.QwtPlot.resizeEvent(self, event)
self.rescale()
def add_curve(self, curve_id, curve_name, values_x, values_y):
curve_id = str(curve_id)
if self._curves.get(curve_id):
return
curve_object = Qwt.QwtPlotCurve(curve_name)
curve_object.attach(self)
curve_object.setPen(
QPen(self._colors[self._color_index % len(self._colors)]))
self._color_index += 1
self._curves[curve_id] = {
'name': curve_name,
'data': zeros(self._num_value_saved),
'object': curve_object,
}
def remove_curve(self, curve_id):
curve_id = str(curve_id)
if curve_id in self._curves:
self._curves[curve_id]['object'].hide()
self._curves[curve_id]['object'].attach(None)
del self._curves[curve_id]['object']
del self._curves[curve_id]
@Slot(str, list, list)
def update_values(self, curve_id, values_x, values_y):
for value_x, value_y in zip(values_x, values_y):
self.update_value(curve_id, value_x, value_y)
@Slot(str, float, float)
def update_value(self, curve_id, value_x, value_y):
curve_id = str(curve_id)
# update data plot
if curve_id in self._curves:
# TODO: use value_x as timestamp
self._curves[curve_id]['data'] = concatenate(
(self._curves[curve_id]['data'][1:],
self._curves[curve_id]['data'][:1]), 1)
self._curves[curve_id]['data'][-1] = float(value_y)
def redraw(self):
for curve_id in self._curves.keys():
self._curves[curve_id]['object'].setData(
self._time_axis, self._curves[curve_id]['data']
[self._data_offset_x:self._data_offset_x +
len(self._time_axis)])
#self._curves[curve_id]['object'].setStyle(Qwt.QwtPlotCurve.CurveStyle(3))
self.replot()
def rescale(self):
y_num_ticks = self.height() / 40
y_lower_limit = self._canvas_offset_y - (self._canvas_display_height /
2)
y_upper_limit = self._canvas_offset_y + (self._canvas_display_height /
2)
# calculate a fitting step size for nice, round tick labels, depending on the displayed value area
y_delta = y_upper_limit - y_lower_limit
exponent = int(math.log10(y_delta))
presicion = -(exponent - 2)
y_step_size = round(y_delta / y_num_ticks, presicion)
self.setAxisScale(Qwt.QwtPlot.yLeft, y_lower_limit, y_upper_limit,
y_step_size)
self.setAxisScale(Qwt.QwtPlot.xBottom, 0, len(self._time_axis))
self.redraw()
def rescale_axis_x(self, delta__x):
new_len = len(self._time_axis) + delta__x
new_len = max(10, min(new_len, self._num_value_saved))
self._time_axis = arange(new_len)
self._data_offset_x = max(
0,
min(self._data_offset_x,
self._num_value_saved - len(self._time_axis)))
self.rescale()
def scale_axis_y(self, max_value):
self._canvas_display_height = max_value
self.rescale()
def move_canvas(self, delta_x, delta_y):
self._data_offset_x += delta_x * len(self._time_axis) / float(
self.canvas().width())
self._data_offset_x = max(
0,
min(self._data_offset_x,
self._num_value_saved - len(self._time_axis)))
self._canvas_offset_x += delta_x * self._canvas_display_width / self.canvas(
).width()
self._canvas_offset_y += delta_y * self._canvas_display_height / self.canvas(
).height()
self.rescale()
def mousePressEvent(self, event):
self._last_canvas_x = event.x() - self.canvas().x()
self._last_canvas_y = event.y() - self.canvas().y()
self._pressed_canvas_y = event.y() - self.canvas().y()
def mouseMoveEvent(self, event):
canvas_x = event.x() - self.canvas().x()
canvas_y = event.y() - self.canvas().y()
if event.buttons() & Qt.MiddleButton: # middle button moves the canvas
delta_x = self._last_canvas_x - canvas_x
delta_y = canvas_y - self._last_canvas_y
self.move_canvas(delta_x, delta_y)
elif event.buttons() & Qt.RightButton: # right button zooms
zoom_factor = max(
-0.6, min(0.6, (self._last_canvas_y - canvas_y) / 20.0 / 2.0))
delta_y = (self.canvas().height() / 2.0) - self._pressed_canvas_y
self.move_canvas(0, zoom_factor * delta_y * 1.0225)
self.scale_axis_y(
max(
0.005, self._canvas_display_height -
(zoom_factor * self._canvas_display_height)))
self.rescale_axis_x(self._last_canvas_x - canvas_x)
self._last_canvas_x = canvas_x
self._last_canvas_y = canvas_y
def wheelEvent(self, event): # mouse wheel zooms the y-axis
canvas_y = event.y() - self.canvas().y()
zoom_factor = max(-0.6, min(0.6, (event.delta() / 120) / 6.0))
delta_y = (self.canvas().height() / 2.0) - canvas_y
self.move_canvas(0, zoom_factor * delta_y * 1.0225)
self.scale_axis_y(
max(
0.0005, self._canvas_display_height -
zoom_factor * self._canvas_display_height))
def __init__(self, spline, label, label_center, from_node, to_node, parent=None, **kwargs):
super(EdgeItem, self).__init__(parent, **kwargs)
self._edge_pen_width = kwargs.get('edge_pen_width', self.EDGE_PEN_WIDTH)
self.from_node = from_node
self.from_node.add_outgoing_edge(self)
self.to_node = to_node
self.to_node.add_incoming_edge(self)
self._brush = QBrush(self._color)
self._label_pen = QPen()
self._label_pen.setColor(self._color)
self._label_pen.setJoinStyle(Qt.RoundJoin)
self._label_pen.setWidthF(self._label_pen_width)
self._edge_pen = QPen()
self._edge_pen.setColor(self._color)
self._edge_pen.setWidthF(self._edge_pen_width)
self._sibling_edges = set()
self._label = None
if label is not None:
self._label = QGraphicsSimpleTextItem(label)
font = self._label.font()
font.setPointSize(8)
self._label.setFont(font)
label_rect = self._label.boundingRect()
label_rect.moveCenter(label_center)
self._label.setPos(label_rect.x(), label_rect.y())
# spline specification according to http://www.graphviz.org/doc/info/attrs.html#k:splineType
coordinates = spline.split(' ')
# extract optional end_point
end_point = None
if coordinates[0].startswith('e,'):
parts = coordinates.pop(0)[2:].split(',')
end_point = QPointF(float(parts[0]), -float(parts[1]))
# extract optional start_point
if coordinates[0].startswith('s,'):
parts = coordinates.pop(0).split(',')
# first point
parts = coordinates.pop(0).split(',')
point = QPointF(float(parts[0]), -float(parts[1]))
path = QPainterPath(point)
while len(coordinates) > 2:
# extract triple of points for a cubic spline
parts = coordinates.pop(0).split(',')
point1 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point2 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point3 = QPointF(float(parts[0]), -float(parts[1]))
path.cubicTo(point1, point2, point3)
self._arrow = None
if end_point is not None:
# draw arrow
self._arrow = QGraphicsPolygonItem()
polygon = QPolygonF()
polygon.append(point3)
offset = QPointF(end_point - point3)
corner1 = QPointF(-offset.y(), offset.x()) * 0.35
corner2 = QPointF(offset.y(), -offset.x()) * 0.35
polygon.append(point3 + corner1)
polygon.append(end_point)
polygon.append(point3 + corner2)
self._arrow.setPolygon(polygon)
self._path = QGraphicsPathItem()
self._path.setPath(path)
self.addToGroup(self._path)
self._brush.setColor(self._color)
self._edge_pen.setColor(self._color)
self._label_pen.setColor(self._color)
self._path.setPen(self._edge_pen)
if self._arrow is not None:
self._arrow.setBrush(self._brush)
self._arrow.setPen(self._edge_pen)
if self._label is not None:
self._label.setBrush(self._brush)
self._label.setPen(self._label_pen)
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))
def addGraph(self, graph_str):
graph = graph_str.splitlines()
header = graph[0].split()
if header[0] != 'graph':
raise Exception('wrong graph format', 'header is: ' + graph[0])
self.scale_factor = 100.0
self.width = float(header[2])
self.height = float(header[3])
print "QGraphicsScene size:", self.width, self.height
self.scene = GraphScene(
QRectF(0, 0, self.scX(self.width), self.scY(self.height)))
for l in graph:
items = l.split()
if len(items) == 0:
continue
elif items[0] == 'stop':
break
elif items[0] == 'node':
#node CImp 16.472 5.25 0.86659 0.5 CImp filled ellipse lightblue lightblue
if len(items) != 11:
raise Exception('wrong number of items in line',
'line is: ' + l)
name = items[6]
if name == "\"\"":
name = ""
w = self.scX(items[4])
h = self.scY(items[5])
x = self.tfX(items[2])
y = self.tfY(items[3])
self.nodes[name] = self.scene.addEllipse(
x - w / 2, y - h / 2, w, h)
self.nodes[name].setData(0, name)
text_item = self.scene.addSimpleText(name)
br = text_item.boundingRect()
text_item.setPos(x - br.width() / 2, y - br.height() / 2)
elif items[0] == 'edge':
# without label:
# edge CImp Ts 4 16.068 5.159 15.143 4.9826 12.876 4.5503 11.87 4.3583 solid black
#
# with label:
# edge b_stSplit TorsoVelAggregate 7 7.5051 6.3954 7.7054 6.3043 7.9532 6.1899 8.1728 6.0833 8.4432 5.9522 8.7407 5.8012 8.9885 5.6735 aa 8.6798 5.9792 solid black
line_len = int(items[3])
label_text = None
label_pos = None
if (line_len * 2 + 6) == len(items):
# no label
pass
elif (line_len * 2 + 9) == len(items):
# edge with label
label_text = items[4 + line_len * 2]
label_pos = QPointF(self.tfX(items[4 + line_len * 2 + 1]),
self.tfY(items[4 + line_len * 2 + 2]))
else:
raise Exception(
'wrong number of items in line',
'should be: ' + str(line_len * 2 + 6) + " or " +
str(line_len * 2 + 9) + ', line is: ' + l)
line = []
for i in range(line_len):
line.append((self.tfX(items[4 + i * 2]),
self.tfY(items[5 + i * 2])))
control_points_idx = 1
path = QPainterPath(QPointF(line[0][0], line[0][1]))
while True:
q1 = line[control_points_idx]
q2 = line[control_points_idx + 1]
p2 = line[control_points_idx + 2]
path.cubicTo(q1[0], q1[1], q2[0], q2[1], p2[0], p2[1])
control_points_idx = control_points_idx + 3
if control_points_idx >= len(line):
break
edge = self.scene.addPath(path)
edge.setData(0, (items[1], items[2]))
self.edges.append(edge)
end_p = QPointF(line[-1][0], line[-1][1])
p0 = end_p - QPointF(line[-2][0], line[-2][1])
p0_norm = math.sqrt(p0.x() * p0.x() + p0.y() * p0.y())
p0 = p0 / p0_norm
p0 = p0 * self.scale_factor * 0.15
p1 = QPointF(p0.y(), -p0.x()) * 0.25
p2 = -p1
poly = QPolygonF()
poly.append(p0 + end_p)
poly.append(p1 + end_p)
poly.append(p2 + end_p)
poly.append(p0 + end_p)
# poly_path = QPainterPath()
# poly_path.addPolygon(poly)
# painter = QPainter()
self.scene.addPolygon(poly)
if label_text and label_pos:
if label_text[0] == "\"":
label_text = label_text[1:]
if label_text[-1] == "\"":
label_text = label_text[:-1]
label_text = label_text.replace("\\n", "\n")
label_item = self.scene.addSimpleText(label_text)
br = label_item.boundingRect()
label_item.setPos(label_pos.x() - br.width() / 2,
label_pos.y() - br.height() / 2)
class QwtDataPlot(Qwt.QwtPlot):
mouseCoordinatesChanged = Signal(QPointF)
_num_value_saved = 1000
_num_values_ploted = 1000
limits_changed = Signal()
def __init__(self, *args):
super(QwtDataPlot, self).__init__(*args)
self.setCanvasBackground(Qt.white)
self.insertLegend(Qwt.QwtLegend(), Qwt.QwtPlot.BottomLegend)
self._curves = {}
# TODO: rejigger these internal data structures so that they're easier
# to work with, and easier to use with set_xlim and set_ylim
# this may also entail rejiggering the _time_axis so that it's
# actually time axis data, or just isn't required any more
# new data structure
self._x_limits = [0.0, 10.0]
self._y_limits = [0.0, 10.0]
# old data structures
self._last_canvas_x = 0
self._last_canvas_y = 0
self._pressed_canvas_y = 0
self._pressed_canvas_x = 0
self._last_click_coordinates = None
marker_axis_y = Qwt.QwtPlotMarker()
marker_axis_y.setLabelAlignment(Qt.AlignRight | Qt.AlignTop)
marker_axis_y.setLineStyle(Qwt.QwtPlotMarker.HLine)
marker_axis_y.setYValue(0.0)
marker_axis_y.attach(self)
self._picker = Qwt.QwtPlotPicker(Qwt.QwtPlot.xBottom,
Qwt.QwtPlot.yLeft,
Qwt.QwtPicker.PolygonSelection,
Qwt.QwtPlotPicker.PolygonRubberBand,
Qwt.QwtPicker.AlwaysOn, self.canvas())
self._picker.setRubberBandPen(QPen(Qt.blue))
self._picker.setTrackerPen(QPen(Qt.blue))
# Initialize data
self.rescale()
#self.move_canvas(0, 0)
self.canvas().setMouseTracking(True)
self.canvas().installEventFilter(self)
def eventFilter(self, _, event):
if event.type() == QEvent.MouseButtonRelease:
x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x())
y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y())
self._last_click_coordinates = QPointF(x, y)
self.limits_changed.emit()
elif event.type() == QEvent.MouseMove:
x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x())
y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y())
coords = QPointF(x, y)
if self._picker.isActive(
) and self._last_click_coordinates is not None:
toolTip = 'origin x: %.5f, y: %.5f' % (
self._last_click_coordinates.x(),
self._last_click_coordinates.y())
delta = coords - self._last_click_coordinates
toolTip += '\ndelta x: %.5f, y: %.5f\nlength: %.5f' % (
delta.x(), delta.y(), QVector2D(delta).length())
else:
toolTip = 'buttons\nleft: measure\nmiddle: move\nright: zoom x/y\nwheel: zoom y'
self.setToolTip(toolTip)
self.mouseCoordinatesChanged.emit(coords)
return False
def log(self, level, message):
self.emit(SIGNAL('logMessage'), level, message)
def resizeEvent(self, event):
Qwt.QwtPlot.resizeEvent(self, event)
self.rescale()
def add_curve(self,
curve_id,
curve_name,
curve_color=QColor(Qt.blue),
markers_on=False):
curve_id = str(curve_id)
if curve_id in self._curves:
return
curve_object = Qwt.QwtPlotCurve(curve_name)
curve_object.attach(self)
curve_object.setPen(curve_color)
if markers_on:
curve_object.setSymbol(
Qwt.QwtSymbol(Qwt.QwtSymbol.Ellipse, QBrush(curve_color),
QPen(Qt.black), QSize(4, 4)))
self._curves[curve_id] = curve_object
def remove_curve(self, curve_id):
curve_id = str(curve_id)
if curve_id in self._curves:
self._curves[curve_id].hide()
self._curves[curve_id].attach(None)
del self._curves[curve_id]
def set_values(self, curve_id, data_x, data_y):
curve = self._curves[curve_id]
curve.setData(data_x, data_y)
def redraw(self):
self.replot()
# ----------------------------------------------
# begin qwtplot internal methods
# ----------------------------------------------
def rescale(self):
self.setAxisScale(Qwt.QwtPlot.yLeft, self._y_limits[0],
self._y_limits[1])
self.setAxisScale(Qwt.QwtPlot.xBottom, self._x_limits[0],
self._x_limits[1])
self._canvas_display_height = self._y_limits[1] - self._y_limits[0]
self._canvas_display_width = self._x_limits[1] - self._x_limits[0]
self.redraw()
def rescale_axis_x(self, delta__x):
"""
add delta_x to the length of the x axis
"""
x_width = self._x_limits[1] - self._x_limits[0]
x_width += delta__x
self._x_limits[1] = x_width + self._x_limits[0]
self.rescale()
def scale_axis_y(self, max_value):
"""
set the y axis height to max_value, about the current center
"""
canvas_display_height = max_value
canvas_offset_y = (self._y_limits[1] + self._y_limits[0]) / 2.0
y_lower_limit = canvas_offset_y - (canvas_display_height / 2)
y_upper_limit = canvas_offset_y + (canvas_display_height / 2)
self._y_limits = [y_lower_limit, y_upper_limit]
self.rescale()
def move_canvas(self, delta_x, delta_y):
"""
move the canvas by delta_x and delta_y in SCREEN COORDINATES
"""
canvas_offset_x = delta_x * self._canvas_display_width / self.canvas(
).width()
canvas_offset_y = delta_y * self._canvas_display_height / self.canvas(
).height()
self._x_limits = [l + canvas_offset_x for l in self._x_limits]
self._y_limits = [l + canvas_offset_y for l in self._y_limits]
self.rescale()
def mousePressEvent(self, event):
self._last_canvas_x = event.x() - self.canvas().x()
self._last_canvas_y = event.y() - self.canvas().y()
self._pressed_canvas_y = event.y() - self.canvas().y()
def mouseMoveEvent(self, event):
canvas_x = event.x() - self.canvas().x()
canvas_y = event.y() - self.canvas().y()
if event.buttons() & Qt.MiddleButton: # middle button moves the canvas
delta_x = self._last_canvas_x - canvas_x
delta_y = canvas_y - self._last_canvas_y
self.move_canvas(delta_x, delta_y)
elif event.buttons() & Qt.RightButton: # right button zooms
zoom_factor = max(
-0.6, min(0.6, (self._last_canvas_y - canvas_y) / 20.0 / 2.0))
delta_y = (self.canvas().height() / 2.0) - self._pressed_canvas_y
self.move_canvas(0, zoom_factor * delta_y * 1.0225)
self.scale_axis_y(
max(
0.005, self._canvas_display_height -
(zoom_factor * self._canvas_display_height)))
self.rescale_axis_x(self._last_canvas_x - canvas_x)
self._last_canvas_x = canvas_x
self._last_canvas_y = canvas_y
def wheelEvent(self, event): # mouse wheel zooms the y-axis
# y position of pointer in graph coordinates
canvas_y = event.y() - self.canvas().y()
zoom_factor = max(-0.6, min(0.6, (event.delta() / 120) / 6.0))
delta_y = (self.canvas().height() / 2.0) - canvas_y
self.move_canvas(0, zoom_factor * delta_y * 1.0225)
self.scale_axis_y(
max(
0.0005, self._canvas_display_height -
zoom_factor * self._canvas_display_height))
self.limits_changed.emit()
def vline(self, x, color):
qWarning("QwtDataPlot.vline is not implemented yet")
def set_xlim(self, limits):
self.setAxisScale(Qwt.QwtPlot.xBottom, limits[0], limits[1])
self._x_limits = limits
def set_ylim(self, limits):
self.setAxisScale(Qwt.QwtPlot.yLeft, limits[0], limits[1])
self._y_limits = limits
def get_xlim(self):
return self._x_limits
def get_ylim(self):
return self._y_limits
class QwtDataPlot(Qwt.QwtPlot):
mouseCoordinatesChanged = Signal(QPointF)
_colors = [Qt.red, Qt.green, Qt.blue, Qt.magenta]
_num_value_saved = 5000
_num_values_ploted = 1000
def __init__(self, *args):
super(QwtDataPlot, self).__init__(*args)
self.setCanvasBackground(Qt.white)
self.insertLegend(Qwt.QwtLegend(), Qwt.QwtPlot.BottomLegend)
self._curves = {}
self._data_offset_x = 0
self._canvas_offset_x = 0
self._canvas_offset_y = 0
self._last_canvas_x = 0
self._last_canvas_y = 0
self._pressed_canvas_y = 0
self._last_click_coordinates = None
self._color_index = 0
marker_axis_y = Qwt.QwtPlotMarker()
marker_axis_y.setLabelAlignment(Qt.AlignRight | Qt.AlignTop)
marker_axis_y.setLineStyle(Qwt.QwtPlotMarker.HLine)
marker_axis_y.setYValue(0.0)
marker_axis_y.attach(self)
self._picker = Qwt.QwtPlotPicker(
Qwt.QwtPlot.xBottom, Qwt.QwtPlot.yLeft, Qwt.QwtPicker.PolygonSelection,
Qwt.QwtPlotPicker.PolygonRubberBand, Qwt.QwtPicker.AlwaysOn, self.canvas()
)
self._picker.setRubberBandPen(QPen(self._colors[-1]))
self._picker.setTrackerPen(QPen(self._colors[-1]))
# Initialize data
self._time_axis = arange(0, 10)
self._canvas_display_height = 1000
self._canvas_display_width = self.canvas().width()
self._data_offset_x = self._num_value_saved - self._num_values_ploted
self.redraw()
self.move_canvas(0, 0)
self.canvas().setMouseTracking(True)
self.canvas().installEventFilter(self)
def eventFilter(self, _, event):
if event.type() == QEvent.MouseButtonRelease:
x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x())
y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y())
self._last_click_coordinates = QPointF(x, y)
elif event.type() == QEvent.MouseMove:
x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x())
y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y())
coords = QPointF(x, y)
if self._picker.isActive() and self._last_click_coordinates is not None:
toolTip = 'origin x: %.5f, y: %.5f' % (self._last_click_coordinates.x(), self._last_click_coordinates.y())
delta = coords - self._last_click_coordinates
toolTip += '\ndelta x: %.5f, y: %.5f\nlength: %.5f' % (delta.x(), delta.y(), QVector2D(delta).length())
else:
toolTip = 'buttons\nleft: measure\nmiddle: move\nright: zoom x/y\nwheel: zoom y'
self.setToolTip(toolTip)
self.mouseCoordinatesChanged.emit(coords)
return False
def log(self, level, message):
self.emit(SIGNAL('logMessage'), level, message)
def resizeEvent(self, event):
#super(QwtDataPlot, self).resizeEvent(event)
Qwt.QwtPlot.resizeEvent(self, event)
self.rescale()
def add_curve(self, curve_id, curve_name, values_x, values_y):
curve_id = str(curve_id)
if self._curves.get(curve_id):
return
curve_object = Qwt.QwtPlotCurve(curve_name)
curve_object.attach(self)
curve_object.setPen(QPen(self._colors[self._color_index % len(self._colors)]))
self._color_index += 1
self._curves[curve_id] = {
'name': curve_name,
'data': zeros(self._num_value_saved),
'time': zeros(self._num_value_saved),
'object': curve_object,
}
def remove_curve(self, curve_id):
curve_id = str(curve_id)
if curve_id in self._curves:
self._curves[curve_id]['object'].hide()
self._curves[curve_id]['object'].attach(None)
del self._curves[curve_id]['object']
del self._curves[curve_id]
@Slot(str, list, list)
def update_values(self, curve_id, values_x, values_y):
for value_x, value_y in zip(values_x, values_y):
self.update_value(curve_id, value_x, value_y)
@Slot(str, float, float)
def update_value(self, curve_id, value_x, value_y):
curve_id = str(curve_id)
# update data plot
if curve_id in self._curves:
# TODO: use value_x as timestamp
self._curves[curve_id]['data'] = concatenate((self._curves[curve_id]['data'][1:], self._curves[curve_id]['data'][:1]), 1)
self._curves[curve_id]['data'][-1] = float(value_y)
self._curves[curve_id]['time'] = concatenate((self._curves[curve_id]['time'][1:], self._curves[curve_id]['time'][:1]), 1)
self._curves[curve_id]['time'][-1] = float(value_x)
def redraw(self):
for curve_id in self._curves.keys():
x_data = self._time_axis
x_data = self._curves[curve_id]['time'][self._data_offset_x: -1]
y_data = self._curves[curve_id]['data'][self._data_offset_x: -1]
self._curves[curve_id]['object'].setData(x_data, y_data)
#self._curves[curve_id]['object'].setStyle(Qwt.QwtPlotCurve.CurveStyle(3))
self.replot()
def rescale(self):
y_num_ticks = self.height() / 40
y_lower_limit = self._canvas_offset_y - (self._canvas_display_height / 2)
y_upper_limit = self._canvas_offset_y + (self._canvas_display_height / 2)
# calculate a fitting step size for nice, round tick labels, depending on the displayed value area
y_delta = y_upper_limit - y_lower_limit
exponent = int(math.log10(y_delta))
presicion = -(exponent - 2)
y_step_size = round(y_delta / y_num_ticks, presicion)
self.setAxisScale(Qwt.QwtPlot.yLeft, y_lower_limit, y_upper_limit, y_step_size)
self.setAxisScale(Qwt.QwtPlot.xBottom, self._time_axis[0], self._time_axis[-1]+1)
self.redraw()
def rescale_axis_x(self, delta__x):
new_len = len(self._time_axis) + delta__x
new_len = max(10, min(new_len, self._num_value_saved))
self._time_axis = arange(new_len)
self._data_offset_x = max(0, min(self._data_offset_x, self._num_value_saved - len(self._time_axis)))
self.rescale()
def scale_axis_y(self, min_value, max_value):
self._canvas_display_height = max_value - min_value
self._canvas_offset_y = (max_value + min_value)/2
self.rescale()
def rescale_axis_y(self):
y_min = [-0.1]
y_max = [0.1]
for curve_id in self._curves.keys():
y_min.append(min(self._curves[curve_id]['data']))
y_max.append(max(self._curves[curve_id]['data']))
time = self._curves[curve_id]['time'][-1]
min_value = min(y_min)
max_value = max(y_max)
if time > 10:
self._time_axis = arange(time-10, time)
self._canvas_display_height = max_value - min_value
self._canvas_offset_y = (max_value + min_value)/2
self.rescale()
def move_canvas(self, delta_x, delta_y):
self._data_offset_x += delta_x * len(self._time_axis) / float(self.canvas().width())
self._data_offset_x = max(0, min(self._data_offset_x, self._num_value_saved - len(self._time_axis)))
self._canvas_offset_x += delta_x * self._canvas_display_width / self.canvas().width()
self._canvas_offset_y += delta_y * self._canvas_display_height / self.canvas().height()
self.rescale()
def mousePressEvent(self, event):
self._last_canvas_x = event.x() - self.canvas().x()
self._last_canvas_y = event.y() - self.canvas().y()
self._pressed_canvas_y = event.y() - self.canvas().y()
def mouseMoveEvent(self, event):
canvas_x = event.x() - self.canvas().x()
canvas_y = event.y() - self.canvas().y()
if event.buttons() & Qt.MiddleButton: # middle button moves the canvas
delta_x = self._last_canvas_x - canvas_x
delta_y = canvas_y - self._last_canvas_y
self.move_canvas(delta_x, delta_y)
elif event.buttons() & Qt.RightButton: # right button zooms
zoom_factor = max(-0.6, min(0.6, (self._last_canvas_y - canvas_y) / 20.0 / 2.0))
delta_y = (self.canvas().height() / 2.0) - self._pressed_canvas_y
self.move_canvas(0, zoom_factor * delta_y * 1.0225)
self.scale_axis_y(0, max(0.005, self._canvas_display_height - (zoom_factor * self._canvas_display_height)))
self.rescale_axis_x(self._last_canvas_x - canvas_x)
self._last_canvas_x = canvas_x
self._last_canvas_y = canvas_y
def wheelEvent(self, event): # mouse wheel zooms the y-axis
canvas_y = event.y() - self.canvas().y()
zoom_factor = max(-0.6, min(0.6, (event.delta() / 120) / 6.0))
delta_y = (self.canvas().height() / 2.0) - canvas_y
self.move_canvas(0, zoom_factor * delta_y * 1.0225)
self.scale_axis_y(0, max(0.0005, self._canvas_display_height - zoom_factor * self._canvas_display_height))
def __init__(self,
spline,
label,
label_center,
from_node,
to_node,
parent=None,
**kwargs):
super(EdgeItem, self).__init__(parent, **kwargs)
self._edge_pen_width = kwargs.get('edge_pen_width',
self.EDGE_PEN_WIDTH)
self.from_node = from_node
self.from_node.add_outgoing_edge(self)
self.to_node = to_node
self.to_node.add_incoming_edge(self)
self._brush = QBrush(self._color)
self._label_pen = QPen()
self._label_pen.setColor(self._color)
self._label_pen.setJoinStyle(Qt.RoundJoin)
self._label_pen.setWidthF(self._label_pen_width)
self._edge_pen = QPen()
self._edge_pen.setColor(self._color)
self._edge_pen.setWidthF(self._edge_pen_width)
self._sibling_edges = set()
self._label = None
if label is not None:
self._label = QGraphicsSimpleTextItem(label)
font = self._label.font()
font.setPointSize(8)
self._label.setFont(font)
label_rect = self._label.boundingRect()
label_rect.moveCenter(label_center)
self._label.setPos(label_rect.x(), label_rect.y())
# spline specification according to http://www.graphviz.org/doc/info/attrs.html#k:splineType
coordinates = spline.split(' ')
# extract optional end_point
end_point = None
if coordinates[0].startswith('e,'):
parts = coordinates.pop(0)[2:].split(',')
end_point = QPointF(float(parts[0]), -float(parts[1]))
# extract optional start_point
if coordinates[0].startswith('s,'):
parts = coordinates.pop(0).split(',')
# first point
parts = coordinates.pop(0).split(',')
point = QPointF(float(parts[0]), -float(parts[1]))
path = QPainterPath(point)
while len(coordinates) > 2:
# extract triple of points for a cubic spline
parts = coordinates.pop(0).split(',')
point1 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point2 = QPointF(float(parts[0]), -float(parts[1]))
parts = coordinates.pop(0).split(',')
point3 = QPointF(float(parts[0]), -float(parts[1]))
path.cubicTo(point1, point2, point3)
self._arrow = None
if end_point is not None:
# draw arrow
self._arrow = QGraphicsPolygonItem()
polygon = QPolygonF()
polygon.append(point3)
offset = QPointF(end_point - point3)
corner1 = QPointF(-offset.y(), offset.x()) * 0.35
corner2 = QPointF(offset.y(), -offset.x()) * 0.35
polygon.append(point3 + corner1)
polygon.append(end_point)
polygon.append(point3 + corner2)
self._arrow.setPolygon(polygon)
self._path = QGraphicsPathItem()
self._path.setPath(path)
self.addToGroup(self._path)
self._brush.setColor(self._color)
self._edge_pen.setColor(self._color)
self._label_pen.setColor(self._color)
self._path.setPen(self._edge_pen)
if self._arrow is not None:
self._arrow.setBrush(self._brush)
self._arrow.setPen(self._edge_pen)
if self._label is not None:
self._label.setBrush(self._brush)
self._label.setPen(self._label_pen)
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)
