C++ shared_ptr::AddPredicate方法代码示例

void
SexpParser::SexpToPredicate
(boost::shared_ptr<PredicateList>& predList, const sexp_t* const sexp)
{
    // throw away outer brackets (i.e. we have a list at the top
    // level)
    if (sexp->ty != SEXP_LIST)
    {
        return;
    }

    sexp_t* s = const_cast<sexp_t*>(sexp->list);

    if (
        (s == 0) ||
        (s->ty != SEXP_VALUE)
        )
        {
            return;
        }

    Predicate& predicate = predList->AddPredicate();
    predicate.name = string(s->val);
    SexpToList(predicate.parameter,s->next);
}

bool ImagePerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
  mRender->RequestRender();

  int size = mRender->GetDataSize();
  if (size == 0)
    return false;

  Predicate &predicate = predList->AddPredicate();
  predicate.name = mPredicateName + "IMG";
  predicate.parameter.Clear();

  ParameterList &sizeElement = predicate.parameter.AddList();
  sizeElement.AddValue(std::string("s"));
  sizeElement.AddValue(mRender->GetWidth());
  sizeElement.AddValue(mRender->GetHeight());

  ParameterList &dataElement = predicate.parameter.AddList();
  dataElement.AddValue(std::string("d"));
  const char* data = mRender->GetData();
  string datacode = mB64Encoder.encode(data, size);
  dataElement.AddValue(datacode);

  return true;
}

bool
GyroRatePerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
    Predicate &predicate = predList->AddPredicate();
    predicate.name = "GYR";
    predicate.parameter.Clear();
    
    ParameterList &nameElement = predicate.parameter.AddList();
    nameElement.AddValue(std::string("n"));
    nameElement.AddValue(GetName());       

    Matrix invRot = mBody->GetRotation();
    invRot.InvertRotationMatrix();
    Vector3f rate = invRot * mBody->GetAngularVelocity();

    ParameterList &ratesElement = predicate.parameter.AddList();    
    ratesElement.AddValue(std::string("rt"));
    ratesElement.AddValue(gRadToDeg(rate.x()));
    ratesElement.AddValue(gRadToDeg(rate.y()));
    ratesElement.AddValue(gRadToDeg(rate.z()));

   // What should be done when yrotate is around 90? in that case, the parameters of the atan2 are 0!
//    const dReal* q = dBodyGetQuaternion(mBody->GetODEBody());
//    float xrotate = gArcTan2(2*(q[0]*q[1]+q[2]*q[3]), 1-2*(q[1]*q[1]+q[2]*q[2]));
//    float yrotate = gArcSin(2*(q[0]*q[2] - q[3]*q[1]));
//    float zrotate = gArcTan2(2*(q[0]*q[3] + q[1]*q[2]), 1-2*(q[2]*q[2]+q[3]*q[3]));
//    
//    ParameterList & anglesElement = predicate.parameter.AddList();    
//    anglesElement.AddValue(std::string("ang"));
//    anglesElement.AddValue(gRadToDeg(xrotate));
//    anglesElement.AddValue(gRadToDeg(yrotate));
//    anglesElement.AddValue(gRadToDeg(zrotate));
    
    return true;
}

bool
VisionPerceptor::StaticAxisPercept(boost::shared_ptr<PredicateList> predList)
{
    Predicate& predicate = predList->AddPredicate();
    predicate.name       = mPredicateName;
    predicate.parameter.Clear();

    TTeamIndex  ti       = mAgentState->GetTeamIndex();
    salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();

    TObjectList visibleObjects;
    SetupVisibleObjects(visibleObjects);

    for (std::list<ObjectData>::iterator i = visibleObjects.begin();
         i != visibleObjects.end(); ++i)
    {
        ObjectData& od = (*i);

        od.mRelPos = SoccerBase::FlipView(od.mRelPos, ti);
        if (mAddNoise)
            {
                od.mRelPos += mError;
            }

        if (
            (od.mRelPos.Length() <= 0.1) ||
            (CheckOcclusion(myPos,od))
            )
        {
            // object is occluded or too close
            continue;
        }

        // theta is the angle in the X-Y (horizontal) plane
        od.mTheta = salt::gRadToDeg(salt::gArcTan2(od.mRelPos[1], od.mRelPos[0]));

        // latitude
        od.mPhi = 90.0 - salt::gRadToDeg(salt::gArcCos(od.mRelPos[2]/od.mDist));

        // make some noise
        ApplyNoise(od);

        // generate a sense entry
        AddSense(predicate,od);
    }

    if (mSenseMyPos)
    {
        Vector3f sensedMyPos = SoccerBase::FlipView(myPos, ti);

        ParameterList& element = predicate.parameter.AddList();
        element.AddValue(std::string("mypos"));
        element.AddValue(sensedMyPos[0]);
        element.AddValue(sensedMyPos[1]);
        element.AddValue(sensedMyPos[2]);
    }

    return true;
}

