00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00074 if (gAbs(pan) > mMaxPanAngleDelta)
00075 {
00076 pan = gSign(pan) * mMaxPanAngleDelta;
00077 }
00078
00079 float tilt = panTiltAction->GetTiltAngle();
00080
00081 if (gAbs(tilt) > mMaxTiltAngleDelta)
00082 {
00083 tilt = gSign(tilt) * mMaxTiltAngleDelta;
00084 }
00085
00086
00087 if (mActuatorErrorRNG.get() != 0)
00088 {
00089 pan += (*(mActuatorErrorRNG.get()))();
00090 tilt += (*(mActuatorErrorRNG.get()))();
00091 }
00092
00093
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