-
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