monitor.cpp

robocup rcssserver 运行防真机器人足球比赛所用的服务器端

    case 'g':
        // Kick goalies if they are on the goals
        KickGoalies();
        break;
    case 'p' :
        // pause simulation
        mPauseGame = !mPauseGame;
        if (mPauseGame)
        {
            cout <<"(rcssmonitor3d) -- Pausing Simulation --" << endl;
            mCommServer->SendPauseCmd();
        } else {
            cout <<"(rcssmonitor3d) -- Running Simulation --" << endl;
            mCommServer->SendRunCmd();
        }
        break;
    case ' ':
        mKickOff = NextKickOffMode(mKickOff);
        break;
    case 'l' :
        // assign free kick to right team
        cerr << "(rcssmonitor3d) " << mGameState.GetTime()
             << ": Freekick left not implemented\n";
        //mCommServer->SendToWorldModel("(playMode free_kick_left)");
        break;
    case 'r' :
        // assign free kick to right team
        cerr << "(rcssmonitor3d) " << mGameState.GetTime()
             << ": Freekick right not implemented\n";
        //mCommServer->SendToWorldModel("(playMode free_kick_right)");
        break;
    case 't':
        // toggle texture
        mUseTexture = ! mUseTexture;
        break;
    case 'X':
        // select CenterBall camera mode
        mShowSecondView = ! mShowSecondView;
        break;

    case 'm' :
        // toggle single step mode (aka _m_anual advance)
        mSingleStep = !mSingleStep;
        mAdvance = false;
        break;
    case '>' :
        // advance a step
        mAdvance = true;
        mSingleStep = true;
        break;
    case '<' :
        if (mLogserver)
        {
            mCommServer->GoBack(2);
            mCommServer->ReadMessage();
            boost::shared_ptr<oxygen::PredicateList> predicates = mCommServer->GetPredicates();
            mOldGameState = mGameState;
            mGameState.ProcessInput(predicates);
            Display();
        }
        break;


    case '?' :
        // show keybindings
        KeyBindings();
        break;

    case 'f' :
        // fast
    mRealTime = !mRealTime;
        if (mRealTime)
            mDiffTime=DiffTime();
        break;

    case 'F' :
        // slow
    mRealTime = true;
    mDiffTime = DiffTime() ;
        break;

    case 'v':
        mDrawVelocity = !mDrawVelocity;
        //mCommServer->SendToWorldModel("(ball (pos 49 20 1) (vel 6.0 0.0 0.1))");
        break;

    case 'Z':
        glutFullScreen();
        glutPositionWindow(0,0);

        //    mDrawVelocity = !mDrawVelocity;
        //mCommServer->SendToWorldModel("(ball (pos 49 20 1) (vel 6.0 0.0 0.1))");
        break;

    default:
        return;
    }

    glutPostRedisplay();

}

void
Monitor::SpecialKeys(int glutkey, int /*x*/, int /*y*/)
{
    salt::Vector3f pos;
    switch (glutkey) {
    case GLUT_KEY_UP:
        //strafe cam forward
        mGLServer.MoveCamStrafeForward(mCamDelta);
        break;
    case GLUT_KEY_DOWN:
        //strafe cam back
        mGLServer.MoveCamStrafeForward(-mCamDelta);
        break;
    case GLUT_KEY_LEFT:
        //strafe cam left
        mGLServer.MoveCamStrafe(mCamDelta);
        break;
    case GLUT_KEY_RIGHT:
        // strafe cam right
        mGLServer.MoveCamStrafe(-mCamDelta);
        break;
    default:
        return;
    }

    glutPostRedisplay();

}

void
Monitor::Mouse(int /*button*/, int /*state*/, int x, int y)
{
    mMousePosition = Vector2f(static_cast<float>(x),
                              static_cast<float>(y));
}

void
Monitor::Reshape(int width, int height)
{
    mGLServer.Reshape(width,height);
}

long int
Monitor::DiffTime()
{

    int time;
    unsigned int utime, ugametime;
    float gametime;


    time = glutGet(GLUT_ELAPSED_TIME) * 1000;// millisec (0,001) --> micro
    utime = (unsigned int) time;

    gametime = mGameState.GetTime() * 1000000; // sec (in float) -->microsec (0,000001)
    ugametime = (unsigned int) gametime;

    return ugametime - utime;
}

void
Monitor::Idle()
{


    int time;
    unsigned int utime, ugametime;
    float gametime;

    time = glutGet(GLUT_ELAPSED_TIME) * 1000;// millisec (0,001) --> micro
    utime = (unsigned int) time;

    gametime = mGameState.GetTime() * 1000000; // sec (in float) -->microsec (0,000001)
    ugametime = (unsigned int) gametime;


    if ((DiffTime() - mDiffTime  > 0) && mRealTime && mLogserver)
    {
        usleep(DiffTime() - mDiffTime);
    }
    else usleep(10);

    // If we are in singlestep mode and not advancing then return
    if (mLogserver && mSingleStep && !mAdvance)
        return;

    if (mGameState.IsFinished())
    {
        cerr << "simulation finished... monitor exiting\n";
        exit(0);
    }

    if (! mCommServer->ReadMessage())
    {
        return;
    }

    static bool first_time = true;
    if (first_time) SetupCameraPositions();

    static int n = 10; // the first 10 messages should not be skipped
    --n;
    if (!first_time && n > 0) return;
    if (n <= 0)
    {
        n = mSkip;

        // set the goal markers to the right height
        if (first_time)
        {
            mFlagInfo[GameState::eGOAL_1_L].mOffset[2] = mGameState.GetGoalHeight();
            mFlagInfo[GameState::eGOAL_2_L].mOffset[2] = mGameState.GetGoalHeight();
            mFlagInfo[GameState::eGOAL_1_R].mOffset[2] = mGameState.GetGoalHeight();
            mFlagInfo[GameState::eGOAL_2_R].mOffset[2] = mGameState.GetGoalHeight();
        }

        first_time = false;
    }


    boost::shared_ptr<oxygen::PredicateList> predicates =
        mCommServer->GetPredicates();

    //        if (mDrawVelocity)
    mOldGameState = mGameState;

    mGameState.ProcessInput(predicates);

    mAdvance = false;
    glutPostRedisplay();
}


Monitor::ECameraMode
Monitor::NextCameraMode(ECameraMode mode) const
{
    switch (mode)
    {
    case eFree:
        return eFollowBall;
    case eFollowBall:
        return eCenterBall;
    default:
        return eFree;
    }
}

salt::Vector3f
Monitor::NextCameraPosition(const list<salt::Vector3f>& positions) const
{
    if (positions.empty()) return salt::Vector3f(0,0,0);

    list<salt::Vector3f>::const_iterator i;

    i = find(positions.begin(), positions.end(), mGLServer.GetCameraPos());
    if (i != positions.end()) i++;
    if (i == positions.end()) i = positions.begin();

    return *i;
}

CommServerBase::EKickOff
Monitor::NextKickOffMode(CommServerBase::EKickOff mode) const
{
    switch (mode)
    {
    case CommServerBase::eRandom: return CommServerBase::eLeft;
    case CommServerBase::eLeft: return CommServerBase::eRight;
    default: return CommServerBase::eRandom;
    }
}

void
Monitor::KickGoalies()
{
    Vector3f pos;
    int i = 0;
    int unum;
    TTeamIndex side;
    float size;
    std::stringstream cmd;

    cerr << "(rcssmonitor3d) " << mGameState.GetTime()
         << ": Kicking Goalies." << endl;

    while (mGameState.GetPlayer(i, pos, side, unum, size))
    {
        if (unum == 1 && pos[2] >= mGameState.GetGoalHeight())
        {
            // we found a goalie
            cmd << "(agent (team " << (side==TI_RIGHT ? "Right" : "Left")
                << ")(unum " << unum << ")(pos " << pos[0]<< " " << pos[1] << " " << pos[2]
                << ")(vel " << pos[0]/-pos[0] << " 0.0 1.0))";

            mCommServer->SendToWorldModel(cmd.str());
        }
        ++i;
    }
}

// void
// Monitor::DrawReport()
// {
//     char buf[1024];

//     glEnable(GL_BLEND);         // Turn Blending On
//     glColor4fv(sReportColor);
//     mGLServer.DrawGroundRectangle( Vector3f( -0.5, -0.7, 0 ), 1.0, 1.4, 0);
//     glColor3f(1.0, 1.0, 1.0);

