void SoccerBehavior::ParseVision(const Predicate& predicate)
{
ParseObjectVision(predicate);
// find the PerfectVision data about the object
Predicate::Iterator iter(predicate);
// advance to the section about object 'name'
if (! predicate.FindParameter(iter,"mypos"))
{
return;
}
// read my position
VisionSense sense;
predicate.GetValue(iter,mMyPos);
}
void SoccerBehavior::ParseObjectVision(const Predicate& predicate)
{
for (
Predicate::Iterator iter(predicate);
iter != iter.end();
++iter
)
{
// extract the element as a parameter list
Predicate::Iterator paramIter = iter;
if (! predicate.DescentList(paramIter))
{
continue;
}
// read the object name
string name;
if (! predicate.GetValue(paramIter,name))
{
continue;
}
// try read the 'id' section
string strId;
if (predicate.GetValue(paramIter,"id", strId))
{
name += strId;
}
// try to lookup the VisionObject
TVisionObjectMap::iterator visiter = mVisionObjectMap.find(name);
if (visiter == mVisionObjectMap.end())
{
continue;
}
VisionObject vo = (*visiter).second;
// find the 'pol' entry in the object's section
Predicate::Iterator polIter = paramIter;
if (! predicate.FindParameter(polIter,"pol"))
{
continue;
}
// read the position vector
VisionSense sense;
if (
(! predicate.AdvanceValue(polIter,sense.distance)) ||
(! predicate.AdvanceValue(polIter,sense.theta)) ||
(! predicate.AdvanceValue(polIter,sense.phi))
)
{
continue;
}
// update the vision map
// cerr << "+++" << endl;
// cerr << "VO " << vo << endl;
// cerr << "D " << sense.distance << endl;
// cerr << "T " << sense.theta << endl;
// cerr << "P " << sense.phi << endl;
// cerr << "---" << endl;
mVisionMap[vo] = sense;
}
}