Skip to content

Document new _PCL types and usage #762

Open
@dehann

Description

@dehann

Initial example, see here:

function handleRadarPointcloud!(msg::sensor_msgs.msg.PointCloud2, fg::AbstractDFG, systemstate::SystemState)
@info "handleRadarPointcloud" maxlog=10
# assume there is still space (previously cleared)
# add new piece of radar point cloud to queue for later processing.
put!(systemstate.radar_scan_queue, msg)
# check if the queue still has space
@show length(systemstate.radar_scan_queue.data)
if length(systemstate.radar_scan_queue.data) < systemstate.radar_scan_queue.sz_max
# nothing more to do
return nothing
end
# type instability
# Full sweep, lets empty the queue and add a variable
queueScans = Vector{Any}(undef, systemstate.radar_scan_queue.sz_max)
for i in 1:length(systemstate.radar_scan_queue.data)
# something minimal, will do util for transforming PointCloud2 next
println(i)
md = take!(systemstate.radar_scan_queue)
# @info typeof(md) fieldnames(typeof(md))
pc2 = Caesar._PCL.PCLPointCloud2(md)
pc_ = Caesar._PCL.PointCloud(pc2)
# 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
timestamp = Float64(msg.header.stamp.secs) + Float64(msg.header.stamp.nsecs)/1.0e9
systemstate.curtimestamp = timestamp
systemstate.cur_variable = addVariable!(fg, Symbol("x$(systemstate.var_index)"), Pose2, timestamp = unix2datetime(timestamp))
systemstate.var_index += 1
io = IOBuffer()
serialize(io, queueScans)
# @show datablob = pc # queueScans
# and add a data blob of all the scans
# Make a data entry in the graph
addData!( fg, :radar, systemstate.cur_variable.label, :RADARPC,
take!(io), # get base64 binary
# Vector{UInt8}(JSON2.write(datablob)),
mimeType="/application/octet-stream/bson;dataformat=Vector{Caesar._PCL.PCLPointCloud2}",
description="queueScans = Serialize.deserialize(PipeBuffer(readBytes))")
#
end

Metadata

Metadata

Assignees

No one assigned

    Type

    No type

    Projects

    Status

    No status

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions