simspark-cvs Mailing List for Spark - A generic physical simulator (Page 30)

SourceForge Headquarters 225 Broadway Suite 1600 San Diego, CA 92101 +1 (858) 422-6466

Update of /cvsroot/simspark/simspark/contrib/plugin/soccer/visionperceptor
In directory sc8-pr-cvs8.sourceforge.net:/tmp/cvs-serv3579

Added Files:
      Tag: WIN32
	.cvsignore visionperceptor.cpp visionperceptor.h 
	visionperceptor_c.cpp 
Log Message:
- add visionperceptor

--- NEW FILE: visionperceptor.cpp ---
/* -*- 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: visionperceptor.cpp,v 1.1.2.1 2007/02/25 16:54:18 rollmark 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 "visionperceptor.h"
#include <zeitgeist/logserver/logserver.h>
#include <oxygen/sceneserver/scene.h>
#include <oxygen/sceneserver/transform.h>
#include <soccer/soccerbase/soccerbase.h>

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

VisionPerceptor::VisionPerceptor() : Perceptor(),
                                     mSenseMyPos(false),
                                     mAddNoise(true),
                                     mUseRandomNoise(true),
                                     mStaticSenseAxis(true)
{
    // set predicate name
    SetPredicateName("Vision");
    // set some default noise values
    SetNoiseParams(0.0965f, 0.1225f, 0.1480f, 0.005f);
}

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

void
VisionPerceptor::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
VisionPerceptor::OnLink()
{
    SoccerBase::GetTransformParent(*this,mTransformParent);
    SoccerBase::GetAgentState(*this, mAgentState);
    SoccerBase::GetActiveScene(*this,mActiveScene);
}

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

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

void
VisionPerceptor::UseRandomNoise(bool random_noise)
{
    mUseRandomNoise = random_noise;
}

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

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

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

    return true;
}

void
VisionPerceptor::SetupVisibleObjects(TObjectList& visibleObjects)
{
    TLeafList objectList;
    mActiveScene->ListChildrenSupportingClass<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);

            if (od.mObj.get() == 0)
                {
                    GetLog()->Error() << "Error: (VisionPerceptor) 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();

            visibleObjects.push_back(od);
        }
}

void VisionPerceptor::AddSense(oxygen::Predicate& predicate, ObjectData& od) const
{
    ParameterList& element = predicate.parameter.AddList();
    element.AddValue(od.mObj->GetPerceptName());

    if(od.mObj->GetPerceptName() == "Player")
        {
            ParameterList player;
            player.AddValue(std::string("team"));
            player.AddValue
                (std::string
                 (od.mObj->GetPerceptName(ObjectState::PT_Player)
                  )
                 );
            element.AddValue(player);
        }

    if (!od.mObj->GetID().empty())
        {
            ParameterList id;
            id.AddValue(std::string("id"));
            id.AddValue(od.mObj->GetID());
            element.AddValue(id);
        }

    ParameterList& position = element.AddList();
    position.AddValue(std::string("pol"));
    position.AddValue(od.mDist);
    position.AddValue(od.mTheta);
    position.AddValue(od.mPhi);
}

void
VisionPerceptor::ApplyNoise(ObjectData& od) const
{
    if (mAddNoise)
        {
            if (mUseRandomNoise)
                {
                    od.mDist  += (*(mDistRng.get()))() * od.mDist / 100.0f;
                    od.mTheta += (*(mThetaRng.get()))();
                    od.mPhi   += (*(mPhiRng.get()))();
                } else
                {
                    /* This gives a constant random error throughout the whole
                     * match. This behavior was not intended and is a bug and
                     * not an intended feature.
                     * It was kept in the simulator because I discovered this
                     * bug only shortly before the competition. *sigh* oliver
                     */
                    od.mDist  += salt::NormalRNG<>(0.0,mSigmaDist)();
                    od.mTheta += salt::NormalRNG<>(0.0,mSigmaTheta)();
                    od.mPhi  += salt::NormalRNG<>(0.0,mSigmaPhi)();
                }
        }
}

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
VisionPerceptor::DynamicAxisPercept(boost::shared_ptr<PredicateList> predList)
{
    Predicate& predicate = predList->AddPredicate();
    predicate.name       = mPredicateName;
    predicate.parameter.Clear();

    TTeamIndex  ti          = mAgentState->GetTeamIndex();

    const Vector3f& up = mTransformParent->GetWorldTransform().Up();

    // calc the percptors angle in the horizontal plane
    double fwTheta = gNormalizeRad(Vector2f(up[0],up[1]).GetAngleRad());

    // calc the perceptors angle in the vertical plane
    double fwPhi = gNormalizeRad(Vector2f(up[0],up[2]).GetAngleRad());

    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)
        {
            // object is too close
            continue;
        }

        // theta is the angle in horizontal plane, with fwAngle as 0 degree
        od.mTheta = gRadToDeg(gNormalizeRad(
                              Vector2f(od.mRelPos[0],od.mRelPos[1]).GetAngleRad() -
                              fwTheta
                              ));

        // latitude with fwPhi as 0 degreee
        od.mPhi = gRadToDeg(gNormalizeRad(
                            Vector2f(od.mRelPos[0],od.mRelPos[2]).GetAngleRad() -
                            fwPhi
                            ));

        // make some noise
        ApplyNoise(od);

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

    return true;
}

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

    return mStaticSenseAxis ?
        StaticAxisPercept(predList) :
        DynamicAxisPercept(predList);
}

