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

fix

parent 59898290
No related branches found
No related tags found
1 merge request!18Feature/costmap to memory
Hide whitespace changes
Inline Side-by-side
source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+ 53
− 26
View file @ bf004ffd
......@@ -23,21 +23,24 @@
#include "NavigationMemory.h"
// STD / STL
#include <iostream>
#include <fstream>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/armem/client/query.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>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#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>
#include <armarx/navigation/core/aron/Location.aron.generated.h>
......@@ -98,7 +101,11 @@ namespace armarx::navigation
// (Requies the armarx::DebugObserverComponentPluginUser.)
// setDebugObserverBatchModeEnabled(true);
workingMemory().addCoreSegment("Parameterization");
workingMemory().addCoreSegment(
navigation::memory::constants::ParameterizationCoreSegmentName);
workingMemory().addCoreSegment(navigation::memory::constants::CostmapCoreSegmentName,
algorithms::arondto::Costmap::ToAronType());
workingMemory().addCoreSegment("Results_GlobalPlanner",
navigation::core::arondto::Trajectory::ToAronType());
......@@ -121,7 +128,8 @@ namespace armarx::navigation
if (not properties.snapshotToLoad.empty())
{
std::filesystem::path snapshotToLoadPath(ArmarXDataPath::resolvePath(properties.snapshotToLoad));
std::filesystem::path snapshotToLoadPath(
ArmarXDataPath::resolvePath(properties.snapshotToLoad));
if (std::filesystem::is_directory(snapshotToLoadPath))
{
// This section loads the snapshot specified by the scenario parameters
......@@ -133,7 +141,7 @@ namespace armarx::navigation
// Sometimes, we use the date before the snapshotname and use a symlink to the real data (e.g. R003 and 2022-03-01_R003)
auto splitted = simox::alg::split(snapshotToLoadPath.filename().string(), "_");
ARMARX_CHECK_GREATER(splitted.size(), 0);
const std::string providerName = splitted[splitted.size()-1];
const std::string providerName = splitted[splitted.size() - 1];
// 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
......@@ -143,26 +151,33 @@ namespace armarx::navigation
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)
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)
if (std::filesystem::is_regular_file(
location)) // check if its a file (otherwise skip)
{
std::ifstream ifs(location);
const std::string content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
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);
if (j.find("globalRobotPose") == j.end())
{
ARMARX_WARNING << "The file '" << location.string() << "' has no 'globalRobotPose' member. Skipping this file.";
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.";
ARMARX_WARNING
<< "The file '" << location.string()
<< "' has no 'relativeToObject' member. Skipping this file.";
continue;
}
......@@ -170,10 +185,9 @@ namespace armarx::navigation
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
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
// 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,35 +215,44 @@ namespace armarx::navigation
{
armem::Commit c;
armem::MemoryID providerID = workingMemory().id();
providerID.coreSegmentName = "Graph";
providerID.coreSegmentName = memory::constants::GraphCoreSegmentName;
providerID.providerSegmentName = providerName;
for (const auto& graphdir : std::filesystem::directory_iterator(graph)) // iterate over the different graphs (subfolders)
for (const auto& graphdir : std::filesystem::directory_iterator(
graph)) // iterate over the different graphs (subfolders)
{
const std::filesystem::path singleGraph = graphdir.path();
if (std::filesystem::is_directory(singleGraph)) // assure that its a folder. otherwise skip
if (std::filesystem::is_directory(
singleGraph)) // assure that its a folder. otherwise skip
{
navigation::core::arondto::Graph g;
for (const auto& subdir : std::filesystem::directory_iterator(singleGraph)) // iterate over all files in the graph
for (const auto& subdir : std::filesystem::directory_iterator(
singleGraph)) // iterate over all files in the graph
{
const std::filesystem::path vertice = subdir.path();
if (std::filesystem::is_regular_file(vertice)) // assure its a file. otherwise skip
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>()));
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.";
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.";
ARMARX_WARNING << "The file '" << vertice.string()
<< "' has no 'outgoingEdges' member. "
"Skipping this file.";
continue;
}
......@@ -237,7 +260,9 @@ namespace armarx::navigation
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_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;
......@@ -268,7 +293,8 @@ namespace armarx::navigation
up.timeCreated = armem::Time::Now();
up.timeSent = armem::Time::Now();
up.timeArrived = armem::Time::Now();
up.entityID = providerID.withEntityName(singleGraph.filename().string());
up.entityID =
providerID.withEntityName(singleGraph.filename().string());
up.instancesData = {g.toAron()};
}
}
......@@ -287,7 +313,8 @@ namespace armarx::navigation
}
else
{
ARMARX_WARNING << "Could not load NavigationMemory '" << properties.snapshotToLoad << "'. Continue with empty memory.";
ARMARX_WARNING << "Could not load NavigationMemory '" << properties.snapshotToLoad
<< "'. Continue with empty memory.";
}
}
}
......
source/armarx/navigation/graph/constants.h
+ 4
− 1
View file @ bf004ffd
......@@ -23,8 +23,11 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include "armarx/navigation/memory/constants.h"
namespace armarx::navigation::graph
{
inline const armem::MemoryID coreSegmentID{"Navigation", "Graph"};
inline const armem::MemoryID coreSegmentID{memory::constants::NavigationMemoryName,
memory::constants::GraphCoreSegmentName};
} // namespace armarx::navigation::graph
source/armarx/navigation/location/constants.h
+ 3
− 1
View file @ bf004ffd
......@@ -23,8 +23,10 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include "armarx/navigation/memory/constants.h"
namespace armarx::navigation::location
{
inline const armem::MemoryID coreSegmentID{"Navigation", "Location"};
inline const armem::MemoryID coreSegmentID{memory::constants::NavigationMemoryName,
memory::constants::LocationCoreSegmentName};
}
source/armarx/navigation/memory/constants.h
+ 1
− 0
View file @ bf004ffd
......@@ -30,6 +30,7 @@ namespace armarx::navigation::memory::constants
inline const std::string ParameterizationCoreSegmentName = "Parameterization";
inline const std::string EventsCoreSegmentName = "Events";
inline const std::string GraphCoreSegmentName = "Graph";
inline const std::string LocationCoreSegmentName = "Location";
inline const std::string CostmapCoreSegmentName = "Costmap";
} // namespace armarx::navigation::memory::constants
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