restrictedvisionperceptor.cpp

rcssserver3d Robocup 3D比赛官方指定平台

/* -*- 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) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: restrictedvisionperceptor.cpp,v 1.8 2008/05/29 20:12:57 benpwd 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 "restrictedvisionperceptor.h"
#include <zeitgeist/logserver/logserver.h>
#include <oxygen/sceneserver/scene.h>
#include <oxygen/sceneserver/transform.h>
#include <soccer/soccerbase/soccerbase.h>
#include <salt/gmath.h>

using namespace zeitgeist;
using namespace oxygen;
using namespace boost;
using namespace salt;

RestrictedVisionPerceptor::RestrictedVisionPerceptor() : Perceptor(),
                                     mSenseMyPos(false),
                                     mAddNoise(true),
                                     mStaticSenseAxis(true)
{
    // set predicate name
    SetPredicateName("See");
    // set some default noise values
    SetNoiseParams(0.0965f, 0.1225f, 0.1480f, 0.005f);
    SetViewCones(90,90);
    SetPanRange(-90,90);
    SetTiltRange(-20,20);
    SetPanTilt(0,0);
}

RestrictedVisionPerceptor::~RestrictedVisionPerceptor()
{
    mDistRng.reset();
    mPhiRng.reset();
    mThetaRng.reset();
}

void
RestrictedVisionPerceptor::SetNoiseParams(float sigma_dist, float sigma_phi,
                                float sigma_theta, float cal_error_abs)
{
    mSigmaDist = sigma_dist;
    mSigmaPhi = sigma_phi;
    mSigmaTheta = sigma_theta;
    mCalErrorAbs = cal_error_abs;

    NormalRngPtr rng1(new salt::NormalRNG<>(0.0,sigma_dist));
    mDistRng = rng1;
    NormalRngPtr rng2(new salt::NormalRNG<>(0.0,sigma_phi));
    mPhiRng = rng2;
    NormalRngPtr rng3(new salt::NormalRNG<>(0.0,sigma_theta));
    mThetaRng = rng3;

    salt::UniformRNG<float> rng4(-mCalErrorAbs,mCalErrorAbs);
    mError = salt::Vector3f(rng4(),rng4(),rng4());
}

void
RestrictedVisionPerceptor::SetViewCones(unsigned int hAngle, unsigned int vAngle)
{
    mHViewCone = hAngle;
    mVViewCone = vAngle;
}

void
RestrictedVisionPerceptor::SetPanRange(int lower, int upper)
{
    /* The total pan range is 360 degrees, starting at -180 degrees
     * and ending at 180 degrees. For restricting this range:
     * - the normal case is: lower < upper, which means that
     *   the horizontal pan angles should be between lower and upper.
     * - the not so normal case is upper < lower, which means that
     *   the pan angles should be *either* smaller than 180 and > lower, *or*
     *   they should be greater than -180 and < upper.
     */
    mPanLower = static_cast<int>(gNormalizeDeg(lower));
    mPanUpper = static_cast<int>(gNormalizeDeg(upper));
}

void
RestrictedVisionPerceptor::SetTiltRange(int lower, int upper)
{
    mTiltLower = static_cast<int>(gNormalizeDeg(lower));
    mTiltUpper = static_cast<int>(gNormalizeDeg(upper));
}

// this should really go to gmath.h for the full release
template <class TYPE1, class TYPE2>
f_inline TYPE1 gClampAngleDeg(TYPE1 &val, TYPE2 min, TYPE2 max)
{
    val = gNormalizeDeg(val);
    if (min <= max)
    {
        if (val<min) val=min;
        if (val>max) val=max;
    } else {
        if (val>=min || val<=max) return val;
        if (val>=(min+max)/2.0) val = min; else val = max;
    }
    return val;
}



void
RestrictedVisionPerceptor::SetPanTilt(float pan, float tilt)
{
    pan = gNormalizeDeg(pan);
    mPan = gClampAngleDeg(pan,mPanLower,mPanUpper);
    tilt = gNormalizeDeg(tilt);
    mTilt = gClampAngleDeg(tilt,mTiltLower,mTiltUpper);
}

void
RestrictedVisionPerceptor::ChangePanTilt(float pan, float tilt)
{
    SetPanTilt(mPan + pan, mTilt + tilt);
}

float
RestrictedVisionPerceptor::GetPan() const
{
    return mPan;
}

float
RestrictedVisionPerceptor::GetTilt() const
{
    return mTilt;
}

