Merge pull request #114 from NavAbility/22Q2/api/iif_api

Standard addVariable and addFactor API POC

Project.toml

@@ -3,7 +3,7 @@ uuid = "f3e6a059-199c-4ada-8143-fcefb97e6165"
 keywords = ["navability", "navigation", "slam", "sdk", "robotics", "robots"]
 desc = "NavAbility SDK: Access NavAbility Cloud factor graph features. Note that this SDK and the related API are still in development. Please let us know if you have any issues at [email protected]."
 authors = ["NavAbility <[email protected]>"]
-version = "0.4.4"
+version = "0.4.5"
 
 [deps]
 Dates = "ade2ca70-3891-5945-98fb-dc099432e06a"

src/NavAbilitySDK.jl

@@ -1,5 +1,8 @@
 module NavAbilitySDK
 
+const NVA = NavAbilitySDK
+export NVA
+
 # Global imports
 using DocStringExtensions
 using LinearAlgebra
@@ -56,6 +59,7 @@ include("./navability/services/Factor.jl")
 include("./navability/services/Solve.jl")
 include("./navability/services/Status.jl")
 include("./navability/services/Utils.jl")
+include("./navability/services/StandardAPI.jl")
 include("./navability/services/DataBlobs.jl")
 export getVariable, getVariables, listVariables, ls
 export addVariable, addPackedVariable

src/navability/entities/Factor.jl

@@ -40,8 +40,8 @@ Create a prior factor for a ContinuousScalar (a.k.a. Pose1) with a distribution
 
 Default value of Z = `Normal(0.0, 0.1)`.
 """
-function PriorData(;Z::Distribution = Normal(0.0, 0.1))::FactorData
-    data = FactorData(fnc = ZInferenceType(Z), certainhypo = [1])
+function PriorData(;Z::Distribution = Normal(0.0, 0.1), kwargs...)::FactorData
+    data = FactorData(;fnc = ZInferenceType(Z), certainhypo = [1], kwargs...)
     return data
 end
 
@@ -52,8 +52,8 @@ Create a prior factor for a Pose2 with a distribution Z representing (x,y,theta)
 
 Default value of Z = `FullNormal([0.0, 0.0, 0.0], diagm([0.01, 0.01, 0.01]))`.
 """
-function PriorPose2Data(;Z::Distribution = FullNormal([0.0, 0.0, 0.0], diagm([0.01, 0.01, 0.01])))::FactorData
-    data = FactorData(fnc = ZInferenceType(Z), certainhypo = [1])
+function PriorPose2Data(;Z::Distribution = FullNormal([0.0, 0.0, 0.0], diagm([0.01, 0.01, 0.01])), kwargs...)::FactorData
+    data = FactorData(;fnc = ZInferenceType(Z), certainhypo = [1], kwargs...)
     return data
 end
 
@@ -64,8 +64,8 @@ Create a prior factor for a Point2 with a distribution Z representing (x,y) prio
 
 Default value of Z = `FullNormal([0.0, 0.0], diagm([0.01, 0.01]))`.
 """
-function PriorPoint2Data(;Z::Distribution = FullNormal([0.0, 0.0], diagm([0.01, 0.01])))::FactorData
-    data = FactorData(fnc = ZInferenceType(Z), certainhypo = [1])
+function PriorPoint2Data(;Z::Distribution = FullNormal([0.0, 0.0], diagm([0.01, 0.01])), kwargs...)::FactorData
+    data = FactorData(;fnc = ZInferenceType(Z), certainhypo = [1], kwargs...)
     return data
 end
 
@@ -76,8 +76,8 @@ between the variables, e.g. `Normal(1.0, 0.1)`.
 
 Default value of Z = `Normal(1.0, 0.1)`.
 """
-function LinearRelativeData(;Z::Distribution = Normal(1.0, 0.1))::FactorData
-    data = FactorData(fnc = ZInferenceType(Z), certainhypo = [1, 2])
+function LinearRelativeData(;Z::Distribution = Normal(1.0, 0.1), kwargs...)::FactorData
+    data = FactorData(;fnc = ZInferenceType(Z), certainhypo = [1, 2], kwargs...)
     return data
 end
 
