graph builder: + getPose()

source/armarx/navigation/server/GraphBuilder.cpp

@@ -226,6 +226,12 @@ namespace armarx::navigation::server
         return activeVertices_.front();
     }
 
+    core::Pose
+    GraphBuilder::goalPose() const
+    {
+        return getGraph().vertex(goalVertex()).attrib().getPose();
+    }
+
 
     // void
     // GraphBuilder::addEdge(const std::vector<core::Graph::VertexDescriptor>& sources,

source/armarx/navigation/server/GraphBuilder.h

@@ -65,6 +65,8 @@ namespace armarx::navigation::server
 
         core::Graph::VertexDescriptor goalVertex() const;
 
+        core::Pose goalPose() const;
+
     private:
         // void addEdge(const std::vector<core::Graph::VertexDescriptor>& sources,
         //              const std::vector<core::Graph::VertexDescriptor>& targets);

source/armarx/navigation/server/Navigator.cpp

@@ -462,12 +462,14 @@ namespace armarx::navigation::server
         auto startVertex = graphBuilder.startVertex;
         auto goalVertex = graphBuilder.getGraph().vertex(graphBuilder.goalVertex());
 
+        ARMARX_INFO << "Goal pose according to graph is " << graphBuilder.goalPose().matrix();
+
         ARMARX_CHECK(startVertex.has_value());
 
         ARMARX_TRACE;
 
-        goalReachedMonitor = GoalReachedMonitor(
-            goalVertex.attrib().getPose(), *config.scene, GoalReachedMonitorConfig());
+        goalReachedMonitor =
+            GoalReachedMonitor(graphBuilder.goalPose(), *config.scene, GoalReachedMonitorConfig());
 
         if (goalReachedMonitor->goalReached())
         {