shared_ptr<ActionObject>
PanTiltEffector::GetActionObject(const Predicate& predicate)
{
if (predicate.name != GetPredicate())
{
GetLog()->Error() << "ERROR: (PanTiltEffector) invalid predicate"
<< predicate.name << "\n";
return shared_ptr<ActionObject>();
}
Predicate::Iterator iter = predicate.begin();
float pan;
if (! predicate.AdvanceValue(iter, pan))
{
GetLog()->Error() << "ERROR: (PanTiltEffector) 2 float parameters expected\n";
return shared_ptr<ActionObject>(new ActionObject(GetPredicate()));
}
float tilt;
if (! predicate.AdvanceValue(iter, tilt))
{
GetLog()->Error() << "ERROR: (PanTiltEffector) float parameter expected\n";
return shared_ptr<ActionObject>(new ActionObject(GetPredicate()));
}
return shared_ptr<ActionObject>(new PanTiltAction(GetPredicate(),pan,tilt));
}
boost::shared_ptr<ActionObject>
SayEffector::GetActionObject(const Predicate& predicate)
{
if (predicate.name != GetPredicate())
{
GetLog()->Error() << "ERROR: (SayEffector) invalid predicate"
<< predicate.name << "\n";
// some error happened
return boost::shared_ptr<ActionObject>();
}
Predicate::Iterator iter = predicate.begin();
std::string message;
if (! predicate.AdvanceValue(iter, message))
{
GetLog()->Error()
<< "ERROR: (SayEffector) said message expected\n";
// some error happened
return boost::shared_ptr<ActionObject>();
}
// construct the SayAction object
return boost::shared_ptr<SayAction>(new SayAction(GetPredicate(), message));
}
shared_ptr<ActionObject> HingeEffector::GetActionObject(const Predicate& predicate)
{
for(;;)
{
if (mJoint.get() == 0)
{
break;
}
if (predicate.name != GetPredicate())
{
GetLog()->Error()
<< "ERROR: (HingeEffector) invalid predicate"
<< predicate.name << "\n";
break;
}
Predicate::Iterator iter = predicate.begin();
float velocity;
if (! predicate.AdvanceValue(iter, velocity))
{
GetLog()->Error()
<< "ERROR: (HingeEffector) motor velocity expected\n";
break;
}
return shared_ptr<HingeAction>(new HingeAction(GetPredicate(),velocity));
}
return shared_ptr<ActionObject>();
}
shared_ptr<ActionObject>
KickEffector::GetActionObject(const Predicate& predicate)
{
do
{
if (predicate.name != GetPredicate())
{
GetLog()->Error() << "ERROR: (KickEffector) invalid predicate"
<< predicate.name << "\n";
break;
}
Predicate::Iterator iter = predicate.begin();
float angle;
if (! predicate.AdvanceValue(iter, angle))
{
GetLog()->Error()
<< "ERROR: (KickEffector) kick angle parameter expected\n";
break;
}
float power;
if (! predicate.AdvanceValue(iter, power))
{
GetLog()->Error()
<< "ERROR: (KickEffector) kick power expected\n";
break;
}
// construct the KickAction object
return shared_ptr<KickAction>(new KickAction(GetPredicate(),angle,power));
} while (0);
// some error happened
return shared_ptr<ActionObject>();
}
boost::shared_ptr<ActionObject>
BeamEffector::GetActionObject(const Predicate& predicate)
{
if (predicate.name != GetPredicate())
{
GetLog()->Error() << "ERROR: (BeamEffector) invalid predicate"
<< predicate.name << "\n";
return boost::shared_ptr<ActionObject>();
}
Predicate::Iterator iter = predicate.begin();
float posX;
if (! predicate.AdvanceValue(iter, posX))
{
GetLog()->Error()
<< "ERROR: (BeamEffector) float expected for parameter1\n";
return boost::shared_ptr<ActionObject>(new ActionObject(GetPredicate()));
}
float posY;
if (! predicate.AdvanceValue(iter, posY))
{
GetLog()->Error()
<< "ERROR: (BeamEffector) float expected for parameter2\n";
return boost::shared_ptr<ActionObject>(new ActionObject(GetPredicate()));
}
float angle;
if (! predicate.AdvanceValue(iter, angle))
{
GetLog()->Error()
<< "ERROR: (BeamEffector) float expected for parameter3\n";
return boost::shared_ptr<ActionObject>(new ActionObject(GetPredicate()));
}
return boost::shared_ptr<ActionObject>(new BeamAction(GetPredicate(), posX, posY, angle));
}
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;
}
}