@@ -88,8 +88,8 @@ between the variables, e.g. `FullNormal([1,0,0.3333*π], diagm([0.01,0.01,0.01])
 
 Default value of Z = `FullNormal([1,0,0.3333*π], diagm([0.01,0.01,0.01]))`.
 """
-function Pose2Pose2Data(;Z::Distribution = FullNormal([1,0,0.3333*π], diagm([0.01, 0.01, 0.01])))::FactorData
-    data = FactorData(fnc = ZInferenceType(Z), certainhypo = [1, 2])
+function Pose2Pose2Data(;Z::Distribution = FullNormal([1,0,0.3333*π], diagm([0.01, 0.01, 0.01])), kwargs...)::FactorData
+    data = FactorData(;fnc = ZInferenceType(Z), certainhypo = [1, 2], kwargs...)
     return data
 end
 
@@ -99,8 +99,8 @@ Create a Pose2->Point2 bearing+range factor with 1D distributions:
 - bearing: The bearing from the pose to the point, default `Normal(0, 1)`.
 - range: The range from the pose to the point, default `Normal(1, 1)`.
 """
-function Pose2Point2BearingRangeData(;bearing::Distribution = Normal(0, 1), range::Distribution = Normal(1, 1))::FactorData
-    data = FactorData(fnc = Pose2Point2BearingRangeInferenceType(bearing, range), certainhypo = [1, 2])
+function Pose2Point2BearingRangeData(;bearing::Distribution = Normal(0, 1), range::Distribution = Normal(1, 1), kwargs...)::FactorData
+    data = FactorData(;fnc = Pose2Point2BearingRangeInferenceType(bearing, range), certainhypo = [1, 2], kwargs...)
     return data
 end
 
@@ -109,8 +109,8 @@ $(SIGNATURES)
 Create a Point2->Point2 range factor with a 1D distribution:
 - range: The range from the pose to the point, default `Normal(1, 1)`.
 """
-function Point2Point2RangeData(;range::Distribution = Normal(1, 1))::FactorData
-    data = FactorData(fnc = ZInferenceType(range), certainhypo = [1, 2])
+function Point2Point2RangeData(;range::Distribution = Normal(1, 1), kwargs...)::FactorData
+    data = FactorData(;fnc = ZInferenceType(range), certainhypo = [1, 2], kwargs...)
     return data
 end
 
@@ -119,9 +119,9 @@ $(SIGNATURES)
 Create a AprilTags factor that directly relates a Pose2 to the information from an AprilTag reading.
 Corners need to be provided, homography and tag length are defaulted and can be overwritten.
 """
-function Pose2AprilTag4CornersData(id, corners::Vector{Float64}, homography::Vector{Float64}; K::Vector{Float64}=[300.0,0.0,0.0,0.0,300.0,0.0,180.0,120.0,1.0], taglength::Float64=0.25)::FactorData
+function Pose2AprilTag4CornersData(id, corners::Vector{Float64}, homography::Vector{Float64}; K::Vector{Float64}=[300.0,0.0,0.0,0.0,300.0,0.0,180.0,120.0,1.0], taglength::Float64=0.25, kwargs...)::FactorData
     fnc = Pose2AprilTag4CornersInferenceType(;corners,homography,K,taglength,id)
-    data = FactorData(fnc = fnc, certainhypo = [1, 2])
+    data = FactorData(; fnc, certainhypo = [1, 2], kwargs...)
     return data
 end
 
@@ -138,12 +138,13 @@ function ScatterAlignPose2Data(
         bw2::AbstractVector{<:Real}=Float64[];
         mkd1 = ManifoldKernelDensity(; varType, pts=cloud1, bw=bw1 ),
         mkd2 = ManifoldKernelDensity(; varType, pts=cloud2, bw=bw2 ),
-        kw_sap...
+        kw_sap::NamedTuple=(;),
+        kwargs...
     )
     #
 
     fnc = ScatterAlignPose2InferenceType(; cloud1=mkd1, cloud2=mkd2, kw_sap...)
-    data = FactorData(fnc = fnc, certainhypo = [1, 2])
+    data = FactorData(; fnc, certainhypo = [1, 2], kwargs...)
     return data
 end
 
@@ -170,7 +171,7 @@ function MixtureData(
         probabilities::Vector{Float64},
         dims::Integer  # TODO: Confirming we can remove.
         )::FactorData #where T<:FactorData
-    data = FactorData(
+    data = FactorData(;
         fnc = MixtureInferenceType(
             N = length(components),
             # @jim-hill-r this is why I don't like the Data suffix.
@@ -183,21 +184,26 @@ function MixtureData(
     return data
 end
 
+
 function Factor(
         label::String, 
         fncType::String, 
         variableOrderSymbols::Vector{String}, 
         data::FactorData; 
         tags::Vector{String}=["FACTOR"], 
         timestamp::String = string(now(Dates.UTC))*"Z", 
-        multihypo=Float64[],
-        nullhypo::Real=0
+        multihypo=nothing,
+        nullhypo=nothing
     )::Factor
     #
     # TODO: Remove independent updates of this and set certainhypo here.
     data.certainhypo = Vector{Int}(1:size(variableOrderSymbols)[1])
-    data.multihypo = multihypo
-    data.nullhypo = float(nullhypo)
+    if multihypo !== nothing
+        data.multihypo = multihypo
+    end
+    if nullhypo !== nothing
+        data.nullhypo = float(nullhypo)
+    end
 
     result = Factor(
         label,

src/navability/entities/InferenceTypes.jl

@@ -8,6 +8,23 @@ functions inside of factors.
 """
 abstract type InferenceType end
 
+macro nvaZInferenceType(inferencetype)
+  return esc(quote
+      Base.@__doc__ struct $inferencetype <: NVA.InferenceType 
+          Z::Distribution
+      end
+      $inferencetype(;Z) = $inferencetype(Z)
+  end)
+end
+
+@nvaZInferenceType Prior
+@nvaZInferenceType LinearRelative
+@nvaZInferenceType PriorPose2
+@nvaZInferenceType Pose2Pose2
+@nvaZInferenceType PriorPoint2
+@nvaZInferenceType Point2Point2
+@nvaZInferenceType Point2Point2Range
+
 """
 $(TYPEDEF)
 ZInferenceType is used by many factors as a common inference
@@ -25,16 +42,18 @@ $(TYPEDEF)
 Pose2Point2BearingRangeInferenceType is used to represent a bearing
 + range measurement.
 """
-Base.@kwdef struct Pose2Point2BearingRangeInferenceType <: InferenceType
+Base.@kwdef struct Pose2Point2BearingRange <: InferenceType
   bearstr::Distribution
   rangstr::Distribution
 end
 
+const Pose2Point2BearingRangeInferenceType = Pose2Point2BearingRange
+
 """
 $(TYPEDEF)
 InferenceType for Pose2AprilTag4CornersData.
 """
-Base.@kwdef struct Pose2AprilTag4CornersInferenceType <: InferenceType
+Base.@kwdef struct Pose2AprilTag4Corners <: InferenceType
   corners::Vector{Float64}
   homography::Vector{Float64}
   K::Vector{Float64}
@@ -43,8 +62,9 @@ Base.@kwdef struct Pose2AprilTag4CornersInferenceType <: InferenceType
   _type::String = "/application/JuliaLang/PackedPose2AprilTag4Corners"
 end
 
+const Pose2AprilTag4CornersInferenceType = Pose2AprilTag4Corners
 
-Base.@kwdef struct ScatterAlignPose2InferenceType <: InferenceType
+Base.@kwdef struct ScatterAlignPose2 <: InferenceType
   """ This SDK only supports MKD clouds at this time. Note CJL also supports HeatmapGridDensity, TODO """
   cloud1::ManifoldKernelDensity
   cloud2::ManifoldKernelDensity
@@ -67,6 +87,8 @@ Base.@kwdef struct ScatterAlignPose2InferenceType <: InferenceType
   _type::String            = "Caesar.PackedScatterAlignPose2"
 end
 
+const ScatterAlignPose2InferenceType = ScatterAlignPose2
+
 """
 $(TYPEDEF)
 InferenceType for MixtureData.

src/navability/entities/Variable.jl

@@ -7,7 +7,9 @@ _variableTypeConvert = Dict{Symbol, String}(
     :Point2 => "RoME.Point2",
     :Pose2 => "RoME.Pose2",
     :ContinuousScalar => "IncrementalInference.ContinuousScalar",
-    # TBD - https://github.com/JuliaRobotics/Caesar.jl/discussions/810
+    # TBD - https://github.com/JuliaRobotics/Caesar.jl/discussions/807
+    :Position1 => "IncrementalInference.ContinuousScalar",
+    #TODO deprecate
     :Pose1 => "IncrementalInference.ContinuousScalar"
 )
 

src/navability/services/StandardAPI.jl

@@ -0,0 +1,103 @@
+## Wrap to standard API spec
+
+# Leaving as comment for future plans, see https://github.com/NavAbility/nva-sdk/issues/21
+# TODO What to use:
+# - NavAbilityPlatform
+# - NavAbilityConnection
+# - NavAbilityClientContext
+# - 
+# mutable struct NavAbilityPlatform{T}
+#     client
+#     context::T
+# end
+
+# function NavAbilityPlatform(;
+#             apiUrl::AbstractString="https://api.navability.io",
+#             UserId::AbstractString="[email protected]",
+#             RobotId::AbstractString=ENV["USER"],
+#             SessionId::AbstractString="Session_" * string(uuid4())[1:8])
+#     #
+#     client = NavAbilityHttpsClient(apiUrl)
+#     context = Client(UserId, RobotId, SessionId)
+#     return NavAbilityPlatform(client, context)
+# end
+
+"""
+  addVariable
+Add a variable to the NavAbility Platform service
+Example
+```julia
+addVariable(client, context, "x0", NVA.Pose2)
+```
+"""
+function addVariable(client,
+                     context,
+                     label::Union{<:AbstractString,Symbol},
+                     varType::Union{<:AbstractString,Symbol};
+                     tags::Vector{String}=String[],
+                     timestamp::String = string(now(Dates.UTC))*"Z")
+                    # TODO
+                    # solvable::Int=1
+                    # nanosecondtime,
+                    # smalldata,
+    #
+    union!(tags, ["VARIABLE"])
+    v = Variable(string(label), Symbol(varType), tags, timestamp)
+    return addVariable(client, context, v)
+end
+
+
+function assembleFactorName(xisyms::Union{Vector{String},Vector{Symbol}})
+    return string(xisyms...,"f_",string(uuid4())[1:4])
+end
+
+function getFncTypeName(fnc::InferenceType)
+    return split(string(typeof(fnc)),".")[end]
+end
+
+
+#TODO solverParams
+
+function addFactor(client,
+                   context,
+                   xisyms::Union{Vector{String},Vector{Symbol}},
+                   fnc::InferenceType;
+                   multihypo::Vector{Float64}=Float64[],
+                   nullhypo::Float64=0.0,
+                   solvable::Int=1,
+                   tags::Vector{String}=String[],
+                   # timestamp::Union{DateTime,ZonedDateTime}=now(localzone()), #TODO why timestamp difference from IIF 
+                   timestamp::String = string(now(Dates.UTC))*"Z",
+                   inflation::Real=3.0,
+                   namestring::String = assembleFactorName(xisyms),
+                   nstime::String="0"
+                  )
+                   # TODO maybe                 
+                   #  graphinit::Bool=true,
+                   #  threadmodel=SingleThreaded,
+                   #  suppressChecks::Bool=false,
+                   #  _blockRecursion::Bool=!getSolverParams(dfg).attemptGradients
+    #
+    # create factor data
+    factordata = FactorData(;fnc, multihypo, nullhypo, inflation)
+
+    fncType = getFncTypeName(fnc)
+
+    union!(tags, ["FACTOR"])
+    # create factor 
+    factor = Factor(
+        namestring,
+        nstime,
+        fncType,
+        string.(xisyms),
+        factordata,
+        solvable,
+        tags,
+        timestamp,
+        DFG_VERSION
+    )
+    # add factor
+    resultId = addFactor(client, context, factor)
+
+    return resultId
+end

test/integration/testStandardAPI.jl

@@ -0,0 +1,35 @@
+apiUrl = get(ENV,"API_URL","https://api.navability.io")
+userId = get(ENV,"USER_ID","[email protected]")
+robotId = get(ENV,"ROBOT_ID","IntegrationRobot")
+sessionId = get(ENV,"SESSION_ID","TestSession_"*string(uuid4())[1:8])
+
+@testset "nva-sdk-standard-api-testset" begin
+
+    client = NavAbilityHttpsClient(apiUrl)
+    context = Client(userId,robotId,sessionId)
+
+    addVariable(client, context,  "x0", :Pose2)
+    addFactor(client, context, ["x0"], NVA.PriorPose2(Z=FullNormal([0.,1.,0], diagm([1.,1,1]))))
+
+    addVariable(client, context,  :x1, :Pose2)
+    addFactor(client, context, [:x0,:x1], NVA.Pose2Pose2(Z=FullNormal([1.,0.,0], diagm([1.,1,1]))))
+
+    addFactor(client, context, [:x1], NVA.PriorPose2(Z=FullNormal([1.,0.,0], diagm([1.,1,1]))); nullhypo=0.1)
+
+    addFactor(client, context, ["x0"], NVA.PriorPose2(Z=FullNormal([0.5,0.5,0], diagm([1.,1,1]))))
+
+    addVariable(client, context,  :x2_a, :Pose2)
+    addVariable(client, context,  :x2_b, :Pose2)
+    addFactor(client, context, ["x1", "x2_a", "x2_b"], NVA.PriorPose2(Z=FullNormal([0.5,0.5,0], diagm([1.,1,1]))); multihypo=[1,0.1,0.9])
+
+    addVariable(client, context,  "y0", :Position1)
+    addFactor(client, context, ["y0"], NVA.Prior(Z=Normal(0.0, 1.0)))  
+
+
+    addVariable(client, context,  "z0", :Point2)
+    addFactor(client, context, ["z0"], NVA.PriorPoint2(Z=FullNormal([1.,0.], diagm([1.,1]))))
+
+    addVariable(client, context,  "z1", :Point2)
+    addFactor(client, context, ["z0", "z1"], NVA.Point2Point2(Z=FullNormal([1.,0.], diagm([1.,1]))))  
+
+end

test/runtests.jl

@@ -1,2 +1,5 @@
 include("./unit/runtests.jl")
-include("./integration/runtests.jl")
+include("./integration/runtests.jl")
+
+#TODO I'm not familiar with the tests yet, so just dumping it here to get us started.
+include("./integration/testStandardAPI.jl")