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

restrictedvisionperceptor.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    Fri May 9 2003
00005    Copyright (C) 2002,2003 Koblenz University
00006    Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
00007    $Id: restrictedvisionperceptor.cpp,v 1.2 2006/03/01 18:32:23 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 "restrictedvisionperceptor.h"
00023 #include <zeitgeist/logserver/logserver.h>
00024 #include <oxygen/sceneserver/scene.h>
00025 #include <oxygen/sceneserver/transform.h>
00026 #include <soccer/soccerbase/soccerbase.h>
00027 #include <salt/gmath.h>
00028 
00029 using namespace zeitgeist;
00030 using namespace oxygen;
00031 using namespace boost;
00032 using namespace salt;
00033 
00034 RestrictedVisionPerceptor::RestrictedVisionPerceptor() : Perceptor(),
00035                                      mSenseMyPos(false),
00036                                      mAddNoise(true),
00037                                      mStaticSenseAxis(true)
00038 {
00039     // set predicate name
00040     SetPredicateName("Vision");
00041     // set some default noise values
00042     SetNoiseParams(0.0965, 0.1225, 0.1480, 0.005);
00043     SetViewCones(90,90);
00044     SetPanRange(-90,90);
00045     SetTiltRange(-20,20);
00046     SetPanTilt(0,0);
00047 }
00048 
00049 RestrictedVisionPerceptor::~RestrictedVisionPerceptor()
00050 {
00051     mDistRng.reset();
00052     mPhiRng.reset();
00053     mThetaRng.reset();
00054 }
00055 
00056 void
00057 RestrictedVisionPerceptor::SetNoiseParams(float sigma_dist, float sigma_phi,
00058                                 float sigma_theta, float cal_error_abs)
00059 {
00060     mSigmaDist = sigma_dist;
00061     mSigmaPhi = sigma_phi;
00062     mSigmaTheta = sigma_theta;
00063     mCalErrorAbs = cal_error_abs;
00064 
00065     NormalRngPtr rng1(new salt::NormalRNG<>(0.0,sigma_dist));
00066     mDistRng = rng1;
00067     NormalRngPtr rng2(new salt::NormalRNG<>(0.0,sigma_phi));
00068     mPhiRng = rng2;
00069     NormalRngPtr rng3(new salt::NormalRNG<>(0.0,sigma_theta));
00070     mThetaRng = rng3;
00071 
00072     salt::UniformRNG<float> rng4(-mCalErrorAbs,mCalErrorAbs);
00073     mError = salt::Vector3f(rng4(),rng4(),rng4());
00074 }
00075 
00076 void
00077 RestrictedVisionPerceptor::SetViewCones(unsigned int hAngle, unsigned int vAngle)
00078 {
00079     mHViewCone = hAngle;
00080     mVViewCone = vAngle;
00081 }
00082 
00083 void
00084 RestrictedVisionPerceptor::SetPanRange(int lower, int upper)
00085 {
00086     /* The total pan range is 360 degrees, starting at -180 degrees
00087      * and ending at 180 degrees. For restricting this range:
00088      * - the normal case is: lower < upper, which means that
00089      *   the horizontal pan angles should be between lower and upper.
00090      * - the not so normal case is upper < lower, which means that
00091      *   the pan angles should be *either* smaller than 180 and > lower, *or*
00092      *   they should be greater than -180 and < upper.
00093      */
00094     mPanLower = gNormalizeDeg(lower);
00095     mPanUpper = gNormalizeDeg(upper);
00096 }
00097 
00098 void
00099 RestrictedVisionPerceptor::SetTiltRange(int lower, int upper)
00100 {
00101     mTiltLower = gNormalizeDeg(lower);
00102     mTiltUpper = gNormalizeDeg(upper);
00103 }
00104 
00105 // this should really go to gmath.h for the full release
00106 template <class TYPE1, class TYPE2>
00107 f_inline TYPE1 gClampAngleDeg(TYPE1 &val, TYPE2 min, TYPE2 max)
00108 {
00109     val = gNormalizeDeg(val);
00110     if (min <= max)
00111     {
00112         if (val<min) val=min;
00113         if (val>max) val=max;
00114     } else {
00115         if (val>=min || val<=max) return val;
00116         if (val>=(min+max)/2.0) val = min; else val = max;
00117     }
00118     return val;
00119 }
00120 
00121 
00122 
00123 void
00124 RestrictedVisionPerceptor::SetPanTilt(float pan, float tilt)
00125 {
00126     pan = gNormalizeDeg(pan);
00127     mPan = gClampAngleDeg(pan,mPanLower,mPanUpper);
00128     tilt = gNormalizeDeg(tilt);
00129     mTilt = gClampAngleDeg(tilt,mTiltLower,mTiltUpper);
00130 }
00131 
00132 void
00133 RestrictedVisionPerceptor::ChangePanTilt(float pan, float tilt)
00134 {
00135     SetPanTilt(mPan + pan, mTilt + tilt);
00136 }
00137 
00138 float
00139 RestrictedVisionPerceptor::GetPan() const
00140 {
00141     return mPan;
00142 }
00143 
00144 float
00145 RestrictedVisionPerceptor::GetTilt() const
00146 {
00147     return mTilt;
00148 }
00149 
00150 void
00151 RestrictedVisionPerceptor::OnLink()
00152 {
00153     SoccerBase::GetTransformParent(*this,mTransformParent);
00154     SoccerBase::GetAgentState(*this, mAgentState);
00155     SoccerBase::GetActiveScene(*this,mActiveScene);
00156 }
00157 
00158 void
00159 RestrictedVisionPerceptor::OnUnlink()
00160 {
00161     mDistRng.reset();
00162     mPhiRng.reset();
00163     mThetaRng.reset();
00164     mTransformParent.reset();
00165     mAgentState.reset();
00166     mActiveScene.reset();
00167 }
00168 
00169 void
00170 RestrictedVisionPerceptor::AddNoise(bool add_noise)
00171 {
00172     mAddNoise = add_noise;
00173 }
00174 
00175 void
00176 RestrictedVisionPerceptor::SetStaticSenseAxis(bool static_axis)
00177 {
00178     mStaticSenseAxis = static_axis;
00179 }
00180 
00181 bool
00182 RestrictedVisionPerceptor::ConstructInternal()
00183 {
00184     mRay = shared_static_cast<RayCollider>
00185         (GetCore()->New("oxygen/RayCollider"));
00186 
00187     if (mRay.get() == 0)
00188     {
00189         GetLog()->Error() << "Error: (RestrictedVisionPerceptor) cannot create Raycollider. "
00190                           << "occlusion check disabled\n";
00191     }
00192 
00193     return true;
00194 }
00195 
00196 void
00197 RestrictedVisionPerceptor::SetupVisibleObjects(TObjectList& visibleObjects)
00198 {
00199     TLeafList objectList;
00200     mActiveScene->ListChildrenSupportingClass<ObjectState>(objectList, true);
00201 
00202     salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();
00203 
00204     for (TLeafList::iterator i = objectList.begin();
00205          i != objectList.end(); ++i)
00206         {
00207             ObjectData od;
00208             od.mObj = shared_static_cast<ObjectState>(*i);
00209 
00210             if (od.mObj.get() == 0)
00211                 {
00212                     GetLog()->Error() << "Error: (RestrictedVisionPerceptor) skipped: "
00213                                       << (*i)->GetName() << "\n";
00214                     continue; // this should never happen
00215                 }
00216 
00217             shared_ptr<Transform> j = od.mObj->GetTransformParent();
00218 
00219             if (j.get() == 0)
00220                 {
00221                     continue; // this should never happen
00222                 }
00223 
00224             od.mRelPos = j->GetWorldTransform().Pos() - myPos;
00225             od.mDist   = od.mRelPos.Length();
00226 
00227             visibleObjects.push_back(od);
00228         }
00229 }
00230 
00231 void
00232 RestrictedVisionPerceptor::AddSense(Predicate& predicate, ObjectData& od) const
00233 {
00234     ParameterList& element = predicate.parameter.AddList();
00235     element.AddValue(od.mObj->GetPerceptName());
00236 
00237     if(od.mObj->GetPerceptName() == "Player")
00238     {
00239         ParameterList player;
00240         player.AddValue(std::string("team"));
00241         player.AddValue(od.mObj->GetPerceptName(ObjectState::PT_Player));
00242         element.AddValue(player);
00243     }
00244 
00245     if (!od.mObj->GetID().empty())
00246     {
00247         ParameterList id;
00248         id.AddValue(std::string("id"));
00249         id.AddValue(od.mObj->GetID());
00250         element.AddValue(id);
00251     }
00252 
00253     ParameterList& position = element.AddList();
00254     position.AddValue(std::string("pol"));
00255     position.AddValue(od.mDist);
00256     position.AddValue(od.mTheta);
00257     position.AddValue(od.mPhi);
00258 }
00259 
00260 void
00261 RestrictedVisionPerceptor::ApplyNoise(ObjectData& od) const
00262 {
00263     if (mAddNoise)
00264     {
00265         od.mDist  += (*(mDistRng.get()))() * od.mDist / 100.0;
00266         od.mTheta += (*(mThetaRng.get()))();
00267         od.mPhi   += (*(mPhiRng.get()))();
00268     }
00269 }
00270 
00271 
00272 bool
00273 RestrictedVisionPerceptor::StaticAxisPercept(boost::shared_ptr<PredicateList> predList)
00274 {
00275     Predicate& predicate = predList->AddPredicate();
00276     predicate.name       = mPredicateName;
00277     predicate.parameter.Clear();
00278 
00279     TTeamIndex  ti       = mAgentState->GetTeamIndex();
00280     salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();
00281 
00282     TObjectList visibleObjects;
00283     SetupVisibleObjects(visibleObjects);
00284 
00285     for (std::list<ObjectData>::iterator i = visibleObjects.begin();
00286          i != visibleObjects.end(); ++i)
00287     {
00288         ObjectData& od = (*i);
00289 
00290         od.mRelPos = SoccerBase::FlipView(od.mRelPos, ti);
00291         if (mAddNoise)
00292         {
00293             od.mRelPos += mError;
00294         }
00295 
00296         if (
00297             (od.mRelPos.Length() <= 0.1) ||
00298             (CheckOcclusion(myPos,od))
00299             )
00300         {
00301             // object is occluded or too close
00302             continue;
00303         }
00304 
00305         // theta is the angle in the X-Y (horizontal) plane
00306         assert(gAbs(GetPan()) <= 360);
00307         od.mTheta = salt::gRadToDeg(salt::gArcTan2(od.mRelPos[1], od.mRelPos[0])) - GetPan();
00308         od.mTheta = gNormalizeDeg(od.mTheta);
00309         // latitude
00310         assert(gAbs(GetTilt()) <= 360);
00311         od.mPhi = 90.0 - salt::gRadToDeg(salt::gArcCos(od.mRelPos[2]/od.mDist)) - GetTilt();
00312         od.mPhi = gNormalizeDeg(od.mPhi);
00313 
00314         // make some noise
00315         ApplyNoise(od);
00316         // check if the object is in the current field of view
00317         if (gAbs(od.mTheta) > mHViewCone) continue;
00318         if (gAbs(od.mPhi) > mVViewCone) continue;
00319 
00320         // generate a sense entry
00321         AddSense(predicate,od);
00322     }
00323 
00324     if (mSenseMyPos)
00325     {
00326         Vector3f sensedMyPos = SoccerBase::FlipView(myPos, ti);
00327 
00328         ParameterList& element = predicate.parameter.AddList();
00329         element.AddValue(std::string("mypos"));
00330         element.AddValue(sensedMyPos[0]);
00331         element.AddValue(sensedMyPos[1]);
00332         element.AddValue(sensedMyPos[2]);
00333     }
00334 
00335     return true;
00336 }
00337 
00338 bool
00339 RestrictedVisionPerceptor::DynamicAxisPercept(boost::shared_ptr<PredicateList> predList)
00340 {
00341     Predicate& predicate = predList->AddPredicate();
00342     predicate.name       = mPredicateName;
00343     predicate.parameter.Clear();
00344 
00345     const int hAngle_2 = mHViewCone>>1;
00346     const int vAngle_2 = mVViewCone>>1;
00347 
00348     TTeamIndex  ti = mAgentState->GetTeamIndex();
00349 
00350     const Vector3f& up = mTransformParent->GetWorldTransform().Up();
00351 
00352     // calc the percptors angle in the horizontal plane
00353     double fwTheta = gNormalizeRad(Vector2f(up[0],up[1]).GetAngleRad());
00354 
00355     // calc the perceptors angle in the vertical plane
00356     // for this the vector has to rotated, i.e. you cannot just use x and z component
00357     //double fwPhi = gNormalizeRad(Vector2f(Vector2f(up[0],up[1]).Length(),up[2]).GetAngleRad());
00358 
00359     // assume that perceptor is always horizontal.
00360     // FIXME: this is magic
00361     double fwPhi = 0.0;
00362 
00363 
00364     TObjectList visibleObjects;
00365     SetupVisibleObjects(visibleObjects);
00366 
00367     // log for 7th agent of the first team
00368     if ((mAgentState->GetTeamIndex() == 1) && (mAgentState->GetUniformNumber() ==7))
00369         GetLog()->Debug() << "percept: " << visibleObjects.size() << " objects. :::"
00370                           << up << " theta " << gRadToDeg(fwTheta)
00371                           << " phi " << gRadToDeg(fwPhi) << "\n";
00372 
00373     for (std::list<ObjectData>::iterator i = visibleObjects.begin();
00374          i != visibleObjects.end(); ++i)
00375     {
00376         ObjectData& od = (*i);
00377 
00378 
00379         od.mRelPos = SoccerBase::FlipView(od.mRelPos, ti);
00380         if ((mAgentState->GetTeamIndex() == 1) && (mAgentState->GetUniformNumber() ==7))
00381             GetLog()->Debug() << "object " << od.mObj->GetPerceptName()
00382                               << " at : " << od.mRelPos << "\n";
00383 
00384         if (mAddNoise)
00385         {
00386             od.mRelPos += mError;
00387         }
00388 
00389         if (od.mRelPos.Length() <= 0.1)
00390         {
00391             // object is too close
00392             continue;
00393         }
00394 
00395         // theta is the angle in horizontal plane, with fwAngle as 0 degree
00396         od.mTheta = gRadToDeg(gNormalizeRad(
00397                                   Vector2f(od.mRelPos[0],od.mRelPos[1]).GetAngleRad() -
00398                                   fwTheta
00399                                   ));
00400         // flags are always visible
00401         if ((gAbs(od.mTheta) > hAngle_2) && (od.mObj->GetPerceptName() != "Flag"))
00402         {
00403             // object is out of view range
00404 //                                 GetLog()->Debug() << "(RestrictedVisionPerceptor) Omitting "
00405 //                               << od.mObj->GetPerceptName() << od.mObj->GetID()
00406 //                               << ": h-angle = " << od.mTheta << ".\n" ;
00407             continue;
00408         }
00409 
00410 
00411         // latitude with fwPhi as 0 degreee
00412         od.mPhi = gRadToDeg(gNormalizeRad(
00413                                 Vector2f(
00414                                     Vector2f(od.mRelPos[0],od.mRelPos[1]).Length(),
00415                                     od.mRelPos[2]).GetAngleRad() - fwPhi
00416                                 ));
00417 
00418 
00419         if ((gAbs(od.mPhi) > vAngle_2) && (od.mObj->GetPerceptName() != "Flag"))
00420             continue;
00421 
00422         // log for 7th agent of the first team
00423         if ((mAgentState->GetTeamIndex() == 1) && (mAgentState->GetUniformNumber() ==7))
00424             GetLog()->Debug() << "percept: " << "adding object: "
00425                               << gAbs(od.mPhi) << ":" << vAngle_2 << "\n";
00426 
00427         // make some noise
00428         ApplyNoise(od);
00429 
00430         // generate a sense entry
00431         AddSense(predicate,od);
00432 
00433     }
00434 
00435     if (mSenseMyPos)
00436     {
00437         salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();
00438 
00439         Vector3f sensedMyPos = myPos;
00440         SoccerBase::FlipView(sensedMyPos, ti);
00441 
00442         ParameterList& element = predicate.parameter.AddList();
00443         element.AddValue(std::string("mypos"));
00444         element.AddValue(sensedMyPos[0]);
00445         element.AddValue(sensedMyPos[1]);
00446         element.AddValue(sensedMyPos[2]);
00447     }
00448     return true;
00449 }
00450 
00451 bool
00452 RestrictedVisionPerceptor::Percept(boost::shared_ptr<PredicateList> predList)
00453 {
00454     if (
00455         (mTransformParent.get() == 0) ||
00456         (mActiveScene.get() == 0) ||
00457         (mAgentState.get() == 0)
00458         )
00459     {
00460         return false;
00461     }
00462 #if 1
00463     return StaticAxisPercept(predList);
00464 #else
00465     return mStaticSenseAxis ?
00466         StaticAxisPercept(predList) :
00467         DynamicAxisPercept(predList);
00468 #endif
00469 }
00470 
00471 bool RestrictedVisionPerceptor::CheckOcclusion(const Vector3f& my_pos, const ObjectData& od) const
00472 {
00473     // (occlusion test disabled for now, every object is visible)
00474     return false;
00475 
00476 //     if (mRay.get() == 0) return;
00477 
00478 //     // sort objects wrt their distance
00479 //     visible_objects.sort();
00480 //     // check visibility
00481 //     std::list<ObjectData>::iterator start = visible_objects.begin();
00482 //     ++start;
00483 
00484 //     // this is going to be expensive now: to check occlusion of an object,
00485 //     // we have to check all closer objects. For n objects, we have to
00486 //     // check at most n*(n-1)/2 objects.
00487 //     for (std::list<ObjectData>::iterator i = start;
00488 //          i != visible_objects.end(); ++i)
00489 //     {
00490 //         for (std::list<ObjectData>::iterator j = visible_objects.begin();
00491 //              j != i; ++j)
00492 //         {
00493 //             // cast ray from camera to object (j)
00494 //             mRay->SetParams(j->mRelPos,my_pos,j->mDist);
00495 
00496 //             dContactGeom contact;
00497 
00498 //             shared_ptr<Collider> collider = shared_static_cast<Collider>
00499 //                 (i->mObj->GetChildSupportingClass("Collider"));
00500 
00501 //             if (mRay->Intersects(collider))
00502 //                 {
00503 //                     j->mVisible = false;
00504 //                     j = i;
00505 //                 }
00506 //         }
00507 //     }
00508 }
00509 
00510 void
00511 RestrictedVisionPerceptor::SetSenseMyPos(bool sense)
00512 {
00513     mSenseMyPos = sense;
00514 }

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