cleanup; + logging

source/armarx/navigation/components/Navigator/Navigator.cpp

@@ -161,6 +161,8 @@ namespace armarx::navigation::components
 
 
         initialized = true;
+
+        ARMARX_INFO << "Initialized. Will now respond to navigation requests.";
     }
 
     void

source/armarx/navigation/server/GraphBuilder.cpp

@@ -3,12 +3,12 @@
 #include <cstddef>
 #include <optional>
 
-#include "ArmarXCore/util/CPPUtility/trace.h"
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
 
 #include <SemanticObjectRelations/Shapes/Shape.h>
 #include <armarx/navigation/client/PathBuilder.h>
 #include <armarx/navigation/core/Graph.h>
-#include <mongocxx/validation_criteria.hpp>
 
 namespace armarx::navigation::server
 {
@@ -110,8 +110,10 @@ namespace armarx::navigation::server
                                       vertices.end(),
                                       [&attributes](const core::Graph::ConstVertex& vertex)
                                       { return vertex.attrib() == attributes; });
+
         if (vIt != vertices.end()) // vertex exists
         {
+            ARMARX_DEBUG << "Found matching vertex `" << vIt->attrib().getName() << "`";
             return vIt->objectID();
         }
 
@@ -139,7 +141,6 @@ namespace armarx::navigation::server
                 std::vector<semrel::ShapeID> routeShapeIds;
 
                 ARMARX_TRACE;
-                getGraph(); // FIXME remove
 
                 // add all vertices to the graph
                 for (const auto& vertex : route)
@@ -156,7 +157,6 @@ namespace armarx::navigation::server
                 }
 
                 ARMARX_TRACE;
-                getGraph(); // FIXME remove
 
                 // add all edges to the graph (between consecutive vertices)
                 for (size_t i = 0; i < route.size() - 1; i++)
@@ -168,16 +168,10 @@ namespace armarx::navigation::server
                 }
 
                 ARMARX_TRACE;
-                getGraph(); // FIXME remove
-
                 // connect `vertex` an this path
                 graph.addEdge(graph.vertex(vertex).objectID(),
                               routeShapeIds.front(),
                               core::Graph::EdgeAttrib{.strategy = strategy});
-
-
-                ARMARX_TRACE;
-                getGraph(); // FIXME remove
             }
         }
 
@@ -193,6 +187,8 @@ namespace armarx::navigation::server
         const std::optional<semrel::ShapeID> shapeId =
             getVertexByAttributes(routes.front().back().attrib());
         ARMARX_CHECK(shapeId.has_value()) << "Vertex exists. This should not have happened";
+
+
         activeVertices_ = {shapeId.value()};
     }
 

source/armarx/navigation/server/Navigator.cpp

@@ -357,14 +357,6 @@ namespace armarx::navigation::server
         GraphBuilder graphBuilder;
         graphBuilder.initialize(core::Pose(config.scene->robot->getGlobalPose()));
 
-        // from goal to start do the following:
-        // -
-
-        // FIXME remove the following implicit check
-        ARMARX_TRACE;
-        graphBuilder.getGraph();
-        ARMARX_TRACE;
-
         // std::optional<Graph*> activeSubgraph;
         for (const auto& target : targets)
         {
@@ -406,12 +398,6 @@ namespace armarx::navigation::server
             if (target.pose.has_value())
             {
                 graphBuilder.connect(target.pose.value(), target.strategy);
-
-                // FIXME remove the following implicit check
-                ARMARX_TRACE;
-                graphBuilder.getGraph();
-                ARMARX_TRACE;
-
                 continue;
             }
 
@@ -431,6 +417,9 @@ namespace armarx::navigation::server
                 const std::vector<core::GraphPath> routes = core::findPathsTo(vertex, subgraph);
                 // const auto routes = subgraph->getRoutesTo(target.locationId);
 
+                ARMARX_INFO << "Found " << routes.size() << " routes to location `"
+                            << target.locationId.value();
+
                 ARMARX_CHECK(not routes.empty()) << "The location `" << target.locationId.value()
                                                  << "` is not a reachable vertex on the graph!";
 
@@ -438,17 +427,15 @@ namespace armarx::navigation::server
                 graphBuilder.connect(routes,
                                      target.strategy); // TODO: all routes to the same target
 
-                // FIXME remove the following implicit check
-                ARMARX_TRACE;
-                graphBuilder.getGraph();
-                ARMARX_TRACE;
-
                 continue;
             }
 
             ARMARX_ERROR << "Either `location_id` or `pose` has to be provided!";
         }
 
+        const auto goalVertex = graphBuilder.getGraph().vertex(graphBuilder.goalVertex());
+        ARMARX_INFO << "Goal vertex is `" << goalVertex.attrib().getName() << "`";
+
         return graphBuilder;
     }