1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
|
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<title>
Attachment – ros-pkg
</title>
<link rel="search" href="/trac/ros-pkg/search" />
<link rel="help" href="/trac/ros-pkg/wiki/TracGuide" />
<link rel="alternate" href="/trac/ros-pkg/raw-attachment/ticket/4440/point_cloud.py" type="text/x-python; charset=iso-8859-15" title="Original Format" />
<link rel="up" href="/trac/ros-pkg/ticket/4440" title="Ticket #4440" />
<link rel="start" href="/trac/ros-pkg/wiki" />
<link rel="stylesheet" href="/trac/ros-pkg/chrome/common/css/trac.css" type="text/css" /><link rel="stylesheet" href="/trac/ros-pkg/pygments/trac.css" type="text/css" /><link rel="stylesheet" href="/trac/ros-pkg/chrome/common/css/code.css" type="text/css" />
<link rel="shortcut icon" href="/trac/ros-pkg/chrome/common/trac.ico" type="image/x-icon" />
<link rel="icon" href="/trac/ros-pkg/chrome/common/trac.ico" type="image/x-icon" />
<link type="application/opensearchdescription+xml" rel="search" href="/trac/ros-pkg/search/opensearch" title="Search ros-pkg" />
<script type="text/javascript" src="/trac/ros-pkg/chrome/common/js/jquery.js"></script><script type="text/javascript" src="/trac/ros-pkg/chrome/common/js/trac.js"></script><script type="text/javascript" src="/trac/ros-pkg/chrome/common/js/search.js"></script>
<!--[if lt IE 7]>
<script type="text/javascript" src="/trac/ros-pkg/chrome/common/js/ie_pre7_hacks.js"></script>
<![endif]-->
</head>
<body>
<div id="banner">
<div id="header">
<a id="logo" href="http://code.ros.org/trac/ros-pkg/"><img src="/trac/ros-pkg/chrome/common/trac_banner.png" alt="" /></a>
</div>
<form id="search" action="/trac/ros-pkg/search" method="get">
<div>
<label for="proj-search">Search:</label>
<input type="text" id="proj-search" name="q" size="18" value="" />
<input type="submit" value="Search" />
</div>
</form>
<div id="metanav" class="nav">
<ul>
<li class="first"><a href="/trac/ros-pkg/login">Login</a></li><li><a href="/trac/ros-pkg/wiki/TracGuide">Help/Guide</a></li><li><a href="/trac/ros-pkg/about">About Trac</a></li><li class="last"><a href="/trac/ros-pkg/prefs">Preferences</a></li>
</ul>
</div>
</div>
<div id="mainnav" class="nav">
<ul>
<li class="first"><a href="/trac/ros-pkg/wiki">Wiki</a></li><li><a href="/trac/ros-pkg/timeline">Timeline</a></li><li><a href="/trac/ros-pkg/roadmap">Roadmap</a></li><li><a href="/trac/ros-pkg/browser">Browse Source</a></li><li><a href="/trac/ros-pkg/report">View Tickets</a></li><li class="last"><a href="/trac/ros-pkg/search">Search</a></li>
</ul>
</div>
<div id="main">
<div id="ctxtnav" class="nav">
<h2>Context Navigation</h2>
<ul>
<li class="last first"><a href="/trac/ros-pkg/ticket/4440">Back to Ticket #4440</a></li>
</ul>
<hr />
</div>
<div id="content" class="attachment">
<h1><a href="/trac/ros-pkg/ticket/4440">Ticket #4440</a>: point_cloud.py</h1>
<table id="info" summary="Description">
<tbody>
<tr>
<th scope="col">
File point_cloud.py, <span title="5328 bytes">5.2 KB</span>
(added by tfield, <a class="timeline" href="/trac/ros-pkg/timeline?from=2011-07-28T13%3A37%3A39-0700&precision=second" title="2011-07-28T13:37:39-0700 in Timeline">10 months</a> ago)
</th>
</tr>
<tr>
<td class="message searchable">
<p>
tims implementation w/o access restrictions
</p>
</td>
</tr>
</tbody>
</table>
<div id="preview" class="searchable">
<table class="code"><thead><tr><th class="lineno" title="Line numbers">Line</th><th class="content"> </th></tr></thead><tbody><tr><th id="L1"><a href="#L1">1</a></th><td><span class="c">#!/usr/bin/env python</span></td></tr><tr><th id="L2"><a href="#L2">2</a></th><td></td></tr><tr><th id="L3"><a href="#L3">3</a></th><td><span class="sd">"""</span></td></tr><tr><th id="L4"><a href="#L4">4</a></th><td><span class="sd">Serialization of sensor_msgs.PointCloud2 messages.</span></td></tr><tr><th id="L5"><a href="#L5">5</a></th><td><span class="sd"></span></td></tr><tr><th id="L6"><a href="#L6">6</a></th><td><span class="sd">Author: Tim Field</span></td></tr><tr><th id="L7"><a href="#L7">7</a></th><td><span class="sd">"""</span></td></tr><tr><th id="L8"><a href="#L8">8</a></th><td></td></tr><tr><th id="L9"><a href="#L9">9</a></th><td><span class="kn">import</span> <span class="nn">roslib</span><span class="p">;</span> roslib<span class="o">.</span>load_manifest<span class="p">(</span><span class="s">'sensor_msgs'</span><span class="p">)</span></td></tr><tr><th id="L10"><a href="#L10">10</a></th><td></td></tr><tr><th id="L11"><a href="#L11">11</a></th><td><span class="kn">import</span> <span class="nn">ctypes</span></td></tr><tr><th id="L12"><a href="#L12">12</a></th><td><span class="kn">import</span> <span class="nn">math</span></td></tr><tr><th id="L13"><a href="#L13">13</a></th><td><span class="kn">import</span> <span class="nn">struct</span></td></tr><tr><th id="L14"><a href="#L14">14</a></th><td></td></tr><tr><th id="L15"><a href="#L15">15</a></th><td><span class="kn">from</span> <span class="nn">sensor_msgs.msg</span> <span class="kn">import</span> PointCloud2<span class="p">,</span> PointField</td></tr><tr><th id="L16"><a href="#L16">16</a></th><td></td></tr><tr><th id="L17"><a href="#L17">17</a></th><td>_DATATYPES <span class="o">=</span> <span class="p">{}</span></td></tr><tr><th id="L18"><a href="#L18">18</a></th><td>_DATATYPES<span class="p">[</span>PointField<span class="o">.</span>INT8<span class="p">]</span> <span class="o">=</span> <span class="p">(</span><span class="s">'b'</span><span class="p">,</span> <span class="mi">1</span><span class="p">)</span></td></tr><tr><th id="L19"><a href="#L19">19</a></th><td>_DATATYPES<span class="p">[</span>PointField<span class="o">.</span>UINT8<span class="p">]</span> <span class="o">=</span> <span class="p">(</span><span class="s">'B'</span><span class="p">,</span> <span class="mi">1</span><span class="p">)</span></td></tr><tr><th id="L20"><a href="#L20">20</a></th><td>_DATATYPES<span class="p">[</span>PointField<span class="o">.</span>INT16<span class="p">]</span> <span class="o">=</span> <span class="p">(</span><span class="s">'h'</span><span class="p">,</span> <span class="mi">2</span><span class="p">)</span></td></tr><tr><th id="L21"><a href="#L21">21</a></th><td>_DATATYPES<span class="p">[</span>PointField<span class="o">.</span>UINT16<span class="p">]</span> <span class="o">=</span> <span class="p">(</span><span class="s">'H'</span><span class="p">,</span> <span class="mi">2</span><span class="p">)</span></td></tr><tr><th id="L22"><a href="#L22">22</a></th><td>_DATATYPES<span class="p">[</span>PointField<span class="o">.</span>INT32<span class="p">]</span> <span class="o">=</span> <span class="p">(</span><span class="s">'i'</span><span class="p">,</span> <span class="mi">4</span><span class="p">)</span></td></tr><tr><th id="L23"><a href="#L23">23</a></th><td>_DATATYPES<span class="p">[</span>PointField<span class="o">.</span>UINT32<span class="p">]</span> <span class="o">=</span> <span class="p">(</span><span class="s">'I'</span><span class="p">,</span> <span class="mi">4</span><span class="p">)</span></td></tr><tr><th id="L24"><a href="#L24">24</a></th><td>_DATATYPES<span class="p">[</span>PointField<span class="o">.</span>FLOAT32<span class="p">]</span> <span class="o">=</span> <span class="p">(</span><span class="s">'f'</span><span class="p">,</span> <span class="mi">4</span><span class="p">)</span></td></tr><tr><th id="L25"><a href="#L25">25</a></th><td>_DATATYPES<span class="p">[</span>PointField<span class="o">.</span>FLOAT64<span class="p">]</span> <span class="o">=</span> <span class="p">(</span><span class="s">'d'</span><span class="p">,</span> <span class="mi">8</span><span class="p">)</span></td></tr><tr><th id="L26"><a href="#L26">26</a></th><td></td></tr><tr><th id="L27"><a href="#L27">27</a></th><td><span class="k">def</span> <span class="nf">read_points</span><span class="p">(</span>cloud<span class="p">,</span> field_names<span class="o">=</span><span class="bp">None</span><span class="p">,</span> skip_nans<span class="o">=</span><span class="bp">False</span><span class="p">,</span> uvs<span class="o">=</span><span class="p">[]):</span></td></tr><tr><th id="L28"><a href="#L28">28</a></th><td> <span class="sd">"""</span></td></tr><tr><th id="L29"><a href="#L29">29</a></th><td><span class="sd"> Read points from a L{sensor_msgs.PointCloud2} message.</span></td></tr><tr><th id="L30"><a href="#L30">30</a></th><td><span class="sd"></span></td></tr><tr><th id="L31"><a href="#L31">31</a></th><td><span class="sd"> @param cloud: The point cloud to read from.</span></td></tr><tr><th id="L32"><a href="#L32">32</a></th><td><span class="sd"> @type cloud: L{sensor_msgs.PointCloud2}</span></td></tr><tr><th id="L33"><a href="#L33">33</a></th><td><span class="sd"> @param field_names: The names of fields to read. If None, read all fields. [default: None]</span></td></tr><tr><th id="L34"><a href="#L34">34</a></th><td><span class="sd"> @type field_names: iterable</span></td></tr><tr><th id="L35"><a href="#L35">35</a></th><td><span class="sd"> @param skip_nans: If True, then don't return any point with a NaN value.</span></td></tr><tr><th id="L36"><a href="#L36">36</a></th><td><span class="sd"> @type skip_nans: bool [default: False]</span></td></tr><tr><th id="L37"><a href="#L37">37</a></th><td><span class="sd"> @param uvs: If specified, then only return the points at the given coordinates. [default: empty list]</span></td></tr><tr><th id="L38"><a href="#L38">38</a></th><td><span class="sd"> @type uvs: iterable</span></td></tr><tr><th id="L39"><a href="#L39">39</a></th><td><span class="sd"> @return: Generator which yields a list of values for each point.</span></td></tr><tr><th id="L40"><a href="#L40">40</a></th><td><span class="sd"> @rtype: generator</span></td></tr><tr><th id="L41"><a href="#L41">41</a></th><td><span class="sd"> """</span></td></tr><tr><th id="L42"><a href="#L42">42</a></th><td> <span class="k">assert</span> <span class="nb">isinstance</span><span class="p">(</span>cloud<span class="p">,</span> roslib<span class="o">.</span>message<span class="o">.</span>Message<span class="p">)</span> <span class="ow">and</span> cloud<span class="o">.</span>_type <span class="o">==</span> <span class="s">'sensor_msgs/PointCloud2'</span><span class="p">,</span> <span class="s">'cloud is not a sensor_msgs.msg.PointCloud2'</span></td></tr><tr><th id="L43"><a href="#L43">43</a></th><td> fmt <span class="o">=</span> _get_struct_fmt<span class="p">(</span>cloud<span class="o">.</span>is_bigendian<span class="p">,</span> cloud<span class="o">.</span>fields<span class="p">,</span> field_names<span class="p">)</span></td></tr><tr><th id="L44"><a href="#L44">44</a></th><td> width<span class="p">,</span> height<span class="p">,</span> point_step<span class="p">,</span> row_step<span class="p">,</span> data<span class="p">,</span> isnan <span class="o">=</span> cloud<span class="o">.</span>width<span class="p">,</span> cloud<span class="o">.</span>height<span class="p">,</span> cloud<span class="o">.</span>point_step<span class="p">,</span> cloud<span class="o">.</span>row_step<span class="p">,</span> cloud<span class="o">.</span>data<span class="p">,</span> math<span class="o">.</span>isnan</td></tr><tr><th id="L45"><a href="#L45">45</a></th><td> unpack_from <span class="o">=</span> struct<span class="o">.</span>Struct<span class="p">(</span>fmt<span class="p">)</span><span class="o">.</span>unpack_from</td></tr><tr><th id="L46"><a href="#L46">46</a></th><td></td></tr><tr><th id="L47"><a href="#L47">47</a></th><td> <span class="k">if</span> skip_nans<span class="p">:</span></td></tr><tr><th id="L48"><a href="#L48">48</a></th><td> <span class="k">if</span> uvs<span class="p">:</span></td></tr><tr><th id="L49"><a href="#L49">49</a></th><td> <span class="k">for</span> u<span class="p">,</span> v <span class="ow">in</span> uvs<span class="p">:</span></td></tr><tr><th id="L50"><a href="#L50">50</a></th><td> p <span class="o">=</span> unpack_from<span class="p">(</span>data<span class="p">,</span> <span class="p">(</span>row_step <span class="o">*</span> v<span class="p">)</span> <span class="o">+</span> <span class="p">(</span>point_step <span class="o">*</span> u<span class="p">))</span></td></tr><tr><th id="L51"><a href="#L51">51</a></th><td> has_nan <span class="o">=</span> <span class="bp">False</span></td></tr><tr><th id="L52"><a href="#L52">52</a></th><td> <span class="k">for</span> pv <span class="ow">in</span> p<span class="p">:</span></td></tr><tr><th id="L53"><a href="#L53">53</a></th><td> <span class="k">if</span> isnan<span class="p">(</span>pv<span class="p">):</span></td></tr><tr><th id="L54"><a href="#L54">54</a></th><td> has_nan <span class="o">=</span> <span class="bp">True</span></td></tr><tr><th id="L55"><a href="#L55">55</a></th><td> <span class="k">break</span></td></tr><tr><th id="L56"><a href="#L56">56</a></th><td> <span class="k">if</span> <span class="ow">not</span> has_nan<span class="p">:</span></td></tr><tr><th id="L57"><a href="#L57">57</a></th><td> <span class="k">yield</span> p</td></tr><tr><th id="L58"><a href="#L58">58</a></th><td> <span class="k">else</span><span class="p">:</span></td></tr><tr><th id="L59"><a href="#L59">59</a></th><td> <span class="k">for</span> v <span class="ow">in</span> <span class="nb">xrange</span><span class="p">(</span>height<span class="p">):</span></td></tr><tr><th id="L60"><a href="#L60">60</a></th><td> offset <span class="o">=</span> row_step <span class="o">*</span> v</td></tr><tr><th id="L61"><a href="#L61">61</a></th><td> <span class="k">for</span> u <span class="ow">in</span> <span class="nb">xrange</span><span class="p">(</span>width<span class="p">):</span></td></tr><tr><th id="L62"><a href="#L62">62</a></th><td> p <span class="o">=</span> unpack_from<span class="p">(</span>data<span class="p">,</span> offset<span class="p">)</span></td></tr><tr><th id="L63"><a href="#L63">63</a></th><td> has_nan <span class="o">=</span> <span class="bp">False</span></td></tr><tr><th id="L64"><a href="#L64">64</a></th><td> <span class="k">for</span> pv <span class="ow">in</span> p<span class="p">:</span></td></tr><tr><th id="L65"><a href="#L65">65</a></th><td> <span class="k">if</span> isnan<span class="p">(</span>pv<span class="p">):</span></td></tr><tr><th id="L66"><a href="#L66">66</a></th><td> has_nan <span class="o">=</span> <span class="bp">True</span></td></tr><tr><th id="L67"><a href="#L67">67</a></th><td> <span class="k">break</span></td></tr><tr><th id="L68"><a href="#L68">68</a></th><td> <span class="k">if</span> <span class="ow">not</span> has_nan<span class="p">:</span></td></tr><tr><th id="L69"><a href="#L69">69</a></th><td> <span class="k">yield</span> p</td></tr><tr><th id="L70"><a href="#L70">70</a></th><td> offset <span class="o">+=</span> point_step</td></tr><tr><th id="L71"><a href="#L71">71</a></th><td> <span class="k">else</span><span class="p">:</span></td></tr><tr><th id="L72"><a href="#L72">72</a></th><td> <span class="k">if</span> uvs<span class="p">:</span></td></tr><tr><th id="L73"><a href="#L73">73</a></th><td> <span class="k">for</span> u<span class="p">,</span> v <span class="ow">in</span> uvs<span class="p">:</span></td></tr><tr><th id="L74"><a href="#L74">74</a></th><td> <span class="k">yield</span> unpack_from<span class="p">(</span>data<span class="p">,</span> <span class="p">(</span>row_step <span class="o">*</span> v<span class="p">)</span> <span class="o">+</span> <span class="p">(</span>point_step <span class="o">*</span> u<span class="p">))</span></td></tr><tr><th id="L75"><a href="#L75">75</a></th><td> <span class="k">else</span><span class="p">:</span></td></tr><tr><th id="L76"><a href="#L76">76</a></th><td> <span class="k">for</span> v <span class="ow">in</span> <span class="nb">xrange</span><span class="p">(</span>height<span class="p">):</span></td></tr><tr><th id="L77"><a href="#L77">77</a></th><td> offset <span class="o">=</span> row_step <span class="o">*</span> v</td></tr><tr><th id="L78"><a href="#L78">78</a></th><td> <span class="k">for</span> u <span class="ow">in</span> <span class="nb">xrange</span><span class="p">(</span>width<span class="p">):</span></td></tr><tr><th id="L79"><a href="#L79">79</a></th><td> <span class="k">yield</span> unpack_from<span class="p">(</span>data<span class="p">,</span> offset<span class="p">)</span></td></tr><tr><th id="L80"><a href="#L80">80</a></th><td> offset <span class="o">+=</span> point_step</td></tr><tr><th id="L81"><a href="#L81">81</a></th><td></td></tr><tr><th id="L82"><a href="#L82">82</a></th><td><span class="k">def</span> <span class="nf">create_cloud</span><span class="p">(</span>header<span class="p">,</span> fields<span class="p">,</span> points<span class="p">):</span></td></tr><tr><th id="L83"><a href="#L83">83</a></th><td> <span class="sd">"""</span></td></tr><tr><th id="L84"><a href="#L84">84</a></th><td><span class="sd"> Create a L{sensor_msgs.msg.PointCloud2} message.</span></td></tr><tr><th id="L85"><a href="#L85">85</a></th><td><span class="sd"></span></td></tr><tr><th id="L86"><a href="#L86">86</a></th><td><span class="sd"> @param header: The point cloud header.</span></td></tr><tr><th id="L87"><a href="#L87">87</a></th><td><span class="sd"> @type header: L{std_msgs.msg.Header}</span></td></tr><tr><th id="L88"><a href="#L88">88</a></th><td><span class="sd"> @param fields: The point cloud fields.</span></td></tr><tr><th id="L89"><a href="#L89">89</a></th><td><span class="sd"> @type fields: iterable of L{sensor_msgs.msg.PointField}</span></td></tr><tr><th id="L90"><a href="#L90">90</a></th><td><span class="sd"> @param points: The point cloud points.</span></td></tr><tr><th id="L91"><a href="#L91">91</a></th><td><span class="sd"> @type points: list</span></td></tr><tr><th id="L92"><a href="#L92">92</a></th><td><span class="sd"> @return: The point cloud.</span></td></tr><tr><th id="L93"><a href="#L93">93</a></th><td><span class="sd"> @rtype: L{sensor_msgs.msg.PointCloud2}</span></td></tr><tr><th id="L94"><a href="#L94">94</a></th><td><span class="sd"> """</span></td></tr><tr><th id="L95"><a href="#L95">95</a></th><td></td></tr><tr><th id="L96"><a href="#L96">96</a></th><td> cloud_struct <span class="o">=</span> struct<span class="o">.</span>Struct<span class="p">(</span>_get_struct_fmt<span class="p">(</span><span class="bp">False</span><span class="p">,</span> fields<span class="p">))</span></td></tr><tr><th id="L97"><a href="#L97">97</a></th><td></td></tr><tr><th id="L98"><a href="#L98">98</a></th><td> buff <span class="o">=</span> ctypes<span class="o">.</span>create_string_buffer<span class="p">(</span>cloud_struct<span class="o">.</span>size <span class="o">*</span> <span class="nb">len</span><span class="p">(</span>points<span class="p">))</span></td></tr><tr><th id="L99"><a href="#L99">99</a></th><td></td></tr><tr><th id="L100"><a href="#L100">100</a></th><td> point_step<span class="p">,</span> pack_into <span class="o">=</span> cloud_struct<span class="o">.</span>size<span class="p">,</span> cloud_struct<span class="o">.</span>pack_into</td></tr><tr><th id="L101"><a href="#L101">101</a></th><td> offset <span class="o">=</span> <span class="mi">0</span></td></tr><tr><th id="L102"><a href="#L102">102</a></th><td> <span class="k">for</span> p <span class="ow">in</span> points<span class="p">:</span></td></tr><tr><th id="L103"><a href="#L103">103</a></th><td> pack_into<span class="p">(</span>buff<span class="p">,</span> offset<span class="p">,</span> <span class="o">*</span>p<span class="p">)</span></td></tr><tr><th id="L104"><a href="#L104">104</a></th><td> offset <span class="o">+=</span> point_step</td></tr><tr><th id="L105"><a href="#L105">105</a></th><td></td></tr><tr><th id="L106"><a href="#L106">106</a></th><td> <span class="k">return</span> PointCloud2<span class="p">(</span>header<span class="o">=</span>header<span class="p">,</span></td></tr><tr><th id="L107"><a href="#L107">107</a></th><td> height<span class="o">=</span><span class="mi">1</span><span class="p">,</span></td></tr><tr><th id="L108"><a href="#L108">108</a></th><td> width<span class="o">=</span><span class="nb">len</span><span class="p">(</span>points<span class="p">),</span></td></tr><tr><th id="L109"><a href="#L109">109</a></th><td> is_dense<span class="o">=</span><span class="bp">False</span><span class="p">,</span></td></tr><tr><th id="L110"><a href="#L110">110</a></th><td> is_bigendian<span class="o">=</span><span class="bp">False</span><span class="p">,</span></td></tr><tr><th id="L111"><a href="#L111">111</a></th><td> fields<span class="o">=</span>fields<span class="p">,</span></td></tr><tr><th id="L112"><a href="#L112">112</a></th><td> point_step<span class="o">=</span>cloud_struct<span class="o">.</span>size<span class="p">,</span></td></tr><tr><th id="L113"><a href="#L113">113</a></th><td> row_step<span class="o">=</span>cloud_struct<span class="o">.</span>size <span class="o">*</span> <span class="nb">len</span><span class="p">(</span>points<span class="p">),</span></td></tr><tr><th id="L114"><a href="#L114">114</a></th><td> data<span class="o">=</span>buff<span class="o">.</span>raw<span class="p">)</span></td></tr><tr><th id="L115"><a href="#L115">115</a></th><td></td></tr><tr><th id="L116"><a href="#L116">116</a></th><td><span class="k">def</span> <span class="nf">create_cloud_xyz32</span><span class="p">(</span>header<span class="p">,</span> points<span class="p">):</span></td></tr><tr><th id="L117"><a href="#L117">117</a></th><td> <span class="sd">"""</span></td></tr><tr><th id="L118"><a href="#L118">118</a></th><td><span class="sd"> Create a L{sensor_msgs.msg.PointCloud2} message with 3 float32 fields (x, y, z).</span></td></tr><tr><th id="L119"><a href="#L119">119</a></th><td><span class="sd"></span></td></tr><tr><th id="L120"><a href="#L120">120</a></th><td><span class="sd"> @param header: The point cloud header.</span></td></tr><tr><th id="L121"><a href="#L121">121</a></th><td><span class="sd"> @type header: L{std_msgs.msg.Header}</span></td></tr><tr><th id="L122"><a href="#L122">122</a></th><td><span class="sd"> @param points: The point cloud points.</span></td></tr><tr><th id="L123"><a href="#L123">123</a></th><td><span class="sd"> @type points: iterable</span></td></tr><tr><th id="L124"><a href="#L124">124</a></th><td><span class="sd"> @return: The point cloud.</span></td></tr><tr><th id="L125"><a href="#L125">125</a></th><td><span class="sd"> @rtype: L{sensor_msgs.msg.PointCloud2}</span></td></tr><tr><th id="L126"><a href="#L126">126</a></th><td><span class="sd"> """</span></td></tr><tr><th id="L127"><a href="#L127">127</a></th><td> fields <span class="o">=</span> <span class="p">[</span>PointField<span class="p">(</span><span class="s">'x'</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> PointField<span class="o">.</span>FLOAT32<span class="p">,</span> <span class="mi">1</span><span class="p">),</span></td></tr><tr><th id="L128"><a href="#L128">128</a></th><td> PointField<span class="p">(</span><span class="s">'y'</span><span class="p">,</span> <span class="mi">4</span><span class="p">,</span> PointField<span class="o">.</span>FLOAT32<span class="p">,</span> <span class="mi">1</span><span class="p">),</span></td></tr><tr><th id="L129"><a href="#L129">129</a></th><td> PointField<span class="p">(</span><span class="s">'z'</span><span class="p">,</span> <span class="mi">8</span><span class="p">,</span> PointField<span class="o">.</span>FLOAT32<span class="p">,</span> <span class="mi">1</span><span class="p">)]</span></td></tr><tr><th id="L130"><a href="#L130">130</a></th><td> <span class="k">return</span> create_cloud<span class="p">(</span>header<span class="p">,</span> fields<span class="p">,</span> points<span class="p">)</span></td></tr><tr><th id="L131"><a href="#L131">131</a></th><td></td></tr><tr><th id="L132"><a href="#L132">132</a></th><td><span class="k">def</span> <span class="nf">_get_struct_fmt</span><span class="p">(</span>is_bigendian<span class="p">,</span> fields<span class="p">,</span> field_names<span class="o">=</span><span class="bp">None</span><span class="p">):</span></td></tr><tr><th id="L133"><a href="#L133">133</a></th><td> fmt <span class="o">=</span> <span class="s">'>'</span> <span class="k">if</span> is_bigendian <span class="k">else</span> <span class="s">'<'</span></td></tr><tr><th id="L134"><a href="#L134">134</a></th><td></td></tr><tr><th id="L135"><a href="#L135">135</a></th><td> offset <span class="o">=</span> <span class="mi">0</span></td></tr><tr><th id="L136"><a href="#L136">136</a></th><td> <span class="k">for</span> field <span class="ow">in</span> <span class="p">(</span>f <span class="k">for</span> f <span class="ow">in</span> <span class="nb">sorted</span><span class="p">(</span>fields<span class="p">,</span> key<span class="o">=</span><span class="k">lambda</span> f<span class="p">:</span> f<span class="o">.</span>offset<span class="p">)</span> <span class="k">if</span> field_names <span class="ow">is</span> <span class="bp">None</span> <span class="ow">or</span> f<span class="o">.</span>name <span class="ow">in</span> field_names<span class="p">):</span></td></tr><tr><th id="L137"><a href="#L137">137</a></th><td> <span class="k">if</span> offset <span class="o"><</span> field<span class="o">.</span>offset<span class="p">:</span></td></tr><tr><th id="L138"><a href="#L138">138</a></th><td> fmt <span class="o">+=</span> <span class="s">'x'</span> <span class="o">*</span> <span class="p">(</span>field<span class="o">.</span>offset <span class="o">-</span> offset<span class="p">)</span></td></tr><tr><th id="L139"><a href="#L139">139</a></th><td> offset <span class="o">=</span> field<span class="o">.</span>offset</td></tr><tr><th id="L140"><a href="#L140">140</a></th><td> <span class="k">if</span> field<span class="o">.</span>datatype <span class="ow">not</span> <span class="ow">in</span> _DATATYPES<span class="p">:</span></td></tr><tr><th id="L141"><a href="#L141">141</a></th><td> <span class="k">print</span> <span class="o">>></span> sys<span class="o">.</span>stderr<span class="p">,</span> <span class="s">'Skipping unknown PointField datatype [</span><span class="si">%d</span><span class="s">]'</span> <span class="o">%</span> field<span class="o">.</span>datatype</td></tr><tr><th id="L142"><a href="#L142">142</a></th><td> <span class="k">else</span><span class="p">:</span></td></tr><tr><th id="L143"><a href="#L143">143</a></th><td> datatype_fmt<span class="p">,</span> datatype_length <span class="o">=</span> _DATATYPES<span class="p">[</span>field<span class="o">.</span>datatype<span class="p">]</span></td></tr><tr><th id="L144"><a href="#L144">144</a></th><td> fmt <span class="o">+=</span> field<span class="o">.</span>count <span class="o">*</span> datatype_fmt</td></tr><tr><th id="L145"><a href="#L145">145</a></th><td> offset <span class="o">+=</span> field<span class="o">.</span>count <span class="o">*</span> datatype_length</td></tr><tr><th id="L146"><a href="#L146">146</a></th><td></td></tr><tr><th id="L147"><a href="#L147">147</a></th><td> <span class="k">return</span> fmt</td></tr></tbody></table>
</div>
</div>
<div id="altlinks">
<h3>Download in other formats:</h3>
<ul>
<li class="last first">
<a rel="nofollow" href="/trac/ros-pkg/raw-attachment/ticket/4440/point_cloud.py">Original Format</a>
</li>
</ul>
</div>
</div>
<div id="footer" lang="en" xml:lang="en"><hr />
<a id="tracpowered" href="http://trac.edgewall.org/"><img src="/trac/ros-pkg/chrome/common/trac_logo_mini.png" height="30" width="107" alt="Trac Powered" /></a>
<p class="left">
Powered by <a href="/trac/ros-pkg/about"><strong>Trac 0.11.7</strong></a><br />
By <a href="http://www.edgewall.org/">Edgewall Software</a>.
</p>
<p class="right">Visit the Trac open source project at<br /><a href="http://trac.edgewall.org/">http://trac.edgewall.org/</a></p>
</div>
</body>
</html>
|