Merge pull request #73 from JuliaRobotics/fearure/subgraph_dispatch

getSubGraphAroundNode conversions

Author: GearsAD

Project.toml

@@ -31,4 +31,4 @@ RoME = "91fb55c2-4c03-5a59-ba21-f4ea956187b8"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["Test"]
+test = ["Test", "RoME"]

src/LightGraphsDFG/services/LightGraphsDFG.jl

@@ -470,6 +470,22 @@ function getSubgraphAroundNode(dfg::LightGraphsDFG{P}, node::T, distance::Int64=
     return addToDFG
 end
 
+
+function getSubgraphAroundNode(dfg::LightGraphsDFG{<:AbstractParams}, node::DFGNode, distance::Int64=1, includeOrphanFactors::Bool=false, addToDFG::AbstractDFG=LightGraphsDFG{AbstractParams}())::AbstractDFG
+    if !haskey(dfg.g.metaindex[:label], node.label)
+        error("Variable/factor with label '$(node.label)' does not exist in the factor graph")
+    end
+
+    # Get a list of all unique neighbors inside 'distance'
+	ns = neighborhood(dfg.g, dfg.g[node.label,:label], distance)
+
+	# Copy the section of graph we want
+	_copyIntoGraph!(dfg, addToDFG, map(n->get_prop(dfg.g, n, :label), ns), includeOrphanFactors)
+	# _copyIntoGraph!(dfg, addToDFG, ns, includeOrphanFactors)
+    return addToDFG
+end
+
+
 """
     $(SIGNATURES)
 Get a deep subgraph copy from the DFG given a list of variables and factors.

src/services/AbstractDFG.jl

@@ -42,7 +42,9 @@ function _copyIntoGraph!(sourceDFG::G, destDFG::H, variableFactorLabels::Vector{
 
     # Now we have to add all variables first,
     for variable in sourceVariables
-        addVariable!(destDFG, deepcopy(variable))
+        if !exists(destDFG, variable)
+            addVariable!(destDFG, deepcopy(variable))
+        end
     end
     # And then all factors to the destDFG.
     for factor in sourceFactors
@@ -57,7 +59,9 @@ function _copyIntoGraph!(sourceDFG::G, destDFG::H, variableFactorLabels::Vector{
         end
         # Only if we have all of them should we add it (otherwise strange things may happen on evaluation)
         if includeOrphanFactors || length(factVariableIds) == length(sourceFactorVariableIds)
-            addFactor!(destDFG, factVariableIds, deepcopy(factor))
+            if !exists(destDFG, factor)
+                addFactor!(destDFG, factVariableIds, deepcopy(factor))
+            end
         end
     end
     return nothing

test/HexagonalLightGraphs.jl

@@ -1,12 +1,8 @@
-using Revise
-using Neo4j # So that DFG initializes the database driver.
 using RoME
 using DistributedFactorGraphs
-using Test
 
 # start with an empty factor graph object
-# fg = initfg()
-fg = LightGraphsDFG{SolverParams}(    solverParams=SolverParams())
+fg = LightGraphsDFG{SolverParams}(    params=SolverParams())
 
 # Add the first pose :x0
 x0 = addVariable!(fg, :x0, Pose2)

test/runtests.jl

@@ -12,6 +12,14 @@ for api in apis
     end
 end
 
+if !(get(ENV, "TRAVIS", "") == "true")
+    @testset "Local Testsets" begin
+        @testset "HexagonalLightGraphs" begin
+            include("HexagonalLightGraphs.jl")
+        end
+    end
+end
+
 # Test other interfaces that are not yet compatible for the general tests.
 # @testset "CloudGraphsDFG Drive: " begin
 #     include("cloudGraphsDFGTests.jl")