128 lines
21 KiB
HTML
128 lines
21 KiB
HTML
<!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"/>
|
|
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
|
|
<meta name="generator" content="Doxygen 1.8.11"/>
|
|
<title>OMPL Plannification: /home/blue/GitLab Repo/OMPL_Planning/Source Files/mapping_planning_tutorial/control_turtlebot/src/OMPL_Planning/OMPL_Planner/StateChecker.cpp Source File</title>
|
|
<link href="tabs.css" rel="stylesheet" type="text/css"/>
|
|
<script type="text/javascript" src="jquery.js"></script>
|
|
<script type="text/javascript" src="dynsections.js"></script>
|
|
<link href="navtree.css" rel="stylesheet" type="text/css"/>
|
|
<script type="text/javascript" src="resize.js"></script>
|
|
<script type="text/javascript" src="navtreedata.js"></script>
|
|
<script type="text/javascript" src="navtree.js"></script>
|
|
<script type="text/javascript">
|
|
$(document).ready(initResizable);
|
|
$(window).load(resizeHeight);
|
|
</script>
|
|
<link href="search/search.css" rel="stylesheet" type="text/css"/>
|
|
<script type="text/javascript" src="search/searchdata.js"></script>
|
|
<script type="text/javascript" src="search/search.js"></script>
|
|
<script type="text/javascript">
|
|
$(document).ready(function() { init_search(); });
|
|
</script>
|
|
<link href="doxygen.css" rel="stylesheet" type="text/css" />
|
|
</head>
|
|
<body>
|
|
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
|
|
<div id="titlearea">
|
|
<table cellspacing="0" cellpadding="0">
|
|
<tbody>
|
|
<tr style="height: 56px;">
|
|
<td id="projectlogo"><img alt="Logo" src="flylab.png"/></td>
|
|
<td id="projectalign" style="padding-left: 0.5em;">
|
|
<div id="projectname">OMPL Plannification
|
|
</div>
|
|
<div id="projectbrief">Path planning with OMPL</div>
|
|
</td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
</div>
|
|
<!-- end header part -->
|
|
<!-- Generated by Doxygen 1.8.11 -->
|
|
<script type="text/javascript">
|
|
var searchBox = new SearchBox("searchBox", "search",false,'Search');
|
|
</script>
|
|
<div id="navrow1" class="tabs">
|
|
<ul class="tablist">
|
|
<li><a href="index.html"><span>Main Page</span></a></li>
|
|
<li><a href="annotated.html"><span>Classes</span></a></li>
|
|
<li class="current"><a href="files.html"><span>Files</span></a></li>
|
|
<li>
|
|
<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" 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>
|
|
<div id="navrow2" class="tabs2">
|
|
<ul class="tablist">
|
|
<li><a href="files.html"><span>File List</span></a></li>
|
|
</ul>
|
|
</div>
|
|
</div><!-- top -->
|
|
<div id="side-nav" class="ui-resizable side-nav-resizable">
|
|
<div id="nav-tree">
|
|
<div id="nav-tree-contents">
|
|
<div id="nav-sync" class="sync"></div>
|
|
</div>
|
|
</div>
|
|
<div id="splitbar" style="-moz-user-select:none;"
|
|
class="ui-resizable-handle">
|
|
</div>
|
|
</div>
|
|
<script type="text/javascript">
|
|
$(document).ready(function(){initNavTree('_state_checker_8cpp_source.html','');});
|
|
</script>
|
|
<div id="doc-content">
|
|
<!-- window showing the filter options -->
|
|
<div id="MSearchSelectWindow"
|
|
onmouseover="return searchBox.OnSearchSelectShow()"
|
|
onmouseout="return searchBox.OnSearchSelectHide()"
|
|
onkeydown="return searchBox.OnSearchSelectKey(event)">
|
|
</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 class="header">
|
|
<div class="headertitle">
|
|
<div class="title">StateChecker.cpp</div> </div>
|
|
</div><!--header-->
|
|
<div class="contents">
|
|
<a href="_state_checker_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno"> 1</span> </div><div class="line"><a name="l00009"></a><span class="lineno"> 9</span> <span class="preprocessor">#include "StateChecker.hpp"</span></div><div class="line"><a name="l00010"></a><span class="lineno"> 10</span> </div><div class="line"><a name="l00011"></a><span class="lineno"><a class="line" href="class_state_checker.html#ab6848e593de676961143a42df192ab7c"> 11</a></span> <a class="code" href="class_state_checker.html#ab6848e593de676961143a42df192ab7c">StateChecker::StateChecker</a>(<span class="keyword">const</span> ob::SpaceInformationPtr &si): ob::StateValidityChecker(si)</div><div class="line"><a name="l00012"></a><span class="lineno"> 12</span> {</div><div class="line"><a name="l00013"></a><span class="lineno"> 13</span>  planningDimension = si->getStateDimension();</div><div class="line"><a name="l00014"></a><span class="lineno"> 14</span> }</div><div class="line"><a name="l00015"></a><span class="lineno"> 15</span> </div><div class="line"><a name="l00016"></a><span class="lineno"><a class="line" href="class_state_checker.html#abad123b62939fe4e3130cd9a3d0d051f"> 16</a></span> <a class="code" href="class_state_checker.html#abad123b62939fe4e3130cd9a3d0d051f">StateChecker::~StateChecker</a>()</div><div class="line"><a name="l00017"></a><span class="lineno"> 17</span> {</div><div class="line"><a name="l00018"></a><span class="lineno"> 18</span>  <span class="keywordflow">if</span>(_map !=<span class="keyword">nullptr</span>)</div><div class="line"><a name="l00019"></a><span class="lineno"> 19</span>  {</div><div class="line"><a name="l00020"></a><span class="lineno"> 20</span>  <span class="keyword">delete</span> _map;</div><div class="line"><a name="l00021"></a><span class="lineno"> 21</span>  _map = <span class="keyword">nullptr</span>;</div><div class="line"><a name="l00022"></a><span class="lineno"> 22</span>  }</div><div class="line"><a name="l00023"></a><span class="lineno"> 23</span>  </div><div class="line"><a name="l00024"></a><span class="lineno"> 24</span> }</div><div class="line"><a name="l00025"></a><span class="lineno"> 25</span> </div><div class="line"><a name="l00026"></a><span class="lineno"><a class="line" href="class_state_checker.html#aa31f7a73b601d4c16a81ea256ab482ea"> 26</a></span> <span class="keywordtype">void</span> <a class="code" href="class_state_checker.html#aa31f7a73b601d4c16a81ea256ab482ea">StateChecker::loadMap</a>(<span class="keyword">const</span> std::string mapFile) <span class="comment">//Take a look to memory mangaement for map</span></div><div class="line"><a name="l00027"></a><span class="lineno"> 27</span> {</div><div class="line"><a name="l00028"></a><span class="lineno"> 28</span>  <span class="comment">/*</span></div><div class="line"><a name="l00029"></a><span class="lineno"> 29</span> <span class="comment"> if(_map !=nullptr)</span></div><div class="line"><a name="l00030"></a><span class="lineno"> 30</span> <span class="comment"> {</span></div><div class="line"><a name="l00031"></a><span class="lineno"> 31</span> <span class="comment"> delete _map;</span></div><div class="line"><a name="l00032"></a><span class="lineno"> 32</span> <span class="comment"> _map = nullptr;</span></div><div class="line"><a name="l00033"></a><span class="lineno"> 33</span> <span class="comment"> }</span></div><div class="line"><a name="l00034"></a><span class="lineno"> 34</span> <span class="comment"> */</span></div><div class="line"><a name="l00035"></a><span class="lineno"> 35</span>  _map = (<span class="keyword">new</span> octomap::OcTree(mapFile));</div><div class="line"><a name="l00036"></a><span class="lineno"> 36</span>  _map_res = _map->getResolution();</div><div class="line"><a name="l00037"></a><span class="lineno"> 37</span> }</div><div class="line"><a name="l00038"></a><span class="lineno"> 38</span> </div><div class="line"><a name="l00039"></a><span class="lineno"> 39</span> <span class="keywordtype">void</span> <a class="code" href="class_state_checker.html#aa31f7a73b601d4c16a81ea256ab482ea">StateChecker::loadMap</a>(octomap::OcTree* octree)</div><div class="line"><a name="l00040"></a><span class="lineno"> 40</span> {</div><div class="line"><a name="l00041"></a><span class="lineno"> 41</span>  <span class="comment">/*</span></div><div class="line"><a name="l00042"></a><span class="lineno"> 42</span> <span class="comment"> if(_map !=nullptr)</span></div><div class="line"><a name="l00043"></a><span class="lineno"> 43</span> <span class="comment"> {</span></div><div class="line"><a name="l00044"></a><span class="lineno"> 44</span> <span class="comment"> delete _map;</span></div><div class="line"><a name="l00045"></a><span class="lineno"> 45</span> <span class="comment"> _map = nullptr;</span></div><div class="line"><a name="l00046"></a><span class="lineno"> 46</span> <span class="comment"> }</span></div><div class="line"><a name="l00047"></a><span class="lineno"> 47</span> <span class="comment"> */</span></div><div class="line"><a name="l00048"></a><span class="lineno"> 48</span>  _map = octree;</div><div class="line"><a name="l00049"></a><span class="lineno"> 49</span>  _map_res = _map->getResolution();</div><div class="line"><a name="l00050"></a><span class="lineno"> 50</span> }</div><div class="line"><a name="l00051"></a><span class="lineno"> 51</span> </div><div class="line"><a name="l00052"></a><span class="lineno"><a class="line" href="class_state_checker.html#a7fc1bee2971992901eed0517a8fffaa8"> 52</a></span> <span class="keywordtype">void</span> <a class="code" href="class_state_checker.html#a7fc1bee2971992901eed0517a8fffaa8">StateChecker::setCollisionMask</a>(<span class="keywordtype">double</span> x, <span class="keywordtype">double</span> y, <span class="keywordtype">double</span> z)</div><div class="line"><a name="l00053"></a><span class="lineno"> 53</span> {</div><div class="line"><a name="l00054"></a><span class="lineno"> 54</span>  _check_box.x=x;</div><div class="line"><a name="l00055"></a><span class="lineno"> 55</span>  _check_box.y=y;</div><div class="line"><a name="l00056"></a><span class="lineno"> 56</span>  _check_box.z=z;</div><div class="line"><a name="l00057"></a><span class="lineno"> 57</span> }</div><div class="line"><a name="l00058"></a><span class="lineno"> 58</span> </div><div class="line"><a name="l00059"></a><span class="lineno"><a class="line" href="class_state_checker.html#af9db3b2571cf0294c07e81dd2b07eca9"> 59</a></span> <span class="keywordtype">void</span> <a class="code" href="class_state_checker.html#af9db3b2571cf0294c07e81dd2b07eca9">StateChecker::setDepthLimit</a>(<span class="keywordtype">double</span> depth_limit)</div><div class="line"><a name="l00060"></a><span class="lineno"> 60</span> {</div><div class="line"><a name="l00061"></a><span class="lineno"> 61</span>  _depth_limit = depth_limit;</div><div class="line"><a name="l00062"></a><span class="lineno"> 62</span> }</div><div class="line"><a name="l00063"></a><span class="lineno"> 63</span> </div><div class="line"><a name="l00064"></a><span class="lineno"><a class="line" href="class_state_checker.html#aa7454eae37b6fea9743cd26ddee94238"> 64</a></span> <span class="keywordtype">bool</span> <a class="code" href="class_state_checker.html#aa7454eae37b6fea9743cd26ddee94238">StateChecker::isValid</a>(<span class="keyword">const</span> ob::State *state)<span class="keyword"> const</span></div><div class="line"><a name="l00065"></a><span class="lineno"> 65</span> <span class="keyword"></span>{</div><div class="line"><a name="l00066"></a><span class="lineno"> 66</span>  OcTreeNode* result;</div><div class="line"><a name="l00067"></a><span class="lineno"> 67</span>  point3d query; <span class="comment">//Octomap</span></div><div class="line"><a name="l00068"></a><span class="lineno"> 68</span>  <span class="keywordtype">bool</span> collision(<span class="keyword">false</span>);</div><div class="line"><a name="l00069"></a><span class="lineno"> 69</span>  <span class="keywordtype">double</span> node_occupancy;</div><div class="line"><a name="l00070"></a><span class="lineno"> 70</span> </div><div class="line"><a name="l00071"></a><span class="lineno"> 71</span>  <span class="comment">// extract the component of the state and cast it to what we expect</span></div><div class="line"><a name="l00072"></a><span class="lineno"> 72</span>  <span class="keyword">const</span> ob::RealVectorStateSpace::StateType *pos = state->as<ob::RealVectorStateSpace::StateType>();</div><div class="line"><a name="l00073"></a><span class="lineno"> 73</span> </div><div class="line"><a name="l00074"></a><span class="lineno"> 74</span>  <span class="comment">//Constant depth for 2D planning (There might be a better way to implement this)</span></div><div class="line"><a name="l00075"></a><span class="lineno"> 75</span>  </div><div class="line"><a name="l00076"></a><span class="lineno"> 76</span>  <span class="keywordflow">if</span>(planningDimension==2)</div><div class="line"><a name="l00077"></a><span class="lineno"> 77</span>  {</div><div class="line"><a name="l00078"></a><span class="lineno"> 78</span>  pos->values[2]=_depth_limit;</div><div class="line"><a name="l00079"></a><span class="lineno"> 79</span>  }</div><div class="line"><a name="l00080"></a><span class="lineno"> 80</span>  </div><div class="line"><a name="l00081"></a><span class="lineno"> 81</span>  <span class="comment">//Recherche dans le collision mask autour de pos</span></div><div class="line"><a name="l00082"></a><span class="lineno"> 82</span>  <span class="keywordflow">for</span>(<span class="keywordtype">double</span> xi = pos->values[0]-(_check_box.x/2.0);xi <= pos->values[0]+(_check_box.x/2.0);xi=xi+_map_res)</div><div class="line"><a name="l00083"></a><span class="lineno"> 83</span>  <span class="keywordflow">for</span>(<span class="keywordtype">double</span> yi = pos->values[1]-(_check_box.y/2.0);yi <= pos->values[1]+(_check_box.y/2.0);yi=yi+_map_res)</div><div class="line"><a name="l00084"></a><span class="lineno"> 84</span>  <span class="keywordflow">for</span>(<span class="keywordtype">double</span> zi = pos->values[2]-(_check_box.z/2.0);zi <= pos->values[2]+(_check_box.z/2.0);zi=zi+_map_res){ <span class="comment">//Trop sensible sur z</span></div><div class="line"><a name="l00085"></a><span class="lineno"> 85</span>  query.x() = xi;</div><div class="line"><a name="l00086"></a><span class="lineno"> 86</span>  query.y() = yi;</div><div class="line"><a name="l00087"></a><span class="lineno"> 87</span>  query.z() = zi;</div><div class="line"><a name="l00088"></a><span class="lineno"> 88</span>  result = _map->search (query); <span class="comment">//Retourne le pointeur de la node à la position query, NULL si elle n'existe pas (terrain inconnu)</span></div><div class="line"><a name="l00089"></a><span class="lineno"> 89</span> </div><div class="line"><a name="l00090"></a><span class="lineno"> 90</span>  <span class="keywordflow">if</span>(result != NULL){ <span class="comment">// Node connue</span></div><div class="line"><a name="l00091"></a><span class="lineno"> 91</span> </div><div class="line"><a name="l00092"></a><span class="lineno"> 92</span>  node_occupancy = result->getOccupancy();</div><div class="line"><a name="l00093"></a><span class="lineno"> 93</span>  </div><div class="line"><a name="l00094"></a><span class="lineno"> 94</span>  <span class="keywordflow">if</span> (node_occupancy > 0.4)</div><div class="line"><a name="l00095"></a><span class="lineno"> 95</span>  {</div><div class="line"><a name="l00096"></a><span class="lineno"> 96</span>  collision = <span class="keyword">true</span>;</div><div class="line"><a name="l00097"></a><span class="lineno"> 97</span>  <span class="keywordflow">break</span>;</div><div class="line"><a name="l00098"></a><span class="lineno"> 98</span>  }</div><div class="line"><a name="l00099"></a><span class="lineno"> 99</span>  }</div><div class="line"><a name="l00100"></a><span class="lineno"> 100</span>  }</div><div class="line"><a name="l00101"></a><span class="lineno"> 101</span> </div><div class="line"><a name="l00102"></a><span class="lineno"> 102</span>  <span class="keywordflow">return</span> !collision;</div><div class="line"><a name="l00103"></a><span class="lineno"> 103</span> }</div><div class="ttc" id="class_state_checker_html_ab6848e593de676961143a42df192ab7c"><div class="ttname"><a href="class_state_checker.html#ab6848e593de676961143a42df192ab7c">StateChecker::StateChecker</a></div><div class="ttdeci">StateChecker(const ob::SpaceInformationPtr &si)</div><div class="ttdoc">Constructor. </div><div class="ttdef"><b>Definition:</b> <a href="_state_checker_8cpp_source.html#l00011">StateChecker.cpp:11</a></div></div>
|
|
<div class="ttc" id="class_state_checker_html_af9db3b2571cf0294c07e81dd2b07eca9"><div class="ttname"><a href="class_state_checker.html#af9db3b2571cf0294c07e81dd2b07eca9">StateChecker::setDepthLimit</a></div><div class="ttdeci">void setDepthLimit(double depth_limit)</div><div class="ttdoc">Set the depth limit (constant for 2D checking) </div><div class="ttdef"><b>Definition:</b> <a href="_state_checker_8cpp_source.html#l00059">StateChecker.cpp:59</a></div></div>
|
|
<div class="ttc" id="class_state_checker_html_a7fc1bee2971992901eed0517a8fffaa8"><div class="ttname"><a href="class_state_checker.html#a7fc1bee2971992901eed0517a8fffaa8">StateChecker::setCollisionMask</a></div><div class="ttdeci">void setCollisionMask(double x, double y, double z=0.2)</div><div class="ttdoc">Set the area around every state considered for state checking. </div><div class="ttdef"><b>Definition:</b> <a href="_state_checker_8cpp_source.html#l00052">StateChecker.cpp:52</a></div></div>
|
|
<div class="ttc" id="class_state_checker_html_aa7454eae37b6fea9743cd26ddee94238"><div class="ttname"><a href="class_state_checker.html#aa7454eae37b6fea9743cd26ddee94238">StateChecker::isValid</a></div><div class="ttdeci">virtual bool isValid(const ob::State *state) const </div><div class="ttdoc">State validator. </div><div class="ttdef"><b>Definition:</b> <a href="_state_checker_8cpp_source.html#l00064">StateChecker.cpp:64</a></div></div>
|
|
<div class="ttc" id="class_state_checker_html_aa31f7a73b601d4c16a81ea256ab482ea"><div class="ttname"><a href="class_state_checker.html#aa31f7a73b601d4c16a81ea256ab482ea">StateChecker::loadMap</a></div><div class="ttdeci">void loadMap(const std::string mapFile)</div><div class="ttdoc">Load the octomap where the states are checked. </div><div class="ttdef"><b>Definition:</b> <a href="_state_checker_8cpp_source.html#l00026">StateChecker.cpp:26</a></div></div>
|
|
<div class="ttc" id="class_state_checker_html_abad123b62939fe4e3130cd9a3d0d051f"><div class="ttname"><a href="class_state_checker.html#abad123b62939fe4e3130cd9a3d0d051f">StateChecker::~StateChecker</a></div><div class="ttdeci">~StateChecker()</div><div class="ttdoc">Destructor. </div><div class="ttdef"><b>Definition:</b> <a href="_state_checker_8cpp_source.html#l00016">StateChecker.cpp:16</a></div></div>
|
|
</div><!-- fragment --></div><!-- contents -->
|
|
</div><!-- doc-content -->
|
|
<!-- start footer part -->
|
|
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
|
|
<ul>
|
|
<li class="navelem"><a class="el" href="dir_a2ed51f7c7095589dc0c78a28547d1d4.html">Source Files</a></li><li class="navelem"><a class="el" href="dir_50758af9fa44b143c39a433e879132a5.html">mapping_planning_tutorial</a></li><li class="navelem"><a class="el" href="dir_2b04689da8f92abcb133442f492c8f6b.html">control_turtlebot</a></li><li class="navelem"><a class="el" href="dir_5fee08efe95e6d722a092087df25e95a.html">src</a></li><li class="navelem"><a class="el" href="dir_0789743f3a1bac7b4929be0fd81c897b.html">OMPL_Planning</a></li><li class="navelem"><a class="el" href="dir_719ee20e18abf3db039d63d757b6a50c.html">OMPL_Planner</a></li><li class="navelem"><a class="el" href="_state_checker_8cpp.html">StateChecker.cpp</a></li>
|
|
<li class="footer">Generated by
|
|
<a href="http://www.doxygen.org/index.html">
|
|
<img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.8.11 </li>
|
|
</ul>
|
|
</div>
|
|
</body>
|
|
</html>
|