Skip to content
Snippets Groups Projects

Feature/location graph export

Merged Feature/location graph export
feature/location-graph-export into master
Merged Fabian Reister requested to merge feature/location-graph-export into master
Viewing commit 2d4103ff
Prev Next
Show latest version
1 file
+ 8
− 4
Compare changes
  • Side-by-side
  • Inline
source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+ 285
− 111
@@ -23,15 +23,24 @@
#include "NavigationMemory.h"
// STD / STL
#include <cstddef>
#include <filesystem>
#include <fstream>
#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>
#include <RobotAPI/libraries/armem/core/operations.h>
#include <RobotAPI/libraries/armem/server/ltm/disk/Memory.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
@@ -39,7 +48,6 @@
#include "Visu.h"
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
@@ -48,7 +56,7 @@
#include <armarx/navigation/core/aron/Twist.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/location/constants.h>
#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation
{
@@ -135,7 +143,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)
@@ -145,62 +154,63 @@ 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();
providerID.coreSegmentName = "Location";
providerID.providerSegmentName = providerName;
for (const auto& subdir : std::filesystem::directory_iterator(
location)) // iterate over all files in folder (the locations)
const auto now = armem::Time::Now();
{
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)
{
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;
}
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
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 = armem::Time::Now();
up.timeSent = armem::Time::Now();
up.timeArrived = armem::Time::Now();
up.entityID = providerID.withEntityName(location.filename().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()};
}
}
}
@@ -208,7 +218,9 @@ namespace armarx::navigation
iceAdapter().commit(c);
}
// Next we load the graphs. The graph folder may contain several graphs, represented by different folders.
const auto now = armem::Time::Now();
// Next we load the graphs. The graph folder may contain several graphs, represented by different json files.
// Each of those graphs contains a list of files representing the vertices. The filename is the vertice id (ideally starting at 0).
// The file contains a json with the corresponding location name (as path) and the adjacent vertives (representing the directed outgoing edges).
if (std::filesystem::is_directory(graph))
@@ -219,82 +231,58 @@ namespace armarx::navigation
providerID.providerSegmentName = providerName;
for (const auto& graphdir : std::filesystem::directory_iterator(
graph)) // iterate over the different graphs (subfolders)
graph)) // iterate over the different graphs (json files)
{
const std::filesystem::path singleGraph = graphdir.path();
if (std::filesystem::is_directory(
singleGraph)) // assure that its a folder. otherwise skip
if (std::filesystem::is_regular_file(
graphdir.path())) // assure that its a json file
{
const std::string graphName = graphdir.path().stem();
navigation::core::arondto::Graph g;
for (const auto& subdir : std::filesystem::directory_iterator(
singleGraph)) // iterate over all files in the graph
ARMARX_VERBOSE << "Loading graph `" << graphName << " `from file `"
<< graphdir.path() << "`.";
std::ifstream ifs(graphdir.path());
const std::string content((std::istreambuf_iterator<char>(ifs)),
(std::istreambuf_iterator<char>()));
nlohmann::json j = nlohmann::json::parse(content);
const auto& jEdges = j.at("edges");
const auto& jVertices = j.at("vertices");
for (const auto& jVertex : jVertices)
{
const std::filesystem::path vertice = subdir.path();
if (std::filesystem::is_regular_file(
vertice)) // assure its a file. otherwise skip
{
std::ifstream ifs(vertice);
const std::string content((std::istreambuf_iterator<char>(ifs)),
(std::istreambuf_iterator<char>()));
// parse vertice. Each vertice must be a valid json object
nlohmann::json j = nlohmann::json::parse(content);
if (j.find("location") == j.end())
{
ARMARX_WARNING
<< "The file '" << vertice.string()
<< "' has no 'location' member. Skipping this file.";
continue;
}
if (j.find("outgoingEdges") == j.end())
{
ARMARX_WARNING << "The file '" << vertice.string()
<< "' has no 'outgoingEdges' member. "
"Skipping this file.";
continue;
}
std::string location = j.at("location");
int id = std::stoi(vertice.filename());
auto split = simox::alg::split(location, "/");
ARMARX_CHECK_EQUAL(
split.size(),
4); // the location is always a path like Navigation/Location/XXX/YYY
armarx::navigation::core::arondto::Vertex v;
v.vertexID = id;
v.locationID.memoryName = split[0];
v.locationID.coreSegmentName = split[1];
v.locationID.providerSegmentName = split[2];
v.locationID.entityName = split[3];
toAron(v.locationID.timestamp, armem::Time::Invalid());
v.locationID.instanceIndex = 0;
g.vertices.push_back(v);
// add edges of this vertice to graph
std::vector<float> edges = j.at("outgoingEdges");
for (const auto edge_id : edges)
{
armarx::navigation::core::arondto::Edge e;
e.sourceVertexID = id;
e.targetVertexID = edge_id;
g.edges.push_back(e);
}
}
const armarx::armem::MemoryID vertexLocationMemoryId(
jVertex.at("locationID"));
armarx::navigation::core::arondto::Vertex v;
v.vertexID = jVertex.at("vertexID");
toAron(v.locationID, vertexLocationMemoryId);
toAron(v.locationID.timestamp, armem::Time::Invalid());
v.locationID.instanceIndex = 0;
g.vertices.push_back(v);
}
for (const auto& jEdge : jEdges)
{
std::pair<std::int64_t, std::int64_t> edgeSourceTargetPair;
jEdge.get_to(edgeSourceTargetPair);
armarx::navigation::core::arondto::Edge e;
e.sourceVertexID = edgeSourceTargetPair.first;
e.targetVertexID = edgeSourceTargetPair.second;
g.edges.push_back(e);
}
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(singleGraph.filename().string());
up.timeCreated = now;
up.timeSent = now;
up.timeArrived = now;
up.entityID = providerID.withEntityName(graphName);
up.instancesData = {g.toAron()};
}
}
@@ -457,5 +445,191 @@ namespace armarx::navigation
}
}
bool
store(const std::map<armem::MemoryID, core::Graph>& graphs,
const std::filesystem::path& baseDirectory)
{
ARMARX_INFO << "Creating export directory `" << baseDirectory << "`.";
std::filesystem::create_directories(baseDirectory);
for (const auto& [memoryId, graph] : graphs)
{
const std::filesystem::path nestedSubDir =
std::filesystem::path(memoryId.providerSegmentName) / memoryId.coreSegmentName;
const std::filesystem::path dir = baseDirectory / nestedSubDir;
std::filesystem::create_directories(dir);
nlohmann::json j;
// 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;
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");
ARMARX_VERBOSE << "Saving file `" << filename << "`.";
std::ofstream ofs(filename);
ofs << std::setw(4) << j;
}
return true;
}
bool
store(const std::map<armem::MemoryID, location::arondto::Location>& locations,
const std::filesystem::path& baseDirectory)
{
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)
{
auto& j = js[memoryId.providerSegmentName];
if (j.count("locations") == 0)
{
j["locations"] = {};
}
nlohmann::json jLoc;
jLoc["globalRobotPose"] = location.globalRobotPose;
if (location.relativeToObject)
{
armarx::armem::MemoryID memoryId;
fromAron(location.relativeToObject->objectInstanceID, memoryId);
jLoc["relativeToObject"]["objectInstanceID"] = memoryId.str();
jLoc["relativeToObject"]["relativeRobotPose"] =
location.relativeToObject->relativeRobotPose;
}
else
{
jLoc["relativeToObject"] = "null";
}
j["locations"][memoryId.getEntityID().str()] = 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;
if (not std::filesystem::exists(dir))
{
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;
}
bool
NavigationMemory::storeLocationGraph(const armarx::data::PackagePath& packagePath,
const Ice::Current& current)
{
armem::server::wm::CoreSegment& locationCoreSegment =
workingMemory().getCoreSegment(navigation::location::coreSegmentID);
armem::server::wm::CoreSegment& graphCoreSegment =
workingMemory().getCoreSegment(navigation::graph::coreSegmentID);
// obtain locations and graphs
const std::map<armem::MemoryID, location::arondto::Location> locations =
[&locationCoreSegment]()
{
std::map<armem::MemoryID, location::arondto::Location> locations;
locationCoreSegment.doLocked(
[&]()
{
locationCoreSegment.forEachEntity(
[&](const armarx::armem::server::wm::Entity& entity) -> bool
{
if (const armarx::armem::server::wm::EntityInstance* instance =
entity.findLatestInstance())
{
locations[entity.id()].fromAron(instance->data());
}
return true;
});
return true;
});
return locations;
}();
const auto graphs = [&graphCoreSegment]()
{
std::map<armem::MemoryID, core::Graph> graphs;
graphCoreSegment.doLocked(
[&]()
{
graphCoreSegment.forEachEntity(
[&](const armarx::armem::server::wm::Entity& entity) -> bool
{
core::Graph& graph = graphs[entity.id()];
if (const armarx::armem::server::wm::EntityInstance* instance =
entity.findLatestInstance())
{
navigation::core::arondto::Graph aron;
aron.fromAron(instance->data());
fromAron(aron, graph);
}
return true;
});
});
return graphs;
}();
// store on disk
const std::filesystem::path baseDirectory = armarx::PackagePath(packagePath).toSystemPath();
store(locations, baseDirectory);
store(graphs, baseDirectory);
return true;
}
} // namespace armarx::navigation