monitor.cpp

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

 /* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2004 RoboCup Soccer Server 3D Maintenance Group
   $Id: monitor.cpp,v 1.30 2008/03/11 12:29:49 jboedeck Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "monitor.h"
#include "gamestate.h"
#include <getopt.h>
#include <iomanip>
#include <sstream>
#include <soccer/soccertypes.h>
#include <types.h>
#include <unistd.h>
#include <kerosin/openglserver/openglwrapper.h>
#include <queue>

#ifdef __APPLE__
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
#endif

void
display()
{
    Monitor::Instance()->Display();
}

void
mouseMotion(int x, int y)
{
    Monitor::Instance()->MouseMotion(x, y);
}

void
keyboard(unsigned char key, int x, int y)
{
    Monitor::Instance()->Keyboard(key, x, y);
}

void
specialkeys(int glutkey, int x, int y)
{
    Monitor::Instance()->SpecialKeys(glutkey, x, y);
}

void
mouse(int button, int state, int x, int y)
{
    Monitor::Instance()->Mouse(button, state, x, y);
}

void
reshape(int width, int height)
{
    Monitor::Instance()->Reshape(width, height);
}

void
idle()
{
    Monitor::Instance()->Idle();
}
// color constants
const GLfloat
Monitor::sGroundColor[4] = {0.1f, 0.5f, 0.1f, 1.0f};
const GLfloat
Monitor::sSecondGroundColor[4] = {0.05f, 0.43f, 0.07f, 1.0f};
const GLfloat
Monitor::sGoalColor[4]   = {0.8f, 0.8f, 0.8f, 1.0f};
const GLfloat
Monitor::sBorderColor[4] = {0.25f, 0.35f, 0.25f, 1.0f};
const GLfloat
Monitor::sLineColor[4]   = {0.8f, 1.0f, 0.8f, 0.8f};
const GLfloat
Monitor::sSphereDefaultColor[4] = { 0.8, 0.8, 0.2, 1.0 };

const GLfloat
Monitor::sUnumColor[4]      = {1.0, 1.0, 1.0, 1.0};
// color for player of the different teams
const GLfloat
Monitor::sTeamColorLeft[4] = {0.8f, 0.2f, 0.2f, 1.0f};
const GLfloat
Monitor::sTeamColorRight[4] = {0.2f, 0.2f, 0.8f, 1.0f};
// ball color
const GLfloat
Monitor::sBallColor[4] = {1.0f, 0.67f, 0.0f, 1.0f};
// debug color1
const GLfloat
Monitor::sDebugColorCyan[4] = {0.0f, 0.8f, 0.8f, 0.3f};
// debug color2
const GLfloat
Monitor::sDebugColorPink[4] = {1.0f, 0.0f, 1.0f, 0.5f};
//color for ground of overview field
const GLfloat
Monitor::sGroundColor2D[4] = {0.0f, 0.5f, 0.0f, 0.6f};

// radius of center circle
const float
Monitor::sCenterRadius = 9.15;

// length of the goal box
const float
Monitor::sGoalBoxLength = 5.5;

// length of the penalty area
const float
Monitor::sPenaltyLength = 16.5;

// 2D Overview constants
const float
Monitor::s2DLowX = 0.02; // left pos

const float
Monitor::s2DLowY = 0.15; // 0.12; // top pos

const float
Monitor::s2DScale = 0.0075; // Skalierung

Monitor::Monitor(std::string rel_path_prefix)
    : mZeitgeist("." PACKAGE_NAME, rel_path_prefix),
      mOxygen(mZeitgeist),
      mMonitorLib(mZeitgeist)
{
    mWidth = DEFAULT_WIDTH;
    mHeight = DEFAULT_HEIGHT;
    mCamDelta = 0.5;

    mCameraMode = eCenterBall;
    mShowCamIconTime = 0;
    mCamModeMap[eFree] = "[F]"; // F_ree mode
    mCamModeMap[eFollowBall] = "[A]"; // A_utomatic cam
    mCamModeMap[eCenterBall] = "[X]" ; // keep cam at X_coord of ball

    mDrawUnums = true;
    mDrawOverview = false;
    mDrawDebug = false;
    mDrawVelocity = false;
    mPauseGame = false;
    mServer = DEFAULT_HOST;
    mPort = DEFAULT_PORT;
    mSkip = 1;

    mCenterBallCameraY = -32.0;
    mCenterBallCameraZ = 20.0;
    mShowSecondView = false;
    mUseTexture = false;

    mLogserver = false;
    mSingleStep = false;
    mAdvance = false;
    mRealTime = true;
    mDiffTime = 0;
    mSecondHalfKickOff = CommServerBase::eRandom;

    FlagInfo fi;
    fi.mOffset = Vector3f(0,0,0);
    fi.mRadius = 0.1;
    memcpy(fi.mColor, sSphereDefaultColor, sizeof(fi.mColor));

    // corner flags left team
    mDefaultFlagInfo = fi;
    fi.mOffset = Vector3f(0,0,1.5);
    memcpy(fi.mColor, sTeamColorLeft, sizeof(fi.mColor));
    mFlagInfo[GameState::eFLAG_1_L] = fi;
    mFlagInfo[GameState::eFLAG_2_L] = fi;
    // goal flags left team
    fi.mOffset = Vector3f(0,0,1.6);
    mFlagInfo[GameState::eGOAL_1_L] = fi;
    mFlagInfo[GameState::eGOAL_2_L] = fi;
    // goal flags right team
    memcpy(fi.mColor, sTeamColorRight, sizeof(fi.mColor));
    mFlagInfo[GameState::eGOAL_1_R] = fi;
    mFlagInfo[GameState::eGOAL_2_R] = fi;
    // corner flags right team
    fi.mOffset = Vector3f(0,0,1.5);
    mFlagInfo[GameState::eFLAG_1_R] = fi;
    mFlagInfo[GameState::eFLAG_2_R] = fi;

    mTextureFile="grass.ppm";
}

Monitor::~Monitor()
{
}

Monitor*
Monitor::Instance()
{
    static std::auto_ptr<Monitor> the_monitor(new Monitor());
    return the_monitor.get();
}

Monitor::EReturnType
Monitor::Init(int argc, char* argv[])
{
    int c = 0;
    EReturnType status = eOK;
    int option_index;
    //bool logplayer = false;
    bool server = false;

    option long_options[] = {
        // name, has_arg, flag, val
        { "help", no_argument, 0, 'h' },
        { "port", required_argument, 0, 'p' },
        { "server", required_argument, 0, 's' },
        { "logfile", required_argument, 0, 'l' },
        { "msgskip", required_argument, 0, 'm' },
        { "texture", required_argument, 0, 't' },
        { 0, 0, 0, 0 }
    };

    while (true)
    {
        c = getopt_long(argc, argv, "", long_options, &option_index);

        if (c == -1) break;

        switch (c)
        {
        case 0: // a long option with a non-NULL flag has been given.
            break;
        case 'h': // --help
            status = eOptions;
            break;
        case 'p': // --port
            mPort = atoi(optarg);
            break;
        case 's': // --server
            mServer = std::string(optarg);
            server = true;
            break;
        case 'l': // -- logplayer
            mServer = std::string(optarg);
            mLogserver = true;
            break;
        case 'm': // --msgskip
            mSkip = atoi(optarg);
            break;
        case 't': // --texture
            mTextureFile = std::string(optarg);
            break;
        default:
            status = eErrInit;
        }
    }

//    if (logplayer && server)
    if (mLogserver && server)
    {
        status = eErrInit;
        mZeitgeist.GetCore()->GetLogServer()->Error()
            << "ERROR: cannot play logfiles and connect to simulator at the same time\n";
    }

    mZeitgeist.GetCore()->GetScriptServer()->RunInitScript
        ("rcssmonitor3D-lite.rb", "app/rcssmonitor3d/lite");

    if (status == eOK)
    {
        shared_ptr<CoreContext> context = mZeitgeist.GetCore()->CreateContext();
        if (mLogserver)
        {
            context->New("rcssmonitor3d/LogfileServer", "/sys/server/comm");
        } else {
            context->New("rcssmonitor3d/CommServer", "/sys/server/comm");
        }

        status = InitInternal(argc,argv);
    }

    return status;
}

Monitor::EReturnType
Monitor::Run()
{
    glutMainLoop();
    return eOK;
}

void
Monitor::Usage()
{
    std::cerr <<
"Usage: rcssserver3D-lite [options]\n"
"Simple visualization of rcssserver3D soccer matches\n\n"
"   --help      Print this message and exit.\n"
"   --port      Specify the port number (default is " << DEFAULT_PORT << ").\n"
"   --server    Specify the server host (default is '" << DEFAULT_HOST << "').\n"
"   --logfile   Specify the logfile to read (not with --server).\n"
"   --msgskip   Every but the nth message should be discarded (default is 1).\n"
"   --texture   Set the name of the texturefile to use.  If no absolute path is given\n"
"               the file is assumed to reside in your ~/.rcssserver3d/ directory.\n"
"\n"
"Press '?' for a list of keybindings.\n"
"\n";
}

void
Monitor::Copyleft()
{
    std::cerr << "rcssmonitor3D-lite written by\n"
              << "Heni Ben Amor, Oliver Obst, Christoph Ringelstein, Markus Rollmann & Jan Murray;\n"
              << "Copyright (C) 2003 Universitaet Koblenz, Germany.\n"
              << "Copyright (C) 2004-2007 The RoboCup Soccer Server Maintenance Group.\n\n"
              << "Special additions by Tobias Warden from TZI Bremen/virtual werder.\n\n";

}

void
Monitor::KeyBindings()
{
    std::cerr <<
        "\n\nKeybindings for rcssmonitor3D-lite:\n"
        "\n"
        "----------------------------------------------------\n"
        "GENERAL\n"
        "----------------------------------------------------\n"
        "q          | quit the monitor\n"
        "p          | pause the simulation/logplayer\n"
        //"r          | unpause the simulation/logplayer\n"
        "n          | toggle display of uniform numbers\n"
        "1          | toggle display of debug information\n"
        "2          | toggle display of 2D overview\n"
        "v          | toggle display of velocities\n"
        "t          | toggle display of texture\n"
        "Z          | switch to fullscreen\n"
        "X          | toggle display of 2nd view (pip)\n"
        "?          | display keybindings\n"
        "----------------------------------------------------\n"
        "CAMERA MOVEMENT\n"
        "----------------------------------------------------\n"
        "c          | switch through camera modes (F,A,X)\n"
        "Arrow keys | move camera\n"
        "w/s        | move camera forward/back (zoomlike)\n"
        "a/d        | move camera left/right\n"
        "(/)        | rotate camera left/right\n"
        "[/]        | adjust y-coord of camera in autocam mode (X)\n"
        "{/}        | adjust height of camera in autocam mode (X)\n"
        "+/-        | move camera up/down\n"
        "3          | move camera behind left goal\n"
        "4          | move camera behind right goal\n"
        "5          | cycle through stationary cam positions on\n"
        "           | left half\n"
        "6          | cycle through stationary cam positions on\n"
        "           | right half\n"
        "----------------------------------------------------\n"
        "SIMULATION\n"
        "----------------------------------------------------\n"
        "k          | kick off (start the game)\n"
        "b          | drop the ball at its current position\n"
        "g          | kick the goalies towards the field,\n"
        "           | if they sit on a goal\n"
        "[SPACE]    | toggle kick off side (left-right-random)\n"
        "----------------------------------------------------\n"
        "LOGPLAYER\n"
        "----------------------------------------------------\n"
        "f          | toggle fast/realtime replay\n"
        "F          | Realtime replay\n"
        "m          | toggle single step mode for logplayer\n"
        ">          | move one step forward\n"
        "<          | move one step back\n"
        "----------------------------------------------------\n"
        "\n";

}

Monitor::EReturnType
Monitor::InitInternal(int argc, char* argv[])
{
    // init the commserver
    mCommServer = shared_dynamic_cast<CommServerBase>
        (mZeitgeist.GetCore()->Get("/sys/server/comm"));

    if (mCommServer.get() == 0)
    {
        mZeitgeist.GetCore()->GetLogServer()->Error()
            << "ERROR: CommServer not found\n";
        return eErrInit;
    }

    mCommServer->Init("SexpParser",mServer,mPort);

    // init glut
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
    glutInitWindowSize(mWidth, mHeight);
    glutCreateWindow("rcssmonitor3D");
    glutDisplayFunc(display);
    glutMotionFunc(mouseMotion);
    glutKeyboardFunc(keyboard);
    glutSpecialFunc(specialkeys);
    glutMouseFunc(mouse);
    glutReshapeFunc(reshape);
    glutIdleFunc(idle);
    glutSetCursor(GLUT_CURSOR_NONE);


    // setup the GLserver with camera coordinates
    // and texture
    salt::Vector3f pos(0.0,-30.0, 12.0);
    salt::Vector3f lookAt(0.0,0.0,0.0);
    salt::Vector3f up(0.0,0.0,1.0);
    mGLServer = GLServer(mWidth, mHeight, pos, lookAt, up, false);

    mGLServer.InitTexture(mTextureFile);

    mGLServer.InitGL();

    glBlendFunc(GL_SRC_ALPHA,GL_ONE);



// Somewhere in the initialization part of your program
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);

// Create light components
GLfloat ambientLight[] = { 0.2f, 0.2f, 0.2f, 1.0f };
GLfloat diffuseLight[] = { 0.8f, 0.8f, 0.8, 1.0f };
GLfloat specularLight[] = { 0.5f, 0.5f, 0.5f, 1.0f };
GLfloat position[] = { 0.0f, 0.0f, 10.0f, 2.0f };

// Assign created components to GL_LIGHT0
glLightfv(GL_LIGHT0, GL_AMBIENT, ambientLight);
glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuseLight);
glLightfv(GL_LIGHT0, GL_SPECULAR, specularLight);
glLightfv(GL_LIGHT0, GL_POSITION, position);




    return eOK;
}

void
Monitor::SetupCameraPositions()
{
    mLeftCamPositions.clear();
    mRightCamPositions.clear();
    float hfl = mGameState.GetFieldLength()/2;
    float hfw = mGameState.GetFieldWidth()/2;

    salt::Vector3f lp, rp;

    // first position somewhere in the middle of the respective half
    // on the side of the field
    lp[0] = -1*hfl/2;        rp[0] = -lp[0];
    lp[1] = -hfw-4;          rp[1] = lp[1];
    lp[2] = 5.5;             rp[2] = lp[2];
    mLeftCamPositions.push_back(lp);
    mRightCamPositions.push_back(rp);

    // second position somewhere in the middle of the respective half
    // but zoomed into the field
    lp[0] = -1*hfl/2;        rp[0] = -lp[0];
    lp[1] = -hfw+4;          rp[1] = lp[1];
    lp[2] = 10.0;             rp[2] = lp[2];
    mLeftCamPositions.push_back(lp);