simspark-cvs

[Oliver O. <fr...@us...>]

Update of /cvsroot/simspark/simspark/simulations/soccer/plugin/pantilteffector
In directory sc8-pr-cvs8.sourceforge.net:/tmp/cvs-serv24718/pantilteffector

Added Files:
      Tag: projectx
	pantiltaction.h pantilteffector.cpp pantilteffector.h 
	pantilteffector_c.cpp 
Log Message:
soccer plugins from rcssserver3D

--- NEW FILE: pantilteffector_c.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Mon May 9 2005
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: pantilteffector_c.cpp,v 1.1.2.1 2007/03/07 10:39:39 fruit 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 "pantilteffector.h"

using namespace oxygen;

FUNCTION(PanTiltEffector,setMaxPanAngleDelta)
{
    int inMaxPanAngleDelta;
    if (
        (in.GetSize() != 1) ||
        (! in.GetValue(in.begin(), inMaxPanAngleDelta))
        )
    {
        return false;
    }

    obj->SetMaxPanAngleDelta(inMaxPanAngleDelta);
    return true;
}

FUNCTION(PanTiltEffector,setMaxTiltAngleDelta)
{
    int inMaxTiltAngleDelta;
    if (
        (in.GetSize() != 1) ||
        (! in.GetValue(in.begin(), inMaxTiltAngleDelta))
        )
    {
        return false;
    }

    obj->SetMaxTiltAngleDelta(inMaxTiltAngleDelta);
    return true;
}

FUNCTION(PanTiltEffector,setSigma)
{
    float inSigma;
    if (
        (in.GetSize() != 1) ||
        (! in.GetValue(in.begin(), inSigma))
        )
    {
        return false;
    }

    obj->SetSigma(inSigma);
    return true;
}

void
CLASS(PanTiltEffector)::DefineClass()
{
    DEFINE_BASECLASS(oxygen/Effector);
    DEFINE_FUNCTION(setSigma);
    DEFINE_FUNCTION(setMaxPanAngleDelta);
    DEFINE_FUNCTION(setMaxTiltAngleDelta);
}

--- NEW FILE: pantilteffector.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Mon May 9 2005
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: pantilteffector.h,v 1.1.2.1 2007/03/07 10:39:39 fruit 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 PANTILTEFFECTOR_H
#define PANTILTEFFECTOR_H

#include <salt/random.h>
#include <oxygen/agentaspect/effector.h>
#include <oxygen/physicsserver/body.h>
#include <soccer/agentstate/agentstate.h>

class PanTiltEffector : public oxygen::Effector
{
public:
    PanTiltEffector();
    virtual ~PanTiltEffector();

    /** realizes the action described by the ActionObject */
    virtual bool Realize(boost::shared_ptr<oxygen::ActionObject> action);

    /** returns the name of the predicate this effector implements. */
    virtual std::string GetPredicate() { return "pantilt"; }

    /** constructs an Actionobject, describing a predicate */
    virtual boost::shared_ptr<oxygen::ActionObject>
    GetActionObject(const oxygen::Predicate& predicate);

    /** Set the maximum pan angle change.
     *  The camera can pan to any angle, but the absolute pan angle
     *  used for one 'pan' action may be restricted. A maximum pan
     *  angle of 0 effectively disables panning, a value of 360 (or
     *  greater) means no restriction.
     * @param max_pan_angle the maximum pan angle (in degrees) for one action
     */
    void SetMaxPanAngleDelta(unsigned char max_pan_angle);

    /** Set the maximum tilt angle change.
     *  Set the maximum absolute tilt angle change for one 'tilt' action.
     * @param max_tilt_angle the maximum tilt angle (in degrees) for one action
     */
    void SetMaxTiltAngleDelta(unsigned char max_tilt_angle);

    /** Set the angle error parameter.
     *  The error is normally distributed around zero.
     * @param sigma the sigma for the random number distribution
     */
    void SetSigma(float sigma);

protected:
    /** setup the reference to the agents body node */
    virtual void OnLink();

    /** remove the reference to the agents body node */
    virtual void OnUnlink();

protected:
    typedef boost::shared_ptr<salt::NormalRNG<> > NormalRngPtr;

    /** the reference to the parent transform node */
    boost::shared_ptr<oxygen::Transform> mTransformParent;
    /** the reference to the parents body node */
    boost::shared_ptr<oxygen::Body> mBody;
    //! a reference to the agent state
    boost::shared_ptr<AgentState> mAgentState;

    /** random number generator for the error distribution of pan/tilt actions */
    NormalRngPtr mActuatorErrorRNG;

    /** The maximum absolute value of the pan angle change */
    unsigned char mMaxPanAngleDelta;

    /** The maximum absolute value of the tilt angle change */
    unsigned char mMaxTiltAngleDelta;
};

DECLARE_CLASS(PanTiltEffector);

#endif // PANTILTEFFECTOR_H

--- NEW FILE: pantiltaction.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Mon May 9 2005
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: pantiltaction.h,v 1.1.2.1 2007/03/07 10:39:39 fruit 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 DRIVEACTION_H
#define DRIVEACTION_H

#include <oxygen/gamecontrolserver/actionobject.h>
#include <salt/vector.h>

