Merge pull request #10 from JuliaRobotics/23Q3/refac/handlers

handlers and tests

Project.toml

@@ -5,16 +5,25 @@ authors = ["dehann <dehann@navability.io>"]
 version = "0.1.0"
 
 [deps]
+BSON = "fbb218c0-5317-5bc6-957e-2ee96dd4b1f0"
 Caesar = "62eebf14-49bc-5f46-9df9-f7b7ef379406"
+Colors = "5ae59095-9a9b-59fe-a467-6f913c188581"
 Dates = "ade2ca70-3891-5945-98fb-dc099432e06a"
 DistributedFactorGraphs = "b5cc3c7e-6572-11e9-2517-99fb8daf2f04"
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
+FileIO = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549"
+FixedPointNumbers = "53c48c17-4a7d-5ca2-90c5-79b7896eea93"
+ImageMagick = "6218d12a-5da1-5696-b52f-db25d2ecc6d1"
 JSON3 = "0f8b85d8-7281-11e9-16c2-39a750bddbf1"
+OpenSSL = "4d8831e6-92b7-49fb-bdf8-b643e874388c"
 Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f"
 ProgressMeter = "92933f4c-e287-5a05-a399-4b506db050ca"
 PyCall = "438e738f-606a-5dbb-bf0a-cddfbfd45ab0"
+Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 SHA = "ea8e919c-243c-51af-8825-aaa63cd721ce"
+StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 TensorCast = "02d47bb6-7ce6-556a-be16-bb1710789e2b"
+TimeZones = "f269a46b-ccf7-5d73-abea-4c690281aa53"
 UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4"
 Unmarshal = "cbff2730-442d-58d7-89d1-8e530c41eb02"
 
@@ -43,4 +52,4 @@ Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 TestImages = "5e47fb64-e119-507b-a336-dd2b206d9990"
 
 [targets]
-test = ["RobotOS", "Test", "TestImages"]
+test = ["Images", "RobotOS", "Test", "TestImages"]

ext/PyCaesarRobotOSExt.jl

@@ -1,55 +1,76 @@
 module PyCaesarRobotOSExt
 
 using PyCall
+using OpenSSL # workaround attempt, https://github.com/JuliaWeb/OpenSSL.jl/issues/9
 using RobotOS
-using Caesar
 using Dates
 using DocStringExtensions
 
-import Base: convert
+using Colors
+using ImageMagick, FileIO
+# using Images
+# likely packages needed to ingest various different sensor data types and serialize them for NVA
+using TimeZones
+using Random
+using JSON3
+using BSON
+# , Serialization
+using FixedPointNumbers
+using StaticArrays
+
+# dont load Caesar until after rostypegen
+using Caesar
+# import Caesar: unmarshal
 
 # FIXME DEPRECATE UPGRADE TBD
 import Unmarshal: unmarshal
 
 import Caesar._PCL as _PCL
 
+import Base: convert
+
 # weakdeps type and memver prototype import for overwritten definition pattern 
 import PyCaesar: RosbagSubscriber, RosbagWriter
 import PyCaesar: loop!, getROSPyMsgTimestamp, nanosecond2datetime
+import PyCaesar: handleMsg!, handleMsg_OVRLPRECOMP!
 
-
+##
 
 @rosimport std_msgs.msg: Header
 
+@rosimport tf2_msgs.msg: TFMessage
+
+@rosimport nav_msgs.msg: Odometry
+
 # standard types
 @rosimport sensor_msgs.msg: PointCloud2
+@rosimport sensor_msgs.msg: PointField
 @rosimport sensor_msgs.msg: LaserScan
 @rosimport sensor_msgs.msg: CompressedImage
 @rosimport sensor_msgs.msg: Image
 @rosimport sensor_msgs.msg: Imu
-@rosimport tf2_msgs.msg: TFMessage
-@rosimport geometry_msgs.msg: TransformStamped
-@rosimport geometry_msgs.msg: Transform
+@rosimport sensor_msgs.msg: NavSatFix
+@rosimport sensor_msgs.msg: RegionOfInterest
+@rosimport sensor_msgs.msg: CameraInfo
+
 @rosimport geometry_msgs.msg: Vector3
 @rosimport geometry_msgs.msg: Quaternion
-@rosimport nav_msgs.msg: Odometry
-
-# FIXME, note this is not working, functions using Main. as workaround
-@rosimport std_msgs.msg: Header
-@rosimport sensor_msgs.msg: PointField
-@rosimport sensor_msgs.msg: PointCloud2
-
-# FIXME not sure if this should be run here
+@rosimport geometry_msgs.msg: Point
+@rosimport geometry_msgs.msg: Pose
+@rosimport geometry_msgs.msg: Transform
+@rosimport geometry_msgs.msg: TransformStamped
+@rosimport geometry_msgs.msg: Twist
+@rosimport geometry_msgs.msg: TwistWithCovariance
+@rosimport geometry_msgs.msg: TwistWithCovarianceStamped
 
-# thismodule(m::Module, args...) = (@info("this module is getting", m); m)
-# macro thismodule(args...)
-#   :( thismodule(__module__) )
-# end
+# load type definitions into this modules context
 rostypegen(@__MODULE__)
 
+##
 
 include("Utils/RosbagSubscriber.jl")
 include("services/ROSConversions.jl")
 include("services/PCLROSConversions.jl")
+include("services/IngestROSSetup.jl")
 
 end

ext/services/IngestROSSetup.jl

@@ -0,0 +1,143 @@
+
+
+
+include("StdROSHandlers/ROSJSONtoJSON.jl")
+
+# http://wiki.ros.org/laser_geometry
+lg = pyimport("laser_geometry.laser_geometry")
+
+include("StdROSHandlers/SystemState.jl")
+
+# actual handlers
+include("StdROSHandlers/IngestLidar3D.jl")
+include("StdROSHandlers/IngestRadar2D.jl")
+include("StdROSHandlers/IngestLaserScan2D.jl")
+include("StdROSHandlers/IngestIMU.jl")
+include("StdROSHandlers/IngestCameraRaw.jl")
+include("StdROSHandlers/IngestCameraInfo.jl")
+include("StdROSHandlers/IngestTFMessage.jl")
+include("StdROSHandlers/IngestOdometry.jl")
+include("StdROSHandlers/IngestGPS.jl")
+include("StdROSHandlers/IngestTwist.jl")
+
+
+
+## Automation code that converts user options on which topics and types to subscribe to in a ROSbag.
+
+function SubscribeROSTopicOptions(;kw...)
+  di = Dict{String,Any}()
+  for (k,v) in kw
+      di[string(k)] = v
+  end
+  di
+end
+
+
+Base.@kwdef struct SubscribeROSTopicInput
+  topicname::String
+  msgtype
+  callback
+end
+
+
+function addROSSubscriber!(bagSub::RosbagSubscriber, inp::SubscribeROSTopicInput, subsargs::Tuple=() )
+  @show inp.msgtype
+  bagSub(inp.topicname, inp.msgtype, inp.callback, subsargs )
+end
+
+"""
+    _suppressPushExisting!
+
+Piece of defensive coding, if graph upload from ROS with BashingMapper breaks during process, this helps resume the "upload"
+"""
+function _suppressPushExisting!(
+  nfg::AbstractDFG,
+  state::SystemState,
+  vars::AbstractVector{Symbol},
+  userPushNva::Bool,
+  userPushBlobs::Bool
+)
+  # skip if not allow suppress
+  !state.suppressPush && (return false)
+  lbl = Symbol("x$(state.var_index)")
+  if lbl in vars
+    state.pushNva = false
+    # not a perfect check, assumes all or nothing is already up there
+    state.pushBlobs = if 0 === length(listBlobEntries(nfg, lbl))
+      @warn "might upload blobs for $lbl"
+      userPushBlobs
+      return false
+    else
+      @warn "skipping uploads of $lbl"
+      false
+      return true
+    end
+  else
+    # regular case of nothing up on nfg yet so just add as stuff as normal
+    state.pushNva = userPushNva
+    state.pushBlobs = userPushBlobs
+    return false
+  end
+  #
+  error("should get here")
+end
+
+
+## a common main loop
+
+function main(
+  nfg::AbstractDFG, 
+  bagfile, 
+  userWantsSubsOnROS; 
+  iters::Integer=50, 
+  stripeKeyframe=5, 
+  pushNva=true,
+  pushBlobs=false, 
+  cur_variable = "x0", # should already be uploaded in a previous step
+  var_index = 1        # this is the next variable to add -- i.e. "x1"
+)
+  #
+
+  @info "Hit CTRL+C to exit and save the graph..."
+
+  if pushNva
+    x0 = NvaSDK.getVariable(client, context, cur_variable) |> fetch
+    @assert !(x0 === nothing) "The indicated starting variable $cur_variable was not found"
+  end
+
+  # subscriber for the bagfile
+  bagSubscriber = RosbagSubscriber(bagfile)
+
+  # System state
+  systemstate = SystemState(;
+                    stripeKeyframe,  # TODO remove, use handler option instead
+                    pushBlobs, 
+                    pushNva,
+                    cur_variable,
+                    var_index
+                )
+
+  # all the subscribers the user requested
+  for (usrInp, usrOpt) in userWantsSubsOnROS
+    @info "Adding ROS subscriber " context usrInp usrOpt
+    addROSSubscriber!(bagSubscriber, usrInp, (client, context, systemstate, usrOpt))
+  end
+
+  vars = listVariables(nfg)
+
+  @info "subscribers have been set up; entering main loop"
+  # loop_rate = Rate(20.0)
+  while loop!(bagSubscriber)
+    # defensive suppress state.pushNva and .pushBlobs if variable already there -- i.e. trying to resume an upload
+    _suppressPushExisting!(nfg, systemstate, vars, pushNva, pushBlobs)
+    # regular processing steps
+    iters -= 1
+    iters < 0 ? break : nothing
+  end
+
+  @info "Exiting"
+  systemstate
+end
+
+
+##

