ompl/control/planners/ltl/ProductGraph.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2012, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Matt Maly */ 00036 00037 #ifndef OMPL_CONTROL_PLANNERS_LTL_PRODUCTGRAPH_ 00038 #define OMPL_CONTROL_PLANNERS_LTL_PRODUCTGRAPH_ 00039 00040 #include "ompl/base/State.h" 00041 #include "ompl/control/planners/ltl/Automaton.h" 00042 #include "ompl/control/planners/ltl/PropositionalDecomposition.h" 00043 #include "ompl/util/ClassForward.h" 00044 #include <boost/function.hpp> 00045 #include <boost/graph/adjacency_list.hpp> 00046 #include <boost/unordered_map.hpp> 00047 #include <map> 00048 #include <ostream> 00049 #include <vector> 00050 00051 namespace ompl 00052 { 00053 namespace control 00054 { 00056 00057 OMPL_CLASS_FORWARD(ProductGraph); 00059 00067 class ProductGraph 00068 { 00069 public: 00074 class State 00075 { 00076 friend class ProductGraph; 00077 public: 00080 State(void) 00081 : decompRegion(-1), 00082 cosafeState(-1), 00083 safeState(-1) { } 00084 00086 State(const State& s) 00087 : decompRegion(s.decompRegion), 00088 cosafeState(s.cosafeState), 00089 safeState(s.safeState) { } 00090 00094 bool operator==(const State& s) const; 00095 00099 bool isValid(void) const; 00100 00102 00103 friend std::size_t hash_value(const ProductGraph::State& s); 00105 00107 friend std::ostream& operator<<(std::ostream& out, const State& s); 00108 00110 int getDecompRegion(void) const; 00111 00113 int getCosafeState(void) const; 00114 00116 int getSafeState(void) const; 00117 00118 private: 00119 int decompRegion; 00120 int cosafeState; 00121 int safeState; 00122 }; 00123 00126 ProductGraph( 00127 const PropositionalDecompositionPtr& decomp, 00128 const AutomatonPtr& cosafetyAut, 00129 const AutomatonPtr& safetyAut 00130 ); 00131 00135 ProductGraph( 00136 const PropositionalDecompositionPtr& decomp, 00137 const AutomatonPtr& cosafetyAut 00138 ); 00139 00140 ~ProductGraph(); 00141 00144 const PropositionalDecompositionPtr& getDecomp() const; 00145 00148 const AutomatonPtr& getCosafetyAutom() const; 00149 00152 const AutomatonPtr& getSafetyAutom() const; 00153 00162 std::vector<State*> computeLead(State* start, const boost::function<double(State*, State*)>& edgeWeight); 00163 00165 void clear(); 00166 00172 void buildGraph(State* start, const boost::function<void(State*)>& initialize = ProductGraph::noInit); 00173 00180 bool isSolution(const State* s) const; 00181 00183 State* getStartState(void) const; 00184 00187 double getRegionVolume(const State* s); 00188 00191 int getCosafeAutDistance(const State* s) const; 00192 00195 int getSafeAutDistance(const State* s) const; 00196 00200 State* getState(const base::State* cs) const; 00201 00205 State* getState(const base::State* cs, int cosafe, int safe) const; 00206 00210 State* getState(const State* parent, int nextRegion) const; 00211 00216 State* getState(const State* parent, const base::State* cs) const; 00217 00220 State* getState(int region, int cosafe, int safe) const 00221 { 00222 State s; 00223 s.decompRegion = region; 00224 s.cosafeState = cosafe; 00225 s.safeState = safe; 00226 State*& ret = stateToPtr_[s]; 00227 if (ret == NULL) ret = new State(s); 00228 return ret; 00229 } 00230 00231 protected: 00232 static void noInit(State* s) 00233 { 00234 } 00235 struct Edge 00236 { 00237 double cost; 00238 }; 00239 00240 typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, State*, Edge> GraphType; 00241 typedef boost::graph_traits<GraphType>::vertex_descriptor Vertex; 00242 typedef boost::graph_traits<GraphType>::vertex_iterator VertexIter; 00243 typedef boost::property_map<GraphType, boost::vertex_index_t>::type VertexIndexMap; 00244 typedef boost::graph_traits<GraphType>::edge_iterator EdgeIter; 00245 00246 PropositionalDecompositionPtr decomp_; 00247 AutomatonPtr cosafety_; 00248 AutomatonPtr safety_; 00249 GraphType graph_; 00250 State* startState_; 00251 std::vector<State*> solutionStates_; 00252 00253 /* Only one State pointer will be allocated for each possible State 00254 in the ProductGraph. There will exist situations in which 00255 all we have are the component values (region, automaton states) 00256 of a State and we want the actual State pointer. 00257 We use this map to access it. */ 00258 mutable boost::unordered_map<State, State*> stateToPtr_; 00259 00260 /* Map from State pointer to the index of the corresponding vertex 00261 in the graph. */ 00262 boost::unordered_map<State*, int> stateToIndex_; 00263 }; 00264 } 00265 } 00266 #endif