<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd"> <html xmlns="http://www.w3.org/1999/xhtml"> <head> <meta http-equiv="Content-Type" content="text/xhtml; charset=UTF-8"/> <title>OMPL: src/ompl/geometric/planners/prm/PRM.h Source File</title> <meta name="author" content="Ioan A. Șucan, Mark Moll, Lydia E. Kavraki"> <link rel="stylesheet" href="../css/screen.css" type="text/css" media="screen, projection"> <link rel="stylesheet" href="../css/print.css" type="text/css" media="print"> <!--[if lt IE 7]> <script type="text/javascript" src="../js/jquery/jquery.js"></script> <script type="text/javascript" src="../js/jquery/jquery.dropdown.js"></script> <![endif]--> <script type="text/javaScript" src="search/search.js"></script> <script type="text/javascript"> var _gaq = _gaq || []; _gaq.push(['_setAccount', 'UA-9156598-2']); _gaq.push(['_trackPageview']); (function() { var ga = document.createElement('script'); ga.type = 'text/javascript'; ga.async = true; ga.src = ('https:' == document.location.protocol ? 'https://ssl' : 'http://www') + '.google-analytics.com/ga.js'; var s = document.getElementsByTagName('script')[0]; s.parentNode.insertBefore(ga, s); })(); </script> </head> <body onload='searchBox.OnSelectItem(0);'> <script type="text/javascript"><!-- var searchBox = new SearchBox("searchBox", "search",false,'Search API'); --></script> <div class="navigation" id="top"> <div class="tabs" id="ompltitle"> <ul class="tablist"> <li>The Open Motion Planning Library</li> <li id="searchli"> <div id="MSearchBox" class="MSearchBoxInactive"> <span class="left"> <img id="MSearchSelect" src="search/mag_sel.png" onmouseover="return searchBox.OnSearchSelectShow()" onmouseout="return searchBox.OnSearchSelectHide()" alt=""/> <input type="text" id="MSearchField" value="Search API" accesskey="S" onfocus="searchBox.OnSearchFieldFocus(true)" onblur="searchBox.OnSearchFieldFocus(false)" onkeyup="searchBox.OnSearchFieldChange(event)"/> </span><span class="right"> <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a> </span> </div> </li> </ul> </div> <ul id="nav" class="dropdown"> <li class="first"><a href="index.html">Home</a></li> <li><a href="download.html">Download</a></li> <li><a href="documentation.html">Documentation</a></li> <li><span class="dir">Code API</span> <ul> <li><a href="api_overview.html">API Overview</a></li> <li><a href="namespaces.html">Namespaces</a></li> <li><a href="annotated.html">Classes</a></li> <li><a href="files.html">Files</a></li> <li><a href="dirs.html">Directories</a></li> </ul> </li> <li><span class="dir">Community</span> <ul> <li><a href="developers.html">Developers</a></li> <li><a href="thirdparty.html">Contributions</a></li> <li><a href="education.html">Education</a></li> <li><a href="gallery.html">Gallery</a></li> </ul> </li> <li><span class="dir">About</span> <ul> <li><a href="license.html">License</a></li> <li><a href="citations.html">Citations</a></li> <li><a href="acknowledgements.html">Acknowledgments</a></li> <li><a href="contact.html">Contact Us</a></li> </ul> </li> </ul> </div> <!--- window showing the filter options --> <div id="MSearchSelectWindow" onmouseover="return searchBox.OnSearchSelectShow()" onmouseout="return searchBox.OnSearchSelectHide()" onkeydown="return searchBox.OnSearchSelectKey(event)"> <a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(0)"><span class="SelectionMark"> </span>All</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(1)"><span class="SelectionMark"> </span>Classes</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(2)"><span class="SelectionMark"> </span>Namespaces</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(3)"><span class="SelectionMark"> </span>Functions</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(4)"><span class="SelectionMark"> </span>Variables</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(5)"><span class="SelectionMark"> </span>Typedefs</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(6)"><span class="SelectionMark"> </span>Enumerations</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(7)"><span class="SelectionMark"> </span>Enumerator</a></div> <!-- iframe showing the search results (closed by default) --> <div id="MSearchResultsWindow"> <iframe src="" frameborder="0"name="MSearchResults" id="MSearchResults"></iframe> </div> <div class="container"> <div class="span-22 push-2 first last"> <div> <!-- Generated by Doxygen 1.7.4 --> <script type="text/javascript"><!-- var searchBox = new SearchBox("searchBox", "search",false,'Search'); --></script> <div id="nav-path" class="navpath"> <ul> <li class="navelem"><a class="el" href="dir_f5421e52a658cd938113ed6044324834.html">src</a> </li> <li class="navelem"><a class="el" href="dir_ae92c2ff78847f0cb49b545f9089bbbc.html">ompl</a> </li> <li class="navelem"><a class="el" href="dir_09c87af92e12dad9ad42621ff12ca21c.html">geometric</a> </li> <li class="navelem"><a class="el" href="dir_ea5fd944e8f903b1c975832ff4982cc4.html">planners</a> </li> <li class="navelem"><a class="el" href="dir_0bc9285a285c1ab505d89a54e821cc99.html">prm</a> </li> </ul> </div> </div> <div class="header"> <div class="headertitle"> <div class="title">PRM.h</div> </div> </div> <div class="contents"> <div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/*********************************************************************</span> <a name="l00002"></a>00002 <span class="comment">* Software License Agreement (BSD License)</span> <a name="l00003"></a>00003 <span class="comment">*</span> <a name="l00004"></a>00004 <span class="comment">* Copyright (c) 2011, Rice University</span> <a name="l00005"></a>00005 <span class="comment">* All rights reserved.</span> <a name="l00006"></a>00006 <span class="comment">*</span> <a name="l00007"></a>00007 <span class="comment">* Redistribution and use in source and binary forms, with or without</span> <a name="l00008"></a>00008 <span class="comment">* modification, are permitted provided that the following conditions</span> <a name="l00009"></a>00009 <span class="comment">* are met:</span> <a name="l00010"></a>00010 <span class="comment">*</span> <a name="l00011"></a>00011 <span class="comment">* * Redistributions of source code must retain the above copyright</span> <a name="l00012"></a>00012 <span class="comment">* notice, this list of conditions and the following disclaimer.</span> <a name="l00013"></a>00013 <span class="comment">* * Redistributions in binary form must reproduce the above</span> <a name="l00014"></a>00014 <span class="comment">* copyright notice, this list of conditions and the following</span> <a name="l00015"></a>00015 <span class="comment">* disclaimer in the documentation and/or other materials provided</span> <a name="l00016"></a>00016 <span class="comment">* with the distribution.</span> <a name="l00017"></a>00017 <span class="comment">* * Neither the name of the Rice University nor the names of its</span> <a name="l00018"></a>00018 <span class="comment">* contributors may be used to endorse or promote products derived</span> <a name="l00019"></a>00019 <span class="comment">* from this software without specific prior written permission.</span> <a name="l00020"></a>00020 <span class="comment">*</span> <a name="l00021"></a>00021 <span class="comment">* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS</span> <a name="l00022"></a>00022 <span class="comment">* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT</span> <a name="l00023"></a>00023 <span class="comment">* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS</span> <a name="l00024"></a>00024 <span class="comment">* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE</span> <a name="l00025"></a>00025 <span class="comment">* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,</span> <a name="l00026"></a>00026 <span class="comment">* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,</span> <a name="l00027"></a>00027 <span class="comment">* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;</span> <a name="l00028"></a>00028 <span class="comment">* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER</span> <a name="l00029"></a>00029 <span class="comment">* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT</span> <a name="l00030"></a>00030 <span class="comment">* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN</span> <a name="l00031"></a>00031 <span class="comment">* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE</span> <a name="l00032"></a>00032 <span class="comment">* POSSIBILITY OF SUCH DAMAGE.</span> <a name="l00033"></a>00033 <span class="comment">*********************************************************************/</span> <a name="l00034"></a>00034 <a name="l00035"></a>00035 <span class="comment">/* Author: Ioan Sucan, James D. Marble */</span> <a name="l00036"></a>00036 <a name="l00037"></a>00037 <span class="preprocessor">#ifndef OMPL_GEOMETRIC_PLANNERS_PRM_PRM_</span> <a name="l00038"></a>00038 <span class="preprocessor"></span><span class="preprocessor">#define OMPL_GEOMETRIC_PLANNERS_PRM_PRM_</span> <a name="l00039"></a>00039 <span class="preprocessor"></span> <a name="l00040"></a>00040 <span class="preprocessor">#include "ompl/geometric/planners/PlannerIncludes.h"</span> <a name="l00041"></a>00041 <span class="preprocessor">#include "ompl/datastructures/NearestNeighbors.h"</span> <a name="l00042"></a>00042 <span class="preprocessor">#include <boost/graph/graph_traits.hpp></span> <a name="l00043"></a>00043 <span class="preprocessor">#include <boost/graph/adjacency_list.hpp></span> <a name="l00044"></a>00044 <span class="preprocessor">#include <boost/pending/disjoint_sets.hpp></span> <a name="l00045"></a>00045 <span class="preprocessor">#include <boost/function.hpp></span> <a name="l00046"></a>00046 <span class="preprocessor">#include <utility></span> <a name="l00047"></a>00047 <span class="preprocessor">#include <vector></span> <a name="l00048"></a>00048 <span class="preprocessor">#include <map></span> <a name="l00049"></a>00049 <a name="l00050"></a>00050 <span class="keyword">namespace </span>ompl <a name="l00051"></a>00051 { <a name="l00052"></a>00052 <a name="l00053"></a>00053 <span class="keyword">namespace </span>geometric <a name="l00054"></a>00054 { <a name="l00055"></a>00055 <a name="l00079"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html">00079</a> <span class="keyword">class </span><a class="code" href="classompl_1_1geometric_1_1PRM.html" title="Probabilistic RoadMap planner.">PRM</a> : <span class="keyword">public</span> base::Planner <a name="l00080"></a>00080 { <a name="l00081"></a>00081 <span class="keyword">public</span>: <a name="l00082"></a>00082 <a name="l00083"></a><a class="code" href="structompl_1_1geometric_1_1PRM_1_1vertex__state__t.html">00083</a> <span class="keyword">struct </span><a class="code" href="structompl_1_1geometric_1_1PRM_1_1vertex__state__t.html">vertex_state_t</a> { <a name="l00084"></a>00084 <span class="keyword">typedef</span> boost::vertex_property_tag kind; <a name="l00085"></a>00085 }; <a name="l00086"></a>00086 <a name="l00087"></a><a class="code" href="structompl_1_1geometric_1_1PRM_1_1vertex__total__connection__attempts__t.html">00087</a> <span class="keyword">struct </span><a class="code" href="structompl_1_1geometric_1_1PRM_1_1vertex__total__connection__attempts__t.html">vertex_total_connection_attempts_t</a> { <a name="l00088"></a>00088 <span class="keyword">typedef</span> boost::vertex_property_tag kind; <a name="l00089"></a>00089 }; <a name="l00090"></a>00090 <a name="l00091"></a><a class="code" href="structompl_1_1geometric_1_1PRM_1_1vertex__successful__connection__attempts__t.html">00091</a> <span class="keyword">struct </span><a class="code" href="structompl_1_1geometric_1_1PRM_1_1vertex__successful__connection__attempts__t.html">vertex_successful_connection_attempts_t</a> { <a name="l00092"></a>00092 <span class="keyword">typedef</span> boost::vertex_property_tag kind; <a name="l00093"></a>00093 }; <a name="l00094"></a>00094 <a name="l00110"></a>00110 <span class="keyword">typedef</span> boost::adjacency_list < <a name="l00111"></a>00111 boost::vecS, boost::vecS, boost::undirectedS, <a name="l00112"></a>00112 boost::property < <a class="code" href="structompl_1_1geometric_1_1PRM_1_1vertex__state__t.html">vertex_state_t</a>, <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">base::State</a>*, <a name="l00113"></a>00113 boost::property < <a class="code" href="structompl_1_1geometric_1_1PRM_1_1vertex__total__connection__attempts__t.html">vertex_total_connection_attempts_t</a>, <span class="keywordtype">unsigned</span> int, <a name="l00114"></a>00114 boost::property < <a class="code" href="structompl_1_1geometric_1_1PRM_1_1vertex__successful__connection__attempts__t.html">vertex_successful_connection_attempts_t</a>, <span class="keywordtype">unsigned</span> int, <a name="l00115"></a>00115 boost::property < boost::vertex_predecessor_t, <span class="keywordtype">unsigned</span> <span class="keywordtype">long</span> int, <a name="l00116"></a>00116 boost::property < boost::vertex_rank_t, unsigned long int > > > > >, <a name="l00117"></a>00117 boost::property < boost::edge_weight_t, double, <a name="l00118"></a>00118 boost::property < boost::edge_index_t, unsigned int> > <a name="l00119"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#ad5e78674f3419bb018a7f94d80d128fb">00119</a> > <a class="code" href="classompl_1_1geometric_1_1PRM.html#ad5e78674f3419bb018a7f94d80d128fb" title="The underlying roadmap graph.">Graph</a>; <a name="l00120"></a>00120 <a name="l00121"></a>00121 <span class="keyword">typedef</span> boost::graph_traits<Graph>::vertex_descriptor Vertex; <a name="l00122"></a>00122 <span class="keyword">typedef</span> boost::graph_traits<Graph>::edge_descriptor Edge; <a name="l00123"></a>00123 <a name="l00124"></a>00124 <span class="keyword">typedef</span> boost::shared_ptr< NearestNeighbors<Vertex> > RoadmapNeighbors; <a name="l00125"></a>00125 <a name="l00132"></a>00132 <span class="keyword">typedef</span> boost::function1<std::vector<Vertex>&, <span class="keyword">const</span> Vertex> <a name="l00133"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#ad7ecc062d1cfcd1d4496846de2ec8a08">00133</a> <a class="code" href="classompl_1_1geometric_1_1PRM.html#ad7ecc062d1cfcd1d4496846de2ec8a08" title="A function returning the milestones that should be attempted to connect to.">ConnectionStrategy</a>; <a name="l00134"></a>00134 <a name="l00140"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#ac0b88385d600799a0ef7db709cf077b8">00140</a> <span class="keyword">typedef</span> boost::function2<bool, const Vertex&, const Vertex&> <a class="code" href="classompl_1_1geometric_1_1PRM.html#ac0b88385d600799a0ef7db709cf077b8" title="A function that can reject connections.">ConnectionFilter</a>; <a name="l00141"></a>00141 <a name="l00143"></a>00143 <a class="code" href="classompl_1_1geometric_1_1PRM.html#a5db629bb4a2bf02fb76a4cabbc5e0dcb" title="Constructor.">PRM</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1SpaceInformationPtr.html" title="A boost shared pointer wrapper for ompl::base::SpaceInformation.">base::SpaceInformationPtr</a> &si, <span class="keywordtype">bool</span> starStrategy = <span class="keyword">false</span>); <a name="l00144"></a>00144 <a name="l00145"></a>00145 <span class="keyword">virtual</span> ~<a class="code" href="classompl_1_1geometric_1_1PRM.html" title="Probabilistic RoadMap planner.">PRM</a>(<span class="keywordtype">void</span>) <a name="l00146"></a>00146 { <a name="l00147"></a>00147 <a class="code" href="classompl_1_1geometric_1_1PRM.html#a6134f963a94d1ecf831dbac17b04be42" title="Free all the memory allocated by the planner.">freeMemory</a>(); <a name="l00148"></a>00148 } <a name="l00149"></a>00149 <a name="l00150"></a>00150 <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#aefef4c0c54de26622152347fa0078961" title="Set the problem definition for the planner. The problem needs to be set before calling solve()...">setProblemDefinition</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1ProblemDefinitionPtr.html" title="A boost shared pointer wrapper for ompl::base::ProblemDefinition.">base::ProblemDefinitionPtr</a> &pdef); <a name="l00151"></a>00151 <a name="l00165"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#a8a7ce65640dd499ba32d80d3495d38bf">00165</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a8a7ce65640dd499ba32d80d3495d38bf" title="Set the connection strategy function that specifies the milestones that connection attempts will be m...">setConnectionStrategy</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#ad7ecc062d1cfcd1d4496846de2ec8a08" title="A function returning the milestones that should be attempted to connect to.">ConnectionStrategy</a>& connectionStrategy) <a name="l00166"></a>00166 { <a name="l00167"></a>00167 <a class="code" href="classompl_1_1geometric_1_1PRM.html#a834d99372562ad29dafb42c2c5758299" title="Function that returns the milestones to attempt connections with.">connectionStrategy_</a> = connectionStrategy; <a name="l00168"></a>00168 <a class="code" href="classompl_1_1geometric_1_1PRM.html#aad9c9af7b63886f0d866da2953320e2d" title="Flag indicating whether the employed connection strategy was set by the user (or defaults are assumed...">userSetConnectionStrategy_</a> = <span class="keyword">true</span>; <a name="l00169"></a>00169 } <a name="l00173"></a>00173 <span class="keywordtype">void</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#ab8832a466b3178de436acbb000712f2b" title="Convenience function that sets the connection strategy to the default one with k nearest neighbors...">setMaxNearestNeighbors</a>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> k); <a name="l00174"></a>00174 <a name="l00188"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#ad1e0bed0774005cf08bca672eec73abf">00188</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#ad1e0bed0774005cf08bca672eec73abf" title="Set the function that can reject a milestone connection.">setConnectionFilter</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#ac0b88385d600799a0ef7db709cf077b8" title="A function that can reject connections.">ConnectionFilter</a>& connectionFilter) <a name="l00189"></a>00189 { <a name="l00190"></a>00190 <a class="code" href="classompl_1_1geometric_1_1PRM.html#afdcd00e44119e8519bb39900f881332f" title="Function that can reject a milestone connection.">connectionFilter_</a> = connectionFilter; <a name="l00191"></a>00191 } <a name="l00192"></a>00192 <a name="l00193"></a>00193 <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a294b21109833dbbc80f5eb322e9c4fbc" title="Get information about the current run of the motion planner. Repeated calls to this function will upd...">getPlannerData</a>(<a class="code" href="classompl_1_1base_1_1PlannerData.html" title="Datatype holding data a planner can expose for debug purposes.">base::PlannerData</a> &data) <span class="keyword">const</span>; <a name="l00194"></a>00194 <a name="l00198"></a>00198 <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a9f15e6dafe26d97547526317b457c201" title="If the user desires, the roadmap can be improved for a specified amount of time. The solve() method w...">growRoadmap</a>(<span class="keywordtype">double</span> growTime); <a name="l00199"></a>00199 <a name="l00203"></a>00203 <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a718ea717e0928e0d4555683021508e46" title="Attempt to connect disjoint components in the roadmap using random bounding motions (the PRM expansio...">expandRoadmap</a>(<span class="keywordtype">double</span> expandTime); <a name="l00204"></a>00204 <a name="l00205"></a>00205 <span class="keyword">virtual</span> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#af622f66347e33f4bb6a2fe7f5df97b5f" title="Function that can solve the motion planning problem. This function can be called multiple times on th...">solve</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1PlannerTerminationCondition.html" title="Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...">base::PlannerTerminationCondition</a> &ptc); <a name="l00206"></a>00206 <a name="l00207"></a>00207 <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a54c8858fdff3da173a0650e66f962f94" title="Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...">clear</a>(<span class="keywordtype">void</span>); <a name="l00208"></a>00208 <a name="l00210"></a>00210 <span class="keyword">template</span><<span class="keyword">template</span><<span class="keyword">typename</span> T> <span class="keyword">class </span>NN> <a name="l00211"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#a3501a3f473485b8c4f577e7f6a7d3b9f">00211</a> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a3501a3f473485b8c4f577e7f6a7d3b9f" title="Set a different nearest neighbors datastructure.">setNearestNeighbors</a>(<span class="keywordtype">void</span>) <a name="l00212"></a>00212 { <a name="l00213"></a>00213 <a class="code" href="classompl_1_1geometric_1_1PRM.html#a3c4074fd06b96a0b12d39b11bf19f310" title="Nearest neighbors data structure.">nn_</a>.reset(<span class="keyword">new</span> NN<Vertex>()); <a name="l00214"></a>00214 <span class="keywordflow">if</span> (!<a class="code" href="classompl_1_1geometric_1_1PRM.html#aad9c9af7b63886f0d866da2953320e2d" title="Flag indicating whether the employed connection strategy was set by the user (or defaults are assumed...">userSetConnectionStrategy_</a>) <a name="l00215"></a>00215 <a class="code" href="classompl_1_1geometric_1_1PRM.html#a834d99372562ad29dafb42c2c5758299" title="Function that returns the milestones to attempt connections with.">connectionStrategy_</a>.clear(); <a name="l00216"></a>00216 <span class="keywordflow">if</span> (<a class="code" href="classompl_1_1base_1_1Planner.html#a2e8966fccd27780c580fd80b89c3221e" title="Check if setup() was called for this planner.">isSetup</a>()) <a name="l00217"></a>00217 <a class="code" href="classompl_1_1geometric_1_1PRM.html#a52ccbc48b63859db436ad03d95091fa9" title="Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...">setup</a>(); <a name="l00218"></a>00218 } <a name="l00219"></a>00219 <a name="l00220"></a>00220 <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a52ccbc48b63859db436ad03d95091fa9" title="Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...">setup</a>(<span class="keywordtype">void</span>); <a name="l00221"></a>00221 <a name="l00222"></a>00222 <span class="keyword">const</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#ad5e78674f3419bb018a7f94d80d128fb" title="The underlying roadmap graph.">Graph</a>& getRoadmap(<span class="keywordtype">void</span>)<span class="keyword"> const</span> <a name="l00223"></a>00223 <span class="keyword"> </span>{ <a name="l00224"></a>00224 <span class="keywordflow">return</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#aa25666bfa02ff8d549b967d411202ea0" title="Connectivity graph.">g_</a>; <a name="l00225"></a>00225 } <a name="l00226"></a>00226 <a name="l00228"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#a983bce5dd72ba208357b5518f4515125">00228</a> <span class="keywordtype">double</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a983bce5dd72ba208357b5518f4515125" title="Compute distance between two milestones (this is simply distance between the states of the milestones...">distanceFunction</a>(<span class="keyword">const</span> Vertex a, <span class="keyword">const</span> Vertex b)<span class="keyword"> const</span> <a name="l00229"></a>00229 <span class="keyword"> </span>{ <a name="l00230"></a>00230 <span class="keywordflow">return</span> <a class="code" href="classompl_1_1base_1_1Planner.html#aa3ceb9471163b6c96f6eeadbcfd3694e" title="The space information for which planning is done.">si_</a>->distance(<a class="code" href="classompl_1_1geometric_1_1PRM.html#ad88ed69d1ac8d376f91639eb5b0b638c" title="Access to the internal base::state at each Vertex.">stateProperty_</a>[a], <a class="code" href="classompl_1_1geometric_1_1PRM.html#ad88ed69d1ac8d376f91639eb5b0b638c" title="Access to the internal base::state at each Vertex.">stateProperty_</a>[b]); <a name="l00231"></a>00231 } <a name="l00232"></a>00232 <a name="l00234"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#add6627598d515cbc5c77ac7f669f2721">00234</a> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#add6627598d515cbc5c77ac7f669f2721" title="Compute distance between two milestones (this is simply distance between the states of the milestones...">milestoneCount</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span> <a name="l00235"></a>00235 <span class="keyword"> </span>{ <a name="l00236"></a>00236 <span class="keywordflow">return</span> boost::num_vertices(<a class="code" href="classompl_1_1geometric_1_1PRM.html#aa25666bfa02ff8d549b967d411202ea0" title="Connectivity graph.">g_</a>); <a name="l00237"></a>00237 } <a name="l00238"></a>00238 <a name="l00239"></a>00239 <span class="keyword">const</span> RoadmapNeighbors& getNearestNeighbors(<span class="keywordtype">void</span>) <a name="l00240"></a>00240 { <a name="l00241"></a>00241 <span class="keywordflow">return</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a3c4074fd06b96a0b12d39b11bf19f310" title="Nearest neighbors data structure.">nn_</a>; <a name="l00242"></a>00242 } <a name="l00243"></a>00243 <a name="l00244"></a>00244 <span class="keyword">protected</span>: <a name="l00245"></a>00245 <a name="l00247"></a>00247 <span class="keywordtype">void</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a6134f963a94d1ecf831dbac17b04be42" title="Free all the memory allocated by the planner.">freeMemory</a>(<span class="keywordtype">void</span>); <a name="l00248"></a>00248 <a name="l00250"></a>00250 Vertex <a class="code" href="classompl_1_1geometric_1_1PRM.html#aa393c5591243c7d166fc0f73b46627f2" title="Construct a milestone for a given state (state) and store it in the nearest neighbors data structure...">addMilestone</a>(<a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">base::State</a> *state); <a name="l00251"></a>00251 <a name="l00253"></a>00253 <span class="keywordtype">void</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a8b699f5d6c9ffd15094ef3854f1eb7b4" title="Make two milestones (m1 and m2) be part of the same connected component. The component with fewer ele...">uniteComponents</a>(Vertex m1, Vertex m2); <a name="l00254"></a>00254 <a name="l00256"></a>00256 <span class="keywordtype">void</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a9f15e6dafe26d97547526317b457c201" title="If the user desires, the roadmap can be improved for a specified amount of time. The solve() method w...">growRoadmap</a>(<span class="keyword">const</span> std::vector<Vertex> &start, <span class="keyword">const</span> std::vector<Vertex> &goal, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1PlannerTerminationCondition.html" title="Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...">base::PlannerTerminationCondition</a> &ptc, <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">base::State</a> *workState); <a name="l00257"></a>00257 <a name="l00261"></a>00261 <span class="keywordtype">void</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a718ea717e0928e0d4555683021508e46" title="Attempt to connect disjoint components in the roadmap using random bounding motions (the PRM expansio...">expandRoadmap</a>(<span class="keyword">const</span> std::vector<Vertex> &starts, <span class="keyword">const</span> std::vector<Vertex> &goals, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1PlannerTerminationCondition.html" title="Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...">base::PlannerTerminationCondition</a> &ptc, std::vector<base::State*> &workStates); <a name="l00262"></a>00262 <a name="l00264"></a>00264 <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#acdfe06f2ca9b6e13149a0ce0aa228e59" title="Check if there exists a solution, i.e., there exists a pair of milestones such that the first is in s...">haveSolution</a>(<span class="keyword">const</span> std::vector<Vertex> &start, <span class="keyword">const</span> std::vector<Vertex> &goal, std::pair<Vertex, Vertex> *endpoints = NULL); <a name="l00265"></a>00265 <a name="l00267"></a>00267 <a class="code" href="classompl_1_1base_1_1PathPtr.html" title="A boost shared pointer wrapper for ompl::base::Path.">base::PathPtr</a> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a3f13f8a7e5ba67332f4734b037870c33" title="Given two milestones from the same connected component, construct a path connecting them and set it a...">constructSolution</a>(<span class="keyword">const</span> Vertex start, <span class="keyword">const</span> Vertex goal); <a name="l00268"></a>00268 <a name="l00270"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#a03722f1e038d5e2e7dcbb97832f6458b">00270</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a03722f1e038d5e2e7dcbb97832f6458b" title="Flag indicating whether the default strategy is the Star trategy or not.">starStrategy_</a>; <a name="l00271"></a>00271 <a name="l00273"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#ad440cded7058f4b8fd703891d151922c">00273</a> <a class="code" href="classompl_1_1base_1_1ValidStateSamplerPtr.html" title="A boost shared pointer wrapper for ompl::base::ValidStateSampler.">base::ValidStateSamplerPtr</a> <a class="code" href="classompl_1_1geometric_1_1PRM.html#ad440cded7058f4b8fd703891d151922c" title="Sampler user for generating valid samples in the state space.">sampler_</a>; <a name="l00274"></a>00274 <a name="l00276"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#a2e22fbb5fdd9f79211631cfad8c6e52d">00276</a> <a class="code" href="classompl_1_1base_1_1StateSamplerPtr.html" title="A boost shared pointer wrapper for ompl::base::StateSampler.">base::StateSamplerPtr</a> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a2e22fbb5fdd9f79211631cfad8c6e52d" title="Sampler user for generating random in the state space.">simpleSampler_</a>; <a name="l00277"></a>00277 <a name="l00279"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#a3c4074fd06b96a0b12d39b11bf19f310">00279</a> RoadmapNeighbors <a class="code" href="classompl_1_1geometric_1_1PRM.html#a3c4074fd06b96a0b12d39b11bf19f310" title="Nearest neighbors data structure.">nn_</a>; <a name="l00280"></a>00280 <a name="l00282"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#aa25666bfa02ff8d549b967d411202ea0">00282</a> <a class="code" href="classompl_1_1geometric_1_1PRM.html#ad5e78674f3419bb018a7f94d80d128fb" title="The underlying roadmap graph.">Graph</a> <a class="code" href="classompl_1_1geometric_1_1PRM.html#aa25666bfa02ff8d549b967d411202ea0" title="Connectivity graph.">g_</a>; <a name="l00283"></a>00283 <a name="l00285"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#a084b230c442b1f7e0a22febdf2892ee8">00285</a> std::vector<Vertex> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a084b230c442b1f7e0a22febdf2892ee8" title="Array of start milestones.">startM_</a>; <a name="l00286"></a>00286 <a name="l00288"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#ac8d91585b24b60381244e928a3e8d0f2">00288</a> std::vector<Vertex> <a class="code" href="classompl_1_1geometric_1_1PRM.html#ac8d91585b24b60381244e928a3e8d0f2" title="Array of goal milestones.">goalM_</a>; <a name="l00289"></a>00289 <a name="l00291"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#aebf9bf1cc13173276301a3413eb92f89">00291</a> <a class="code" href="classompl_1_1base_1_1PathPtr.html" title="A boost shared pointer wrapper for ompl::base::Path.">base::PathPtr</a> <a class="code" href="classompl_1_1geometric_1_1PRM.html#aebf9bf1cc13173276301a3413eb92f89" title="Storage for approximate solutions.">approxsol_</a>; <a name="l00292"></a>00292 <a name="l00294"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#aaf99a585bd59f84e43aa64a41de2605c">00294</a> <span class="keywordtype">double</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#aaf99a585bd59f84e43aa64a41de2605c" title="The length of the approximate solution.">approxlen_</a>; <a name="l00295"></a>00295 <a name="l00297"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#ad88ed69d1ac8d376f91639eb5b0b638c">00297</a> boost::property_map<Graph, vertex_state_t>::type <a class="code" href="classompl_1_1geometric_1_1PRM.html#ad88ed69d1ac8d376f91639eb5b0b638c" title="Access to the internal base::state at each Vertex.">stateProperty_</a>; <a name="l00298"></a>00298 <a name="l00300"></a>00300 boost::property_map<<a class="code" href="classompl_1_1geometric_1_1PRM.html#ad5e78674f3419bb018a7f94d80d128fb" title="The underlying roadmap graph.">Graph</a>, <a name="l00301"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#ad279ee7f7070426d6835dd03a970f3a8">00301</a> <a class="code" href="structompl_1_1geometric_1_1PRM_1_1vertex__total__connection__attempts__t.html">vertex_total_connection_attempts_t</a>>::type <a class="code" href="classompl_1_1geometric_1_1PRM.html#ad279ee7f7070426d6835dd03a970f3a8" title="Access to the number of total connection attempts for a vertex.">totalConnectionAttemptsProperty_</a>; <a name="l00302"></a>00302 <a name="l00304"></a>00304 boost::property_map<<a class="code" href="classompl_1_1geometric_1_1PRM.html#ad5e78674f3419bb018a7f94d80d128fb" title="The underlying roadmap graph.">Graph</a>, <a name="l00305"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#aa2bbda9d19b0cb3c4b848bdebdfeecb1">00305</a> <a class="code" href="structompl_1_1geometric_1_1PRM_1_1vertex__successful__connection__attempts__t.html">vertex_successful_connection_attempts_t</a>>::type <a class="code" href="classompl_1_1geometric_1_1PRM.html#aa2bbda9d19b0cb3c4b848bdebdfeecb1" title="Access to the number of successful connection attempts for a vertex.">successfulConnectionAttemptsProperty_</a>; <a name="l00306"></a>00306 <a name="l00308"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#aef27444f5bb1cd1735a6653a00232f18">00308</a> boost::property_map<Graph, boost::edge_weight_t>::type <a class="code" href="classompl_1_1geometric_1_1PRM.html#aef27444f5bb1cd1735a6653a00232f18" title="Access to the weights of each Edge.">weightProperty_</a>; <a name="l00309"></a>00309 <a name="l00311"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#aa73e5b42a13b926bfe22826656a88550">00311</a> boost::property_map<Graph, boost::edge_index_t>::type <a class="code" href="classompl_1_1geometric_1_1PRM.html#aa73e5b42a13b926bfe22826656a88550" title="Access to the indices of each Edge.">edgeIDProperty_</a>; <a name="l00312"></a>00312 <a name="l00314"></a>00314 boost::disjoint_sets< <a name="l00315"></a>00315 boost::property_map<Graph, boost::vertex_rank_t>::type, <a name="l00316"></a>00316 boost::property_map<Graph, boost::vertex_predecessor_t>::type > <a name="l00317"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#a287f90dddb41fabd54c8840901dcf551">00317</a> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a287f90dddb41fabd54c8840901dcf551" title="Data structure that maintains the connected components.">disjointSets_</a>; <a name="l00318"></a>00318 <a name="l00320"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#ad2cd6c46e940a4b22051408c0dd8e05b">00320</a> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#ad2cd6c46e940a4b22051408c0dd8e05b" title="Maximum unique id number used so for for edges.">maxEdgeID_</a>; <a name="l00321"></a>00321 <a name="l00323"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#a834d99372562ad29dafb42c2c5758299">00323</a> <a class="code" href="classompl_1_1geometric_1_1PRM.html#ad7ecc062d1cfcd1d4496846de2ec8a08" title="A function returning the milestones that should be attempted to connect to.">ConnectionStrategy</a> <a class="code" href="classompl_1_1geometric_1_1PRM.html#a834d99372562ad29dafb42c2c5758299" title="Function that returns the milestones to attempt connections with.">connectionStrategy_</a>; <a name="l00324"></a>00324 <a name="l00326"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#afdcd00e44119e8519bb39900f881332f">00326</a> <a class="code" href="classompl_1_1geometric_1_1PRM.html#ac0b88385d600799a0ef7db709cf077b8" title="A function that can reject connections.">ConnectionFilter</a> <a class="code" href="classompl_1_1geometric_1_1PRM.html#afdcd00e44119e8519bb39900f881332f" title="Function that can reject a milestone connection.">connectionFilter_</a>; <a name="l00327"></a>00327 <a name="l00329"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#aad9c9af7b63886f0d866da2953320e2d">00329</a> <span class="keywordtype">bool</span> <a class="code" href="classompl_1_1geometric_1_1PRM.html#aad9c9af7b63886f0d866da2953320e2d" title="Flag indicating whether the employed connection strategy was set by the user (or defaults are assumed...">userSetConnectionStrategy_</a>; <a name="l00330"></a>00330 <a name="l00332"></a><a class="code" href="classompl_1_1geometric_1_1PRM.html#adca46f63edd3ad7ff1a13483242c1bf2">00332</a> <a class="code" href="classompl_1_1RNG.html" title="Random number generation. An instance of this class cannot be used by multiple threads at once (membe...">RNG</a> <a class="code" href="classompl_1_1geometric_1_1PRM.html#adca46f63edd3ad7ff1a13483242c1bf2" title="Random number generator.">rng_</a>; <a name="l00333"></a>00333 }; <a name="l00334"></a>00334 <a name="l00335"></a>00335 } <a name="l00336"></a>00336 } <a name="l00337"></a>00337 <a name="l00338"></a>00338 <span class="preprocessor">#endif</span> </pre></div></div> </div> <!-- window showing the filter options --> <div id="MSearchSelectWindow" onmouseover="return searchBox.OnSearchSelectShow()" onmouseout="return searchBox.OnSearchSelectHide()" onkeydown="return searchBox.OnSearchSelectKey(event)"> <a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(0)"><span class="SelectionMark"> </span>All</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(1)"><span class="SelectionMark"> </span>Classes</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(2)"><span class="SelectionMark"> </span>Namespaces</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(3)"><span class="SelectionMark"> </span>Functions</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(4)"><span class="SelectionMark"> </span>Variables</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(5)"><span class="SelectionMark"> </span>Typedefs</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(6)"><span class="SelectionMark"> </span>Enumerations</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(7)"><span class="SelectionMark"> </span>Enumerator</a></div> <!-- iframe showing the search results (closed by default) --> <div id="MSearchResultsWindow"> <iframe src="javascript:void(0)" frameborder="0" name="MSearchResults" id="MSearchResults"> </iframe> </div> </div> <div class="footer span-22 push-2 last"> <a href="http://www.kavrakilab.org">Physical and Biological Computing Group</a> • <a href="http://www.cs.rice.edu">Department of Computer Science</a> • <a href="http://www.rice.edu">Rice University</a><br> <div class="gray">Generated on Sun Oct 9 2011 23:04:40 by <a href="http://www.doxygen.org/index.html">doxygen</a> 1.7.4</div> </div> </div> </body> </html>