diff --git a/src/3rdParty/_PCL/entities/PCLTypes.jl b/src/3rdParty/_PCL/entities/PCLTypes.jl index 27520d549..3df65fd0c 100644 --- a/src/3rdParty/_PCL/entities/PCLTypes.jl +++ b/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 diff --git a/src/3rdParty/_PCL/services/PointCloud.jl b/src/3rdParty/_PCL/services/PointCloud.jl index 4c7278920..91cd1d710 100644 --- a/src/3rdParty/_PCL/services/PointCloud.jl +++ b/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 diff --git a/src/3rdParty/_PCL/services/ROSConversions.jl b/src/3rdParty/_PCL/services/ROSConversions.jl index 9d3eaae75..cbc51cea0 100644 --- a/src/3rdParty/_PCL/services/ROSConversions.jl +++ b/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 # \ No newline at end of file