//     mGLServer.DrawTextPix("--   rcssmonitor3D Game Report   --",
//                           Vector2f( 0, 4*mGLServer.GetTextHeight() + 100),
//                           GLServer::eCENTER);

//     sprintf(buf, "Ball Existence In Left Field : %'.2f %%", 100.0*mBallInLeft/(mBallInLeft+mBallInCenter+mBallInRight) );
//     mGLServer.DrawTextPix(buf, Vector2f( 0, 7*mGLServer.GetTextHeight() + 100), GLServer::eCENTER);

//     sprintf(buf, "Ball Existence In Field's Center : %'.2f %%", 100.0*mBallInCenter/(mBallInLeft+mBallInCenter+mBallInRight) );
//     mGLServer.DrawTextPix(buf, Vector2f( 0, 8*mGLServer.GetTextHeight() + 100), GLServer::eCENTER);

//     sprintf(buf, "Ball Existence In Right Field : %'.2f %%", 100.0*mBallInRight/(mBallInLeft+mBallInCenter+mBallInRight) );
//     mGLServer.DrawTextPix(buf, Vector2f( 0, 9*mGLServer.GetTextHeight() + 100), GLServer::eCENTER);

//     glDisable(GL_BLEND);         // Turn Blending Off
// }

void
Monitor::checkAndShow()
{
    if (! mShowSecondView) return;

    Vector3f pos;
    float size;
    mGameState.GetBall(pos, size);

    if (pos[0] > 35)
        drawSecondView(20, 30, Vector3f(48.0f,-10.0f,5.0f), Vector3f(50.0f,18.0f,-15.0f) );
    else if (pos[0] < -35)
        drawSecondView(20, 30, Vector3f(-48.0f,-10.0f,5.0f), Vector3f(-50.0f,18.0f,-15.0f) );
    else
        drawSecondView(20, 30, Vector3f(0.0f,-6.0f,4.0f) + pos , Vector3f(0.0f,6.0f,-6.0f) + pos );
}

