Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Document new _PCL types and usage #762

Open
dehann opened this issue Nov 2, 2021 · 0 comments
Open

Document new _PCL types and usage #762

dehann opened this issue Nov 2, 2021 · 0 comments

Comments

@dehann
Copy link
Member

dehann commented Nov 2, 2021

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

@dehann dehann added this to the v0.0.x milestone Nov 2, 2021
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
Status: No status
Development

No branches or pull requests

1 participant