simulationserver.cpp

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

void SimulationServer::Init(int argc, char** argv)
{
    GetLog()->Normal() << "(SimulationServer) init\n";
    mExit = false;

    // cache argc and argv, to make it accessible for registerd
    // SimControlNodes
    mArgC = argc;
    mArgV = argv;

    ControlEvent(CE_Init);
}

void SimulationServer::Run(int argc, char** argv)
{
    Init(argc, argv);
    GetLog()->Normal() << "(SimulationServer) entering runloop\n";

    if ( mMultiThreads )
        {
            GetLog()->Normal()<< "(SimulationServer) running in multi-threads\n";
            RunMultiThreaded();
        }
    else
        {
            GetLog()->Normal()<< "(SimulationServer) running in single thread\n";
            shared_ptr<SimControlNode> inputCtr = GetControlNode("InputControl");
            if ( !mAutoTime && inputCtr.get() == 0 )
                {
                    GetLog()->Error()<< "(SimulationServer) ERROR: can not get InputControl\n";
                }
            else
                {
                    while (! mExit)
                        {
                            Cycle(inputCtr);
                        }
                }
        }

    Done();
}

void SimulationServer::Cycle(shared_ptr<SimControlNode> &inputCtr)
{
    ++mCycle;

    ControlEvent(CE_StartCycle);
    ControlEvent(CE_SenseAgent);
    ControlEvent(CE_ActAgent);

    if (mAutoTime)
        {
            AdvanceTime(mSimStep);
        }
    else
        {
            if (inputCtr.get() != 0)
                {
                    while (int(mSumDeltaTime*100) < int(mSimStep*100))
                        {
                            inputCtr->StartCycle();// advance the time
                        }
                }
        }
    Step();

    ControlEvent(CE_EndCycle);
}

void SimulationServer::Done()
{
    ControlEvent(CE_Done);

    mArgC = 0;
    mArgV = 0;

    GetLog()->Normal()
        << "(SimulationServer) leaving runloop at t="
        << mSimTime << "\n";
}

shared_ptr<GameControlServer> SimulationServer::GetGameControlServer()
{
    return mGameControlServer.get();
}

shared_ptr<MonitorServer> SimulationServer::GetMonitorServer()
{
    return mMonitorServer.get();
}

shared_ptr<SceneServer> SimulationServer::GetSceneServer()
{
    return mSceneServer.get();
}

void SimulationServer::Loops()
{
    if (
        (mSceneServer.get() == 0) ||
        (mGameControlServer.get() == 0)
        )
        {
            return;
        }

    bool isStep = false;
    while (! mExit)
        {
            if( isStep)
                {
                    mSceneServer->PhysicsUpdate(mSimStep);
                }

            // lock all SimControlNode threads
            vector< boost::shared_ptr<boost::mutex::scoped_lock> > locks;
            for ( TLeafList::iterator iter=begin(); iter != end(); ++iter )
                {
                    shared_ptr<SimControlNode> ctrNode =  shared_dynamic_cast<SimControlNode>(*iter);
                    if (ctrNode.get() == 0) continue;
                    boost::shared_ptr<boost::mutex::scoped_lock> lp(new boost::mutex::scoped_lock(ctrNode->mMutex));
                    locks.push_back(lp);
                    ctrNode->Wait(*lp);
                }

            ++mCycle;

            if ( isStep )
                {
                    mSceneServer->PostPhysicsUpdate();
                    mGameControlServer->Update(mSimStep);
                    mSumDeltaTime -= mSimStep;
                    mSimTime += mSimStep;
                    isStep = false;
                }

            if( int(mSumDeltaTime*100) >= int(mSimStep*100) )
                {
                    mSceneServer->PrePhysicsUpdate(mSimStep);
                    isStep = true;
                }

            // notify all SimControlNode threads
            for ( TLeafList::iterator iter=begin(); iter != end(); ++iter )
                {
                    shared_ptr<SimControlNode> ctrNode =  shared_dynamic_cast<SimControlNode>(*iter);
                    if (ctrNode.get() == 0) continue;
                    ctrNode->NotifyOne();
                }
        }
}

void SimulationServer::RunMultiThreaded()
{
    if (mSimStep == 0)
        {
            GetLog()->Error() << "(SimulationServer) ERROR: multi-threaded "
                    << "mode supports descreet simulations only.\n";
            return;
        }

    boost::thread_group ctrThrdGroup;

    // count valid SimControlNodes.
    int count = 1;
    for ( TLeafList::iterator iter=begin(); iter != end(); ++iter )
        {
            if (shared_dynamic_cast<SimControlNode>(*iter))
                count++;
        }
    mThreadBarrier = new barrier(count);

    // create new threads for each SimControlNode
    for ( TLeafList::iterator iter=begin(); iter != end(); ++iter )
        {
            shared_ptr<SimControlNode> ctrNode =  shared_dynamic_cast<SimControlNode>(*iter);
            if (ctrNode.get() == 0) continue;
//            ctrThrdGroup.create_thread(boost::bind(&SimControlNode::Run, ctrNode.get()));
            ctrThrdGroup.create_thread(boost::bind(&SimulationServer::SimControlThread,
                                                   this, ctrNode));
        }
    // Loops();

    shared_ptr<SimControlNode> renderControl = GetControlNode("RenderControl");

    while (true)
        {
            ++mCycle;
            mThreadBarrier->wait();
            mSceneServer->PrePhysicsUpdate(mSimStep);
            mThreadBarrier->wait();
            mSceneServer->PhysicsUpdate(mSimStep);
            if (mAutoTime)
                AdvanceTime(mSimStep);

            mThreadBarrier->wait();

            if (mExit) // this check should be here so that all threads will quit
                break;

            UpdateDeltaTimeAfterStep();

            float finalStep = mSimStep;
            while (int(mSumDeltaTime*100) >= int(mSimStep*100))
                {
                    mSceneServer->PhysicsUpdate(mSimStep);
                    UpdateDeltaTimeAfterStep();
                    finalStep += mSimStep;
                }
            mSceneServer->PostPhysicsUpdate();
            mGameControlServer->Update(finalStep);
            mSimTime += finalStep;
            if (renderControl
                && int(renderControl->GetTime()*100) < int(mSimTime*100))
                renderControl->EndCycle();
            mThreadBarrier->wait();
        }

    // wait for threads
    ctrThrdGroup.join_all();
}

void SimulationServer::SimControlThread(shared_ptr<SimControlNode> controlNode)
{
    if (!mThreadBarrier)
        {
            GetLog()->Error()
                << "(SimulationServer) mThreadBarrier is not initialized.\n";
            return;
        }

    bool isInputControl = (controlNode->GetName() == "InputControl");
    bool isRenderControl = (controlNode->GetName() == "RenderControl");
    bool newCycle = false;

    while ( true )
        {
            mThreadBarrier->wait();
            // wait for PrePhysicsUpdate()
            mThreadBarrier->wait();
            newCycle = false;
            if (int(controlNode->GetTime()*100) < int(mSimTime*100))
                {
                    newCycle = true;
                    controlNode->StartCycle();
                    controlNode->SenseAgent();
                    controlNode->ActAgent();
                    controlNode->SetSimTime(mSimTime);
                }
            if (isInputControl)
                {
                    while (int(mSumDeltaTime*100) < int(mSimStep*100))
                        controlNode->StartCycle(); // advance the time
                }
            mThreadBarrier->wait();
            if (mExit)
                break;
            // wait for physics update
            mThreadBarrier->wait();
            if (!isRenderControl && newCycle)
                controlNode->EndCycle();
        }
}

void SimulationServer::SetMultiThreads(bool isMThreas)
{
    mMultiThreads = isMThreas;
}

void SimulationServer::SetAdjustSpeed(bool adjustSpeed)
{
    mAdjustSpeed = adjustSpeed;
}

void SimulationServer::SetMaxStepsPerCycle(int max)
{
    mMaxStepsPerCycle = max;
}

inline void SimulationServer::UpdateDeltaTimeAfterStep()
{
    if (mAdjustSpeed && mSumDeltaTime > mMaxStepsPerCycle
            * mSimStep)
        {
            GetLog()->Error()
                    << "(SimulationServer) ERROR: Skipping remaining time: "
                    << mSumDeltaTime - mSimStep << '\n';
            mSumDeltaTime = 0;
        }
    else
        mSumDeltaTime -= mSimStep;
}