self.cpp

agentspark 机器人模拟代码 适用robocup 机器人步态模拟仿真（机器人动作在NAOGETUP.cpp下修改）

/*
   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 "self.h"
#include "./Action/action.h"
#include "worldmodel.h"
#include "fieldinfo.h"
#include "./RobotModel/nao.h"
#include "./RobotModel/soccerbot.h"

#include "./Action/NaoGetUp.h"


using namespace std;
using namespace salt;
using namespace boost;
//using namespace zeitgeist;

Self::Self() :
    CurrentCycle(0),
    mRobotModel(EUnknownModel)
{	
}

Self::~Self()
{
}

void Self::OnUnlink()
{
    mRobot.reset();
}

string Self::GetModelFileName() const
{
    string name;

    switch (mRobotModel)
    {
        case ENaoModel:
            name = "rsg/agent/nao/nao.rsg"; break;
        case ESoccerbotModel:
            name = "rsg/agent/soccerbot056.rsg"; break;
        default:
           cerr << "(OnLink) Robot model unknown\n"; break;
    }
    return name;
}

bool Self::Init()
{	
    //robot type according to name
	string type = "nao";
    	if (type == "nao") 
	{	mRobotModel = ENaoModel;
		if (! NAO->Init())
    		{
       			cerr << "[Self] ERROR: Robot init failed\n";
        		return false;
    		}
	}
    	else if (type == "soccerbot") mRobotModel = ESoccerbotModel;
    	else mRobotModel = EUnknownModel;
    

    /** robot type according to dynamic cast */
   /* if (shared_dynamic_cast<Nao>(mRobot) != 0)
        mRobotModel = ENaoModel;
    else if (shared_dynamic_cast<Soccerbot>(mRobot) != 0)
        mRobotModel = ESoccerbotModel;
    else mRobotModel = EUnknownModel;*/

    return true;
}

shared_ptr<Action> Self::GetAction(const string& actionType) const
{
    shared_ptr<Action> action(new NaoGetUp); /*= shared_dynamic_cast<Action>
        //(mRobot->GetChildOfClass(actionType));
        (GetCore()->Get(mRobot->GetFullPath() + actionType));
    if (action.get() == 0)
    {
       cerr
            << "Robot " << mRobot->GetName()
            << " has no action named " << actionType << endl;
    }*/
    return action;
}

void Self::setGyroAngularVel ( int i, float value )
{	mGyroAngularVel[i] = value;
}

void Self::setLFCenterVel ( int i,float value )
{	mLFCenter[i] = value;
}

void Self::setRFCenterVel ( int i,float value )
{	mRFCenter[i] = value;
}

void Self::setLFForce( int i, float value )
{	mLFForce[i] = value;
}

void Self::setRFForce( int i, float value )
{	mRFForce[i] = value;
}

void Self::UpdateRobotMatrix(const Matrix& mat)
{	
    ++CurrentCycle;
    pos = mat.Pos();
    NAO->UpdateLink(mat);
}