00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "visionperceptor.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
00028 using namespace zeitgeist;
00029 using namespace oxygen;
00030 using namespace boost;
00031 using namespace salt;
00032
00033 VisionPerceptor::VisionPerceptor() : Perceptor(),
00034 mSenseMyPos(false),
00035 mAddNoise(true),
00036 mUseRandomNoise(true),
00037 mStaticSenseAxis(true)
00038 {
00039
00040 SetPredicateName("Vision");
00041
00042 SetNoiseParams(0.0965, 0.1225, 0.1480, 0.005);
00043 }
00044
00045 VisionPerceptor::~VisionPerceptor()
00046 {
00047 mDistRng.reset();
00048 mPhiRng.reset();
00049 mThetaRng.reset();
00050 }
00051
00052 void
00053 VisionPerceptor::SetNoiseParams(float sigma_dist, float sigma_phi,
00054 float sigma_theta, float cal_error_abs)
00055 {
00056 mSigmaDist = sigma_dist;
00057 mSigmaPhi = sigma_phi;
00058 mSigmaTheta = sigma_theta;
00059 mCalErrorAbs = cal_error_abs;
00060
00061 NormalRngPtr rng1(new salt::NormalRNG<>(0.0,sigma_dist));
00062 mDistRng = rng1;
00063 NormalRngPtr rng2(new salt::NormalRNG<>(0.0,sigma_phi));
00064 mPhiRng = rng2;
00065 NormalRngPtr rng3(new salt::NormalRNG<>(0.0,sigma_theta));
00066 mThetaRng = rng3;
00067
00068 salt::UniformRNG<float> rng4(-mCalErrorAbs,mCalErrorAbs);
00069 mError = salt::Vector3f(rng4(),rng4(),rng4());
00070 }
00071
00072 void
00073 VisionPerceptor::OnLink()
00074 {
00075 SoccerBase::GetTransformParent(*this,mTransformParent);
00076 SoccerBase::GetAgentState(*this, mAgentState);
00077 SoccerBase::GetActiveScene(*this,mActiveScene);
00078 }
00079
00080 void
00081 VisionPerceptor::OnUnlink()
00082 {
00083 mDistRng.reset();
00084 mPhiRng.reset();
00085 mThetaRng.reset();
00086 mTransformParent.reset();
00087 mAgentState.reset();
00088 mActiveScene.reset();
00089 }
00090
00091 void
00092 VisionPerceptor::AddNoise(bool add_noise)
00093 {
00094 mAddNoise = add_noise;
00095 }
00096
00097 void
00098 VisionPerceptor::UseRandomNoise(bool random_noise)
00099 {
00100 mUseRandomNoise = random_noise;
00101 }
00102
00103 void
00104 VisionPerceptor::SetStaticSenseAxis(bool static_axis)
00105 {
00106 mStaticSenseAxis = static_axis;
00107 }
00108
00109 bool
00110 VisionPerceptor::ConstructInternal()
00111 {
00112 mRay = shared_static_cast<oxygen::RayCollider>
00113 (GetCore()->New("oxygen/RayCollider"));
00114
00115 if (mRay.get() == 0)
00116 {
00117 GetLog()->Error() << "Error: (VisionPerceptor) cannot create Raycollider. "
00118 << "occlusion check disabled\n";
00119 }
00120
00121 return true;
00122 }
00123
00124 void
00125 VisionPerceptor::SetupVisibleObjects(TObjectList& visibleObjects)
00126 {
00127 TLeafList objectList;
00128 mActiveScene->ListChildrenSupportingClass<ObjectState>(objectList, true);
00129
00130 salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();
00131
00132 for (TLeafList::iterator i = objectList.begin();
00133 i != objectList.end(); ++i)
00134 {
00135 ObjectData od;
00136 od.mObj = shared_static_cast<ObjectState>(*i);
00137
00138 if (od.mObj.get() == 0)
00139 {
00140 GetLog()->Error() << "Error: (VisionPerceptor) skipped: "
00141 << (*i)->GetName() << "\n";
00142 continue;
00143 }
00144
00145 shared_ptr<Transform> j = od.mObj->GetTransformParent();
00146
00147 if (j.get() == 0)
00148 {
00149 continue;
00150 }
00151
00152 od.mRelPos = j->GetWorldTransform().Pos() - myPos;
00153 od.mDist = od.mRelPos.Length();
00154
00155 visibleObjects.push_back(od);
00156 }
00157 }
00158
00159 void VisionPerceptor::AddSense(oxygen::Predicate& predicate, ObjectData& od) const
00160 {
00161 ParameterList& element = predicate.parameter.AddList();
00162 element.AddValue(od.mObj->GetPerceptName());
00163
00164 if(od.mObj->GetPerceptName() == "Player")
00165 {
00166 ParameterList player;
00167 player.AddValue(std::string("team"));
00168 player.AddValue
00169 (std::string
00170 (od.mObj->GetPerceptName(ObjectState::PT_Player)
00171 )
00172 );
00173 element.AddValue(player);
00174 }
00175
00176 if (!od.mObj->GetID().empty())
00177 {
00178 ParameterList id;
00179 id.AddValue(std::string("id"));
00180 id.AddValue(od.mObj->GetID());
00181 element.AddValue(id);
00182 }
00183
00184 ParameterList& position = element.AddList();
00185 position.AddValue(std::string("pol"));
00186 position.AddValue(od.mDist);
00187 position.AddValue(od.mTheta);
00188 position.AddValue(od.mPhi);
00189 }
00190
00191 void
00192 VisionPerceptor::ApplyNoise(ObjectData& od) const
00193 {
00194 if (mAddNoise)
00195 {
00196 if (mUseRandomNoise)
00197 {
00198 od.mDist += (*(mDistRng.get()))() * od.mDist / 100.0;
00199 od.mTheta += (*(mThetaRng.get()))();
00200 od.mPhi += (*(mPhiRng.get()))();
00201 } else
00202 {
00203
00204
00205
00206
00207
00208
00209 od.mDist += salt::NormalRNG<>(0.0,mSigmaDist)();
00210 od.mTheta += salt::NormalRNG<>(0.0,mSigmaTheta)();
00211 od.mPhi += salt::NormalRNG<>(0.0,mSigmaPhi)();
00212 }
00213 }
00214 }
00215
00216
00217 bool
00218 VisionPerceptor::StaticAxisPercept(boost::shared_ptr<PredicateList> predList)
00219 {
00220 Predicate& predicate = predList->AddPredicate();
00221 predicate.name = mPredicateName;
00222 predicate.parameter.Clear();
00223
00224 TTeamIndex ti = mAgentState->GetTeamIndex();
00225 salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();
00226
00227 TObjectList visibleObjects;
00228 SetupVisibleObjects(visibleObjects);
00229
00230 for (std::list<ObjectData>::iterator i = visibleObjects.begin();
00231 i != visibleObjects.end(); ++i)
00232 {
00233 ObjectData& od = (*i);
00234
00235 od.mRelPos = SoccerBase::FlipView(od.mRelPos, ti);
00236 if (mAddNoise)
00237 {
00238 od.mRelPos += mError;
00239 }
00240
00241 if (
00242 (od.mRelPos.Length() <= 0.1) ||
00243 (CheckOcclusion(myPos,od))
00244 )
00245 {
00246
00247 continue;
00248 }
00249
00250
00251 od.mTheta = salt::gRadToDeg(salt::gArcTan2(od.mRelPos[1], od.mRelPos[0]));
00252
00253
00254 od.mPhi = 90.0 - salt::gRadToDeg(salt::gArcCos(od.mRelPos[2]/od.mDist));
00255
00256
00257 ApplyNoise(od);
00258
00259
00260 AddSense(predicate,od);
00261 }
00262
00263 if (mSenseMyPos)
00264 {
00265 Vector3f sensedMyPos = SoccerBase::FlipView(myPos, ti);
00266
00267 ParameterList& element = predicate.parameter.AddList();
00268 element.AddValue(std::string("mypos"));
00269 element.AddValue(sensedMyPos[0]);
00270 element.AddValue(sensedMyPos[1]);
00271 element.AddValue(sensedMyPos[2]);
00272 }
00273
00274 return true;
00275 }
00276
00277 bool
00278 VisionPerceptor::DynamicAxisPercept(boost::shared_ptr<PredicateList> predList)
00279 {
00280 Predicate& predicate = predList->AddPredicate();
00281 predicate.name = mPredicateName;
00282 predicate.parameter.Clear();
00283
00284 TTeamIndex ti = mAgentState->GetTeamIndex();
00285
00286 const Vector3f& up = mTransformParent->GetWorldTransform().Up();
00287
00288
00289 double fwTheta = gNormalizeRad(Vector2f(up[0],up[1]).GetAngleRad());
00290
00291
00292 double fwPhi = gNormalizeRad(Vector2f(up[0],up[2]).GetAngleRad());
00293
00294 TObjectList visibleObjects;
00295 SetupVisibleObjects(visibleObjects);
00296
00297 for (std::list<ObjectData>::iterator i = visibleObjects.begin();
00298 i != visibleObjects.end(); ++i)
00299 {
00300 ObjectData& od = (*i);
00301
00302 od.mRelPos = SoccerBase::FlipView(od.mRelPos, ti);
00303 if (mAddNoise)
00304 {
00305 od.mRelPos += mError;
00306 }
00307
00308 if (od.mRelPos.Length() <= 0.1)
00309 {
00310
00311 continue;
00312 }
00313
00314
00315 od.mTheta = gRadToDeg(gNormalizeRad(
00316 Vector2f(od.mRelPos[0],od.mRelPos[1]).GetAngleRad() -
00317 fwTheta
00318 ));
00319
00320
00321 od.mPhi = gRadToDeg(gNormalizeRad(
00322 Vector2f(od.mRelPos[0],od.mRelPos[2]).GetAngleRad() -
00323 fwPhi
00324 ));
00325
00326
00327 ApplyNoise(od);
00328
00329
00330 AddSense(predicate,od);
00331 }
00332
00333 return true;
00334 }
00335
00336 bool
00337 VisionPerceptor::Percept(boost::shared_ptr<PredicateList> predList)
00338 {
00339 if (
00340 (mTransformParent.get() == 0) ||
00341 (mActiveScene.get() == 0) ||
00342 (mAgentState.get() == 0)
00343 )
00344 {
00345 return false;
00346 }
00347
00348 return mStaticSenseAxis ?
00349 StaticAxisPercept(predList) :
00350 DynamicAxisPercept(predList);
00351 }
00352
00353 bool VisionPerceptor::CheckOcclusion(const Vector3f& my_pos, const ObjectData& od) const
00354 {
00355
00356 return false;
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390 }
00391
00392 void
00393 VisionPerceptor::SetSenseMyPos(bool sense)
00394 {
00395 mSenseMyPos = sense;
00396 }