void
SexpMonitor::AddPredicates(std::ostringstream& ss, const PredicateList& pList)
{
for (
PredicateList::TList::const_iterator iter = pList.begin();
iter != pList.end();
++iter
)
{
const Predicate& pred = (*iter);
ss << "(";
ss << pred.name;
const ParameterList& paramList = pred.parameter;
ParameterList::TVector::const_iterator pIter = paramList.begin();
std::string param;
while (
(pIter != paramList.end()) &&
(paramList.AdvanceValue(pIter, param))
)
{
ss << " ";
ss << param;
}
ss << ")";
}
}
double can_load(const ParameterList & parameterList,
predicateCallbackType predicateCallback, numericalFluentCallbackType numericalFluentCallback, int )
{
ROS_DEBUG("Calling module %s", __func__);
static int count = 0;
count++;
//cout << "Count: " << count << endl;
if(DEBUG_MOD)
cout << parameterList << endl;
PredicateList* list = NULL;
predicateCallback(list);
if(list == NULL) {
exit(1);
return false;
}
if(DEBUG_MOD) {
cout << "Predicates are: " << endl;
cout << *list << endl;
}
string me = parameterList.front().value;
string pack = parameterList.back().value;
deque<string> packs_in_truck;
for(PredicateList::iterator it = list->begin(); it != list->end(); it++) {
Predicate p = *it;
if(!p.value)
continue;
if(p.name != "in")
continue;
if(p.parameters.back().value == me)
packs_in_truck.push_back(p.parameters.front().value);
}
if(DEBUG_MOD) {
for(deque<string>::iterator it = packs_in_truck.begin(); it != packs_in_truck.end(); it++) {
cout << *it << endl;
}
if(packs_in_truck.empty()) {
cout << "No packs in " << me << endl;
} else {
cout << packs_in_truck.size() << " packs i truck " << me << endl;
}
}
packs_in_truck.push_back(pack);
NumericalFluentList* nfl = new NumericalFluentList();
for(deque<string>::iterator it = packs_in_truck.begin(); it != packs_in_truck.end(); it++) {
ParameterList pl;
pl.push_back(Parameter("p", "package", *it));
nfl->push_back(NumericalFluent("package-size", pl));
}
ParameterList plt;
plt.push_back(Parameter("v", "vehicle", me));
nfl->push_back(NumericalFluent("capacity", plt));
if(!numericalFluentCallback(nfl)) {
exit(1);
return false;
}
if(DEBUG_MOD)
cout << *nfl << endl;
// ideally: get sizes 1 - n-2, size n-1 == package, size n == capcity
// 1..n-2 + n == full_load
deque<double> packs;
for(NumericalFluentList::iterator it = nfl->begin(); it != nfl->end(); it++) {
packs.push_back((*it).value);
}
double cap = packs.back();
packs.erase(packs.end() - 1);
double pSize = packs.back();
packs.erase(packs.end() - 1);
if(DEBUG_MOD)
cout << "Packs" << packs.size() <<endl;
double sumPacks = 0;
for(deque<double>::iterator it = packs.begin(); it != packs.end(); it++) {
if(DEBUG_MOD)
cout << *it << endl;
sumPacks += *it;
}
if(DEBUG_MOD) {
cout << "newpack " << pSize << endl;
cout << "cap " << cap << endl;
}
double loadCap = sumPacks + cap;
if(DEBUG_MOD) {
cout << "loadCap " << loadCap;
cout << endl;
}
packs.push_back(pSize);
bool loadPoss = checkLoading(packs, loadCap);
return loadPoss;
}