Exemplo n.º 1
0
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;
}