ext/services/StdROSHandlers/IngestCamera.jl

@@ -0,0 +1,48 @@
+
+"""
+$SIGNATURES
+
+Message callback for camera images.
+"""
+function handleMsg!(
+    msg::sensor_msgs.msg.CompressedImage, 
+    dfg, 
+    state::SystemState, 
+    options=Dict()
+)
+
+    lbl = "IMG_CENTER_$(state.cur_variable)_$(msg.header.seq)"
+    @info "handleCameraCompressed!" lbl maxlog=10
+
+    entries = Tuple{String, String}[]
+
+    prvLbl = state.prv_variable === "" ? "x0" : state.prv_variable
+    if state.pushBlobs
+        res = addData(dfg, lbl, msg.data)
+        # not super well synched, but good enough for basic demonstration
+        addBlobEntry!(dfg, prvLbl, res, lbl, "image/jpeg")
+        #   description="ImageMagick.readblob(imgBytes)"
+        push!(entries, (lbl, res))
+    end
+
+    # add the data entries to the local factor graph too
+    if state.pushBlobs && state.pushLocal
+        vari = Caesar.getVariable(state.localfg, Symbol(prvLbl))
+        timestamp = now(localzone())
+        for (lb,id) in entries
+            fde = Caesar.BlobStoreEntry(
+                Symbol(lb),
+                UUID(id),
+                :nva,
+                "",
+                "", # $(userId)|$(robotId)|$(sessionId)|$(prvLbl)",
+                "",
+                "image/jpeg",
+                now(localzone())
+            )
+            de = addBlobEntry!(vari, fde)
+        end
+    end
+
+    nothing
+end

ext/services/StdROSHandlers/IngestCameraInfo.jl

@@ -0,0 +1,63 @@
+
+
+
+"""
+    $SIGNATURES
+
+Message callback for Radar pings. Adds a variable to the factor graph and appends the scan as a bigdata element.
+"""
+function handleMsg!(
+  msg::sensor_msgs.msg.CameraInfo, 
+  dfg, 
+  state::SystemState, 
+  options=Dict()
+)
+  @info "handleCamInfo" state.cur_variable state.prv_variable maxlog=10
+
+  if state.cur_variable === nothing
+    # skip if we don't have a variable yet.
+    return nothing
+  end
+
+  if !haskey(state.workspace, "CamInfo_keyframe")
+    state.workspace["CamInfo_keyframe"] = state.prv_variable
+  end
+
+  # blob label is user specified or Mapper default
+  bllb = get(options, "name", CAMINFO_BLOBNAME())
+
+  # quick and dirty synchronization of camera messages to lidar messages.
+  # NOTE storing only the first image after a new pose is created
+  if state.workspace["CamInfo_keyframe"] == state.prv_variable
+    return nothing
+  end
+
+  msgdict = unmarshal(msg)
+  # @info "TRYING TO ADD" msg msgdict
+
+  timestamp = Float64(msg.header.stamp.secs) + Float64(msg.header.stamp.nsecs)/10^9
+  @info "[$timestamp] CamInfo sample on $(state.cur_variable)"
+
+  bllb *= "_"*string(msg.header.seq) #msgdict["header"]["seq"])
+  blob = JSON.json(msgdict) |> Vector{UInt8}
+  mime = "application/json/octet-stream"
+
+  if state.pushBlobs
+    blobid = addData(dfg, bllb, blob)
+
+    #
+    addBlobEntry!(
+      dfg,
+      # next of pair for now as rest works like that
+      state.cur_variable,
+      blobid,
+      bllb,
+      length(blob),
+      mime
+    )
+  end
+
+  state.workspace["CamInfo_keyframe"] = state.prv_variable
+
+  nothing
+end