bool VisionPerceptor::CheckOcclusion(const Vector3f& my_pos, const ObjectData& od) const
{
    // (occlusion test disabled for now, every object is visible)
    return false;
}

void
VisionPerceptor::SetSenseMyPos(bool sense)
{
    mSenseMyPos = sense;
}

--- NEW FILE: visionperceptor.h ---
/* -*- 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: visionperceptor.h,v 1.1.2.1 2007/02/25 16:54:18 rollmark 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.
*/
#ifndef VISIONPERCEPTOR_H
#define VISIONPERCEPTOR_H

#include <salt/random.h>
#include <oxygen/agentaspect/perceptor.h>
#include <oxygen/physicsserver/raycollider.h>
#include <oxygen/sceneserver/sceneserver.h>
#include <oxygen/sceneserver/transform.h>
#include <soccer/agentstate/agentstate.h>

class VisionPerceptor : public oxygen::Perceptor
{
protected:
    typedef boost::shared_ptr<salt::NormalRNG<> > NormalRngPtr;

    struct ObjectData
    {
        boost::shared_ptr<ObjectState> mObj;

        float mTheta;  // angle in the X-Y (horizontal) plane
        float mPhi;    // latitude angle
        float mDist;  // distance between perceptor and object
        salt::Vector3f mRelPos;

        ObjectData& operator=(const ObjectData& rhs)
        {
            mObj     = rhs.mObj;
            mRelPos  = rhs.mRelPos;
            mTheta   = rhs.mTheta;
            mPhi     = rhs.mPhi;
            mDist    = rhs.mDist;
        }

        int operator==(const ObjectData& rhs) const
        { return mDist == rhs.mDist; }

        int operator<(const ObjectData& rhs) const
        { return mDist < rhs.mDist; }
    };

    typedef std::list<ObjectData> TObjectList;

public:
    VisionPerceptor();
    virtual ~VisionPerceptor();

    //! \return true, if valid data is available and false otherwise.
    bool Percept(boost::shared_ptr<oxygen::PredicateList> predList);

    /** Set the noise parameters of the vision perceptor.
     *
     * This will always create new calibration error values.
     * The random noise added each step is normally distributed around 0.0.
     * The (fixed) calibration error is calculated once for each axis. It
     * is uniformly distributed between -cal_error_abs and cal_error_abs and
     * added to the "camera" coordinates.
     *
     * \param sigma_dist the sigma for the distance error distribution
     * \param sigma_phi the sigma for the horizontal angle error distribution
     * \param sigma_theta the sigma for the latitudal angle error distribution
     * \param cal_error_abs absolute value of the maximum calibration error
     *                      along each axis.
     */
    void SetNoiseParams(float sigma_dist, float sigma_phi,
                        float sigma_theta, float cal_error_abs);

    //! Turn sensing of agent position on/off
    void SetSenseMyPos(bool sense);

    /** Turn noise off/on.
        \param add_noise flag if noise should be used at all.
    */
    void AddNoise(bool add_noise);

    /** Turn randomization off/on.
        \param random_noise flag if the measurement noise is randomized each step.
    */
    void UseRandomNoise(bool random_noise);

    //! Turn senses relative to the X-axis of the team off/on
    void SetStaticSenseAxis(bool static_axis);

protected:
    /** constructs the internal ray collider */
    virtual bool ConstructInternal();

    /** prepares a list of visible objects */
    void SetupVisibleObjects(TObjectList& visibleObjects);

    /** Percept implementation for a static relative axis */
    bool StaticAxisPercept(boost::shared_ptr<oxygen::PredicateList> predList);

    /** Percept implementation relative to the current orientation of
        the VisionPerceptor node
    */
    bool DynamicAxisPercept(boost::shared_ptr<oxygen::PredicateList> predList);

    /** Checks if the given object is occluded, seen from from my_pos */
    bool CheckOcclusion(const salt::Vector3f& my_pos, const ObjectData& od) const;

    /** constructs a sense entry for the given object in the given
        predicate*/
    void AddSense(oxygen::Predicate& predicate, ObjectData& od) const;

    /** applies noise to the setup ObjectData */
    void ApplyNoise(ObjectData& od) const;

    virtual void OnLink();
    virtual void OnUnlink();

protected:
    //! vision calibration error
    salt::Vector3f mError;

