00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00040 SetPredicateName("Vision");
00041
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
00087
00088
00089
00090
00091
00092
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
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;
00215 }
00216
00217 shared_ptr<Transform> j = od.mObj->GetTransformParent();
00218
00219 if (j.get() == 0)
00220 {
00221 continue;
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
00302 continue;
00303 }
00304
00305
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
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
00315 ApplyNoise(od);
00316
00317 if (gAbs(od.mTheta) > mHViewCone) continue;
00318 if (gAbs(od.mPhi) > mVViewCone) continue;
00319
00320
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
00353 double fwTheta = gNormalizeRad(Vector2f(up[0],up[1]).GetAngleRad());
00354
00355
00356
00357
00358
00359
00360
00361 double fwPhi = 0.0;
00362
00363
00364 TObjectList visibleObjects;
00365 SetupVisibleObjects(visibleObjects);
00366
00367
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
00392 continue;
00393 }
00394
00395
00396 od.mTheta = gRadToDeg(gNormalizeRad(
00397 Vector2f(od.mRelPos[0],od.mRelPos[1]).GetAngleRad() -
00398 fwTheta
00399 ));
00400
00401 if ((gAbs(od.mTheta) > hAngle_2) && (od.mObj->GetPerceptName() != "Flag"))
00402 {
00403
00404
00405
00406
00407 continue;
00408 }
00409
00410
00411
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
00423 if ((mAgentState->GetTeamIndex() == 1) && (mAgentState->GetUniformNumber() ==7))
00424 GetLog()->Debug() << "percept: " << "adding object: "
00425 << gAbs(od.mPhi) << ":" << vAngle_2 << "\n";
00426
00427
00428 ApplyNoise(od);
00429
00430
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
00474 return false;
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508 }
00509
00510 void
00511 RestrictedVisionPerceptor::SetSenseMyPos(bool sense)
00512 {
00513 mSenseMyPos = sense;
00514 }