void
Monitor::drawSecondView( int sx, int sy, Vector3f campos, Vector3f lookat )
{
    const Vector3f szGoal1 = mGameState.GetGoalSize(false);
    const Vector3f szGoal2 = mGameState.GetGoalSize(true);

    glColor3f (1, 1, 1);
    glLoadIdentity();

		Vector3f look = mGLServer.GetLookAtPos();
		Vector3f pos = mGLServer.GetCameraPos();

    mGLServer.SetCameraPos( campos );
    mGLServer.SetLookAtPos( lookat );
		mGLServer.ApplyCamera();
		glViewport(sx, sy, mWidth/3 - 60, 160);

    const float& fl = mGameState.GetFieldLength();
    const float& fw = mGameState.GetFieldWidth();
    const float& fh = mGameState.GetFieldHeight();
    const float& lw = mGameState.GetLineWidth();
    const float& bs = mGameState.GetBorderSize();
    const float& gw = mGameState.GetGoalWidth();

    // ground
    glColor4fv(sSecondGroundColor);

    // left half
    mGLServer.DrawGroundRectangle(Vector3f(-fl/2 + lw, -fw/2 + lw, 0),
                                  fl/2 - lw - lw/2, fw - 2*lw, 0);

    // right half
    mGLServer.DrawGroundRectangle(Vector3f(lw/2, -fw/2 + lw, 0),
                                  fl/2 - lw - lw/2, fw - 2*lw, 0);

    // border
    glColor4fv(sBorderColor);
    glDisable (GL_DEPTH_TEST);

    // border at left goal
    mGLServer.DrawGroundRectangle(Vector3f(-fl/2-bs,-fw/2-bs,0),
                                  bs, fw+2*bs, 0);

    // border at right goal
    mGLServer.DrawGroundRectangle(Vector3f(fl/2,-fw/2-bs,0),
                                  bs, fw+2*bs, 0);

    // long top border
    mGLServer.DrawGroundRectangle(Vector3f(-fl/2,-fw/2-bs,0),
                                  fl, bs, 0);

    // long bottom border
    mGLServer.DrawGroundRectangle(Vector3f(-fl/2,fw/2,0),
                                  fl, bs, 0);

    // lines
    glColor4fv(sLineColor);

    // left goal
    mGLServer.DrawGroundRectangle(Vector3f(-fl/2,-fw/2,0), lw, fw, 0);

    // right goal
    mGLServer.DrawGroundRectangle(Vector3f(fl/2 - lw,-fw/2,0), lw, fw, 0);

    // long top border
    mGLServer.DrawGroundRectangle(Vector3f(-fl/2 + lw,-fw/2,0),
                                  fl - 2*lw, lw, 0);

    // long bottom border
    mGLServer.DrawGroundRectangle(Vector3f(-fl/2 + lw,fw/2 - lw,0),
                                  fl - 2*lw, lw, 0);

    // middle line
    mGLServer.DrawGroundRectangle(Vector3f(-lw/2,-fw/2 + lw,0),
                                  lw, fw - 2*lw, 0);

    // goal box area right side
    mGLServer.DrawGroundRectangle(Vector3f(fl/2 - sGoalBoxLength, -gw/2 - sGoalBoxLength, 0.05), lw, gw + 11, 0);
    mGLServer.DrawGroundRectangle(Vector3f(fl/2 - sGoalBoxLength, -gw/2 - sGoalBoxLength, 0.05), sGoalBoxLength, lw, 0);
    mGLServer.DrawGroundRectangle(Vector3f(fl/2 - sGoalBoxLength, gw/2 + sGoalBoxLength, 0.05), sGoalBoxLength, lw, 0);

    // goal box area left side
    mGLServer.DrawGroundRectangle(Vector3f(-fl/2 + sGoalBoxLength, -gw/2 - sGoalBoxLength, 0.05), lw, gw + 11, 0);
    mGLServer.DrawGroundRectangle(Vector3f(-fl/2 + sGoalBoxLength, -gw/2 - sGoalBoxLength, 0.05), -sGoalBoxLength, lw, 0);
    mGLServer.DrawGroundRectangle(Vector3f(-fl/2 + sGoalBoxLength, gw/2 + sGoalBoxLength, 0.05), -sGoalBoxLength, lw, 0);

    // penalty area right side
    mGLServer.DrawGroundRectangle(Vector3f(fl/2 - sPenaltyLength, -gw/2 - sPenaltyLength, 0.05), lw, gw + 33, 0);
    mGLServer.DrawGroundRectangle(Vector3f(fl/2 - sPenaltyLength, -gw/2 - sPenaltyLength, 0.05), sPenaltyLength, lw, 0);
    mGLServer.DrawGroundRectangle(Vector3f(fl/2 - sPenaltyLength, gw/2 + sPenaltyLength, 0.05), sPenaltyLength, lw, 0);

    // penalty area left side
    mGLServer.DrawGroundRectangle(Vector3f(-fl/2 + sPenaltyLength, -gw/2 - sPenaltyLength, 0.05), lw, gw + 33, 0);
    mGLServer.DrawGroundRectangle(Vector3f(-fl/2 + sPenaltyLength, -gw/2 - sPenaltyLength, 0.05), -sPenaltyLength, lw, 0);
    mGLServer.DrawGroundRectangle(Vector3f(-fl/2 + sPenaltyLength, gw/2 + sPenaltyLength, 0.05), -sPenaltyLength, lw, 0);

    // center circle
    mGLServer.DrawCircle(Vector3f(0.0,0.0,0.05), sCenterRadius);

    // fieldBox
    mGLServer.DrawWireBox(Vector3f(-fl/2.0,-fw/2.0,0.0),
                          Vector3f(fl,fw,fh));

    // draw shadows of cached positions
    DrawScene(0);
    glDisable(GL_DEPTH_TEST);

    // goal
    glColor4fv(sGoalColor);
    mGLServer.DrawWireBox(Vector3f(-fl/2,-gw/2.0,0),szGoal1);
    mGLServer.DrawGoal(Vector3f(-fl/2,-gw/2.0,0),szGoal1,lw/2.0);

    // goal
    glColor4fv(sGoalColor);
    mGLServer.DrawWireBox(Vector3f(fl/2,-gw/2.0,0),szGoal2);
    mGLServer.DrawGoal(Vector3f(fl/2,-gw/2.0,0),szGoal2,lw/2.0);

    // draw cached positions
    DrawScene(1);

    // JAN
    if (mDrawVelocity)
        DrawVelocities();

    glLoadIdentity();

    mGLServer.SetCameraPos( pos );
    mGLServer.SetLookAtPos( look );
		mGLServer.ApplyCamera();
}