void
RestrictedVisionPerceptor::OnLink()
{
    SoccerBase::GetTransformParent(*this,mTransformParent);
//     SoccerBase::GetAgentState(*this, mAgentState);
    SoccerBase::GetActiveScene(*this,mActiveScene);
    
    shared_ptr<AgentAspect> agent_aspect =
        make_shared(FindParentSupportingClass<AgentAspect>());
    if (agent_aspect == 0)
    {
        GetLog()->Error()
            << "Error: (RestrictedVisionPerceptor) cannot find AgentAspect.\n";
    }
    else
    {
        mAgentAspect = agent_aspect;
        agent_aspect = make_shared(agent_aspect->FindParentSupportingClass<AgentAspect>());
        if (agent_aspect != 0)
        {
            mAgentAspect = agent_aspect;
        }
        
        mAgentState = shared_static_cast<AgentState>
            (mAgentAspect->GetChildOfClass("AgentState",true));
        if (mAgentState == 0)
        {
            GetLog()->Error()
                << "Error: (RestrictedVisionPerceptor) cannot find AgentState.\n";
        }
    }
}

void
RestrictedVisionPerceptor::OnUnlink()
{
    mDistRng.reset();
    mPhiRng.reset();
    mThetaRng.reset();
    mTransformParent.reset();
    mAgentAspect.reset();
    mAgentState.reset();
    mActiveScene.reset();
}

void
RestrictedVisionPerceptor::AddNoise(bool add_noise)
{
    mAddNoise = add_noise;
}

void
RestrictedVisionPerceptor::SetStaticSenseAxis(bool static_axis)
{
    mStaticSenseAxis = static_axis;
}

bool
RestrictedVisionPerceptor::ConstructInternal()
{
    mRay = shared_static_cast<RayCollider>
        (GetCore()->New("oxygen/RayCollider"));

    if (mRay.get() == 0)
    {
        GetLog()->Error() << "Error: (RestrictedVisionPerceptor) cannot create Raycollider. "
                          << "occlusion check disabled\n";
    }

    return true;
}

void
RestrictedVisionPerceptor::SetupVisibleNodes(TNodeObjectsMap& visibleNodes)
{
    TLeafList objectList;
    //mActiveScene->ListChildrenSupportingClass<ObjectState>(objectList, true);
    mActiveScene->GetChildrenOfClass("ObjectState", objectList, true);

    salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();

    for (TLeafList::iterator i = objectList.begin();
        i != objectList.end(); ++i)
    {
        ObjectData od;

        od.mObj = shared_static_cast<ObjectState>(*i);

        shared_ptr<BaseNode> node = shared_dynamic_cast<BaseNode>(mActiveScene);
        shared_ptr<AgentAspect> agent_aspect = make_shared(
                od.mObj->FindParentSupportingClass<AgentAspect>());
        if (agent_aspect != 0)
        {
            shared_ptr<AgentAspect> aspect = make_shared(
                agent_aspect->FindParentSupportingClass<AgentAspect>());
            if (aspect != 0)
            {
                agent_aspect = aspect;
            }

            // if (agent_aspect == mAgentAspect) // exclude node of self
            // {
            //     continue;
            // }

            // GetLog()->Normal()
            //    << "skipping agentAspect " << agent_aspect->GetFullPath() << std::endl;
            node = shared_dynamic_cast<BaseNode>(agent_aspect);
        }

        if (od.mObj.get() == 0)
        {
            GetLog()->Error() << "Error: (RestrictedVisionPerceptor) skipped: "
                << (*i)->GetName() << "\n";
            continue; // this should never happen
        }

        shared_ptr<Transform> j = od.mObj->GetTransformParent();
        
        if (j.get() == 0)
        {
            continue; // this should never happen
        }

        od.mRelPos = j->GetWorldTransform().Pos() - myPos;
        od.mDist   = od.mRelPos.Length();
        
        visibleNodes[node].push_back(od);
	}
}

void
RestrictedVisionPerceptor::AddSense(Predicate& predicate,
                                    shared_ptr<BaseNode> node,
                                    TObjectList& objectList) const
{
    if (objectList.empty())
    {
        return;
    }

    shared_ptr<AgentAspect> agent_aspect =
        shared_dynamic_cast<AgentAspect>(node);
    if (agent_aspect != 0)
    {        
        shared_ptr<AgentAspect> aspect =
            make_shared(agent_aspect->FindParentSupportingClass<AgentAspect>());
        if (aspect != 0)
        {
            agent_aspect = aspect;
        }
        
        shared_ptr<AgentState> agent_state = shared_static_cast<AgentState>
            (agent_aspect->GetChildOfClass("AgentState",true));
        if (agent_state.get() == 0 ||
            (agent_state->GetPerceptName(ObjectState::PT_Player).empty())
           )
        {
            return;
        }