class PanTiltAction : public oxygen::ActionObject
{
public:
    PanTiltAction(const std::string& predicate, float pan, float tilt)
        : ActionObject(predicate), mPan(pan), mTilt(tilt) {}

    virtual ~PanTiltAction() {}

    //! @return the pan angle
    float GetPanAngle() const { return mPan; }

    //! @return the tilt angle
    float GetTiltAngle() const { return mTilt; }

protected:
    //! the pan angle (in degrees)
    float mPan;
    //! the tilt angle (in degrees)
    float mTilt;
};

#endif // DRIVEACTION_H

--- NEW FILE: pantilteffector.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Mon May 9 2005
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: pantilteffector.cpp,v 1.1.2.1 2007/03/07 10:39:39 fruit 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 "pantiltaction.h"
#include "pantilteffector.h"
#include <salt/gmath.h>
#include <zeitgeist/logserver/logserver.h>
#include <soccer/soccerbase/soccerbase.h>
#include <soccer/restrictedvisionperceptor/restrictedvisionperceptor.h>

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

PanTiltEffector::PanTiltEffector() : oxygen::Effector(),
                                     mMaxPanAngleDelta(90),
                                     mMaxTiltAngleDelta(10)
{
    SetSigma(0.25);
}

PanTiltEffector::~PanTiltEffector()
{
}

bool
PanTiltEffector::Realize(boost::shared_ptr<ActionObject> action)
{
    if (mBody.get() == 0)
    {
        return false;
    }

    shared_ptr<BaseNode> parent =
        shared_dynamic_cast<BaseNode>(make_shared(GetParent()));

    if (parent.get() == 0)
    {
        GetLog()->Error() << "ERROR: (PanTiltEffector) "
                          << "parent node is not derived from BaseNode\n";
        return false;
    }

    shared_ptr<PanTiltAction> panTiltAction =
        shared_dynamic_cast<PanTiltAction>(action);

    if (panTiltAction.get() == 0)
    {
        GetLog()->Error() << "ERROR: (PanTiltEffector) "
                          << "cannot realize an unknown ActionObject\n";
        return false;
    }

    float pan = panTiltAction->GetPanAngle();

    // check for NAN
    if (std::isnan(pan))
    {
        return true;
    }

    // cut down the pan angle if necessary
    if (gAbs(pan) > mMaxPanAngleDelta)
    {
        pan = gSign(pan) * mMaxPanAngleDelta;
    }

    float tilt = panTiltAction->GetTiltAngle();

    // check for NAN
    if (std::isnan(tilt))
    {
        return true;
    }

    // cut down the tilt angle if necessary
    if (gAbs(tilt) > mMaxTiltAngleDelta)
    {
        tilt = gSign(tilt) * mMaxTiltAngleDelta;
    }

    // apply random error if there is a RNG
    if (mActuatorErrorRNG.get() != 0)
    {
        pan += (*(mActuatorErrorRNG.get()))();
        tilt += (*(mActuatorErrorRNG.get()))();
    }

    // look for vision perceptor and apply change
    shared_ptr<RestrictedVisionPerceptor> rvp =
        parent->FindChildSupportingClass<RestrictedVisionPerceptor>(false);
    if (rvp.get() == 0)
    {
        GetLog()->Error() << "ERROR: (PanTiltEffector) "
                          << "cannot find RestrictedVisionPerceptor instance\n";
        return false;
    }

    rvp->ChangePanTilt(pan,tilt);
    return true;
}

shared_ptr<ActionObject>
PanTiltEffector::GetActionObject(const Predicate& predicate)
{
    if (predicate.name != GetPredicate())
    {
        GetLog()->Error() << "ERROR: (PanTiltEffector) invalid predicate"
                          << predicate.name << "\n";
        return shared_ptr<ActionObject>();
    }

    Predicate::Iterator iter = predicate.begin();

    float pan;
    if (! predicate.AdvanceValue(iter, pan))
    {
        GetLog()->Error() << "ERROR: (PanTiltEffector) 2 float parameters expected\n";
        return shared_ptr<ActionObject>(new ActionObject(GetPredicate()));
    }
    float tilt;
    if (! predicate.AdvanceValue(iter, tilt))
    {
        GetLog()->Error() << "ERROR: (PanTiltEffector) float parameter expected\n";
        return shared_ptr<ActionObject>(new ActionObject(GetPredicate()));
    }
    return shared_ptr<ActionObject>(new PanTiltAction(GetPredicate(),pan,tilt));
}

void
PanTiltEffector::OnLink()
{
    SoccerBase::GetTransformParent(*this,mTransformParent);
    SoccerBase::GetBody(*this,mBody);
    SoccerBase::GetAgentState(*this,mAgentState);
}

void
PanTiltEffector::OnUnlink()
{
    mActuatorErrorRNG.reset();
    mTransformParent.reset();
    mBody.reset();
}

void
PanTiltEffector::SetMaxPanAngleDelta(unsigned char max_pan_angle)
{
    mMaxPanAngleDelta = max_pan_angle;
}

void
PanTiltEffector::SetMaxTiltAngleDelta(unsigned char max_tilt_angle)
{
    mMaxTiltAngleDelta = max_tilt_angle;
}

void
PanTiltEffector::SetSigma(float sigma)
{
    NormalRngPtr rng(new salt::NormalRNG<>(0.0,sigma));
    mActuatorErrorRNG = rng;
}