Feature/location graph export

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

@@ -142,7 +142,8 @@ namespace armarx::navigation
                 // This section loads the snapshot specified by the scenario parameters
                 // resolve the paths for the locations and graphs
                 const std::filesystem::path graph = snapshotToLoadPath / "Graph";
-                const std::filesystem::path location = snapshotToLoadPath / "Location";
+                const std::filesystem::path locationsFilename =
+                    snapshotToLoadPath / "locations.json";
 
                 // remove date from folder name (if present)
                 // Sometimes, we use the date before the snapshotname and use a symlink to the real data (e.g. R003 and 2022-03-01_R003)
@@ -152,7 +153,7 @@ namespace armarx::navigation
 
                 // This if statement loads the location. Each location is a single file (without extension). The filename is the name of the location.
                 // The file contains json with the globalRobotPose (4x4 matrix) and relativeToObject information
-                if (std::filesystem::is_directory(location))
+                if (std::filesystem::is_regular_file(locationsFilename))
                 {
                     armem::Commit c;
                     armem::MemoryID providerID = workingMemory().id();
@@ -161,54 +162,53 @@ namespace armarx::navigation
 
                     const auto now = armem::Time::Now();
 
-                    for (const auto& subdir : std::filesystem::directory_iterator(
-                             location)) // iterate over all files in folder (the locations)
                     {
-                        const std::filesystem::path location = subdir.path();
                         if (std::filesystem::is_regular_file(
-                                location)) // check if its a file (otherwise skip)
+                                locationsFilename)) // check if its a file (otherwise skip)
                         {
-                            ARMARX_VERBOSE << "Loading " << location;
-                            std::ifstream ifs(location);
+                            ARMARX_VERBOSE << "Loading " << locationsFilename;
+                            std::ifstream ifs(locationsFilename);
                             const std::string content((std::istreambuf_iterator<char>(ifs)),
                                                       (std::istreambuf_iterator<char>()));
 
                             // parse location as json. All files in Location folder must be valid json objects!
-                            nlohmann::json j = nlohmann::json::parse(content);
+                            nlohmann::json js = nlohmann::json::parse(content);
 
-                            if (j.find("globalRobotPose") == j.end())
+                            for (const auto& [locationMemoryIdStr, j] : js["locations"].get<std::map<std::string, nlohmann::json>>())
                             {
-                                ARMARX_WARNING
-                                    << "The file '" << location.string()
-                                    << "' has no 'globalRobotPose' member. Skipping this file.";
-                                continue;
-                            }
-
-                            if (j.find("relativeToObject") == j.end())
-                            {
-                                ARMARX_WARNING
-                                    << "The file '" << location.string()
-                                    << "' has no 'relativeToObject' member. Skipping this file.";
-                                continue;
-                            }
-
-                            navigation::location::arondto::Location loc;
-                            j.at("globalRobotPose")
-                                .get_to(loc.globalRobotPose); // load the 4x4 matrix
+                                if (j.find("globalRobotPose") == j.end())
+                                {
+                                    ARMARX_WARNING
+                                        << "The element '" << locationMemoryIdStr
+                                        << "' has no 'globalRobotPose' member. Skipping this entity.";
+                                    continue;
+                                }
 
-                            // TODO: All location I have seen were null.
-                            // I don't know how this member should look like (von @Fabian Peller to @Fabian Reister)
-                            loc.relativeToObject = std::nullopt;
+                                if (j.find("relativeToObject") == j.end())
+                                {
+                                    ARMARX_WARNING << "The element '" << locationMemoryIdStr
+                                                   << "' has no 'relativeToObject' member. "
+                                                      "Skipping this entity.";
+                                    continue;
+                                }
 
-                            // send commit to memory
-                            auto& up = c.add();
-                            up.confidence = 1.0;
-                            up.timeCreated = now;
-                            up.timeSent = now;
-                            up.timeArrived = now;
-                            up.entityID =
-                                providerID.withEntityName(location.filename().stem().string());
-                            up.instancesData = {loc.toAron()};
+                                navigation::location::arondto::Location loc;
+                                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)
+                                loc.relativeToObject = std::nullopt;
+
+                                // send commit to memory
+                                auto& up = c.add();
+                                up.confidence = 1.0;
+                                up.timeCreated = now;
+                                up.timeSent = now;
+                                up.timeArrived = now;
+                                up.entityID = armarx::armem::MemoryID(locationMemoryIdStr);
+                                up.instancesData = {loc.toAron()};
+                            }
                         }
                     }
 
@@ -563,7 +563,7 @@ namespace armarx::navigation
             const std::filesystem::path subDir = std::filesystem::path(providerId);
             const std::filesystem::path dir = baseDirectory / subDir;
 
-            if(not std::filesystem::exists(dir))
+            if (not std::filesystem::exists(dir))
             {
                 std::filesystem::create_directories(dir);
             }