ompl/control/planners/ltl/src/World.cpp
00001 #include "ompl/control/planners/ltl/World.h" 00002 #include "ompl/util/Console.h" 00003 #include <boost/functional/hash.hpp> 00004 #include <boost/lexical_cast.hpp> 00005 #include <boost/unordered_map.hpp> 00006 #include <string> 00007 00008 ompl::control::World::World(unsigned int np) : numProps_(np) 00009 { 00010 } 00011 00012 bool ompl::control::World::operator[](unsigned int i) const 00013 { 00014 boost::unordered_map<unsigned int, bool>::const_iterator p = props_.find(i); 00015 if (p == props_.end()) 00016 OMPL_ERROR("Proposition %u is not set in world", i); 00017 return p->second; 00018 } 00019 00020 bool& ompl::control::World::operator[](unsigned int i) 00021 { 00022 return props_[i]; 00023 } 00024 00025 unsigned int ompl::control::World::numProps() const 00026 { 00027 return numProps_; 00028 } 00029 00030 bool ompl::control::World::satisfies(const World& w) const 00031 { 00032 boost::unordered_map<unsigned int, bool>::const_iterator p, q; 00033 for (p = w.props_.begin(); p != w.props_.end(); ++p) 00034 { 00035 q = props_.find(p->first); 00036 if (q == props_.end() || *q != *p) 00037 return false; 00038 } 00039 return true; 00040 } 00041 00042 std::string ompl::control::World::formula(void) const 00043 { 00044 if (props_.empty()) 00045 return "true"; 00046 boost::unordered_map<unsigned int, bool>::const_iterator p = props_.begin(); 00047 std::string f = std::string(p->second ? "p" : "!p") + boost::lexical_cast<std::string>(p->first); 00048 ++p; 00049 for (; p != props_.end(); ++p) 00050 f += std::string(p->second ? " & p" : " & !p") + boost::lexical_cast<std::string>(p->first); 00051 return f; 00052 } 00053 00054 const boost::unordered_map<unsigned int, bool>& ompl::control::World::props(void) const 00055 { 00056 return props_; 00057 } 00058 00059 bool ompl::control::World::operator==(const World& w) const 00060 { 00061 return numProps_ == w.numProps_ && props_ == w.props_; 00062 } 00063 00064 void ompl::control::World::clear(void) 00065 { 00066 props_.clear(); 00067 } 00068 00069 namespace ompl 00070 { 00071 namespace control 00072 { 00073 std::size_t hash_value(const ompl::control::World& w) 00074 { 00075 std::size_t hash = 0; 00076 boost::unordered_map<unsigned int, bool>::const_iterator p; 00077 for (p = w.props_.begin(); p != w.props_.end(); ++p) 00078 boost::hash_combine(hash, *p); 00079 return hash; 00080 } 00081 } 00082 }