compact export of graph

source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp

@@ -28,12 +28,15 @@
 #include <iostream>
 #include <iterator>
 
+#include <SimoxUtility/json/json.hpp>
+
 #include "ArmarXCore/core/PackagePath.h"
 #include "ArmarXCore/core/logging/Logging.h"
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 #include <ArmarXCore/core/time/CycleUtil.h>
 #include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
 #include "RobotAPI/libraries/armem/core/wm/aron_conversions.h"
 #include <RobotAPI/libraries/armem/client/query.h>
 #include <RobotAPI/libraries/armem/core/aron_conversions.h>
@@ -155,6 +158,9 @@ namespace armarx::navigation
                     armem::MemoryID providerID = workingMemory().id();
                     providerID.coreSegmentName = "Location";
                     providerID.providerSegmentName = providerName;
+
+                    const auto now = armem::Time::Now();
+
                     for (const auto& subdir : std::filesystem::directory_iterator(
                              location)) // iterate over all files in folder (the locations)
                     {
@@ -186,13 +192,9 @@ namespace armarx::navigation
                                 continue;
                             }
 
-                            std::vector<float> p = j.at("globalRobotPose");
-                            ARMARX_CHECK_EQUAL(p.size(), 16);
-
                             navigation::location::arondto::Location loc;
-                            loc.globalRobotPose << p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7],
-                                p[8], p[9], p[10], p[11], p[12], p[13], p[14],
-                                p[15]; // load the 4x4 matrix
+                            j.at("globalRobotPose")
+                                .get_to(loc.globalRobotPose); // load the 4x4 matrix
 
                             // TODO: All location I have seen were null.
                             // I don't know how this member should look like (von @Fabian Peller to @Fabian Reister)
@@ -201,10 +203,11 @@ namespace armarx::navigation
                             // send commit to memory
                             auto& up = c.add();
                             up.confidence = 1.0;
-                            up.timeCreated = armem::Time::Now();
-                            up.timeSent = armem::Time::Now();
-                            up.timeArrived = armem::Time::Now();
-                            up.entityID = providerID.withEntityName(location.filename().stem().string());
+                            up.timeCreated = now;
+                            up.timeSent = now;
+                            up.timeArrived = now;
+                            up.entityID =
+                                providerID.withEntityName(location.filename().stem().string());
                             up.instancesData = {loc.toAron()};
                         }
                     }
@@ -479,14 +482,29 @@ namespace armarx::navigation
 
             nlohmann::json j;
 
-            // std::vector<std::size_t> outgoingEdges;
-            // std::transform(graph.m_edges.begin(),
-            //                graph.m_edges.end(),
-            //                std::back_inserter(outgoingEdges),
-            //                [](const auto& edge) { return edge.m_target; });
+            // source -> target
+            std::vector<std::pair<std::size_t, std::size_t>> outgoingEdges;
+            std::transform(graph.m_edges.begin(),
+                           graph.m_edges.end(),
+                           std::back_inserter(outgoingEdges),
+                           [](const auto& edge)
+                           { return std::make_pair(edge.m_source, edge.m_target); });
 
-            j["edges"] = outgoingEdges; // TODO pairs of vertices as list
-            j["location"] = memoryId.getEntityID().str();
+            j["edges"] = outgoingEdges;
+            j["vertices"] = {};
+
+            for (const auto& vertex : graph.m_vertices)
+            {
+
+                armarx::armem::MemoryID locationId;
+                fromAron(vertex.m_property.aron.locationID, locationId);
+
+                nlohmann::json jVertex;
+                jVertex["locationID"] = locationId.getEntityID().str();
+                jVertex["vertexID"] = vertex.m_property.aron.vertexID;
+
+                j["vertices"].push_back(jVertex);
+            }
 
             // save to disk
             const std::filesystem::path filename = dir / (memoryId.entityName + ".json");
@@ -505,38 +523,54 @@ namespace armarx::navigation
         ARMARX_INFO << "Creating export directory `" << baseDirectory << "`.";
         std::filesystem::create_directories(baseDirectory);
 
+        // key: provider id
+        std::map<std::string, nlohmann::json> js;
+
+
         for (const auto& [memoryId, location] : locations)
         {
-            const std::filesystem::path nestedSubDir =
-                std::filesystem::path(memoryId.providerSegmentName) / memoryId.coreSegmentName;
-            const std::filesystem::path dir = baseDirectory / nestedSubDir;
+            auto& j = js[memoryId.coreSegmentName];
 
-            std::filesystem::create_directories(dir);
+            if (j.count("locations") == 0)
+            {
+                j["locations"] = nlohmann::json::array();
+            }
 
-            nlohmann::json j;
-            j["globalRobotPose"] = location.globalRobotPose;
+            nlohmann::json jLoc;
+
+            jLoc["globalRobotPose"] = location.globalRobotPose;
 
             if (location.relativeToObject)
             {
                 armarx::armem::MemoryID memoryId;
                 fromAron(location.relativeToObject->objectInstanceID, memoryId);
 
-                j["relativeToObject"]["objectInstanceID"] = memoryId.str();
-                j["relativeToObject"]["relativeRobotPose"] =
+                jLoc["relativeToObject"]["objectInstanceID"] = memoryId.str();
+                jLoc["relativeToObject"]["relativeRobotPose"] =
                     location.relativeToObject->relativeRobotPose;
             }
             else
             {
-                j["relativeToObject"] = "null";
+                jLoc["relativeToObject"] = "null";
             }
 
-            // save to disk
-            const std::filesystem::path filename = dir / (memoryId.entityName + ".json");
+            j["locations"].push_back(jLoc);
+        }
+
+        // save to disk
+        for (const auto& [providerId, j] : js)
+        {
+            const std::filesystem::path subDir = std::filesystem::path(providerId);
+            const std::filesystem::path dir = baseDirectory / subDir;
+            std::filesystem::create_directories(dir);
+
+            const std::filesystem::path filename = dir / "locations.json";
             ARMARX_VERBOSE << "Saving file `" << filename << "`.";
             std::ofstream ofs(filename);
             ofs << std::setw(4) << j;
         }
 
+
         return true;
     }