-
Notifications
You must be signed in to change notification settings - Fork 32
Open
Labels
Milestone
Description
Initial example, see here:
Caesar.jl/examples/marine/asv/philos/IngestROSbag.jl
Lines 137 to 186 in 1f7d843
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
Labels
Type
Projects
Status
No status