bool
AgentStatePerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
    if (mAgentState.get() == 0)
    {
        return false;
    }

    --mSenses;

    if (mSenses > 0)
    {
        return false;
    }

    mSenses = mPerceptRate;

    Predicate& predicate = predList->AddPredicate();
    predicate.name = "AgentState";
    predicate.parameter.Clear();

    boost::shared_ptr<BaseNode> parent =
        dynamic_pointer_cast<BaseNode>(GetParent().lock());

    if (parent.get() == 0)
    {
        GetLog()->Warning() << "WARNING: (AgentStatePerceptor) "
                            << "parent node is not derived from BaseNode\n";
    } else {
        boost::shared_ptr<RestrictedVisionPerceptor> rvp =
            parent->FindChildSupportingClass<RestrictedVisionPerceptor>(false);
        if (rvp.get() == 0)
        {
            GetLog()->Warning() << "WARNING: (AgentStatePerceptor) "
                                << "cannot find RestrictedVisionPerceptor instance\n";
        } else {
            // pan tilt information
            ParameterList& pantiltElement = predicate.parameter.AddList();
            pantiltElement.AddValue(std::string("pan_tilt"));
            pantiltElement.AddValue(gRound(rvp->GetPan()));
            pantiltElement.AddValue(gRound(rvp->GetTilt()));
        }
    }

    // battery
    ParameterList& batteryElement = predicate.parameter.AddList();
    batteryElement.AddValue(std::string("battery"));
    batteryElement.AddValue(mAgentState->GetBattery());

    // temperature
    ParameterList& tempElement = predicate.parameter.AddList();
    tempElement.AddValue(std::string("temp"));
    tempElement.AddValue(mAgentState->GetTemperature());

    return true;
}

bool ForceResistancePerceptor::Percept(
        boost::shared_ptr<oxygen::PredicateList> predList)
{
    if (mContactList.empty())
    {
        mLastForce.Zero();
        mLastCenter.Zero();
        return false;
    }

    Vector3f force(0,0,0);
    Vector3f center(0,0,0);
    float sumLength = 0;
    for (TContactList::const_iterator i = mContactList.begin();
         i!= mContactList.end();
         ++i)
        {
            Vector3f forceVec(i->second.f1[0], i->second.f1[1], i->second.f1[2]);
            force += forceVec;

            float forcevalue = forceVec.Length();
            center += Vector3f(i->first.pos[0], i->first.pos[1], i->first.pos[2])
                    * forcevalue;
            sumLength += forcevalue;
        }

    if (sumLength != 0) // sumLength will be zero when the body is disabled
        {
            Matrix invRot = mBody->GetLocalTransform();
            invRot.InvertRotationMatrix();
            mLastCenter = invRot * (center / sumLength);
            mLastForce = invRot.Rotate(force);
        }

    Predicate& predicate = predList->AddPredicate();
    predicate.name = "FRP";
    predicate.parameter.Clear();

    ParameterList& nameElement = predicate.parameter.AddList();
    nameElement.AddValue(std::string("n"));
    nameElement.AddValue(GetName());

    ParameterList& posElement = predicate.parameter.AddList();
    posElement.AddValue(std::string("c"));
    posElement.AddValue(mLastCenter.x());
    posElement.AddValue(mLastCenter.y());
    posElement.AddValue(mLastCenter.z());

    ParameterList& forceElement = predicate.parameter.AddList();
    forceElement.AddValue(std::string("f"));
    forceElement.AddValue(mLastForce.x());
    forceElement.AddValue(mLastForce.y());
    forceElement.AddValue(mLastForce.z());

    return true;
}

bool
GameStatePerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
    if (
        (mGameState.get() == 0) ||
        (mAgentState.get() == 0)
        )
    {
        return false;
    }

    Predicate& predicate = predList->AddPredicate();
    predicate.name = "GS";
    predicate.parameter.Clear();

    // with the first GameState percept after the player is assigned
    // to a team it receives info about it's team and unum assignment
    // along with outher soccer parameters
    if (
        (mFirstPercept) &&
        (mAgentState->GetTeamIndex() != TI_NONE)
        )
    {
        mFirstPercept = false;
        InsertInitialPercept(predicate);
    }

    if (mReportScore) {
        // score left
        ParameterList& slElement = predicate.parameter.AddList();
        slElement.AddValue(string("sl"));
        slElement.AddValue(mGameState->GetScore(TI_LEFT));
        
        // score right
        ParameterList& srElement = predicate.parameter.AddList();
        srElement.AddValue(string("sr"));
        srElement.AddValue(mGameState->GetScore(TI_RIGHT));
    }

    // time
    ParameterList& timeElement = predicate.parameter.AddList();
    timeElement.AddValue(string("t"));
    timeElement.AddValue(mGameState->GetTime());

    // playmode
    ParameterList& pmElement = predicate.parameter.AddList();
    pmElement.AddValue(string("pm"));
    pmElement.AddValue(SoccerBase::PlayMode2Str(mGameState->GetPlayMode()));

    return true;
}

bool ImagePerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
    if ( !Render() )
        return false;
    
    Predicate &predicate = predList->AddPredicate();
    predicate.name = "Image";
    predicate.parameter.Clear();
    
    // ParameterList &sizeElement = predicate.parameter.AddList();
    // sizeElement.AddValue(std::string("s"));
    // sizeElement.AddValue(mCamera->GetViewportWidth());
    // sizeElement.AddValue(mCamera->GetViewportHeight());

    predicate.parameter.AddValue(mData);
    return true;
}

bool Accelerometer::Percept(boost::shared_ptr<PredicateList> predList)
{
    Predicate &predicate = predList->AddPredicate();
    predicate.name = "ACC";
    predicate.parameter.Clear();

    ParameterList &nameElement = predicate.parameter.AddList();
    nameElement.AddValue(std::string("n"));
    nameElement.AddValue(GetName());

    ParameterList &ratesElement = predicate.parameter.AddList();
    ratesElement.AddValue(std::string("a"));
    ratesElement.AddValue(mAcc.x());
    ratesElement.AddValue(mAcc.y());
    ratesElement.AddValue(mAcc.z());

    return true;
}

bool
GameStatePerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
    if (
        (mGameState.get() == 0) ||
        (mAgentState.get() == 0)
        )
    {
        return false;
    }

    Predicate& predicate = predList->AddPredicate();
    predicate.name = "GS";
    predicate.parameter.Clear();

    // with the first GameState percept after the player is assigned
    // to a team it receives info about it's team and unum assignment
    // along with outher soccer parameters
    if (
        (mFirstPercept) &&
        (mAgentState->GetTeamIndex() != TI_NONE)
        )
    {
        mFirstPercept = false;
        InsertInitialPercept(predicate);
    }

    // time
    ParameterList& timeElement = predicate.parameter.AddList();
    timeElement.AddValue(string("t"));
    timeElement.AddValue(mGameState->GetTime());

    // playmode
    ParameterList& pmElement = predicate.parameter.AddList();
    pmElement.AddValue(string("pm"));
    if (mAgentState->IsPenalized())
        pmElement.AddValue(string("penalized"));
    else
        pmElement.AddValue(mGameState->GetPlayModeStr());

    return true;
}

