Skip to content
Snippets Groups Projects
Commit c60c1287 authored by Fabian Reister's avatar Fabian Reister
Browse files

loading new locations.json

parent 2d4103ff
No related branches found
No related tags found
1 merge request!62Feature/location graph export
This commit is part of merge request !62. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+ 40
− 40
View file @ c60c1287
...@@ -142,7 +142,8 @@ namespace armarx::navigation ...@@ -142,7 +142,8 @@ namespace armarx::navigation
// This section loads the snapshot specified by the scenario parameters // This section loads the snapshot specified by the scenario parameters
// resolve the paths for the locations and graphs // resolve the paths for the locations and graphs
const std::filesystem::path graph = snapshotToLoadPath / "Graph"; 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) // 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) // 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 ...@@ -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. // 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 // 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::Commit c;
armem::MemoryID providerID = workingMemory().id(); armem::MemoryID providerID = workingMemory().id();
...@@ -161,54 +162,53 @@ namespace armarx::navigation ...@@ -161,54 +162,53 @@ namespace armarx::navigation
const auto now = armem::Time::Now(); 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( 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; ARMARX_VERBOSE << "Loading " << locationsFilename;
std::ifstream ifs(location); std::ifstream ifs(locationsFilename);
const std::string content((std::istreambuf_iterator<char>(ifs)), const std::string content((std::istreambuf_iterator<char>(ifs)),
(std::istreambuf_iterator<char>())); (std::istreambuf_iterator<char>()));
// parse location as json. All files in Location folder must be valid json objects! // 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 if (j.find("globalRobotPose") == j.end())
<< "The file '" << location.string() {
<< "' has no 'globalRobotPose' member. Skipping this file."; ARMARX_WARNING
continue; << "The element '" << locationMemoryIdStr
} << "' has no 'globalRobotPose' member. Skipping this entity.";
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
// TODO: All location I have seen were null. if (j.find("relativeToObject") == j.end())
// I don't know how this member should look like (von @Fabian Peller to @Fabian Reister) {
loc.relativeToObject = std::nullopt; ARMARX_WARNING << "The element '" << locationMemoryIdStr
<< "' has no 'relativeToObject' member. "
"Skipping this entity.";
continue;
}
// send commit to memory navigation::location::arondto::Location loc;
auto& up = c.add(); j.at("globalRobotPose")
up.confidence = 1.0; .get_to(loc.globalRobotPose); // load the 4x4 matrix
up.timeCreated = now;
up.timeSent = now; // TODO: All location I have seen were null.
up.timeArrived = now; // I don't know how this member should look like (von @Fabian Peller to @Fabian Reister)
up.entityID = loc.relativeToObject = std::nullopt;
providerID.withEntityName(location.filename().stem().string());
up.instancesData = {loc.toAron()}; // 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 ...@@ -563,7 +563,7 @@ namespace armarx::navigation
const std::filesystem::path subDir = std::filesystem::path(providerId); const std::filesystem::path subDir = std::filesystem::path(providerId);
const std::filesystem::path dir = baseDirectory / subDir; const std::filesystem::path dir = baseDirectory / subDir;
if(not std::filesystem::exists(dir)) if (not std::filesystem::exists(dir))
{ {
std::filesystem::create_directories(dir); std::filesystem::create_directories(dir);
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment