snapshot

source/armarx/navigation/core/Graph.cpp

@@ -143,6 +143,44 @@ namespace armarx::navigation
         ARMARX_CHECK_EQUAL(dto.edges.size(), bo.numEdges());
     }
 
+    core::Graph::ConstVertex
+    getVertexByName(const std::string& vertexName, const core::Graph& graph)
+    {
+        auto vertices = graph.vertices(); // begin() and end() are not const ...
+        const auto vertexIt =
+            std::find_if(vertices.begin(),
+                         vertices.end(),
+                         [&vertexName](const core::Graph::ConstVertex& vertex) -> bool
+                         { return vertex.attrib().getName() == vertexName; });
+
+        ARMARX_CHECK(vertexIt != vertices.end())
+            << "No vertex found with id `" << vertexName << "`";
+        return *vertexIt;
+    }
+
+    bool
+    core::hasVertex(const std::string& vertexName, const core::Graph& graph)
+    {
+        auto vertices = graph.vertices(); // begin() and end() are not const ...
+        return std::any_of(vertices.begin(),
+                           vertices.end(),
+                           [&vertexName](const core::Graph::ConstVertex& vertex) -> bool
+                           { return vertex.attrib().getName() == vertexName; });
+    }
+
+    const core::Graph&
+    core::getSubgraph(const std::string& vertexName, const core::Graphs& graphs)
+    {
+        auto graphIt = std::find_if(graphs.begin(),
+                                    graphs.end(),
+                                    [&vertexName](const core::Graph& graph) -> bool
+                                    { return hasVertex(vertexName, graph); });
+
+        ARMARX_CHECK(graphIt != graphs.end())
+            << "No subgraph found for vertex `" << vertexName << "`";
+        return *graphIt;
+    }
+
 
     void
     core::fromAron(const arondto::Graph& dto, Graph& bo)

source/armarx/navigation/core/Graph.h

@@ -62,6 +62,10 @@ namespace armarx::navigation::core
     using GraphPath = std::vector<Graph::ConstVertex>;
     std::vector<GraphPath> findPathsTo(Graph::ConstVertex vertex, const Graph& graph);
 
+    Graph::ConstVertex getVertexByName(const std::string& vertexName, const Graph& graph);
+
+    bool hasVertex(const std::string& vertexName, const Graph& graph);
+    const core::Graph& getSubgraph(const std::string& vertexName, const Graphs& graphs);
 
     void toAron(arondto::Graph& dto, const Graph& bo);
     void fromAron(const arondto::Graph& dto, Graph& bo);

source/armarx/navigation/core/test/coreTest.cpp

@@ -153,6 +153,4 @@ BOOST_AUTO_TEST_CASE(testGraphRoutes)
                                 [&gtPath, &isMatchingPath](const auto& path)
                                 { return isMatchingPath(path, gtPath); }));
     }
-
-    // TODO check if sequences match
 }

source/armarx/navigation/core/types.h

@@ -76,12 +76,8 @@ namespace armarx::navigation::core
 
     class TimeServerInterface;
 
-    class SceneGraph
+    struct SceneGraph
     {
-    public:
-        auto getSubgraph(const std::string& locationId);
-
-    private:
         core::Graphs subgraphs;
     };
 

source/armarx/navigation/server/Navigator.cpp

@@ -96,28 +96,28 @@ namespace armarx::navigation::server
         for (const auto& target : targets)
         {
             // if the last location was on a subgraph, that we are about to leave
-            const bool leavingSubgraph = [&]() -> bool
-            {
-                if (not activeSubgraph.has_value())
-                {
-                    return false;
-                }
-
-                // if a user specifies a pose, then this pose is not meant to be part of a graph
-                // -> can be reached directly
-                if (target.pose.has_value())
-                {
-                    return true;
-                }
-
-                if (not target.locationId->empty())
-                {
-                    const auto& subgraph = config.scene->graph->getSubgraph(target.locationId);
-                    return subgraph->getName() != activeSubgraph;
-                }
-
-                throw LocalException("this line should not be reachable");
-            }();
+            // const bool leavingSubgraph = [&]() -> bool
+            // {
+            //     if (not activeSubgraph.has_value())
+            //     {
+            //         return false;
+            //     }
+
+            //     // if a user specifies a pose, then this pose is not meant to be part of a graph
+            //     // -> can be reached directly
+            //     if (target.pose.has_value())
+            //     {
+            //         return true;
+            //     }
+
+            //     if (not target.locationId->empty())
+            //     {
+            //         const auto& subgraph = core::getSubgraph(target.locationId.value(),config.scene->graph->subgraphs);
+            //         return subgraph.name() != activeSubgraph;
+            //     }
+
+            //     throw LocalException("this line should not be reachable");
+            // }();
 
             // if(leavingSubgraph)
             // {
@@ -140,8 +140,12 @@ namespace armarx::navigation::server
             // instead. Thus, we collect all routes to reach the node.
             if (not target.locationId->empty())
             {
-                const auto& subgraph = config.scene->graphs->getSubgraph(target.locationId);
-                const auto routes = subgraph->getRoutesTo(target.locationId);
+                const auto& subgraph =
+                    core::getSubgraph(target.locationId.value(), config.scene->graph->subgraphs);
+
+                const auto vertex = core::getVertexByName(target.locationId, subgraph);
+                const auto routes = core::findPathsTo(vertex, subgraph);
+                // const auto routes = subgraph->getRoutesTo(target.locationId);
 
                 ARMARX_CHECK(not routes.empty()) << "The location `" << target.locationId
                                                  << "` is not a reachable vertex on the graph!";