Merge pull request #760 from JuliaRobotics/21Q4/pcl/pointcloud2

adding PCL PointCloud (Julia) types

Author: dehann

Project.toml

@@ -80,6 +80,7 @@ julia = "1.4"
 
 [extras]
 AprilTags = "f0fec3d5-a81e-5a6a-8c28-d2b34f3659de"
+Colors = "5ae59095-9a9b-59fe-a467-6f913c188581"
 Images = "916415d5-f1e6-5110-898d-aaa5f9f070e0"
 PyCall = "438e738f-606a-5dbb-bf0a-cddfbfd45ab0"
 RoMEPlotting = "238d586b-a4bf-555c-9891-eda6fc5e55a2"
@@ -88,4 +89,4 @@ Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 ZMQ = "c2297ded-f4af-51ae-bb23-16f91089e4e1"
 
 [targets]
-test = ["Test", "PyCall", "RobotOS", "ZMQ", "AprilTags", "Images"]
+test = ["AprilTags", "Colors", "Images", "PyCall", "RobotOS", "Test", "ZMQ"]

src/3rdParty/_PCL/_PCL.jl

@@ -0,0 +1,117 @@
+"""
+  Software License Agreement (BSD License)
+
+  Point Cloud Library (PCL) - www.pointclouds.org
+  Copyright (c) 2010, Willow Garage, Inc.
+  Copyright (c) 2012-, Open Perception, Inc.
+
+  All rights reserved.
+
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions
+  are met:
+
+  * Redistributions of source code must retain the above copyright
+    notice, this list of conditions and the following disclaimer.
+  * Redistributions in binary form must reproduce the above
+    copyright notice, this list of conditions and the following
+    disclaimer in the documentation and/or other materials provided
+    with the distribution.
+  * Neither the name of the copyright holder(s) nor the names of its
+    contributors may be used to endorse or promote products derived
+    from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+  POSSIBILITY OF SUCH DAMAGE.
+"""
+
+@info "Caesar.jl is loading (but not exporting) tools requiring Colors.jl; e.g. Caesar._PCL"
+
+module _PCL
+
+using DocStringExtensions
+using StaticArrays
+using ..Colors
+
+## hold off on exports, users can in the mean-time use/import via e.g. _PCL.PointXYZ
+# export PointT, PointXYZ, PointXYZRGB, PointXYZRGBA
+# export PCLHeader, PointCloud
+
+abstract type PointT end
+
+"""
+    $TYPEDEF
+
+See https://pointclouds.org/documentation/point__types_8hpp_source.html
+"""
+Base.@kwdef struct PointXYZ{C <: Colorant, T <: Number} <: PointT
+  color::C           = RGBA(1,1,1,1)
+  data::SVector{4,T} = SVector(0,0,0,Float32(1))
+end
+
+# Construct helpers nearest to PCL
+PointXYZRGBA( x::Real=0, y::Real=0, z::Real=Float32(1); 
+              r::Real=1,g::Real=1,b::Real=1, alpha::Real=1,
+              color::Colorant=RGBA(r,g,b,alpha), 
+              pos=SA[x,y,z], data=SA[pos...,1] ) = PointXYZ(;color,data)
+#
+PointXYZRGB(  x::Real=0, y::Real=0, z::Real=Float32(1); 
+              r::Real=1,g::Real=1,b::Real=1, 
+              color::Colorant=RGB(r,g,b), 
+              pos=SA[x,y,z], data=SA[pos...,1] ) = PointXYZ(;color,data)
+#
+
+function Base.getproperty(p::PointXYZ, f::Symbol)
+  if f == :x 
+    return getfield(p, :data)[1]
+  elseif f == :y 
+    return getfield(p, :data)[2]
+  elseif f == :z
+    return getfield(p, :data)[3]
+  elseif f == :r
+    return getfield(p, :color).r
+  elseif f == :g
+    return getfield(p, :color).g
+  elseif f == :b
+    return getfield(p, :color).b
+  elseif f == :alpha
+    return getfield(p, :color).alpha
+  elseif f == :data || f == :pos
+    return getfield(p, :data)
+  elseif f == :color || f == :rgb
+    return getfield(p, :color)
+  end
+  error("PointXYZ has no field $f")
+end
+
+
+"""
+    $TYPEDEF
+
+See https://pointclouds.org/documentation/structpcl_1_1_p_c_l_header.html
+"""
+Base.@kwdef struct PCLHeader
+  seq::UInt32      = UInt32(0)
+  stamp::UInt64    = UInt64(0)
+  frame_id::String = ""
+end
+
+Base.@kwdef struct PointCloud
+  header::PCLHeader        = PCLHeader()
+  """ vector of points representing the point cloud """
+  points::Vector{<:PointT} = Vector{PointXYZ{RGBA{Colors.FixedPointNumbers.N0f8}, Float32}}()
+end
+
+
+
+end

src/Caesar.jl

@@ -92,6 +92,7 @@ function __init__()
     include("images/ScanMatcherPose2.jl")
     include("images/ScatterAlignPose2.jl")
   end
+  @require Colors="5ae59095-9a9b-59fe-a467-6f913c188581" include("3rdParty/_PCL/_PCL.jl")
   @require Distributed="8ba89e20-285c-5b6f-9357-94700520ee1b" include("images/DistributedUtils.jl")
 end