124 lines
24 KiB
HTML
124 lines
24 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/laserscan_to_pointcloud.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('laserscan__to__pointcloud_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">laserscan_to_pointcloud.cpp</div> </div>
|
|
</div><!--header-->
|
|
<div class="contents">
|
|
<div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno"> 1</span> <span class="comment">// ROS</span></div><div class="line"><a name="l00002"></a><span class="lineno"> 2</span> <span class="preprocessor">#include <ros/ros.h></span></div><div class="line"><a name="l00003"></a><span class="lineno"> 3</span> </div><div class="line"><a name="l00004"></a><span class="lineno"> 4</span> <span class="comment">// ROS LaserScan tools</span></div><div class="line"><a name="l00005"></a><span class="lineno"> 5</span> <span class="preprocessor">#include <laser_geometry/laser_geometry.h></span></div><div class="line"><a name="l00006"></a><span class="lineno"> 6</span> </div><div class="line"><a name="l00007"></a><span class="lineno"> 7</span> <span class="comment">// ROS messages</span></div><div class="line"><a name="l00008"></a><span class="lineno"> 8</span> <span class="preprocessor">#include <geometry_msgs/Pose.h></span></div><div class="line"><a name="l00009"></a><span class="lineno"> 9</span> <span class="preprocessor">#include <message_filters/subscriber.h></span></div><div class="line"><a name="l00010"></a><span class="lineno"> 10</span> <span class="preprocessor">#include <nav_msgs/Odometry.h></span></div><div class="line"><a name="l00011"></a><span class="lineno"> 11</span> <span class="preprocessor">#include <sensor_msgs/LaserScan.h></span></div><div class="line"><a name="l00012"></a><span class="lineno"> 12</span> <span class="preprocessor">#include <sensor_msgs/PointCloud.h></span></div><div class="line"><a name="l00013"></a><span class="lineno"> 13</span> <span class="preprocessor">#include <std_msgs/ColorRGBA.h></span></div><div class="line"><a name="l00014"></a><span class="lineno"> 14</span> <span class="preprocessor">#include <std_msgs/Bool.h></span></div><div class="line"><a name="l00015"></a><span class="lineno"> 15</span> <span class="preprocessor">#include <visualization_msgs/MarkerArray.h></span></div><div class="line"><a name="l00016"></a><span class="lineno"> 16</span> </div><div class="line"><a name="l00017"></a><span class="lineno"> 17</span> <span class="comment">// ROS services</span></div><div class="line"><a name="l00018"></a><span class="lineno"> 18</span> <span class="preprocessor">#include <std_srvs/Empty.h></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="comment">// ROS tf</span></div><div class="line"><a name="l00021"></a><span class="lineno"> 21</span> <span class="preprocessor">#include <tf/message_filter.h></span></div><div class="line"><a name="l00022"></a><span class="lineno"> 22</span> <span class="preprocessor">#include <tf/transform_listener.h></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> <span class="comment">// Octomap</span></div><div class="line"><a name="l00025"></a><span class="lineno"> 25</span> <span class="preprocessor">#include <octomap/octomap.h></span></div><div class="line"><a name="l00026"></a><span class="lineno"> 26</span> <span class="preprocessor">#include <octomap_msgs/conversions.h></span></div><div class="line"><a name="l00027"></a><span class="lineno"> 27</span> <span class="comment">//#include <octomap_msgs/OctomapBinary.h></span></div><div class="line"><a name="l00028"></a><span class="lineno"> 28</span> <span class="preprocessor">#include <octomap_msgs/GetOctomap.h></span></div><div class="line"><a name="l00029"></a><span class="lineno"> 29</span> <span class="keyword">typedef</span> octomap_msgs::GetOctomap OctomapSrv;</div><div class="line"><a name="l00030"></a><span class="lineno"> 30</span> <span class="comment">//#include <laser_octomap/BoundingBoxQuery.h></span></div><div class="line"><a name="l00031"></a><span class="lineno"> 31</span> <span class="comment">//typedef laser_octomap::BoundingBoxQuery BBXSrv;</span></div><div class="line"><a name="l00032"></a><span class="lineno"> 32</span> <span class="preprocessor">#include <octomap_ros/conversions.h></span></div><div class="line"><a name="l00033"></a><span class="lineno"> 33</span> </div><div class="line"><a name="l00034"></a><span class="lineno"> 34</span> <span class="preprocessor">#include <signal.h></span></div><div class="line"><a name="l00035"></a><span class="lineno"> 35</span> </div><div class="line"><a name="l00036"></a><span class="lineno"> 36</span> </div><div class="line"><a name="l00037"></a><span class="lineno"> 37</span> <span class="keywordtype">void</span> stopNode(<span class="keywordtype">int</span> sig)</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>  ros::shutdown();</div><div class="line"><a name="l00040"></a><span class="lineno"> 40</span>  exit(0);</div><div class="line"><a name="l00041"></a><span class="lineno"> 41</span> }</div><div class="line"><a name="l00042"></a><span class="lineno"> 42</span> </div><div class="line"><a name="l00044"></a><span class="lineno"><a class="line" href="class_laser_scan_to_point_cloud.html"> 44</a></span> <span class="keyword">class </span><a class="code" href="class_laser_scan_to_point_cloud.html">LaserScanToPointCloud</a></div><div class="line"><a name="l00045"></a><span class="lineno"> 45</span> {</div><div class="line"><a name="l00046"></a><span class="lineno"> 46</span>  <span class="keyword">public</span>:</div><div class="line"><a name="l00047"></a><span class="lineno"> 47</span>  <span class="comment">// Constructor and destructor</span></div><div class="line"><a name="l00048"></a><span class="lineno"> 48</span>  <a class="code" href="class_laser_scan_to_point_cloud.html#aa39e6795adce055a15dfe8f0a3aba993">LaserScanToPointCloud</a>();</div><div class="line"><a name="l00049"></a><span class="lineno"> 49</span> <span class="comment">// virtual ~LaserScanToPointCloud();</span></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>  <span class="comment">// Callbacks</span></div><div class="line"><a name="l00052"></a><span class="lineno"> 52</span>  <span class="keywordtype">void</span> laserScanCallback(<span class="keyword">const</span> sensor_msgs::LaserScanConstPtr& scan);</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>  <span class="keyword">private</span>:</div><div class="line"><a name="l00055"></a><span class="lineno"> 55</span> </div><div class="line"><a name="l00056"></a><span class="lineno"> 56</span>  <span class="comment">// ROS</span></div><div class="line"><a name="l00057"></a><span class="lineno"> 57</span>  ros::NodeHandle node_hand_;</div><div class="line"><a name="l00058"></a><span class="lineno"> 58</span>  ros::Subscriber laser_scan_sub_;</div><div class="line"><a name="l00059"></a><span class="lineno"> 59</span>  ros::Publisher pc_pub_;</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>  <span class="comment">//TF</span></div><div class="line"><a name="l00062"></a><span class="lineno"> 62</span>  tf::TransformListener listener_;</div><div class="line"><a name="l00063"></a><span class="lineno"> 63</span> </div><div class="line"><a name="l00064"></a><span class="lineno"> 64</span>  <span class="comment">//PC</span></div><div class="line"><a name="l00065"></a><span class="lineno"> 65</span>  laser_geometry::LaserProjection projector_;</div><div class="line"><a name="l00066"></a><span class="lineno"> 66</span> };</div><div class="line"><a name="l00067"></a><span class="lineno"> 67</span> </div><div class="line"><a name="l00069"></a><span class="lineno"><a class="line" href="class_laser_scan_to_point_cloud.html#aa39e6795adce055a15dfe8f0a3aba993"> 69</a></span> <a class="code" href="class_laser_scan_to_point_cloud.html#aa39e6795adce055a15dfe8f0a3aba993">LaserScanToPointCloud::LaserScanToPointCloud</a>(){</div><div class="line"><a name="l00070"></a><span class="lineno"> 70</span>  <span class="comment">//=======================================================================</span></div><div class="line"><a name="l00071"></a><span class="lineno"> 71</span>  <span class="comment">// Subscribers</span></div><div class="line"><a name="l00072"></a><span class="lineno"> 72</span>  <span class="comment">//=======================================================================</span></div><div class="line"><a name="l00073"></a><span class="lineno"> 73</span>  <span class="comment">//Mission Flag (feedback)</span></div><div class="line"><a name="l00074"></a><span class="lineno"> 74</span>  laser_scan_sub_ = node_hand_.subscribe(<span class="stringliteral">"/scan"</span>, 2, &LaserScanToPointCloud::laserScanCallback, <span class="keyword">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="comment">//=======================================================================</span></div><div class="line"><a name="l00077"></a><span class="lineno"> 77</span>  <span class="comment">// Publishers</span></div><div class="line"><a name="l00078"></a><span class="lineno"> 78</span>  <span class="comment">//=======================================================================</span></div><div class="line"><a name="l00079"></a><span class="lineno"> 79</span>  pc_pub_ = node_hand_.advertise<sensor_msgs::PointCloud2>(<span class="stringliteral">"pc_from_scan"</span>, 2, <span class="keyword">true</span>);</div><div class="line"><a name="l00080"></a><span class="lineno"> 80</span> <span class="comment">// // Waiting for mission flag</span></div><div class="line"><a name="l00081"></a><span class="lineno"> 81</span> <span class="comment">// ros::Rate loop_rate(10);</span></div><div class="line"><a name="l00082"></a><span class="lineno"> 82</span> <span class="comment">// if(!mission_flag_available_)</span></div><div class="line"><a name="l00083"></a><span class="lineno"> 83</span> <span class="comment">// ROS_WARN("Waiting for mission_flag");</span></div><div class="line"><a name="l00084"></a><span class="lineno"> 84</span> <span class="comment">// while (ros::ok() && !mission_flag_available_)</span></div><div class="line"><a name="l00085"></a><span class="lineno"> 85</span> <span class="comment">// {</span></div><div class="line"><a name="l00086"></a><span class="lineno"> 86</span> <span class="comment">// ros::spinOnce();</span></div><div class="line"><a name="l00087"></a><span class="lineno"> 87</span> <span class="comment">// loop_rate.sleep();</span></div><div class="line"><a name="l00088"></a><span class="lineno"> 88</span> <span class="comment">// }</span></div><div class="line"><a name="l00089"></a><span class="lineno"> 89</span> <span class="comment">// //Navigation data (feedback)</span></div><div class="line"><a name="l00090"></a><span class="lineno"> 90</span> <span class="comment">// odomSub_ = node_hand_.subscribe("/pose_ekf_slam/odometry", 1, &LaserScanToPointCloud::odomCallback, this);</span></div><div class="line"><a name="l00091"></a><span class="lineno"> 91</span> <span class="comment">// nav_sts_available_ = false;</span></div><div class="line"><a name="l00092"></a><span class="lineno"> 92</span> <span class="comment">// while (ros::ok() && !nav_sts_available_)</span></div><div class="line"><a name="l00093"></a><span class="lineno"> 93</span> <span class="comment">// {</span></div><div class="line"><a name="l00094"></a><span class="lineno"> 94</span> <span class="comment">// ros::spinOnce();</span></div><div class="line"><a name="l00095"></a><span class="lineno"> 95</span> <span class="comment">// loop_rate.sleep();</span></div><div class="line"><a name="l00096"></a><span class="lineno"> 96</span> <span class="comment">// }</span></div><div class="line"><a name="l00097"></a><span class="lineno"> 97</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> <span class="keywordtype">void</span> LaserScanToPointCloud::laserScanCallback(<span class="keyword">const</span> sensor_msgs::LaserScanConstPtr &scan){</div><div class="line"><a name="l00100"></a><span class="lineno"> 100</span>  <span class="keywordflow">if</span>(!listener_.waitForTransform(</div><div class="line"><a name="l00101"></a><span class="lineno"> 101</span>  scan->header.frame_id,</div><div class="line"><a name="l00102"></a><span class="lineno"> 102</span>  <span class="stringliteral">"/odom"</span>,</div><div class="line"><a name="l00103"></a><span class="lineno"> 103</span>  scan->header.stamp + ros::Duration().fromSec(scan->ranges.size()*scan->time_increment),</div><div class="line"><a name="l00104"></a><span class="lineno"> 104</span>  ros::Duration(1.0))){</div><div class="line"><a name="l00105"></a><span class="lineno"> 105</span>  <span class="keywordflow">return</span>;</div><div class="line"><a name="l00106"></a><span class="lineno"> 106</span>  }</div><div class="line"><a name="l00107"></a><span class="lineno"> 107</span> </div><div class="line"><a name="l00108"></a><span class="lineno"> 108</span>  sensor_msgs::PointCloud2 cloud;</div><div class="line"><a name="l00109"></a><span class="lineno"> 109</span>  projector_.transformLaserScanToPointCloud(<span class="stringliteral">"/base_link"</span>,*scan,</div><div class="line"><a name="l00110"></a><span class="lineno"> 110</span>  cloud,listener_);</div><div class="line"><a name="l00111"></a><span class="lineno"> 111</span> </div><div class="line"><a name="l00112"></a><span class="lineno"> 112</span>  <span class="comment">// Do something with cloud.</span></div><div class="line"><a name="l00113"></a><span class="lineno"> 113</span>  pc_pub_.publish(cloud);</div><div class="line"><a name="l00114"></a><span class="lineno"> 114</span> }</div><div class="line"><a name="l00115"></a><span class="lineno"> 115</span> </div><div class="line"><a name="l00117"></a><span class="lineno"> 117</span> <span class="keywordtype">int</span> main(<span class="keywordtype">int</span> argc, <span class="keywordtype">char</span>** argv){</div><div class="line"><a name="l00118"></a><span class="lineno"> 118</span> </div><div class="line"><a name="l00119"></a><span class="lineno"> 119</span>  <span class="comment">//=======================================================================</span></div><div class="line"><a name="l00120"></a><span class="lineno"> 120</span>  <span class="comment">// Override SIGINT handler</span></div><div class="line"><a name="l00121"></a><span class="lineno"> 121</span>  <span class="comment">//=======================================================================</span></div><div class="line"><a name="l00122"></a><span class="lineno"> 122</span>  signal(SIGINT, stopNode);</div><div class="line"><a name="l00123"></a><span class="lineno"> 123</span> </div><div class="line"><a name="l00124"></a><span class="lineno"> 124</span>  <span class="comment">// Init ROS node</span></div><div class="line"><a name="l00125"></a><span class="lineno"> 125</span>  ros::init(argc, argv, <span class="stringliteral">"laserscan_to_pointcloud"</span>);</div><div class="line"><a name="l00126"></a><span class="lineno"> 126</span>  ros::NodeHandle private_nh(<span class="stringliteral">"~"</span>);</div><div class="line"><a name="l00127"></a><span class="lineno"> 127</span> </div><div class="line"><a name="l00128"></a><span class="lineno"> 128</span>  <span class="comment">// Constructor</span></div><div class="line"><a name="l00129"></a><span class="lineno"> 129</span>  <a class="code" href="class_laser_scan_to_point_cloud.html">LaserScanToPointCloud</a> laserscan_to_pc;</div><div class="line"><a name="l00130"></a><span class="lineno"> 130</span> </div><div class="line"><a name="l00131"></a><span class="lineno"> 131</span>  <span class="comment">// Spin</span></div><div class="line"><a name="l00132"></a><span class="lineno"> 132</span>  ros::spin();</div><div class="line"><a name="l00133"></a><span class="lineno"> 133</span> </div><div class="line"><a name="l00134"></a><span class="lineno"> 134</span>  <span class="comment">// Exit main function without errors</span></div><div class="line"><a name="l00135"></a><span class="lineno"> 135</span>  <span class="keywordflow">return</span> 0;</div><div class="line"><a name="l00136"></a><span class="lineno"> 136</span> }</div><div class="ttc" id="class_laser_scan_to_point_cloud_html"><div class="ttname"><a href="class_laser_scan_to_point_cloud.html">LaserScanToPointCloud</a></div><div class="ttdoc">CLASS DEFINITION ===========================================================. </div><div class="ttdef"><b>Definition:</b> <a href="laserscan__to__pointcloud_8cpp_source.html#l00044">laserscan_to_pointcloud.cpp:44</a></div></div>
|
|
<div class="ttc" id="class_laser_scan_to_point_cloud_html_aa39e6795adce055a15dfe8f0a3aba993"><div class="ttname"><a href="class_laser_scan_to_point_cloud.html#aa39e6795adce055a15dfe8f0a3aba993">LaserScanToPointCloud::LaserScanToPointCloud</a></div><div class="ttdeci">LaserScanToPointCloud()</div><div class="ttdoc">Constructor and destructor =================================================. </div><div class="ttdef"><b>Definition:</b> <a href="laserscan__to__pointcloud_8cpp_source.html#l00069">laserscan_to_pointcloud.cpp:69</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"><b>laserscan_to_pointcloud.cpp</b></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>
|