static float domainFunction(const PRUstate& fromState, const PRUstate& toState,
const string& kind, float parameter) {
if (kind == "null")
return 0;
if (kind == "distance") {
PRUstate::const_iterator itF = fromState.find("location");
PRUstate::const_iterator itT = toState.find("location");
if ((itF==fromState.end()) || (itT==toState.end())) {
std::cerr << "Unable to compute distance, locations not specified" << std::endl;
return 0;
}
return app->getDistance(itF->second, itT->second); // gets the distance from the movebase cache
}
std::cerr << "Unknown function kind " << kind << "!" << std::endl;
return 0;
}
bool PRU2MDPprogress::isMatching(const PRUstate &stateVariables) const {
for (PRUstate::const_iterator itV = this->stateVariables.begin();
itV != this->stateVariables.end(); ++itV) {
PRUstate::const_iterator itF = stateVariables.find(itV->first);
if (itF == stateVariables.end()) {
if (itV->second != "nil")
return false;
} else if (itF->second != itV->second)
return false;
}
return true; // layer name matches and any specified variable does...
} // isMatching(...)