symbolicState.cpp
Go to the documentation of this file.
00001 #include "continual_planning_executive/symbolicState.h"
00002 #include <sstream>
00003 #include <ros/ros.h>
00004 
00005 bool Predicate::operator<(const Predicate & p) const
00006 {
00007 #if 0
00008     if(name < p.name)
00009         return true;
00010     else if(name > p.name)
00011         return false;
00012     if(parameters.size() < p.parameters.size())
00013         return true;
00014     else if(parameters.size() > p.parameters.size())
00015         return false;
00016     for(unsigned int i = 0; i < parameters.size(); i++) {
00017         if(parameters[i] < p.parameters[i])
00018             return true;
00019         else if(parameters[i] > p.parameters[i])
00020             return false;
00021     }
00022     // equal -> false (not <)
00023     return false;
00024 #endif
00025     // sorting procedure for predicates
00026     // no particular reason then to put them in the map somehow
00027     // so: tailored to nice outputs
00028     //
00029     // First: "global" preds = no parameters
00030     // Then: sorted by their first parameter
00031     // -> Idea: If that is an object all preds of the same object in one bunch
00032     // Next: sorted by name
00033     // -> Same predicates for the object together
00034     // finally sorted by the following parameters
00035 
00036     // 0-size params go before all others
00037     if(parameters.empty() && !p.parameters.empty())
00038         return true;
00039     if(!parameters.empty() && p.parameters.empty())
00040         return false;
00041     // if both 0-size, sort by name
00042     if(parameters.empty() && p.parameters.empty())
00043         return name < p.name;
00044 
00045     // now both parameters not empty
00046     // sort by first parameter
00047     if(parameters[0] < p.parameters[0])
00048         return true;
00049     if(parameters[0] > p.parameters[0])
00050         return false;
00051     // first is same, sort by name
00052     if(name < p.name)
00053         return true;
00054     else if(name > p.name)
00055         return false;
00056     // names are same, just sort by the following params
00057     if(parameters.size() < p.parameters.size())
00058         return true;
00059     else if(parameters.size() > p.parameters.size())
00060         return false;
00061     for(unsigned int i = 1; i < parameters.size(); i++) {
00062         if(parameters[i] < p.parameters[i])
00063             return true;
00064         else if(parameters[i] > p.parameters[i])
00065             return false;
00066     }
00067     // equal -> false (not <)
00068     return false;
00069 }
00070 
00071 std::ostream & operator<<(std::ostream & os, const Predicate & p)
00072 {
00073     os << "(" << p.name;
00074     forEach(const string & s, p.parameters) {
00075         os << " " << s;
00076     }
00077     os << ")";
00078     return os;
00079 }
00080 
00081 
00082 SymbolicState::SymbolicState()
00083 {
00084 }
00085 
00086 SymbolicState::~SymbolicState()
00087 {
00088 }
00089 
00090 void SymbolicState::addObject(string obj, string type)
00091 {
00092     typedef pair< multimap<string,string>::iterator, multimap<string,string>::iterator > StringMapRange;
00093     multimap<string,string>::iterator mapIt;
00094    
00095     // insert obj for its type and all its supertypes
00096     StringMapRange ret = _superTypes.equal_range(type);
00097     for(mapIt = ret.first; mapIt != ret.second; mapIt++) {
00098         string st = mapIt->second;
00099         // should now insert (st, obj) into _typedObjects
00100 
00101         StringMapRange objRange = _typedObjects.equal_range(st);
00102         bool alreadyIn = false;
00103         for(multimap<string,string>::iterator it = objRange.first; it != objRange.second; it++) {
00104             if(it->second == obj) {
00105                 alreadyIn = true;
00106                 break;
00107             }
00108         }
00109         if(!alreadyIn) {
00110             _typedObjects.insert(make_pair(st, obj));
00111         }
00112     }
00113 
00114     // This code is only for backwards compatibility
00115     // If no types have been inserted into _superTypes, we
00116     // still add the pair (type, obj) into _typedObjects
00117 
00118     ret = _typedObjects.equal_range(type);
00119     // check if map contains typed obj of same name
00120     for(multimap<string,string>::iterator it = ret.first; it != ret.second; it++) {
00121         if(it->second == obj)
00122             return;
00123     }
00124 
00125     // insert it
00126     _typedObjects.insert(std::make_pair(type, obj));
00127 }
00128 
00129 void SymbolicState::addSuperType(string type, string supertype)
00130 {
00131     // door_location - location
00132     multimap<string,string>::iterator mapIt;
00133     pair< multimap<string,string>::iterator, multimap<string,string>::iterator > ret;
00134 
00135     deque<pair<string, string> > queue; // all new connections
00136     queue.push_back(make_pair(type, supertype));
00137     queue.push_back(make_pair(type, type));
00138     queue.push_back(make_pair(supertype, supertype));
00139 
00140     // add supertype pairs in graph and readd their
00141     // dependencies to the queue to compute the transitive closure
00142     // as a fixed point computation (no more new pairs)
00143     while(!queue.empty()) {
00144         pair<string, string> stp = queue.front();
00145         queue.pop_front();
00146 
00147         // check if stp is already in
00148         ret = _superTypes.equal_range(stp.first);
00149         bool alreadyIn = false;
00150         for(mapIt = ret.first; mapIt != ret.second; mapIt++) {
00151             string st = mapIt->second;
00152             if(stp.second == st) {
00153                 alreadyIn = true;
00154                 break;
00155             }
00156         }
00157         if(alreadyIn)
00158             continue;
00159 
00160         // this is a new pair, insert it
00161         _superTypes.insert(stp);
00162 
00163         // now for all pairs (t, st) in the map
00164         // 1. if stp.second == t, make sure the pair (stp.first, st) is in
00165         // 2. if stp.first == st, make sure the pair (t, stp.second) is in
00166         for(mapIt = _superTypes.begin(); mapIt != _superTypes.end(); mapIt++) {
00167             // mapIt = (t, st)
00168             if(stp.second == mapIt->first)      // 1.
00169                 queue.push_back(make_pair(stp.first, mapIt->second));
00170             if(stp.first == mapIt->second)      // 2.
00171                 queue.push_back(make_pair(mapIt->first, stp.second));
00172         }
00173     }
00174 }
00175 
00176 bool SymbolicState::isMostSpecificType(string obj, string type) const
00177 {
00178     // check if there is a pair (t, st) where type == st (!= t), then t is more specific
00179     // (given that (t, obj) is in _typedObjects)
00180     multimap<string,string>::const_iterator mapIt;
00181     for(mapIt = _superTypes.begin(); mapIt != _superTypes.end(); mapIt++) {
00182         // mapIt = (t, st)
00183         if(type == mapIt->second && mapIt->first != mapIt->second) { // t is a true subtype of type
00184             string t = mapIt->first;    // check if there is an entry (t, obj) in _typedObjects
00185 
00186             pair< multimap<string,string>::const_iterator, multimap<string,string>::const_iterator > ret;
00187             ret = _typedObjects.equal_range(t);
00188 
00189             multimap<string,string>::const_iterator objectIt;
00190             for(objectIt = ret.first; objectIt != ret.second; objectIt++) {
00191                 if(objectIt->second == obj) {   // (t, obj) is a valid pair, this type is not most specific
00192                     return false;
00193                 }
00194             }
00195         }
00196     }
00197 
00198     // if we found no more specific one or 
00199     // if there is no _superTypes entry, return true
00200     return true;
00201 }
00202 
00203 void SymbolicState::printSuperTypes() const
00204 {
00205     printf("Type Hierarchy:\n");
00206     // desired output:
00207     // door_location is a (object, door_location, location)
00208     // manipulation_location is a (object, manipulation_location, location)
00209     // location is a (object, location)
00210     string currentType;
00211     multimap<string,string>::const_iterator mapIt;
00212     for(mapIt = _superTypes.begin(); mapIt != _superTypes.end(); mapIt++) {
00213         // mapIt = (t, st)
00214         if(mapIt->first != currentType) {   // different type
00215             if(!currentType.empty()) {  // this is not the first, close the last one
00216                 printf(")\n");
00217             }
00218             currentType = mapIt->first;
00219             printf("%s is a (object", currentType.c_str());
00220         }
00221         printf(", %s", mapIt->second.c_str());
00222     }
00223     if(!_superTypes.empty())        // close the last one
00224         printf(")\n");
00225 }
00226 
00227 void SymbolicState::removeObject(string obj, bool removePredicates)
00228 {
00229     for(multimap<string,string>::iterator it = _typedObjects.begin(); it != _typedObjects.end(); ) {
00230         multimap<string, string>::iterator current = it;    // remember current for deletion
00231         it++;       // but forward it now, before deleting that
00232         if(current->second == obj) {
00233             _typedObjects.erase(current);    // it invalid now
00234         }
00235     }
00236 
00237     if(!removePredicates)
00238         return;
00239 
00240     bool removedSomething = true;   // true, to get in loop
00241     // unfortunately the erase function returning an iterator is only C++11
00242     // so for now we do the ugly (loop until match, erase once), until nothing changed - approach
00243     while(removedSomething) {
00244         removedSomething = false;
00245         for(map<Predicate, bool>::iterator it = _booleanPredicates.begin(); it != _booleanPredicates.end(); it++) {
00246             bool foundObj = false;
00247             for(vector<string>::const_iterator paramIt = it->first.parameters.begin();
00248                     paramIt != it->first.parameters.end(); paramIt++) {
00249                 if(obj == *paramIt) {
00250                     foundObj = true;
00251                     break;
00252                 }
00253             }
00254             if(foundObj) {
00255                 _booleanPredicates.erase(it);    // it invalid
00256                 removedSomething = true;
00257                 break;
00258             }
00259         }
00260     }
00261     // the same for numerical fluents
00262     removedSomething = true;
00263     while(removedSomething) {
00264         removedSomething = false;
00265         for(map<Predicate, double>::iterator it = _numericalFluents.begin(); it != _numericalFluents.end(); it++) {
00266             bool foundObj = false;
00267             for(vector<string>::const_iterator paramIt = it->first.parameters.begin();
00268                     paramIt != it->first.parameters.end(); paramIt++) {
00269                 if(obj == *paramIt) {
00270                     foundObj = true;
00271                     break;
00272                 }
00273             }
00274             if(foundObj) {
00275                 _numericalFluents.erase(it);    // it invalid
00276                 removedSomething = true;
00277                 break;
00278             }
00279         }
00280     }
00281 }
00282 
00283 void SymbolicState::setBooleanPredicate(string name, vector<string> parameters, bool value)
00284 {
00285     Predicate bp;
00286     bp.name = name;
00287     bp.parameters = parameters;
00288 
00289     _booleanPredicates[bp] = value;
00290 }
00291 
00292 void SymbolicState::setBooleanPredicate(string name, string parameters, bool value)
00293 {
00294     vector<string> params = buildParameterList(parameters);
00295     setBooleanPredicate(name, params, value);
00296 }
00297 
00298 void SymbolicState::setAllBooleanPredicates(string name, bool value)
00299 {
00300     forEach(SymbolicState::BooleanPredicateEntry & bp, _booleanPredicates) {
00301         if(bp.first.name == name)
00302             bp.second = value;
00303     }
00304 }
00305 
00306 void SymbolicState::setNumericalFluent(string name, vector<string> parameters, double value)
00307 {
00308     Predicate bp;
00309     bp.name = name;
00310     bp.parameters = parameters;
00311 
00312     _numericalFluents[bp] = value;
00313 }
00314 
00315 void SymbolicState::setNumericalFluent(string name, string parameters, double value)
00316 {
00317     vector<string> params = buildParameterList(parameters);
00318     setNumericalFluent(name, params, value);
00319 }
00320 
00321 void SymbolicState::setAllNumericalFluents(string name, double value)
00322 {
00323     forEach(SymbolicState::NumericalFluentEntry & nf, _numericalFluents) {
00324         if(nf.first.name == name)
00325             nf.second = value;
00326     }
00327 }
00328 
00329 void SymbolicState::setObjectFluent(string name, vector<string> parameters, string value)
00330 {
00331     Predicate bp;
00332     bp.name = name;
00333     bp.parameters = parameters;
00334 
00335     _objectFluents[bp] = value;
00336 }
00337 
00338 void SymbolicState::setObjectFluent(string name, string parameters, string value)
00339 {
00340     vector<string> params = buildParameterList(parameters);
00341     setObjectFluent(name, params, value);
00342 }
00343 
00344 void SymbolicState::setAllObjectFluents(string name, string value)
00345 {
00346     forEach(SymbolicState::ObjectFluentEntry & of, _objectFluents) {
00347         if(of.first.name == name)
00348             of.second = value;
00349     }
00350 }
00351 
00352 void SymbolicState::setForEachGoalStatement(string objectType, string predicateName, bool value)
00353 {
00354     _forEachGoalStatements.insert(pair<string, pair<string, bool> >(objectType, pair<string, bool>(predicateName, value)));
00355 }
00356 
00357 bool SymbolicState::hasBooleanPredicate(const Predicate & p, bool* value) const
00358 {
00359     map<Predicate, bool>::const_iterator it = _booleanPredicates.find(p);
00360     if(it == _booleanPredicates.end())
00361         return false;
00362     if(value != NULL)
00363         *value = it->second;
00364     return true;
00365 }
00366 
00367 bool SymbolicState::hasNumericalFluent(const Predicate & p, double* value) const
00368 {
00369     map<Predicate, double>::const_iterator it = _numericalFluents.find(p);
00370     if(it == _numericalFluents.end())
00371         return false;
00372     if(value != NULL)
00373         *value = it->second;
00374     return true;
00375 }
00376 
00377 bool SymbolicState::hasObjectFluent(const Predicate & p, string* value) const
00378 {
00379     map<Predicate, string>::const_iterator it = _objectFluents.find(p);
00380     if(it == _objectFluents.end())
00381         return false;
00382     if(value != NULL)
00383         *value = it->second;
00384     return true;
00385 }
00386 
00387 
00388 bool SymbolicState::isFulfilledBy(const SymbolicState & state) const
00389 {
00390     forEach(const BooleanPredicateEntry & bp, _booleanPredicates) {
00391         bool value;
00392         // state needs to have every goal predicate
00393         if(!state.hasBooleanPredicate(bp.first, &value))
00394             return false;
00395         // and they need to be the same truth value
00396         if(value != bp.second)
00397             return false;
00398     }
00399 
00400     forEach(const NumericalFluentEntry & nf, _numericalFluents) {
00401         double value;
00402         // state needs to have every goal predicate
00403         if(!state.hasNumericalFluent(nf.first, &value))
00404             return false;
00405         // and they need to be the same value
00406         if(!double_equals(value, nf.second))
00407             return false;
00408     }
00409 
00410     forEach(const ObjectFluentEntry & of, _objectFluents) {
00411         string value;
00412         // state needs to have every goal predicate
00413         if(!state.hasObjectFluent(of.first, &value))
00414             return false;
00415         // and they need to be the same value
00416         if(value != of.second)
00417             return false;
00418     }
00419 
00420     // evaluate foreach goal statements
00421     forEach(const ForEachGoalStatements::value_type & fobp, _forEachGoalStatements) {
00422         string objectType = fobp.first;
00423         // check for all objects of the specified type
00424         pair< multimap<string,string>::const_iterator, multimap<string,string>::const_iterator > ret;
00425         ret = _typedObjects.equal_range(objectType);
00426         for(multimap<string,string>::const_iterator objectIt = ret.first; objectIt != ret.second; objectIt++)
00427         {
00428             Predicate bp;
00429             bp.name = fobp.second.first;
00430             bp.parameters = buildParameterList(objectIt->second);
00431             bool value;
00432             // state needs to have every goal predicate
00433             if(!state.hasBooleanPredicate(bp, &value))
00434                 return false;
00435             // and they need to be the same truth value
00436             if(value != fobp.second.second)
00437                 return false;
00438         }
00439     }
00440 
00441     return true;
00442 }
00443 
00444 bool SymbolicState::booleanEquals(const SymbolicState & other) const
00445 {
00446     // better: collect merged list (should be same) and then only check truth
00447     // all our predicates are the same in other
00448     forEach(const BooleanPredicateEntry & bp, _booleanPredicates) {
00449         bool value;
00450         // state needs to have every predicate
00451         if(!other.hasBooleanPredicate(bp.first, &value))
00452             return false;
00453         // and they need to be the same truth value
00454         if(value != bp.second)
00455             return false;
00456     }
00457     // all other's predicates are the same in ours
00458     forEach(const BooleanPredicateEntry & bp, other._booleanPredicates) {
00459         bool value;
00460         // state needs to have every predicate
00461         if(!hasBooleanPredicate(bp.first, &value))
00462             return false;
00463         // and they need to be the same truth value
00464         if(value != bp.second)
00465             return false;
00466     }
00467     return true;
00468 }
00469 
00470 bool SymbolicState::numericalEquals(const SymbolicState & other) const
00471 {
00472     forEach(const NumericalFluentEntry & nf, _numericalFluents) {
00473         double value;
00474         // state needs to have every predicate
00475         if(!other.hasNumericalFluent(nf.first, &value))
00476             return false;
00477         // and they need to be the same truth value
00478         if(!double_equals(value, nf.second))
00479             return false;
00480     }
00481     // all other's predicates are the same in ours
00482     forEach(const NumericalFluentEntry & nf, other._numericalFluents) {
00483         double value;
00484         // state needs to have every predicate
00485         if(!hasNumericalFluent(nf.first, &value))
00486             return false;
00487         // and they need to be the same truth value
00488         if(!double_equals(value, nf.second))
00489             return false;
00490     }
00491     return true;
00492 }
00493 
00494 bool SymbolicState::objectFluentsEquals(const SymbolicState & other) const
00495 {
00496     forEach(const ObjectFluentEntry & of, _objectFluents) {
00497         string value;
00498         // state needs to have every predicate
00499         if(!other.hasObjectFluent(of.first, &value))
00500             return false;
00501         // and they need to be the same truth value
00502         if(value != of.second)
00503             return false;
00504     }
00505     // all other's predicates are the same in ours
00506     forEach(const ObjectFluentEntry & of, other._objectFluents) {
00507         string value;
00508         // state needs to have every predicate
00509         if(!hasObjectFluent(of.first, &value))
00510             return false;
00511         // and they need to be the same truth value
00512         if(value != of.second)
00513             return false;
00514     }
00515     return true;
00516 }
00517 
00518 bool SymbolicState::equals(const SymbolicState & other) const
00519 {
00520     return booleanEquals(other) && numericalEquals(other) && objectFluentsEquals(other);
00521 }
00522 
00523 
00524 void SymbolicState::toPDDLProblem(std::ostream & os) const
00525 {
00526     os << "  (:objects" << std::endl << "    ";
00527     string lastType = "";
00528     for(multimap<string,string>::const_iterator it = _typedObjects.begin(); it != _typedObjects.end(); it++) {
00529         if(!isMostSpecificType(it->second, it->first))  // only insert obj entries for its most specific type
00530             continue;
00531 
00532         if(lastType != "" && it->first != lastType) {  // type changed
00533             os << "- " << lastType << std::endl << "    ";
00534         }
00535         lastType = it->first;
00536         os << it->second << " ";
00537     }
00538     if(lastType != "")
00539         os << "- " << lastType;
00540     os << std::endl << "  )" << std::endl;
00541     os << "  (:init" << std::endl;
00542     forEach(const BooleanPredicateEntry & p, _booleanPredicates) {
00543         if(p.second)
00544             os << "    " << p.first << std::endl;
00545     }
00546     forEach(const NumericalFluentEntry & nf, _numericalFluents) {
00547         os << "    (= " << nf.first << " " << nf.second << ")" << std::endl;
00548     }
00549     forEach(const ObjectFluentEntry & of, _objectFluents) {
00550         if(of.second.empty()) {
00551             ROS_ERROR_STREAM(__func__ << ": ObjectFluentEntry for " << of.first << " is empty.");
00552         }
00553         os << "    (= " << of.first << " " << of.second << ")" << std::endl;
00554     }
00555     os << "  )" << std::endl;
00556 }
00557 
00558 void SymbolicState::toPDDLGoal(std::ostream & os) const
00559 {
00560     // prevent empty conjunction in goal
00561     if(_booleanPredicates.empty() && _numericalFluents.empty() && _forEachGoalStatements.empty()) {
00562         os << "  (:goal " << std::endl;
00563         os << "  )" << std::endl;
00564         return;
00565     }
00566 
00567     os << "  (:goal (and" << std::endl;
00568     forEach(const BooleanPredicateEntry & p, _booleanPredicates) {
00569         if(p.second)
00570             os << "    " << p.first << std::endl;
00571         else
00572             os << "    (not " << p.first << ")" << std::endl;
00573     }
00574     forEach(const NumericalFluentEntry & nf, _numericalFluents) {
00575         os << "    (= " << nf.first << " " << nf.second << ")" << std::endl;
00576     }
00577     forEach(const ObjectFluentEntry & of, _objectFluents) {
00578         if(of.second.empty()) {
00579             ROS_ERROR_STREAM(__func__ << ": ObjectFluentEntry for " << of.first << " is empty.");
00580         }
00581         os << "    (= " << of.first << " " << of.second << ")" << std::endl;
00582     }
00583 
00584     // from: multimap<objectType, pair<predicateName, value> >
00585     // produce: (forall (?o - objectType) (predicateName ?o))
00586     // or:      (forall (?o - objectType) (not (predicateName ?o)))
00587     forEach(const ForEachGoalStatements::value_type & p, _forEachGoalStatements) {
00588         os << "    (forall (?o - " << p.first << ") (";
00589         if(p.second.second)
00590             os << p.second.first << " ?o))" << std::endl;
00591         else
00592             os << "not (" << p.second.first << " ?o)))" << std::endl;
00593     }
00594     os << "  ))" << std::endl;
00595 }
00596 
00597 
00598 vector<string> SymbolicState::buildParameterList(string params) const
00599 {
00600     vector<string> ret;
00601 
00602     while(params.size() > 0) {
00603         // strip leading whitespace
00604         while(params.size() > 0 && params[0] == ' ') {
00605             params = params.substr(1);
00606         }
00607         // something left
00608         if(params.size() > 0) {
00609             size_t ind = params.find_first_of(" ");
00610             ret.push_back(params.substr(0, ind));     // insert next word
00611             if(ind == string::npos) {  // nothing left
00612                 params = "";
00613             } else {
00614                 params = params.substr(ind);              // and remove that from string
00615             }
00616         }
00617     }
00618 
00619     return ret;
00620 }
00621 
00622 unsigned int getShellWidth()
00623 {
00624     string cmd = "stty size";               // should output "height width"
00625     // read output from pipe into buf
00626     FILE* p = popen(cmd.c_str(), "r");
00627     if(p == NULL)
00628         return 80;
00629     char buf[1024];
00630     if(fgets(buf, 1024, p) == NULL)
00631         return 80;
00632     pclose(p);
00633 
00634     // parse second part of buf (width) into w
00635     string s(buf);
00636     size_t pos = s.find_first_of(" ");
00637     if(pos == string::npos)
00638         return 80;
00639     s = s.substr(pos + 1);
00640     long w = strtol(s.c_str(), NULL, 10);
00641     if(w <= 0)
00642         return 80;
00643     return w;
00644 }
00645 
00646 bool SymbolicState::OStreamMode::forceNewlines = false;
00647 
00648 std::ostream & operator<<(std::ostream & os, const SymbolicState & ss) {
00649     os << "Objects:" << std::endl;
00650     string lastType = "";
00651     for(multimap<string,string>::const_iterator it = ss._typedObjects.begin(); it != ss._typedObjects.end(); it++) {
00652         if(!ss.isMostSpecificType(it->second, it->first))  // only insert obj entries for its most specific type
00653             continue;
00654 
00655         if(lastType != "" && it->first != lastType) {  // type changed
00656             os << "- " << lastType << "   ";
00657             if(SymbolicState::OStreamMode::forceNewlines) os << std::endl;
00658         }
00659         lastType = it->first;
00660         os << it->second << " ";
00661     }
00662     if(lastType != "")
00663         os << "- " << lastType;
00664 
00665     os << std::endl;
00666     if(SymbolicState::OStreamMode::forceNewlines) os << std::endl;
00667     os << "True Predicates:" << std::endl;
00668     forEach(const SymbolicState::BooleanPredicateEntry & bp, ss._booleanPredicates) {
00669         if(bp.second) {
00670             os << bp.first << " ";
00671             if(SymbolicState::OStreamMode::forceNewlines) os << std::endl;
00672         }
00673     }
00674     os << std::endl;
00675     os << "False Predicates:" << std::endl;
00676     forEach(const SymbolicState::BooleanPredicateEntry & bp, ss._booleanPredicates) {
00677         if(!bp.second) {
00678             os << bp.first << " ";
00679             if(SymbolicState::OStreamMode::forceNewlines) os << std::endl;
00680         }
00681     }
00682     os << std::endl;
00683 
00684     unsigned int sw = getShellWidth();
00685     int maxEntriesPerLine = 0;
00686     if(SymbolicState::OStreamMode::forceNewlines)
00687         maxEntriesPerLine = 1;
00688     os << "Numerical Fluents:" << std::endl;
00689     int count = 0;
00690     std::stringstream ssLine;
00691     forEach(const SymbolicState::NumericalFluentEntry & nf, ss._numericalFluents) {
00692         std::stringstream ssBuf;
00693         ssBuf << nf.first << " = " << nf.second << " "; // this is what we want to add to the output line
00694 
00695         if(maxEntriesPerLine > 0) {
00696             os << ssBuf.str();
00697         } else {    // maxEntriesPerLine == 0, use shell width
00698             if(ssLine.str().length() + ssBuf.str().length() > sw) { // added cur output would be too long, so newline
00699                 os << ssLine.str() << std::endl;
00700                 ssLine.str("");         // next line starts empty
00701             }
00702             ssLine << ssBuf.str();
00703         }
00704 
00705         // print newline every maxEntriesPerLine outputs
00706         count++;
00707         if(maxEntriesPerLine > 0 && count >= maxEntriesPerLine) {
00708             count = 0;
00709             os << std::endl;
00710         }
00711     }
00712     if(!ssLine.str().empty())
00713         os << ssLine.str() << std::endl; // output last line
00714     os << std::endl;
00715     os << "Object Fluents:" << std::endl;
00716     count = 0;
00717     ssLine.str("");
00718     forEach(const SymbolicState::ObjectFluentEntry & nf, ss._objectFluents) {
00719         std::stringstream ssBuf;
00720         ssBuf << nf.first << " = " << nf.second << " ";
00721 
00722         if(maxEntriesPerLine > 0) {
00723             os << ssBuf.str();
00724         } else {
00725             if(ssLine.str().length() + ssBuf.str().length() > sw) { // added cur output would be too long, so newline
00726                 os << ssLine.str() << std::endl;
00727                 ssLine.str("");         // next line starts empty
00728             }
00729             ssLine << ssBuf.str();
00730         }
00731 
00732         // print newline every maxEntriesPerLine outputs
00733         count++;
00734         if(maxEntriesPerLine > 0 && count >= maxEntriesPerLine) {
00735             count = 0;
00736             os << std::endl;
00737         }
00738     }
00739     if(!ssLine.str().empty())
00740         os << ssLine.str() << std::endl; // output last line
00741     os << std::endl;
00742 
00743     if(!ss._forEachGoalStatements.empty()) {
00744         os << "ForEachGoalStatements:" << std::endl;
00745         // from: multimap<objectType, pair<predicateName, value> >
00746         // produce: (forall (?o - objectType) (predicateName ?o))
00747         // or:      (forall (?o - objectType) (not (predicateName ?o)))
00748         forEach(const SymbolicState::ForEachGoalStatements::value_type & p, ss._forEachGoalStatements) {
00749             os << "    (forall (?o - " << p.first << ") (";
00750             if(p.second.second)
00751                 os << p.second.first << " ?o))" << std::endl;
00752             else
00753                 os << "not (" << p.second.first << " ?o)))" << std::endl;
00754         }
00755     }
00756 
00757     return os;
00758 }
00759 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


continual_planning_executive
Author(s): Christian Dornhege
autogenerated on Tue Jan 22 2013 12:24:43