~wecacuee/mrol/mrol-dev

« back to all changes in this revision

Viewing changes to rosutils_mrol/point_cloud.py

  • Committer: Vikas Dhiman
  • Date: 2012-05-22 15:26:38 UTC
  • Revision ID: wecacuee@gmail.com-20120522152638-j6srlkgrpdq4kdfe
Added support for userdata in terms of colored voxels. Added benchmark test and corresponding data

Show diffs side-by-side

added added

removed removed

Lines of Context:
 
1
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
 
2
<html xmlns="http://www.w3.org/1999/xhtml">
 
3
  
 
4
  
 
5
 
 
6
 
 
7
  
 
8
 
 
9
  <head>
 
10
    <title>
 
11
      Attachment – ros-pkg
 
12
    </title>
 
13
        <link rel="search" href="/trac/ros-pkg/search" />
 
14
        <link rel="help" href="/trac/ros-pkg/wiki/TracGuide" />
 
15
        <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" />
 
16
        <link rel="up" href="/trac/ros-pkg/ticket/4440" title="Ticket #4440" />
 
17
        <link rel="start" href="/trac/ros-pkg/wiki" />
 
18
        <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" />
 
19
        <link rel="shortcut icon" href="/trac/ros-pkg/chrome/common/trac.ico" type="image/x-icon" />
 
20
        <link rel="icon" href="/trac/ros-pkg/chrome/common/trac.ico" type="image/x-icon" />
 
21
      <link type="application/opensearchdescription+xml" rel="search" href="/trac/ros-pkg/search/opensearch" title="Search ros-pkg" />
 
22
    <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>
 
23
    <!--[if lt IE 7]>
 
24
    <script type="text/javascript" src="/trac/ros-pkg/chrome/common/js/ie_pre7_hacks.js"></script>
 
25
    <![endif]-->
 
26
  </head>
 
27
  <body>
 
28
    <div id="banner">
 
29
      <div id="header">
 
30
        <a id="logo" href="http://code.ros.org/trac/ros-pkg/"><img src="/trac/ros-pkg/chrome/common/trac_banner.png" alt="" /></a>
 
31
      </div>
 
32
      <form id="search" action="/trac/ros-pkg/search" method="get">
 
33
        <div>
 
34
          <label for="proj-search">Search:</label>
 
35
          <input type="text" id="proj-search" name="q" size="18" value="" />
 
36
          <input type="submit" value="Search" />
 
37
        </div>
 
38
      </form>
 
39
      <div id="metanav" class="nav">
 
40
    <ul>
 
41
      <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>
 
42
    </ul>
 
43
  </div>
 
44
    </div>
 
45
    <div id="mainnav" class="nav">
 
46
    <ul>
 
47
      <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>
 
48
    </ul>
 
49
  </div>
 
50
    <div id="main">
 
51
      <div id="ctxtnav" class="nav">
 
52
        <h2>Context Navigation</h2>
 
53
          <ul>
 
54
              <li class="last first"><a href="/trac/ros-pkg/ticket/4440">Back to Ticket #4440</a></li>
 
55
          </ul>
 
56
        <hr />
 
57
      </div>
 
58
    <div id="content" class="attachment">
 
59
        <h1><a href="/trac/ros-pkg/ticket/4440">Ticket #4440</a>: point_cloud.py</h1>
 
60
        <table id="info" summary="Description">
 
61
          <tbody>
 
62
            <tr>
 
63
              <th scope="col">
 
64
                File point_cloud.py, <span title="5328 bytes">5.2 KB</span>
 
65
                (added by tfield,  <a class="timeline" href="/trac/ros-pkg/timeline?from=2011-07-28T13%3A37%3A39-0700&amp;precision=second" title="2011-07-28T13:37:39-0700 in Timeline">10 months</a> ago)
 
66
              </th>
 
67
            </tr>
 
68
            <tr>
 
69
              <td class="message searchable">
 
70
                <p>
 
71
tims implementation w/o access restrictions
 
72
</p>
 
73
 
 
74
              </td>
 
75
            </tr>
 
76
          </tbody>
 
77
        </table>
 
78
        <div id="preview" class="searchable">
 
79
    <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">'&gt;'</span> <span class="k">if</span> is_bigendian <span class="k">else</span> <span class="s">'&lt;'</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">&lt;</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">&gt;&gt;</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>
 
80
        </div>
 
81
    </div>
 
82
    <div id="altlinks">
 
83
      <h3>Download in other formats:</h3>
 
84
      <ul>
 
85
        <li class="last first">
 
86
          <a rel="nofollow" href="/trac/ros-pkg/raw-attachment/ticket/4440/point_cloud.py">Original Format</a>
 
87
        </li>
 
88
      </ul>
 
89
    </div>
 
90
    </div>
 
91
    <div id="footer" lang="en" xml:lang="en"><hr />
 
92
      <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>
 
93
      <p class="left">
 
94
        Powered by <a href="/trac/ros-pkg/about"><strong>Trac 0.11.7</strong></a><br />
 
95
        By <a href="http://www.edgewall.org/">Edgewall Software</a>.
 
96
      </p>
 
97
      <p class="right">Visit the Trac open source project at<br /><a href="http://trac.edgewall.org/">http://trac.edgewall.org/</a></p>
 
98
    </div>
 
99
  </body>
 
100
</html>
 
 
b'\\ No newline at end of file'