示例#1
0
    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)
示例#2
0
    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
示例#3
0
    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
示例#4
0
    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
示例#5
0
    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)
示例#6
0
    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)
示例#7
0
    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
示例#8
0
    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)
示例#9
0
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())
示例#10
0
    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()
示例#11
0
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
示例#12
0
    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))
示例#14
0
    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)
示例#15
0
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))
示例#16
0
    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)
示例#17
0
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
示例#18
0
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))
示例#19
0
文件: edge_item.py 项目: iosp/robil2
    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)
示例#20
0
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)