diff --git a/src/3rdParty/_PCL/entities/PCLTypes.jl b/src/3rdParty/_PCL/entities/PCLTypes.jl index a2e7eabfa..27520d549 100644 --- a/src/3rdParty/_PCL/entities/PCLTypes.jl +++ b/src/3rdParty/_PCL/entities/PCLTypes.jl @@ -125,7 +125,8 @@ Base.@kwdef struct Header """ The sequence number """ seq::UInt32 = UInt32(0) """ A timestamp associated with the time when the data was acquired. - The value represents microseconds since 1970-01-01 00:00:00 (the UNIX epoch). """ + The value represents microseconds since 1970-01-01 00:00:00 (the UNIX epoch). + Suggest: making this relative to UCT to account for timezones in future. """ stamp::UInt64 = UInt64(0) """ Coordinate frame ID. """ frame_id::String = "" diff --git a/src/3rdParty/_PCL/services/PointCloud.jl b/src/3rdParty/_PCL/services/PointCloud.jl index 5dd31e5a7..4c7278920 100644 --- a/src/3rdParty/_PCL/services/PointCloud.jl +++ b/src/3rdParty/_PCL/services/PointCloud.jl @@ -57,6 +57,8 @@ Base.getindex(pc::PointCloud, i) = pc.points[i] Base.setindex!(pc::PointCloud, pt::PointT, idx) = (pc.points[idx] = pt) Base.resize!(pc::PointCloud, s::Integer) = resize!(pc.points, s) +Base.length(pc::PointCloud) = length(pc.points) + ## not very Julian translations from C++ above ## ============================================================================================== @@ -146,7 +148,7 @@ function createMapping(T,msg_fields::AbstractVector{<:PointField}, field_map::Ms # Coalesce adjacent fields into single copy where possible if 1 < length(field_map) - # TODO check accending vs descending order + # TODO check accending vs descending order> sort!(field_map, by = x->x.serialized_offset) # something strange with how C++ does and skips the while loop, disabling the coalescing for now @@ -167,7 +169,7 @@ function createMapping(T,msg_fields::AbstractVector{<:PointField}, field_map::Ms # else # i += 1 # j += 1 - + # _jmap = field_map[j] # _jend = field_map[end] # end @@ -177,6 +179,14 @@ function createMapping(T,msg_fields::AbstractVector{<:PointField}, field_map::Ms return field_map end +# pc2.fields +# 6-element Vector{Caesar._PCL.PointField}: +# 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) +# Caesar._PCL.PointField("timestamp", 0x00000018, Caesar._PCL._PCL_FLOAT64, 0x00000001) +# Caesar._PCL.PointField("ring", 0x00000020, Caesar._PCL._PCL_UINT16, 0x00000001) # https://pointclouds.org/documentation/conversions_8h_source.html#l00166 @@ -193,13 +203,14 @@ function PointCloud( cloudsize = msg.width*msg.height # cloud_data = Vector{UInt8}(undef, cloudsize) - # NOTE assume all fields use the same data type + # FIXME cannot assume all fields use the same data type # off script conversion for XYZ_ data only datatype = asType{msg.fields[1].datatype}() len = trunc(Int, length(msg.data)/field_map[1].size) data_ = Vector{datatype}(undef, len) read!(IOBuffer(msg.data), data_) mat = reshape(data_, :, cloudsize) + # see createMapping above # Check if we can copy adjacent points in a single memcpy. We can do so if there @@ -214,7 +225,7 @@ function PointCloud( error("copy of just one field_map not implemented yet") else # If not, memcpy each group of contiguous fields separately - @assert msg.height == 1 "only decoding msg.height=1 messages, update converter here." + @assert msg.height == 1 "only decoding msg::PCLPointCloud2.height=1 messages, update converter here." for row in 1:msg.height # TODO check might have an off by one error here # row_data = row * msg.row_step + 1 # msg.data[(row-1) * msg.row_step] @@ -223,12 +234,11 @@ function PointCloud( # the slow way of building the point.data entry ptdata = zeros(datatype, 4) for (i,mapping) in enumerate(field_map) + @info "test" i mapping maxlog=3 midx = trunc(Int,mapping.serialized_offset/mapping.size) + 1 - # TODO, why the weird index reversal? + # Copy individual points as columns from mat -- i.e. memcpy + # TODO copy only works if all elements have the same type -- suggestion, ptdata::ArrayPartition instead ptdata[i] = mat[midx, col] - # @info "DO COPY" mapping - # memcpy (cloud_data + mapping.struct_offset, msg_data + mapping.serialized_offset, mapping.size); - # @info "copy" mapping.struct_offset mapping.serialized_offset mapping.size end pt = T(;data=SVector(ptdata...)) push!(cloud.points, pt) @@ -241,6 +251,76 @@ function PointCloud( end +""" +$SIGNATURES + +Convert a pcl::PointCloud object to a PCLPointCloud2 binary data blob. +In: cloud the input pcl::PointCloud +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} + # Ease the user's burden on specifying width/height for unorganized datasets + if (cloud.width == 0 && cloud.height == 0) + height = 1; + width = length(cloud) + else + @assert (length(cloud) == cloud.width * cloud.height) "Input `cloud::PointCloud`'s width*height is not equal to length(cloud)" + height = cloud.height; + width = cloud.width; + end + + # // Fill point cloud binary data (padding and all) + # std::size_t data_size = sizeof (PointT) * cloud.size (); + # msg.data.resize (data_size); + # if (data_size) + # { + # 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)) + _nflds = length(fields) + # # Fill fields metadata + # msg.fields.clear (); + # for_each_type::type> (detail::FieldAdder(msg.fields)); + + # NOTE get the last possible byte location of all fields, assume no padding after last used byte + _fend = fields[end] + point_step = _fend.offset + sizeof(asType{_fend.datatype}()) # = sizeof (PointT); + # msg.row_step = (sizeof (PointT) * msg.width); + # # todo msg.is_bigendian = ?; + + # flatten all point fields as binary + 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) + data[offset:(offset-1+point_step)] .= reinterpret(UInt8, view(pt.data, 1:_nflds)) + end + + # @error("noise") + return PCLPointCloud2(; + header = cloud.header, + height, + width, + fields, + data, + # is_bigendian = , + point_step, + row_step = sizeof(T)*width, + is_dense = cloud.is_dense, + ) +end + + ## ========================================================================================================= ## Coordinate transformations using Manifolds.jl ## ========================================================================================================= @@ -248,9 +328,10 @@ end # 2D, do similar or better for 3D # FIXME, to optimize, this function will likely be slow +# TODO, consolidate with transformPointcloud(::ScatterAlign,..) function function apply( M_::typeof(SpecialEuclidean(2)), - rPp::Union{<:ProductRepr,<:Manifolds.ArrayPartition}, - pc::PointCloud{T} ) where T + rPp::Union{<:ProductRepr,<:Manifolds.ArrayPartition}, + pc::PointCloud{T} ) where T # rTp = affine_matrix(M_, rPp) diff --git a/src/3rdParty/_PCL/services/ROSConversions.jl b/src/3rdParty/_PCL/services/ROSConversions.jl index d37e7388f..9d3eaae75 100644 --- a/src/3rdParty/_PCL/services/ROSConversions.jl +++ b/src/3rdParty/_PCL/services/ROSConversions.jl @@ -6,10 +6,29 @@ using ..RobotOS +@rosimport std_msgs.msg: Header @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" # 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 @@ -23,16 +42,24 @@ function PCLPointCloud2(msg::Main.sensor_msgs.msg.PointCloud2) pfs = PointField[PointField(;name=pf_.name,offset=pf_.offset,datatype=pf_.datatype,count=pf_.count) for pf_ in msg.fields] endian = msg.is_bigendian ? _PCL_ENDIAN_BIG_BYTE : _PCL_ENDIAN_LITTLE_WORD - pc2 = PCLPointCloud2(;header, - height = msg.height, - width = msg.width, - fields = pfs, - data = msg.data, - is_bigendian= endian, - point_step = msg.point_step, - row_step = msg.row_step, - is_dense = UInt8(msg.is_dense) ) + PCLPointCloud2(;header, + height = msg.height, + width = msg.width, + fields = pfs, + data = msg.data, + is_bigendian= endian, + point_step = msg.point_step, + row_step = msg.row_step, + is_dense = UInt8(msg.is_dense) + ) # end +# function toROSPointCloud2(pc2::PCLPointCloud2) +# msg = Main.sensor_msgs.msg.PointCloud2() +# msg.height = pc2.height + +# msg +# end + # \ No newline at end of file diff --git a/test/pcl/testPointCloud2.jl b/test/pcl/testPointCloud2.jl index 1654893ac..81928aa72 100644 --- a/test/pcl/testPointCloud2.jl +++ b/test/pcl/testPointCloud2.jl @@ -92,4 +92,24 @@ show(pc) ## end + +@testset "PandarXT test point cloud conversion test" begin +## + +@warn "TODO, see testdata/_pandar_PCLPointCloud2.jldat which via `Serialization.serialize` of a `Caesar._PCL.PCLPointCloud2` object, at JL 1.7.3, CJL v0.13.1+" +# pc2.fields +# 6-element Vector{Caesar._PCL.PointField}: +# 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) +# Caesar._PCL.PointField("timestamp", 0x00000018, Caesar._PCL._PCL_FLOAT64, 0x00000001) +# Caesar._PCL.PointField("ring", 0x00000020, Caesar._PCL._PCL_UINT16, 0x00000001) + + +## +end + + + # \ No newline at end of file diff --git a/test/testdata/_pandar_PCLPointCloud2.jldat b/test/testdata/_pandar_PCLPointCloud2.jldat new file mode 100644 index 000000000..625ee8790 Binary files /dev/null and b/test/testdata/_pandar_PCLPointCloud2.jldat differ