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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines