Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | Directories | File List | Namespace Members | Class Members | File Members

pantilteffector.cpp

Go to the documentation of this file.
00001 /* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
00002 
00003    this file is part of rcssserver3D
00004    Mon May 9 2005
00005    Copyright (C) 2002,2003 Koblenz University
00006    Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
00007    $Id: pantilteffector.cpp,v 1.1 2006/03/10 13:46:47 fruit Exp $
00008 
00009    This program is free software; you can redistribute it and/or modify
00010    it under the terms of the GNU General Public License as published by
00011    the Free Software Foundation; version 2 of the License.
00012 
00013    This program is distributed in the hope that it will be useful,
00014    but WITHOUT ANY WARRANTY; without even the implied warranty of
00015    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016    GNU General Public License for more details.
00017 
00018    You should have received a copy of the GNU General Public License
00019    along with this program; if not, write to the Free Software
00020    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00021 */
00022 #include "pantiltaction.h"
00023 #include "pantilteffector.h"
00024 #include <salt/gmath.h>
00025 #include <zeitgeist/logserver/logserver.h>
00026 #include <soccer/soccerbase/soccerbase.h>
00027 #include <soccer/restrictedvisionperceptor/restrictedvisionperceptor.h>
00028 
00029 using namespace boost;
00030 using namespace oxygen;
00031 using namespace salt;
00032 
00033 PanTiltEffector::PanTiltEffector() : oxygen::Effector(),
00034                                      mMaxPanAngleDelta(90),
00035                                      mMaxTiltAngleDelta(10)
00036 {
00037     SetSigma(0.25);
00038 }
00039 
00040 PanTiltEffector::~PanTiltEffector()
00041 {
00042 }
00043 
00044 bool
00045 PanTiltEffector::Realize(boost::shared_ptr<ActionObject> action)
00046 {
00047     if (mBody.get() == 0)
00048     {
00049         return false;
00050     }
00051 
00052     shared_ptr<BaseNode> parent =
00053         shared_dynamic_cast<BaseNode>(make_shared(GetParent()));
00054 
00055     if (parent.get() == 0)
00056     {
00057         GetLog()->Error() << "ERROR: (PanTiltEffector) "
00058                           << "parent node is not derived from BaseNode\n";
00059         return false;
00060     }
00061 
00062     shared_ptr<PanTiltAction> panTiltAction =
00063         shared_dynamic_cast<PanTiltAction>(action);
00064 
00065     if (panTiltAction.get() == 0)
00066     {
00067         GetLog()->Error() << "ERROR: (PanTiltEffector) "
00068                           << "cannot realize an unknown ActionObject\n";
00069         return false;
00070     }
00071 
00072     float pan = panTiltAction->GetPanAngle();
00073     // cut down the pan angle if necessary
00074     if (gAbs(pan) > mMaxPanAngleDelta)
00075     {
00076         pan = gSign(pan) * mMaxPanAngleDelta;
00077     }
00078 
00079     float tilt = panTiltAction->GetTiltAngle();
00080     // cut down the tilt angle if necessary
00081     if (gAbs(tilt) > mMaxTiltAngleDelta)
00082     {
00083         tilt = gSign(tilt) * mMaxTiltAngleDelta;
00084     }
00085 
00086     // apply random error if there is a RNG
00087     if (mActuatorErrorRNG.get() != 0)
00088     {
00089         pan += (*(mActuatorErrorRNG.get()))();
00090         tilt += (*(mActuatorErrorRNG.get()))();
00091     }
00092 
00093     // look for vision perceptor and apply change
00094     shared_ptr<RestrictedVisionPerceptor> rvp =
00095         parent->FindChildSupportingClass<RestrictedVisionPerceptor>(false);
00096     if (rvp.get() == 0)
00097     {
00098         GetLog()->Error() << "ERROR: (PanTiltEffector) "
00099                           << "cannot find RestrictedVisionPerceptor instance\n";
00100         return false;
00101     }
00102     GetLog()->Debug() << "Applying a pan tilt change: " << pan << " " << tilt << "\n";
00103     rvp->ChangePanTilt(pan,tilt);
00104     return true;
00105 }
00106 
00107 shared_ptr<ActionObject>
00108 PanTiltEffector::GetActionObject(const Predicate& predicate)
00109 {
00110     if (predicate.name != GetPredicate())
00111     {
00112         GetLog()->Error() << "ERROR: (PanTiltEffector) invalid predicate"
00113                           << predicate.name << "\n";
00114         return shared_ptr<ActionObject>();
00115     }
00116 
00117     Predicate::Iterator iter = predicate.begin();
00118 
00119     float pan;
00120     if (! predicate.AdvanceValue(iter, pan))
00121     {
00122         GetLog()->Error() << "ERROR: (PanTiltEffector) 2 float parameters expected\n";
00123         return shared_ptr<ActionObject>(new ActionObject(GetPredicate()));
00124     }
00125     float tilt;
00126     if (! predicate.AdvanceValue(iter, tilt))
00127     {
00128         GetLog()->Error() << "ERROR: (PanTiltEffector) float parameter expected\n";
00129         return shared_ptr<ActionObject>(new ActionObject(GetPredicate()));
00130     }
00131     return shared_ptr<ActionObject>(new PanTiltAction(GetPredicate(),pan,tilt));
00132 }
00133 
00134 void
00135 PanTiltEffector::OnLink()
00136 {
00137     SoccerBase::GetTransformParent(*this,mTransformParent);
00138     SoccerBase::GetBody(*this,mBody);
00139     SoccerBase::GetAgentState(*this,mAgentState);
00140 }
00141 
00142 void
00143 PanTiltEffector::OnUnlink()
00144 {
00145     mActuatorErrorRNG.reset();
00146     mTransformParent.reset();
00147     mBody.reset();
00148 }
00149 
00150 void
00151 PanTiltEffector::SetMaxPanAngleDelta(unsigned char max_pan_angle)
00152 {
00153     mMaxPanAngleDelta = max_pan_angle;
00154 }
00155 
00156 void
00157 PanTiltEffector::SetMaxTiltAngleDelta(unsigned char max_tilt_angle)
00158 {
00159     mMaxTiltAngleDelta = max_tilt_angle;
00160 }
00161 
00162 void
00163 PanTiltEffector::SetSigma(float sigma)
00164 {
00165     NormalRngPtr rng(new salt::NormalRNG<>(0.0,sigma));
00166     mActuatorErrorRNG = rng;
00167 }
00168 
00169 

Generated on Thu Apr 6 15:25:39 2006 for rcssserver3d by  doxygen 1.4.4