    //! true, if the absolute position of the agent is sensed.
    bool mSenseMyPos;

    //! sigma for random measurement error (distance)
    float mSigmaDist;

    //! sigma for random measurement error (horizontal angle)
    float mSigmaTheta;

    //! sigma for random measurement error (latitudal angle)
    float mSigmaPhi;

    //! absolute maximum value of the calibration error
    float mCalErrorAbs;

    //! flag if we should noisify the data
    bool mAddNoise;

    //! flag if the error should be randomized each step
    bool mUseRandomNoise;

    /** flag if the senses are always relative to the X-axis of the
        team, default true
    */
    bool mStaticSenseAxis;

    //! ray collider to check occlusion
    boost::shared_ptr<oxygen::RayCollider> mRay;

    //! random number generator for distance errors
    NormalRngPtr mDistRng;

    //! random number generator for angle errors
    NormalRngPtr mThetaRng;

    //! random number generator for angle errors
    NormalRngPtr mPhiRng;

    boost::shared_ptr<oxygen::Scene> mActiveScene;

    //! a reference to the next transorm parent
    boost::shared_ptr<oxygen::Transform> mTransformParent;

    //! a reference to the scene server
    boost::shared_ptr<oxygen::SceneServer> mSceneServer;

    //! a reference to the agent state
    boost::shared_ptr<AgentState> mAgentState;
};

DECLARE_CLASS(VisionPerceptor);

#endif //VISIONPERCEPTOR_H

--- NEW FILE: visionperceptor_c.cpp ---
/* -*- 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: visionperceptor_c.cpp,v 1.1.2.1 2007/02/25 16:54:18 rollmark 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 "visionperceptor.h"

using namespace boost;
using namespace oxygen;
using namespace std;

FUNCTION(VisionPerceptor,setNoiseParams)
{
    float inDist;
    float inPhi;
    float inTheta;
    float inErrorAbs;

    if (
        (in.GetSize() != 4) ||
        (! in.GetValue(in[0],inDist)) ||
        (! in.GetValue(in[1],inPhi)) ||
        (! in.GetValue(in[2],inTheta)) ||
        (! in.GetValue(in[3],inErrorAbs))
        )
        {
            return false;
        }

    obj->SetNoiseParams(inDist,inPhi,inTheta,inErrorAbs);
    return true;
}

FUNCTION(VisionPerceptor,addNoise)
{
    bool inAddNoise;

    if (
        (in.GetSize() != 1) ||
        (! in.GetValue(in.begin(),inAddNoise))
        )
    {
        return false;
    }

    obj->AddNoise(inAddNoise);
    return true;
}

FUNCTION(VisionPerceptor,useRandomNoise)
{
    bool inRandomNoise;

    if (
        (in.GetSize() != 1) ||
        (! in.GetValue(in.begin(),inRandomNoise))
        )
    {
        return false;
    }

    obj->UseRandomNoise(inRandomNoise);
    return true;
}

FUNCTION(VisionPerceptor,setSenseMyPos)
{
    bool inSenseMyPos;

    if (
        (in.GetSize() != 1) ||
        (! in.GetValue(in.begin(),inSenseMyPos))
        )
        {
            return false;
        }

    obj->SetSenseMyPos(inSenseMyPos);
    return true;
}

FUNCTION(VisionPerceptor,setStaticSenseAxis)
{
    bool inStaticAxis;

    if (
        (in.GetSize() != 1) ||
        (! in.GetValue(in.begin(),inStaticAxis))
        )
        {
            return false;
        }

    obj->SetStaticSenseAxis(inStaticAxis);
    return true;
}

void CLASS(VisionPerceptor)::DefineClass()
{
    DEFINE_BASECLASS(oxygen/Perceptor);
    DEFINE_FUNCTION(setNoiseParams);
    DEFINE_FUNCTION(addNoise);
    DEFINE_FUNCTION(useRandomNoise);
    DEFINE_FUNCTION(setSenseMyPos);
    DEFINE_FUNCTION(setStaticSenseAxis);
}

--- NEW FILE: .cvsignore ---
*.lo
.deps
.dirstamp
.libs
Makefile
Makefile.in

2005	Jan	Feb	Mar	Apr	May	Jun	Jul	Aug	Sep	Oct	Nov (1)	Dec (153)
2006	Jan (48)	Feb (46)	Mar (12)	Apr (4)	May (4)	Jun	Jul	Aug	Sep	Oct	Nov	Dec
2007	Jan	Feb (263)	Mar (235)	Apr (66)	May (42)	Jun (270)	Jul (65)	Aug (2)	Sep	Oct	Nov	Dec
2013	Jan (1)	Feb	Mar	Apr	May	Jun	Jul	Aug	Sep	Oct	Nov	Dec

simspark-cvs Mailing List for Spark - A generic physical simulator (Page 30)

simspark-cvs — CVS Commit Notifications via Email