add convert and toROSPointCloud2 functions

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

@@ -80,6 +80,7 @@ References
 end
 
 Base.convert(::Type{<:_PCL_POINTFIELD_FORMAT}, val::Integer) = (en=instances(_PCL_POINTFIELD_FORMAT); en[findfirst(Int.(en) .== Int.(convert(UInt8, val)))])
+Base.convert(::Type{<:Integer}, ff::_PCL_POINTFIELD_FORMAT) = findfirst(==(ff), instances(_PCL_POINTFIELD_FORMAT))
 
 struct asType{T} end
 

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

@@ -261,7 +261,7 @@ Out: msg the resultant PCLPointCloud2 binary blob
 DevNotes:
 - TODO, remove hacks, e.g. fields [x,y,z,intensity,]-only.
 """
-function PCLPointCloud2(cloud::PointCloud{T,P,R}) where {T,P,R}
+function PCLPointCloud2(cloud::PointCloud{T,P,R}; datatype = _PCL_FLOAT32) where {T,P,R}
   # Ease the user's burden on specifying width/height for unorganized datasets
   if (cloud.width == 0 && cloud.height == 0)
     height = 1;
@@ -280,13 +280,12 @@ function PCLPointCloud2(cloud::PointCloud{T,P,R}) where {T,P,R}
   #      memcpy(&msg.data[0], &cloud[0], data_size);
   #    }
 
-  hackTypeAll = _PCL_FLOAT32
   fields = Vector{PointField}(undef, 4)
   # TODO assuming all(fields[_].count==1)
-  fields[1] = PointField("x", UInt32(0), hackTypeAll, UInt32(1))
-  fields[2] = PointField("y", UInt32(4), hackTypeAll, UInt32(1))
-  fields[3] = PointField("z", UInt32(8), hackTypeAll, UInt32(1))
-  fields[4] = PointField("intensity", UInt32(12), hackTypeAll, UInt32(1))
+  fields[1] = PointField("x", UInt32(0), datatype, UInt32(1))
+  fields[2] = PointField("y", UInt32(4), datatype, UInt32(1))
+  fields[3] = PointField("z", UInt32(8), datatype, UInt32(1))
+  fields[4] = PointField("intensity", UInt32(12), datatype, UInt32(1))
   _nflds = length(fields)
   #    # Fill fields metadata
   #    msg.fields.clear ();
@@ -302,7 +301,7 @@ function PCLPointCloud2(cloud::PointCloud{T,P,R}) where {T,P,R}
   data = Vector{UInt8}(undef, point_step*width)
   for (i,pt) in enumerate(cloud.points)
     offset = (point_step*(i-1)+1)
-    # NOTE assume continuous data block in struct (all of same hackTypeAll, FIXME)
+    # NOTE assume continuous data block in struct (all of same datatype, FIXME)
     data[offset:(offset-1+point_step)] .= reinterpret(UInt8, view(pt.data, 1:_nflds))
   end
 

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

@@ -7,28 +7,15 @@
 using ..RobotOS
 
 @rosimport std_msgs.msg: Header
+@rosimport sensor_msgs.msg: PointField
 @rosimport sensor_msgs.msg: PointCloud2
 
 
-
 Base.convert(::Type{UInt64}, rost::RobotOS.Time) = UInt64(rost.secs)*1000000 + trunc(UInt64, rost.nsecs*1e-3)
-# FIXME, this converter is making a small mistake, e.g. the loss of 145ns in comment data below
-Base.convert(::Type{RobotOS.Time}, tm::UInt64) = RobotOS.Time(trunc(Int32,tm*1e-6), + trunc(Int32, ((tm*1e-6) % 1)*1_000_000_000) )
-# header: 
-#   seq: 24282
-#   stamp: 
-#     secs: 1646305718
-#     nsecs: 997857000
-#   frame_id: "PandarXT-32"
-##
-# julia> rt = RobotOS.Time(1646305718, 997857000)
-# Time(1646305718, 997857000)
-# julia> xt = convert(UInt64, rt)
-# 0x0005d94e6b929361
-# julia> convert(RobotOS.Time, xt)
-# Time(1646305718, 997856855)
-
-# @assert convert(RobotOS.Time, convert(UInt64, RobotOS.Time(1646305718, 997857000))) == RobotOS.Time(1646305718, 997857000) "conversion to and from RobotOS.Time and UInt64 not consistent"
+Base.convert(::Type{RobotOS.Time}, tm::UInt64) = RobotOS.Time(trunc(Int32,tm*1e-6), Int32((tm % 1_000_000)*1000) )
+
+# embedded test to avoid CI requiring all of ROS and PyCall
+@assert convert(RobotOS.Time, convert(UInt64, RobotOS.Time(1646305718, 997857000))) == RobotOS.Time(1646305718, 997857000) "conversion to and from RobotOS.Time and UInt64 not consistent"
 
 # 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
@@ -55,11 +42,37 @@ function PCLPointCloud2(msg::Main.sensor_msgs.msg.PointCloud2)
   #
 end
 
-# function toROSPointCloud2(pc2::PCLPointCloud2)
-#   msg = Main.sensor_msgs.msg.PointCloud2()
-#   msg.height = pc2.height
+function toROSPointCloud2(pc2::PCLPointCloud2)
+  header = Main.std_msgs.msg.Header();
+  header.seq = pc2.header.seq
+  header.stamp = convert(RobotOS.Time, pc2.header.stamp)
+  header.frame_id = pc2.header.frame_id
+
+  msg = Main.sensor_msgs.msg.PointCloud2();
+
+  msg.header = header
+  msg.height = pc2.height
+  msg.width = pc2.width
 
-#   msg
-# end
+  # all PointField elements
+  fields = Main.sensor_msgs.msg.PointField[]
+  for pf_ in pc2.fields
+    mpf = Main.sensor_msgs.msg.PointField()
+    mpf.name = pf_.name
+    mpf.offset=pf_.offset
+    mpf.datatype= convert(UInt8, pf_.datatype)
+    mpf.count=pf_.count
+    push!(fields, mpf)
+  end
+  msg.fields = fields
+
+  msg.is_bigendian = pc2.is_bigendian == _PCL_ENDIAN_BIG_BYTE
+  msg.point_step = pc2.point_step
+  msg.row_step = pc2.row_step
+  msg.data = pc2.data
+  msg.is_dense = pc2.is_dense
+
+  msg
+end
 
 #