bool HingePerceptor::Percept(boost::shared_ptr<oxygen::PredicateList> predList)
{
    if (mJoint.get() == 0)
        {
            return false;
        }

    Predicate& predicate = predList->AddPredicate();
    predicate.name = "HJ";
    predicate.parameter.Clear();

    ParameterList& nameElement = predicate.parameter.AddList();
    nameElement.AddValue(string("n"));
    nameElement.AddValue(GetName());

    InsertAxisAngle(predicate);
    //InsertAxisRate(predicate);
    //InsertAxisTorque(predicate);
    
    return true;
}

bool TimePerceptor::Percept(boost::shared_ptr<oxygen::PredicateList> predList)
{
    if (mSimulationServer.get() == 0)
        {
            return false;
        }

    Predicate& predicate = predList->AddPredicate();
    predicate.name = "time";
    predicate.parameter.Clear();

    ParameterList& nowElement = predicate.parameter.AddList();
    nowElement.AddValue(string("now"));
    nowElement.AddValue(mSimulationServer->GetTime());

//     ParameterList& stepElement = predicate.parameter.AddList();
//     stepElement.AddValue(string("st"));
//     stepElement.AddValue(mSimulationServer->GetSimStep());

    return true;
}

bool
GPS::Percept(boost::shared_ptr<PredicateList> predList)
{
    Predicate &predicate = predList->AddPredicate();
    predicate.name = "GPS";
    predicate.parameter.Clear();
    
    ParameterList &nameElement = predicate.parameter.AddList();
    nameElement.AddValue(std::string("n"));
    nameElement.AddValue(GetName());       

    const Matrix& tf = mTransformParent->GetWorldTransform();

    ParameterList &tfElement = predicate.parameter.AddList();
    tfElement.AddValue(std::string("tf"));
    for(int i=0; i<4; i++) {
        for(int j=0; j<4; j++) {
            tfElement.AddValue(tf(i, j));
        }
    }
    
    return true;
}

bool
GameTimePerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
    if (mGameState.get() == 0)
    {
        return false;
    }

    Predicate& predicate = predList->AddPredicate();
    predicate.name = "time";
    predicate.parameter.Clear();

    // with the first GameState percept after the player is assigned
    // to a team it receives info about it's team and unum assignment
    // along with outher soccer parameters

    // time
    ParameterList& timeElement = predicate.parameter.AddList();
    timeElement.AddValue(string("now"));
    timeElement.AddValue(mGameState->GetTime());

    return true;
}

bool
HearPerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
    if ( 
        (mAgentState.get() == 0) ||
        (mGameState.get() == 0)
        )		 
    {
        return false;
    }

    bool result = false;

    string message;
    string team;
    float direction;
		
    if (mAgentState->GetSelfMessage(message))
    {
        string self = "self";
        Predicate& predicate = predList->AddPredicate();
        predicate.name = "hear";
        predicate.parameter.Clear();
        if (mLabelMessages) {
            predicate.parameter.AddValue(mAgentState->GetPerceptName(ObjectState::PT_Player));
        }
        predicate.parameter.AddValue(mGameState->GetTime());
        predicate.parameter.AddValue(self);
        predicate.parameter.AddValue(message);
        result = true;
    }

    if (mAgentState->GetMessage(message, team, direction, true))
    {
        Predicate& predicate = predList->AddPredicate();
        predicate.name = "hear";
        predicate.parameter.Clear();
        if (mLabelMessages) {
            predicate.parameter.AddValue(team);
        }
        predicate.parameter.AddValue(mGameState->GetTime());
        predicate.parameter.AddValue(direction);
        predicate.parameter.AddValue(message);
        result = true;
    }

    if (mAgentState->GetMessage(message, team, direction, false))
    {
        Predicate& predicate = predList->AddPredicate();
        predicate.name = "hear";
        predicate.parameter.Clear();
        if (mLabelMessages) {
            predicate.parameter.AddValue(team);
        }
        predicate.parameter.AddValue(mGameState->GetTime());
        predicate.parameter.AddValue(direction);
        predicate.parameter.AddValue(message);
        result = true;
    }

    return result;
}