PointCloud converter good enough for 2D Radar

Author: dehann

examples/marine/asv/philos/IngestROSbag.jl

@@ -61,6 +61,7 @@ using DistributedFactorGraphs.DocStringExtensions
 using Dates
 using JSON2
 using BSON
+using Serialization
 using FixedPointNumbers
 using StaticArrays
 
@@ -161,6 +162,7 @@ function handleRadarPointcloud!(msg::sensor_msgs.msg.PointCloud2, fg::AbstractDF
         # pc = Caesar._PCL.PointCloud(; height=md.height, width=md.width)
 
         queueScans[i] = (pc2,pc_) 
+        # queueScans[i] = pc_
     end
 
     # add a new variable to the graph
@@ -170,7 +172,7 @@ function handleRadarPointcloud!(msg::sensor_msgs.msg.PointCloud2, fg::AbstractDF
     systemstate.var_index += 1
 
     io = IOBuffer()
-    BSON.@save io queueScans
+    serialize(io, queueScans)
 
     # @show datablob = pc # queueScans
     # and add a data blob of all the scans
@@ -179,7 +181,7 @@ function handleRadarPointcloud!(msg::sensor_msgs.msg.PointCloud2, fg::AbstractDF
                 take!(io), # get base64 binary
                 # Vector{UInt8}(JSON2.write(datablob)),  
                 mimeType="/application/octet-stream/bson;dataformat=Vector{Caesar._PCL.PCLPointCloud2}",
-                description="BSON.@load PipeBuffer(readBytes) queueScans")
+                description="queueScans = Serialize.deserialize(PipeBuffer(readBytes))")
     #
 end
 
@@ -354,7 +356,39 @@ addBlobStore!(fg, ds)
 
 de,db = getData(fg, :x0, :RADARPC)
 
-BSON.@load PipeBuffer(db) queueScans
+queueScans = deserialize(PipeBuffer(db)) # BSON.@load
 
-queueScans[1]
+pc2,pc = queueScans[1]
 
+## 
+
+using Gadfly
+Gadfly.set_default_plot_size(40cm,20cm)
+
+##
+
+
+PL = []
+
+for lb in [:x0;:x1;:x2;:x3;:x4;:x5]
+de,db = getData(fg, lb, :RADARPC)
+queueScans = deserialize(PipeBuffer(db)) 
+for (pc2,pc) in queueScans
+X = (c->c.x).(pc.points)
+Y = (c->c.y).(pc.points)
+push!(PL, Gadfly.layer(x=X,y=Y, Geom.point))
+end
+end
+
+Gadfly.plot(PL...)
+
+##
+
+
+
+
+
+
+
+
+##

src/3rdParty/_PCL/entities/PCLTypes.jl

@@ -109,7 +109,7 @@ See
 - https://pointclouds.org/documentation/point__types_8hpp_source.html
 """
 Base.@kwdef struct PointXYZ{C <: Colorant, T <: Number} <: PointT
-  color::C           = RGB(1,1,1)
+  color::C           = RGBA(1,1,1,1)
   data::SVector{4,T} = SVector(0,0,0,1f0)
 end
 

src/3rdParty/_PCL/services/ROSConversions.jl

@@ -14,7 +14,6 @@ Base.convert(::Type{UInt64}, rost::RobotOS.Time) = UInt64(rost.secs)*1000000 + t
 # Really odd constructor, strong assumption that user FIRST ran @rostypegen in Main BEFORE loading Caesar
 # https://docs.ros.org/en/hydro/api/pcl_conversions/html/pcl__conversions_8h_source.html#l00208
 function PCLPointCloud2(msg::Main.sensor_msgs.msg.PointCloud2)
-  @warn("work in progress on PCLPointCloud2(msg::Main.sensor_msgs.msg.PointCloud2)")
   header = Header(;stamp  = msg.header.stamp,
                   seq     = msg.header.seq,
                   frame_id= msg.header.frame_id )

test/pcl/testPointCloud2.jl

@@ -13,43 +13,81 @@ using BSON
 using Test
 using Pkg
 
-import Caesar._PCL: FieldMapper, createMapping, PointCloud, PointField, PCLPointCloud2, Header, asType, _PCL_POINTFIELD_FORMAT, FieldMapping, MsgFieldMap, FieldMatches
+# import Caesar._PCL: FieldMapper, createMapping, PointCloud, PointField, PCLPointCloud2, Header, asType, _PCL_POINTFIELD_FORMAT, FieldMapping, MsgFieldMap, FieldMatches
 
 
 ##
 @testset "test Caesar._PCL.PCLPointCloud2 to Caesar._PCL.PointCloud converter." begin
 ##
 
-testdatafile = joinpath( dirname(dirname(Pkg.pathof(Caesar))), "test", "testdata", "_PCLPointCloud2.bson")
-
+# testdatafile = joinpath( dirname(dirname(Pkg.pathof(Caesar))), "test", "testdata", "_PCLPointCloud2.bson")
 # load presaved test data to test the coverter
-BSON.@load testdatafile PointCloudRef PointCloudTest
+# BSON.@load testdatafile PointCloudRef PointCloudTest
+
+## build PCLPointCloud2 to be converted
+
+datafile = joinpath( dirname(dirname(Pkg.pathof(Caesar))), "test", "testdata", "_PCLPointCloud2_15776.dat")
+fid = open(datafile,"r")
+data = read(fid)
+close(fid)
+
+##
+
+pc2 = Caesar._PCL.PCLPointCloud2(;
+  header = Caesar._PCL.Header(;
+    seq = 92147,
+    stamp = 0x0005b24647c4af10,
+    frame_id = "velodyne"
+  ),
+  height = 1,
+  width = 493,
+  fields = [
+    Caesar._PCL.PointField("x", 0x00000000, Caesar._PCL._PCL_FLOAT32, 0x00000001),
+    Caesar._PCL.PointField("y", 0x00000004, Caesar._PCL._PCL_FLOAT32, 0x00000001),
+    Caesar._PCL.PointField("z", 0x00000008, Caesar._PCL._PCL_FLOAT32, 0x00000001),
+    Caesar._PCL.PointField("intensity", 0x00000010, Caesar._PCL._PCL_FLOAT32, 0x00000001)
+  ],
+  point_step = 32,
+  row_step = 15776,
+  is_dense = 1,
+  data
+)
+
 
-show(PointCloudTest[1])
+##
+
+show(pc2)
 
 # test major unpacking / type conversion needed by ROS.sensor_msgs.pointcloud2
-pc = Caesar._PCL.PointCloud(PointCloudTest[1])
+pc = Caesar._PCL.PointCloud(pc2)
+
+show(pc)
 
 @test pc.height == 1
 @test pc.width == 493
 @test pc.is_dense
 
-@test_broken length(pc.points) == 493
+@test length(pc.points) == 493
+
+##
+
+# truncated copy of reference data on good to 1e-3
+@test isapprox( [-0,0,0],              pc.points[1].data[1:3], atol=5e-3)
+@test isapprox( [-1.56486,1.09851,0],  pc.points[2].data[1:3], atol=5e-3)
+@test isapprox( [-793.383,556.945,0],  pc.points[3].data[1:3], atol=5e-3)
+@test isapprox( [-0,0,0],              pc.points[4].data[1:3], atol=5e-3)
+@test isapprox( [-1.56148,1.10331,0],  pc.points[5].data[1:3], atol=5e-3)
+@test isapprox( [-788.548,557.169,0],  pc.points[6].data[1:3], atol=5e-3)
+@test isapprox( [-790.109,558.273,0],  pc.points[7].data[1:3], atol=5e-3)
+@test isapprox( [-791.671,559.376,0],  pc.points[8].data[1:3], atol=5e-3)
+@test isapprox( [-793.232,560.479,0],  pc.points[9].data[1:3], atol=5e-3)
+@test isapprox( [-794.794,561.583,0],  pc.points[10].data[1:3], atol=5e-3)
 
-for (i,pt) in enumerate(pc.points)
-  @test isapprox( PointCloudRef[1][1,:], pt.data; atol=1e-6)
-end
 
-# 231	    fromPCLPointCloud2 (msg, cloud, field_map);
-# (gdb) print field_map
-# $7 = std::vector of length 1, capacity 4 = {{serialized_offset = 0, 
-#     struct_offset = 0, size = 12}}
+# for (i,pt) in enumerate(pc.points)
+#   @test isapprox( PointCloudRef[1][i,1:3], pt.data[1:3]; atol=1e-6)
+# end
 
-# (gdb) print cloud.points[0]
-# $43 = {<pcl::_PointXYZ> = {{data = {-0, 0, 0, 1}, {x = -0, y = 0, 
-#         z = 0}}}, <No data fields>}
-# (gdb) print sizeof(cloud.points[0])
-# $44 = 16
 
 ##
 end