From cb92fcf27c74597d452cde67af29bc9a52b52dd9 Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit>
Date: Wed, 9 Nov 2022 14:31:42 +0100
Subject: [PATCH 1/8] location graph export draft
---
.../NavigationMemory/CMakeLists.txt | 5 ++
.../NavigationMemory/ComponentInterface.ice | 47 +++++++++++++++++++
.../NavigationMemory/NavigationMemory.cpp | 10 ++++
.../NavigationMemory/NavigationMemory.h | 12 +++--
4 files changed, 71 insertions(+), 3 deletions(-)
create mode 100644 source/armarx/navigation/components/NavigationMemory/ComponentInterface.ice
diff --git a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
index 06bd4080..01196c14 100644
--- a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
+++ b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
@@ -1,4 +1,9 @@
armarx_add_component(navigation_memory
+ ICE_DEPENDENCIES
+ ArmarXCoreInterfaces
+ RobotAPIInterfaces
+ ICE_FILES
+ ComponentInterface.ice
DEPENDENCIES
# ArmarXCore
ArmarXCore
diff --git a/source/armarx/navigation/components/NavigationMemory/ComponentInterface.ice b/source/armarx/navigation/components/NavigationMemory/ComponentInterface.ice
new file mode 100644
index 00000000..b7c4d6d9
--- /dev/null
+++ b/source/armarx/navigation/components/NavigationMemory/ComponentInterface.ice
@@ -0,0 +1,47 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package RobotAPI
+* @author Rainer Kartmann
+* @copyright 2020 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice>
+#include <ArmarXCore/interface/core/PackagePath.ice>
+// #include <ArmarXCore/interface/core/PackagePath.h>
+
+
+module armarx
+{
+ module navigation
+ {
+ module memory
+ {
+ interface NavigationMemoryInterface extends
+ armarx::armem::server::MemoryInterface
+ {
+ bool storeLocationGraph(armarx::data::PackagePath packagePath);
+ };
+
+ };
+ };
+};
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index c3bec2c6..dd8b20b0 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -457,5 +457,15 @@ namespace armarx::navigation
}
}
+ bool NavigationMemory::storeLocationGraph(const armarx::data::PackagePath& packagePath, const Ice::Current& current)
+ {
+ const auto& locationCoreSegment = workingMemory().getCoreSegment(navigation::location::coreSegmentID);
+ workingMemory().getCoreSegment(navigation::graph::coreSegmentID)};
+
+
+ return true;
+ }
+
+
} // namespace armarx::navigation
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
index 11bd41fd..e6464265 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
@@ -25,6 +25,7 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/interface/core/PackagePath.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
@@ -34,6 +35,8 @@
#include <mutex>
+#include <armarx/navigation/components/NavigationMemory/ComponentInterface.h>
+
namespace armarx::navigation
{
@@ -50,17 +53,20 @@ namespace armarx::navigation
* Detailed description of class NavigationMemory.
*/
class NavigationMemory :
- virtual public armarx::Component
+ virtual public armarx::Component,
// , virtual public armarx::DebugObserverComponentPluginUser
- ,
virtual public armarx::LightweightRemoteGuiComponentPluginUser,
virtual public armarx::ArVizComponentPluginUser,
- virtual public armarx::armem::server::ReadWritePluginUser
+ virtual public armarx::armem::server::ReadWritePluginUser,
+ virtual public memory::NavigationMemoryInterface
{
public:
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
+ bool storeLocationGraph(const armarx::data::PackagePath& packagePath, const Ice::Current& current) override;
+
+
protected:
/// @see PropertyUser::createPropertyDefinitions()
--
GitLab
From b977bbde27bee29830dc11e98ce8df6444b7e960 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 9 Nov 2022 20:37:43 +0100
Subject: [PATCH 2/8] successfully exported location graph. still wip
---
.../NavigationMemory/NavigationMemory.cpp | 158 +++++++++++++++++-
.../LocationGraphEditor/CMakeLists.txt | 1 +
.../LocationGraphEditorWidget.ui | 52 ++++++
.../LocationGraphEditor/WidgetController.cpp | 15 ++
.../LocationGraphEditor/WidgetController.h | 1 +
5 files changed, 221 insertions(+), 6 deletions(-)
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index dd8b20b0..c0e62cb7 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -26,12 +26,17 @@
#include <filesystem>
#include <fstream>
#include <iostream>
+#include <iterator>
+#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/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 +44,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 +52,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
{
@@ -158,6 +162,7 @@ namespace armarx::navigation
if (std::filesystem::is_regular_file(
location)) // check if its a file (otherwise skip)
{
+ ARMARX_VERBOSE << "Loading " << location;
std::ifstream ifs(location);
const std::string content((std::istreambuf_iterator<char>(ifs)),
(std::istreambuf_iterator<char>()));
@@ -199,7 +204,7 @@ namespace armarx::navigation
up.timeCreated = armem::Time::Now();
up.timeSent = armem::Time::Now();
up.timeArrived = armem::Time::Now();
- up.entityID = providerID.withEntityName(location.filename().string());
+ up.entityID = providerID.withEntityName(location.filename().stem().string());
up.instancesData = {loc.toAron()};
}
}
@@ -457,15 +462,156 @@ namespace armarx::navigation
}
}
- bool NavigationMemory::storeLocationGraph(const armarx::data::PackagePath& packagePath, const Ice::Current& current)
+ bool
+ store(const std::map<armem::MemoryID, core::Graph>& graphs,
+ const std::filesystem::path& baseDirectory)
{
- const auto& locationCoreSegment = workingMemory().getCoreSegment(navigation::location::coreSegmentID);
- workingMemory().getCoreSegment(navigation::graph::coreSegmentID)};
+ 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;
+
+ // 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; });
+
+ j["edges"] = outgoingEdges; // TODO pairs of vertices as list
+ j["location"] = memoryId.getEntityID().str();
+
+ // 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);
+
+ 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;
+
+ std::filesystem::create_directories(dir);
+
+ nlohmann::json j;
+ j["globalRobotPose"] = location.globalRobotPose;
+
+ if (location.relativeToObject)
+ {
+ armarx::armem::MemoryID memoryId;
+ fromAron(location.relativeToObject->objectInstanceID, memoryId);
+
+ j["relativeToObject"]["objectInstanceID"] = memoryId.str();
+ j["relativeToObject"]["relativeRobotPose"] =
+ location.relativeToObject->relativeRobotPose;
+ }
+ else
+ {
+ j["relativeToObject"] = "null";
+ }
+ // 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
+ 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
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
index e12c2b6a..c8cd2932 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
@@ -56,4 +56,5 @@ armarx_add_qt_plugin(qt_plugin_LocationGraphEditor
armarx_navigation::location
armarx_navigation::graph
+ armarx_navigation::navigation_memory_ice
)
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
index db72fa0c..7173033c 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
@@ -120,6 +120,58 @@
</layout>
</widget>
</item>
+ <item>
+ <widget class="QGroupBox" name="groupBox">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>200</height>
+ </size>
+ </property>
+ <property name="title">
+ <string>Export</string>
+ </property>
+ <widget class="QLineEdit" name="packageNameEdit">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>30</y>
+ <width>113</width>
+ <height>28</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>PriorKnowledgeData</string>
+ </property>
+ </widget>
+ <widget class="QLineEdit" name="packageDirectoryEdit">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>80</y>
+ <width>113</width>
+ <height>28</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>navigation-graphs</string>
+ </property>
+ </widget>
+ <widget class="QPushButton" name="exportButton">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>140</y>
+ <width>84</width>
+ <height>28</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Export</string>
+ </property>
+ </widget>
+ </widget>
+ </item>
<item>
<widget class="QGroupBox" name="robotVisuGroupBox">
<property name="title">
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.cpp
index 33c40dfc..b59ac4cf 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.cpp
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/PackagePath.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
@@ -40,11 +41,13 @@
#include "widgets/graph_scene/Scene.h"
#include "widgets/graph_scene/Widget.h"
#include "widgets/utils.h"
+#include <armarx/navigation/components/NavigationMemory/ComponentInterface.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
#include <armarx/navigation/location/constants.h>
+#include <armarx/navigation/components/NavigationMemory/ComponentInterface.h>
// Qt headers
#include <QDialog>
@@ -165,8 +168,20 @@ namespace armarx::navigation::qt_plugins::location_graph_editor
&This::removeEdgesOfVertex);
connect(view.edgeTable, &EdgeTableWidget::edgeRemovalRequested, this, &This::removeEdges);
+
+ // export
+ connect(widget.exportButton, &QPushButton::clicked, this, &This::exportLocationGraph);
}
+ void WidgetController::exportLocationGraph()
+ {
+ auto navigationMemoryPrx =
+ getProxy<memory::NavigationMemoryInterfacePrx>("NavigationMemory");
+
+ const ::armarx::PackagePath packagePath(widget.packageNameEdit->text().toStdString(), widget.packageDirectoryEdit->text().toStdString());
+
+ navigationMemoryPrx->storeLocationGraph(packagePath.serialize());
+ }
WidgetController::~WidgetController()
{
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.h
index 080b9617..f8f6e97d 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.h
@@ -201,6 +201,7 @@ namespace armarx::navigation::qt_plugins::location_graph_editor
QString getGraphDisplayName(const armem::MemoryID& entityID, bool changed = false) const;
+ void exportLocationGraph();
private:
/// Widget Form
--
GitLab
From 53616cd5a05e1d4f952977ad1606cb8c9711aa89 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 10 Nov 2022 10:03:49 +0100
Subject: [PATCH 3/8] compact export of graph
---
.../NavigationMemory/NavigationMemory.cpp | 90 +++++++++++++------
1 file changed, 62 insertions(+), 28 deletions(-)
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index c0e62cb7..e5eabda3 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/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;
}
--
GitLab
From 2d4103ffc64a024a8c9cbad3c24cd3ec4caffaf3 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 10 Nov 2022 10:14:35 +0100
Subject: [PATCH 4/8] location export
---
.../components/NavigationMemory/NavigationMemory.cpp | 12 ++++++++----
1 file changed, 8 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index e5eabda3..9f70d948 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -529,11 +529,11 @@ namespace armarx::navigation
for (const auto& [memoryId, location] : locations)
{
- auto& j = js[memoryId.coreSegmentName];
+ auto& j = js[memoryId.providerSegmentName];
if (j.count("locations") == 0)
{
- j["locations"] = nlohmann::json::array();
+ j["locations"] = {};
}
nlohmann::json jLoc;
@@ -554,7 +554,7 @@ namespace armarx::navigation
jLoc["relativeToObject"] = "null";
}
- j["locations"].push_back(jLoc);
+ j["locations"][memoryId.getEntityID().str()] = jLoc;
}
// save to disk
@@ -562,7 +562,11 @@ namespace armarx::navigation
{
const std::filesystem::path subDir = std::filesystem::path(providerId);
const std::filesystem::path dir = baseDirectory / subDir;
- std::filesystem::create_directories(dir);
+
+ if(not std::filesystem::exists(dir))
+ {
+ std::filesystem::create_directories(dir);
+ }
const std::filesystem::path filename = dir / "locations.json";
ARMARX_VERBOSE << "Saving file `" << filename << "`.";
--
GitLab
From c60c1287ba3545d6c2695a8b9255d5eec485ecae Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 10 Nov 2022 10:26:48 +0100
Subject: [PATCH 5/8] loading new locations.json
---
.../NavigationMemory/NavigationMemory.cpp | 80 +++++++++----------
1 file changed, 40 insertions(+), 40 deletions(-)
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index 9f70d948..c3cfba42 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/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);
}
--
GitLab
From 6d651aca3478495edf98976b36743a1c11d989df Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 10 Nov 2022 10:48:45 +0100
Subject: [PATCH 6/8] successfully loaded graph from new file
---
.../config/VisionMemory.cfg | 109 +---
.../config/navigation_memory.cfg | 4 +-
.../PlatformNavigation/config/navigator.cfg | 255 ++++----
.../NavigationMemory/NavigationMemory.cpp | 606 +++++++++---------
4 files changed, 438 insertions(+), 536 deletions(-)
diff --git a/scenarios/PlatformNavigation/config/VisionMemory.cfg b/scenarios/PlatformNavigation/config/VisionMemory.cfg
index 22e0d3e6..1b95447f 100644
--- a/scenarios/PlatformNavigation/config/VisionMemory.cfg
+++ b/scenarios/PlatformNavigation/config/VisionMemory.cfg
@@ -210,21 +210,12 @@
# ArmarX.VisionMemory.mem.MemoryName = Vision
-# ArmarX.VisionMemory.mem.ltm..buffer.storeFreq: Frequency to store the buffer to the LTM in Hz.
+# ArmarX.VisionMemory.mem.ltm.configuration:
# Attributes:
-# - Default: 10
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.VisionMemory.mem.ltm..buffer.storeFreq = 10
-
-
-# ArmarX.VisionMemory.mem.ltm.depthImageExtractor.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm.depthImageExtractor.Enabled = true
+# ArmarX.VisionMemory.mem.ltm.configuration = {}
# ArmarX.VisionMemory.mem.ltm.enabled:
@@ -236,100 +227,6 @@
# ArmarX.VisionMemory.mem.ltm.enabled = false
-# ArmarX.VisionMemory.mem.ltm.exrConverter.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm.exrConverter.Enabled = true
-
-
-# ArmarX.VisionMemory.mem.ltm.imageExtractor.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm.imageExtractor.Enabled = true
-
-
-# ArmarX.VisionMemory.mem.ltm.memFreqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm.memFreqFilter.Enabled = false
-
-
-# ArmarX.VisionMemory.mem.ltm.memFreqFilter.WaitingTime: Waiting time in MS after each LTM update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.mem.ltm.memFreqFilter.WaitingTime = -1
-
-
-# ArmarX.VisionMemory.mem.ltm.pngConverter.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm.pngConverter.Enabled = true
-
-
-# ArmarX.VisionMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
-# Attributes:
-# - Default: 1024
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
-
-
-# ArmarX.VisionMemory.mem.ltm.snapEqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm.snapEqFilter.Enabled = false
-
-
-# ArmarX.VisionMemory.mem.ltm.snapEqFilter.MaxWaitingTime: Max Waiting time in MS after each Entity update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
-
-
-# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.Enabled = false
-
-
-# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.WaitingTime: Waiting time in MS after each Entity update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
-
-
-# ArmarX.VisionMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.mem.ltm.storagepath = Default value not mapped.
-
-
# ArmarX.VisionMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
# Set to false to use this memory as a stand-alone.
# Attributes:
diff --git a/scenarios/PlatformNavigation/config/navigation_memory.cfg b/scenarios/PlatformNavigation/config/navigation_memory.cfg
index f808f777..8945e9ca 100644
--- a/scenarios/PlatformNavigation/config/navigation_memory.cfg
+++ b/scenarios/PlatformNavigation/config/navigation_memory.cfg
@@ -217,7 +217,7 @@
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graphs/audimax-science-week-opening
+ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graphs/R003
# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
@@ -285,6 +285,6 @@ ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graph
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
+ArmarX.Verbosity = Verbose
diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg
index f6a497b2..c94f4489 100644
--- a/scenarios/PlatformNavigation/config/navigator.cfg
+++ b/scenarios/PlatformNavigation/config/navigator.cfg
@@ -92,298 +92,323 @@
# ArmarX.LoggingGroup = ""
-# ArmarX.Navigator.ArVizStorageName: Name of the ArViz storage
+# ArmarX.Navigator.ObjectName: No Description
# Attributes:
-# - Default: ArVizStorage
-# - Case sensitivity: yes
+# - Default: navigator
+# - Case sensitivity: no
# - Required: no
-# ArmarX.Navigator.ArVizStorageName = ArVizStorage
+ArmarX.Navigator.ObjectName = navigator
-# ArmarX.Navigator.ArVizTopicName: Name of the ArViz topic
+# ArmarX.Navigator.RobotUnitName: No Description
# Attributes:
-# - Default: ArVizTopic
-# - Case sensitivity: yes
+# - Default: Armar6Unit
+# - Case sensitivity: no
+# - Required: no
+ArmarX.Navigator.RobotUnitName = Armar6Unit
+
+
+# ArmarX.Navigator.cmp.PlatformUnit: No Description
+# Attributes:
+# - Default: Armar6PlatformUnit
+# - Case sensitivity: no
# - Required: no
-# ArmarX.Navigator.ArVizTopicName = ArVizTopic
+ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
-# ArmarX.Navigator.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.Navigator.p.occupancy_grid.occopied_threshold: No Description
# Attributes:
-# - Default: false
+# - Default: 0.8
+# - Case sensitivity: no
+# - Required: no
+ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.Navigator.EnableProfiling = false
+# ArmarX.RedirectStdout = true
-# ArmarX.Navigator.MinimumLoggingLevel: Local logging level only for this component
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
# Attributes:
-# - Default: Undefined
+# - Default: 3000
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Navigator.MinimumLoggingLevel = Undefined
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
-# ArmarX.Navigator.ObjectMemoryName: Name of the object memory.
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
-# - Default: ObjectMemory
+# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.ObjectMemoryName = ObjectMemory
+# ArmarX.SecondsStartupDelay = 0
-# ArmarX.Navigator.ObjectName: Name of IceGrid well-known object
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
# Attributes:
-# - Default: ""
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-ArmarX.Navigator.ObjectName = navigator
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
-# ArmarX.Navigator.RobotUnitName: Name of the RobotUnit
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
# Attributes:
+# - Default: 1
# - Case sensitivity: yes
-# - Required: yes
-ArmarX.Navigator.RobotUnitName = Armar6Unit
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
-# ArmarX.Navigator.cmp.PlatformUnit: No Description
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
# Attributes:
-# - Default: Armar6PlatformUnit
-# - Case sensitivity: no
+# - Default: ""
+# - Case sensitivity: yes
# - Required: no
-ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
+# ArmarX.TopicSuffix = ""
-# ArmarX.Navigator.cmp.RemoteGui: Ice object name of the `RemoteGui` component.
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
# Attributes:
-# - Default: RemoteGuiProvider
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.cmp.RemoteGui = RemoteGuiProvider
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
-# ArmarX.Navigator.mem.nav.costmap.CoreSegment:
+# ArmarX.Verbosity: Global logging level for whole application
# Attributes:
-# - Default: Costmap
+# - Default: Info
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.costmap.CoreSegment = Costmap
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+ArmarX.Verbosity = Verbose
-# ArmarX.Navigator.mem.nav.costmap.Memory:
+# ArmarX.navigator.ArVizStorageName: Name of the ArViz storage
# Attributes:
-# - Default: Navigation
+# - Default: ArVizStorage
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.costmap.Memory = Navigation
+# ArmarX.navigator.ArVizStorageName = ArVizStorage
-# ArmarX.Navigator.mem.nav.costmap.Provider: Name of this provider
+# ArmarX.navigator.ArVizTopicName: Name of the ArViz topic
# Attributes:
-# - Default: ""
+# - Default: ArVizTopic
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.costmap.Provider = ""
+# ArmarX.navigator.ArVizTopicName = ArVizTopic
-# ArmarX.Navigator.mem.nav.events.CoreSegment:
+# ArmarX.navigator.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
-# - Default: Events
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigator.EnableProfiling = false
+
+
+# ArmarX.navigator.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.events.CoreSegment = Events
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.navigator.MinimumLoggingLevel = Undefined
-# ArmarX.Navigator.mem.nav.events.Memory:
+# ArmarX.navigator.ObjectMemoryName: Name of the object memory.
# Attributes:
-# - Default: Navigation
+# - Default: ObjectMemory
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.events.Memory = Navigation
+# ArmarX.navigator.ObjectMemoryName = ObjectMemory
-# ArmarX.Navigator.mem.nav.events.Provider: Name of this provider
+# ArmarX.navigator.ObjectName: Name of IceGrid well-known object
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.events.Provider = ""
+# ArmarX.navigator.ObjectName = ""
-# ArmarX.Navigator.mem.nav.graph.CoreSegment:
+# ArmarX.navigator.RobotUnitName: Name of the RobotUnit
# Attributes:
-# - Default: Location
# - Case sensitivity: yes
-# - Required: no
-# ArmarX.Navigator.mem.nav.graph.CoreSegment = Location
+# - Required: yes
+# ArmarX.navigator.RobotUnitName = ::_NOT_SET_::
-# ArmarX.Navigator.mem.nav.graph.Memory:
+# ArmarX.navigator.cmp.RemoteGui: Ice object name of the `RemoteGui` component.
# Attributes:
-# - Default: Navigation
+# - Default: RemoteGuiProvider
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.graph.Memory = Navigation
+# ArmarX.navigator.cmp.RemoteGui = RemoteGuiProvider
-# ArmarX.Navigator.mem.nav.param.CoreSegment:
+# ArmarX.navigator.mem.nav.costmap.CoreSegment:
# Attributes:
-# - Default: Parameterization
+# - Default: Costmap
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.param.CoreSegment = Parameterization
+# ArmarX.navigator.mem.nav.costmap.CoreSegment = Costmap
-# ArmarX.Navigator.mem.nav.param.Memory:
+# ArmarX.navigator.mem.nav.costmap.Memory:
# Attributes:
# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.param.Memory = Navigation
+# ArmarX.navigator.mem.nav.costmap.Memory = Navigation
-# ArmarX.Navigator.mem.nav.param.Provider: Name of this provider
+# ArmarX.navigator.mem.nav.events.CoreSegment:
# Attributes:
-# - Default: ""
+# - Default: Events
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.param.Provider = ""
+# ArmarX.navigator.mem.nav.events.CoreSegment = Events
-# ArmarX.Navigator.mem.nav.stack_result.CoreSegment:
+# ArmarX.navigator.mem.nav.events.Memory:
# Attributes:
-# - Default: ""
+# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.stack_result.CoreSegment = ""
+# ArmarX.navigator.mem.nav.events.Memory = Navigation
-# ArmarX.Navigator.mem.nav.stack_result.Memory:
+# ArmarX.navigator.mem.nav.events.Provider: Name of this provider
# Attributes:
-# - Default: Navigation
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.stack_result.Memory = Navigation
+# ArmarX.navigator.mem.nav.events.Provider = ""
-# ArmarX.Navigator.mem.nav.stack_result.Provider: Name of this provider
+# ArmarX.navigator.mem.nav.graph.CoreSegment:
# Attributes:
-# - Default: ""
+# - Default: Location
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.stack_result.Provider = ""
+# ArmarX.navigator.mem.nav.graph.CoreSegment = Location
-# ArmarX.Navigator.mem.robot_state.Memory:
+# ArmarX.navigator.mem.nav.graph.Memory:
# Attributes:
-# - Default: RobotState
+# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.robot_state.Memory = RobotState
+# ArmarX.navigator.mem.nav.graph.Memory = Navigation
-# ArmarX.Navigator.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# ArmarX.navigator.mem.nav.param.CoreSegment:
# Attributes:
-# - Default: Localization
+# - Default: Parameterization
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.robot_state.localizationSegment = Localization
+# ArmarX.navigator.mem.nav.param.CoreSegment = Parameterization
-# ArmarX.Navigator.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
+# ArmarX.navigator.mem.nav.param.Memory:
# Attributes:
-# - Default: true
+# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.Navigator.mns.MemoryNameSystemEnabled = true
+# ArmarX.navigator.mem.nav.param.Memory = Navigation
-# ArmarX.Navigator.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# ArmarX.navigator.mem.nav.param.Provider: Name of this provider
# Attributes:
-# - Default: MemoryNameSystem
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.navigator.mem.nav.param.Provider = ""
-# ArmarX.Navigator.p.occupancy_grid.occopied_threshold: Threshold for each cell to be considered occupied. Increase this value to reduce noise.
+# ArmarX.navigator.mem.nav.stack_result.CoreSegment:
# Attributes:
-# - Default: 0.550000012
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
+# ArmarX.navigator.mem.nav.stack_result.CoreSegment = ""
-# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# ArmarX.navigator.mem.nav.stack_result.Memory:
# Attributes:
-# - Default: true
+# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RedirectStdout = true
+# ArmarX.navigator.mem.nav.stack_result.Memory = Navigation
-# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# ArmarX.navigator.mem.nav.stack_result.Provider: Name of this provider
# Attributes:
-# - Default: 3000
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RemoteHandlesDeletionTimeout = 3000
+# ArmarX.navigator.mem.nav.stack_result.Provider = ""
-# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# ArmarX.navigator.mem.robot_state.Memory:
# Attributes:
-# - Default: 0
+# - Default: RobotState
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SecondsStartupDelay = 0
+# ArmarX.navigator.mem.robot_state.Memory = RobotState
-# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# ArmarX.navigator.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
# Attributes:
-# - Default: false
+# - Default: Localization
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.StartDebuggerOnCrash = false
+# ArmarX.navigator.mem.robot_state.localizationSegment = Localization
-# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# ArmarX.navigator.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
# Attributes:
-# - Default: 1
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ThreadPoolSize = 1
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigator.mns.MemoryNameSystemEnabled = true
-# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# ArmarX.navigator.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: ""
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# ArmarX.TopicSuffix = ""
+# ArmarX.navigator.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# ArmarX.navigator.p.disableExecutor: If the executor is disabled, the navigator will only plan the trajectory but won't execute it.
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.UseTimeServer = false
+# ArmarX.navigator.p.disableExecutor = false
-# ArmarX.Verbosity: Global logging level for whole application
+# ArmarX.navigator.p.occupancy_grid.occopied_threshold: Threshold for each cell to be considered occupied. Increase this value to reduce noise.
# Attributes:
-# - Default: Info
+# - Default: 0.550000012
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-ArmarX.Verbosity = Verbose
+# ArmarX.navigator.p.occupancy_grid.occopied_threshold = 0.550000012
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index c3cfba42..d9930d23 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -23,6 +23,7 @@
#include "NavigationMemory.h"
// STD / STL
+#include <cstddef>
#include <filesystem>
#include <fstream>
#include <iostream>
@@ -174,13 +175,14 @@ namespace armarx::navigation
// parse location as json. All files in Location folder must be valid json objects!
nlohmann::json js = nlohmann::json::parse(content);
- for (const auto& [locationMemoryIdStr, j] : js["locations"].get<std::map<std::string, nlohmann::json>>())
+ for (const auto& [locationMemoryIdStr, j] :
+ js["locations"].get<std::map<std::string, nlohmann::json>>())
{
if (j.find("globalRobotPose") == j.end())
{
- ARMARX_WARNING
- << "The element '" << locationMemoryIdStr
- << "' has no 'globalRobotPose' member. Skipping this entity.";
+ ARMARX_WARNING << "The element '" << locationMemoryIdStr
+ << "' has no 'globalRobotPose' member. Skipping "
+ "this entity.";
continue;
}
@@ -216,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))
@@ -227,114 +231,90 @@ 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()};
- }
}
-
- // send graph to memory
- iceAdapter().commit(c);
}
- // LEGACY CODE (Not working anymore since the wm json export changed due to ltm updates and aron updates)
- // armem::wm::Memory memory = armem::server::ltm::disk::load(path.value());
- //armem::server::ltm::disk::Memory ltm(path.value());
- //armem::wm::Memory memory;
- //ltm.loadAll(memory);
- //workingMemory().update(armem::toCommit(memory));
- ARMARX_INFO << "Loaded NavigationMemory '" << properties.snapshotToLoad << "'";
- }
- else
- {
- ARMARX_WARNING << "Could not load NavigationMemory '" << properties.snapshotToLoad
- << "'. Continue with empty memory.";
+ // send graph to memory
+ iceAdapter().commit(c);
}
+
+ // LEGACY CODE (Not working anymore since the wm json export changed due to ltm updates and aron updates)
+ // armem::wm::Memory memory = armem::server::ltm::disk::load(path.value());
+ //armem::server::ltm::disk::Memory ltm(path.value());
+ //armem::wm::Memory memory;
+ //ltm.loadAll(memory);
+ //workingMemory().update(armem::toCommit(memory));
+ ARMARX_INFO << "Loaded NavigationMemory '" << properties.snapshotToLoad << "'";
+ }
+ else
+ {
+ ARMARX_WARNING << "Could not load NavigationMemory '" << properties.snapshotToLoad
+ << "'. Continue with empty memory.";
}
}
+}
- void
- NavigationMemory::onConnectComponent()
- {
- // Do things after connecting to topics and components.
+void
+NavigationMemory::onConnectComponent()
+{
+ // Do things after connecting to topics and components.
- /* (Requies the armarx::DebugObserverComponentPluginUser.)
+ /* (Requies the armarx::DebugObserverComponentPluginUser.)
// Use the debug observer to log data over time.
// The data can be viewed in the ObserverView and the LivePlotter.
// (Before starting any threads, we don't need to lock mutexes.)
@@ -345,311 +325,311 @@ namespace armarx::navigation
}
*/
- // Setup the remote GUI.
- {
- createRemoteGuiTab();
- RemoteGui_startRunningTask();
- }
-
- tasks.visuTask = new SimpleRunningTask<>([this]() { this->visuRun(); }, "Visualization");
- tasks.visuTask->start();
+ // Setup the remote GUI.
+ {
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
}
+ tasks.visuTask = new SimpleRunningTask<>([this]() { this->visuRun(); }, "Visualization");
+ tasks.visuTask->start();
+}
- void
- NavigationMemory::onDisconnectComponent()
- {
- }
+void
+NavigationMemory::onDisconnectComponent()
+{
+}
- void
- NavigationMemory::onExitComponent()
- {
- }
+void
+NavigationMemory::onExitComponent()
+{
+}
- std::string
- NavigationMemory::getDefaultName() const
- {
- return "NavigationMemory";
- }
+std::string
+NavigationMemory::getDefaultName() const
+{
+ return "NavigationMemory";
+}
- void
- NavigationMemory::createRemoteGuiTab()
- {
- using namespace armarx::RemoteGui::Client;
- // Setup the widgets.
- tab.locationGraph.setup(*this);
+void
+NavigationMemory::createRemoteGuiTab()
+{
+ using namespace armarx::RemoteGui::Client;
+ // Setup the widgets.
+ tab.locationGraph.setup(*this);
- // Setup the layout.
- VBoxLayout root = {tab.locationGraph.group, VSpacer()};
- RemoteGui_createTab(getName(), root, &tab);
- }
+ // Setup the layout.
+ VBoxLayout root = {tab.locationGraph.group, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+}
- void
- NavigationMemory::RemoteGui_update()
- {
- tab.locationGraph.update(*this);
- }
+void
+NavigationMemory::RemoteGui_update()
+{
+ tab.locationGraph.update(*this);
+}
- void
- NavigationMemory::RemoteGuiTab::LocationGraph::setup(NavigationMemory& owner)
- {
- using namespace armarx::RemoteGui::Client;
- GridLayout grid;
- int row = 0;
- {
- visuLocations.setValue(owner.properties.locationGraph.visuLocations);
- visuGraphEdges.setValue(owner.properties.locationGraph.visuLocations);
- grid.add(Label("Visualize Locations"), {row, 0}).add(visuLocations, {row, 1});
- ++row;
+void
+NavigationMemory::RemoteGuiTab::LocationGraph::setup(NavigationMemory& owner)
+{
+ using namespace armarx::RemoteGui::Client;
+ GridLayout grid;
+ int row = 0;
+ {
+ visuLocations.setValue(owner.properties.locationGraph.visuLocations);
+ visuGraphEdges.setValue(owner.properties.locationGraph.visuLocations);
- grid.add(Label("Visualize Graph Edges"), {row, 0}).add(visuGraphEdges, {row, 1});
- ++row;
- }
+ grid.add(Label("Visualize Locations"), {row, 0}).add(visuLocations, {row, 1});
+ ++row;
- group = GroupBox({grid});
+ grid.add(Label("Visualize Graph Edges"), {row, 0}).add(visuGraphEdges, {row, 1});
+ ++row;
}
+ group = GroupBox({grid});
+}
- void
- NavigationMemory::RemoteGuiTab::LocationGraph::update(NavigationMemory& owner)
+
+void
+NavigationMemory::RemoteGuiTab::LocationGraph::update(NavigationMemory& owner)
+{
+ if (visuLocations.hasValueChanged() or visuGraphEdges.hasValueChanged())
{
- if (visuLocations.hasValueChanged() or visuGraphEdges.hasValueChanged())
- {
- std::scoped_lock lock(owner.propertiesMutex);
- owner.properties.locationGraph.visuLocations = visuLocations.getValue();
- owner.properties.locationGraph.visuGraphEdges = visuGraphEdges.getValue();
- }
+ std::scoped_lock lock(owner.propertiesMutex);
+ owner.properties.locationGraph.visuLocations = visuLocations.getValue();
+ owner.properties.locationGraph.visuGraphEdges = visuGraphEdges.getValue();
}
+}
- void
- NavigationMemory::visuRun()
+void
+NavigationMemory::visuRun()
+{
+ memory::Visu visu{arviz,
+ workingMemory().getCoreSegment(navigation::location::coreSegmentID),
+ workingMemory().getCoreSegment(navigation::graph::coreSegmentID)};
+
+ Properties::LocationGraph p;
{
- memory::Visu visu{arviz,
- workingMemory().getCoreSegment(navigation::location::coreSegmentID),
- workingMemory().getCoreSegment(navigation::graph::coreSegmentID)};
+ std::scoped_lock lock(propertiesMutex);
+ p = properties.locationGraph;
+ }
- Properties::LocationGraph p;
+ CycleUtil cycle(static_cast<int>(1000 / p.visuFrequency));
+ while (tasks.visuTask and not tasks.visuTask->isStopped())
+ {
{
std::scoped_lock lock(propertiesMutex);
p = properties.locationGraph;
}
- CycleUtil cycle(static_cast<int>(1000 / p.visuFrequency));
- while (tasks.visuTask and not tasks.visuTask->isStopped())
- {
- {
- std::scoped_lock lock(propertiesMutex);
- p = properties.locationGraph;
- }
-
- std::vector<viz::Layer> layers;
+ std::vector<viz::Layer> layers;
- // Locations
- visu.drawLocations(layers, p.visuLocations);
+ // Locations
+ visu.drawLocations(layers, p.visuLocations);
- // Graph Edges
- visu.drawGraphs(layers, p.visuGraphEdges);
+ // Graph Edges
+ visu.drawGraphs(layers, p.visuGraphEdges);
- arviz.commit(layers);
+ arviz.commit(layers);
- cycle.waitForCycleDuration();
- }
+ cycle.waitForCycleDuration();
}
+}
- 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;
+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);
- std::filesystem::create_directories(dir);
+ 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;
- nlohmann::json j;
+ std::filesystem::create_directories(dir);
- // 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); });
+ nlohmann::json j;
- j["edges"] = outgoingEdges;
- j["vertices"] = {};
+ // 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); });
- for (const auto& vertex : graph.m_vertices)
- {
+ j["edges"] = outgoingEdges;
+ j["vertices"] = {};
- armarx::armem::MemoryID locationId;
- fromAron(vertex.m_property.aron.locationID, locationId);
+ for (const auto& vertex : graph.m_vertices)
+ {
- nlohmann::json jVertex;
- jVertex["locationID"] = locationId.getEntityID().str();
- jVertex["vertexID"] = vertex.m_property.aron.vertexID;
+ armarx::armem::MemoryID locationId;
+ fromAron(vertex.m_property.aron.locationID, locationId);
- j["vertices"].push_back(jVertex);
- }
+ nlohmann::json jVertex;
+ jVertex["locationID"] = locationId.getEntityID().str();
+ jVertex["vertexID"] = vertex.m_property.aron.vertexID;
- // 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;
+ j["vertices"].push_back(jVertex);
}
- return true;
+ // 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;
}
- 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);
+ return true;
+}
- // key: provider id
- std::map<std::string, nlohmann::json> js;
+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"] = {};
- }
+ for (const auto& [memoryId, location] : locations)
+ {
+ auto& j = js[memoryId.providerSegmentName];
- nlohmann::json jLoc;
+ if (j.count("locations") == 0)
+ {
+ j["locations"] = {};
+ }
- jLoc["globalRobotPose"] = location.globalRobotPose;
+ nlohmann::json jLoc;
- if (location.relativeToObject)
- {
- armarx::armem::MemoryID memoryId;
- fromAron(location.relativeToObject->objectInstanceID, memoryId);
+ jLoc["globalRobotPose"] = location.globalRobotPose;
- jLoc["relativeToObject"]["objectInstanceID"] = memoryId.str();
- jLoc["relativeToObject"]["relativeRobotPose"] =
- location.relativeToObject->relativeRobotPose;
- }
- else
- {
- jLoc["relativeToObject"] = "null";
- }
+ if (location.relativeToObject)
+ {
+ armarx::armem::MemoryID memoryId;
+ fromAron(location.relativeToObject->objectInstanceID, memoryId);
- j["locations"][memoryId.getEntityID().str()] = jLoc;
+ jLoc["relativeToObject"]["objectInstanceID"] = memoryId.str();
+ jLoc["relativeToObject"]["relativeRobotPose"] =
+ location.relativeToObject->relativeRobotPose;
}
-
- // save to disk
- for (const auto& [providerId, j] : js)
+ else
{
- const std::filesystem::path subDir = std::filesystem::path(providerId);
- const std::filesystem::path dir = baseDirectory / subDir;
+ jLoc["relativeToObject"] = "null";
+ }
- if (not std::filesystem::exists(dir))
- {
- std::filesystem::create_directories(dir);
- }
+ j["locations"][memoryId.getEntityID().str()] = jLoc;
+ }
- const std::filesystem::path filename = dir / "locations.json";
- ARMARX_VERBOSE << "Saving file `" << filename << "`.";
- std::ofstream ofs(filename);
- ofs << std::setw(4) << j;
- }
+ // 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);
+ }
- return true;
+ const std::filesystem::path filename = dir / "locations.json";
+ ARMARX_VERBOSE << "Saving file `" << filename << "`.";
+ std::ofstream ofs(filename);
+ ofs << std::setw(4) << j;
}
- 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);
+ return true;
+}
- // 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
+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())
{
- if (const armarx::armem::server::wm::EntityInstance* instance =
- entity.findLatestInstance())
- {
- locations[entity.id()].fromAron(instance->data());
- }
+ locations[entity.id()].fromAron(instance->data());
+ }
- return true;
- });
+ return true;
+ });
- return true;
- });
+ return true;
+ });
- return locations;
- }();
+ 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
+ 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())
{
- 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);
- }
+ navigation::core::arondto::Graph aron;
+ aron.fromAron(instance->data());
+ fromAron(aron, graph);
+ }
- return true;
- });
- });
+ return true;
+ });
+ });
- return graphs;
- }();
+ return graphs;
+ }();
- // store on disk
+ // store on disk
- const std::filesystem::path baseDirectory = armarx::PackagePath(packagePath).toSystemPath();
+ const std::filesystem::path baseDirectory = armarx::PackagePath(packagePath).toSystemPath();
- store(locations, baseDirectory);
- store(graphs, baseDirectory);
+ store(locations, baseDirectory);
+ store(graphs, baseDirectory);
- return true;
- }
+ return true;
+}
} // namespace armarx::navigation
--
GitLab
From 05daa7b76eeec0d366e9bf9c99d4447970c3b0ed Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 10 Nov 2022 10:59:34 +0100
Subject: [PATCH 7/8] formatting file
---
.../NavigationMemory/NavigationMemory.cpp | 482 +++++++++---------
1 file changed, 241 insertions(+), 241 deletions(-)
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index d9930d23..d000113f 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -284,37 +284,37 @@ namespace armarx::navigation
up.timeArrived = now;
up.entityID = providerID.withEntityName(graphName);
up.instancesData = {g.toAron()};
+ }
}
+
+ // send graph to memory
+ iceAdapter().commit(c);
}
- // send graph to memory
- iceAdapter().commit(c);
+ // LEGACY CODE (Not working anymore since the wm json export changed due to ltm updates and aron updates)
+ // armem::wm::Memory memory = armem::server::ltm::disk::load(path.value());
+ //armem::server::ltm::disk::Memory ltm(path.value());
+ //armem::wm::Memory memory;
+ //ltm.loadAll(memory);
+ //workingMemory().update(armem::toCommit(memory));
+ ARMARX_INFO << "Loaded NavigationMemory '" << properties.snapshotToLoad << "'";
+ }
+ else
+ {
+ ARMARX_WARNING << "Could not load NavigationMemory '" << properties.snapshotToLoad
+ << "'. Continue with empty memory.";
}
-
- // LEGACY CODE (Not working anymore since the wm json export changed due to ltm updates and aron updates)
- // armem::wm::Memory memory = armem::server::ltm::disk::load(path.value());
- //armem::server::ltm::disk::Memory ltm(path.value());
- //armem::wm::Memory memory;
- //ltm.loadAll(memory);
- //workingMemory().update(armem::toCommit(memory));
- ARMARX_INFO << "Loaded NavigationMemory '" << properties.snapshotToLoad << "'";
- }
- else
- {
- ARMARX_WARNING << "Could not load NavigationMemory '" << properties.snapshotToLoad
- << "'. Continue with empty memory.";
}
}
-}
-void
-NavigationMemory::onConnectComponent()
-{
- // Do things after connecting to topics and components.
+ void
+ NavigationMemory::onConnectComponent()
+ {
+ // Do things after connecting to topics and components.
- /* (Requies the armarx::DebugObserverComponentPluginUser.)
+ /* (Requies the armarx::DebugObserverComponentPluginUser.)
// Use the debug observer to log data over time.
// The data can be viewed in the ObserverView and the LivePlotter.
// (Before starting any threads, we don't need to lock mutexes.)
@@ -325,311 +325,311 @@ NavigationMemory::onConnectComponent()
}
*/
- // Setup the remote GUI.
- {
- createRemoteGuiTab();
- RemoteGui_startRunningTask();
- }
+ // Setup the remote GUI.
+ {
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+ }
- tasks.visuTask = new SimpleRunningTask<>([this]() { this->visuRun(); }, "Visualization");
- tasks.visuTask->start();
-}
+ tasks.visuTask = new SimpleRunningTask<>([this]() { this->visuRun(); }, "Visualization");
+ tasks.visuTask->start();
+ }
-void
-NavigationMemory::onDisconnectComponent()
-{
-}
+ void
+ NavigationMemory::onDisconnectComponent()
+ {
+ }
-void
-NavigationMemory::onExitComponent()
-{
-}
+ void
+ NavigationMemory::onExitComponent()
+ {
+ }
-std::string
-NavigationMemory::getDefaultName() const
-{
- return "NavigationMemory";
-}
+ std::string
+ NavigationMemory::getDefaultName() const
+ {
+ return "NavigationMemory";
+ }
-void
-NavigationMemory::createRemoteGuiTab()
-{
- using namespace armarx::RemoteGui::Client;
+ void
+ NavigationMemory::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
- // Setup the widgets.
- tab.locationGraph.setup(*this);
+ // Setup the widgets.
+ tab.locationGraph.setup(*this);
- // Setup the layout.
- VBoxLayout root = {tab.locationGraph.group, VSpacer()};
- RemoteGui_createTab(getName(), root, &tab);
-}
+ // Setup the layout.
+ VBoxLayout root = {tab.locationGraph.group, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
-void
-NavigationMemory::RemoteGui_update()
-{
- tab.locationGraph.update(*this);
-}
+ void
+ NavigationMemory::RemoteGui_update()
+ {
+ tab.locationGraph.update(*this);
+ }
-void
-NavigationMemory::RemoteGuiTab::LocationGraph::setup(NavigationMemory& owner)
-{
- using namespace armarx::RemoteGui::Client;
- GridLayout grid;
- int row = 0;
+ void
+ NavigationMemory::RemoteGuiTab::LocationGraph::setup(NavigationMemory& owner)
{
- visuLocations.setValue(owner.properties.locationGraph.visuLocations);
- visuGraphEdges.setValue(owner.properties.locationGraph.visuLocations);
+ using namespace armarx::RemoteGui::Client;
+ GridLayout grid;
+ int row = 0;
+ {
+ visuLocations.setValue(owner.properties.locationGraph.visuLocations);
+ visuGraphEdges.setValue(owner.properties.locationGraph.visuLocations);
- grid.add(Label("Visualize Locations"), {row, 0}).add(visuLocations, {row, 1});
- ++row;
+ grid.add(Label("Visualize Locations"), {row, 0}).add(visuLocations, {row, 1});
+ ++row;
- grid.add(Label("Visualize Graph Edges"), {row, 0}).add(visuGraphEdges, {row, 1});
- ++row;
- }
+ grid.add(Label("Visualize Graph Edges"), {row, 0}).add(visuGraphEdges, {row, 1});
+ ++row;
+ }
- group = GroupBox({grid});
-}
+ group = GroupBox({grid});
+ }
-void
-NavigationMemory::RemoteGuiTab::LocationGraph::update(NavigationMemory& owner)
-{
- if (visuLocations.hasValueChanged() or visuGraphEdges.hasValueChanged())
+ void
+ NavigationMemory::RemoteGuiTab::LocationGraph::update(NavigationMemory& owner)
{
- std::scoped_lock lock(owner.propertiesMutex);
- owner.properties.locationGraph.visuLocations = visuLocations.getValue();
- owner.properties.locationGraph.visuGraphEdges = visuGraphEdges.getValue();
+ if (visuLocations.hasValueChanged() or visuGraphEdges.hasValueChanged())
+ {
+ std::scoped_lock lock(owner.propertiesMutex);
+ owner.properties.locationGraph.visuLocations = visuLocations.getValue();
+ owner.properties.locationGraph.visuGraphEdges = visuGraphEdges.getValue();
+ }
}
-}
-void
-NavigationMemory::visuRun()
-{
- memory::Visu visu{arviz,
- workingMemory().getCoreSegment(navigation::location::coreSegmentID),
- workingMemory().getCoreSegment(navigation::graph::coreSegmentID)};
-
- Properties::LocationGraph p;
+ void
+ NavigationMemory::visuRun()
{
- std::scoped_lock lock(propertiesMutex);
- p = properties.locationGraph;
- }
+ memory::Visu visu{arviz,
+ workingMemory().getCoreSegment(navigation::location::coreSegmentID),
+ workingMemory().getCoreSegment(navigation::graph::coreSegmentID)};
- CycleUtil cycle(static_cast<int>(1000 / p.visuFrequency));
- while (tasks.visuTask and not tasks.visuTask->isStopped())
- {
+ Properties::LocationGraph p;
{
std::scoped_lock lock(propertiesMutex);
p = properties.locationGraph;
}
- std::vector<viz::Layer> layers;
+ CycleUtil cycle(static_cast<int>(1000 / p.visuFrequency));
+ while (tasks.visuTask and not tasks.visuTask->isStopped())
+ {
+ {
+ std::scoped_lock lock(propertiesMutex);
+ p = properties.locationGraph;
+ }
- // Locations
- visu.drawLocations(layers, p.visuLocations);
+ std::vector<viz::Layer> layers;
- // Graph Edges
- visu.drawGraphs(layers, p.visuGraphEdges);
+ // Locations
+ visu.drawLocations(layers, p.visuLocations);
- arviz.commit(layers);
+ // Graph Edges
+ visu.drawGraphs(layers, p.visuGraphEdges);
- cycle.waitForCycleDuration();
- }
-}
+ arviz.commit(layers);
-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);
+ cycle.waitForCycleDuration();
+ }
+ }
- for (const auto& [memoryId, graph] : graphs)
+ bool
+ store(const std::map<armem::MemoryID, core::Graph>& graphs,
+ const std::filesystem::path& baseDirectory)
{
- const std::filesystem::path nestedSubDir =
- std::filesystem::path(memoryId.providerSegmentName) / memoryId.coreSegmentName;
- const std::filesystem::path dir = baseDirectory / nestedSubDir;
+ ARMARX_INFO << "Creating export directory `" << baseDirectory << "`.";
+ std::filesystem::create_directories(baseDirectory);
- std::filesystem::create_directories(dir);
+ 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;
- nlohmann::json j;
+ std::filesystem::create_directories(dir);
- // 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); });
+ nlohmann::json j;
- j["edges"] = outgoingEdges;
- j["vertices"] = {};
+ // 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); });
- for (const auto& vertex : graph.m_vertices)
- {
+ j["edges"] = outgoingEdges;
+ j["vertices"] = {};
+
+ for (const auto& vertex : graph.m_vertices)
+ {
- armarx::armem::MemoryID locationId;
- fromAron(vertex.m_property.aron.locationID, locationId);
+ 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;
+ nlohmann::json jVertex;
+ jVertex["locationID"] = locationId.getEntityID().str();
+ jVertex["vertexID"] = vertex.m_property.aron.vertexID;
- j["vertices"].push_back(jVertex);
+ 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;
}
- // 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;
}
- 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);
-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;
- // key: provider id
- std::map<std::string, nlohmann::json> js;
+ for (const auto& [memoryId, location] : locations)
+ {
+ auto& j = js[memoryId.providerSegmentName];
- for (const auto& [memoryId, location] : locations)
- {
- auto& j = js[memoryId.providerSegmentName];
+ if (j.count("locations") == 0)
+ {
+ j["locations"] = {};
+ }
- if (j.count("locations") == 0)
- {
- j["locations"] = {};
- }
+ nlohmann::json jLoc;
- nlohmann::json jLoc;
+ jLoc["globalRobotPose"] = location.globalRobotPose;
- jLoc["globalRobotPose"] = location.globalRobotPose;
+ if (location.relativeToObject)
+ {
+ armarx::armem::MemoryID memoryId;
+ fromAron(location.relativeToObject->objectInstanceID, memoryId);
- 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";
+ }
- jLoc["relativeToObject"]["objectInstanceID"] = memoryId.str();
- jLoc["relativeToObject"]["relativeRobotPose"] =
- location.relativeToObject->relativeRobotPose;
- }
- else
- {
- jLoc["relativeToObject"] = "null";
+ j["locations"][memoryId.getEntityID().str()] = jLoc;
}
- 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;
- // 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);
+ }
- 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;
}
- const std::filesystem::path filename = dir / "locations.json";
- ARMARX_VERBOSE << "Saving file `" << filename << "`.";
- std::ofstream ofs(filename);
- ofs << std::setw(4) << j;
- }
-
- return true;
-}
+ 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);
+ 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
+ // 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())
+ 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
{
- locations[entity.id()].fromAron(instance->data());
- }
+ if (const armarx::armem::server::wm::EntityInstance* instance =
+ entity.findLatestInstance())
+ {
+ locations[entity.id()].fromAron(instance->data());
+ }
- return true;
- });
+ return true;
+ });
- return true;
- });
+ return true;
+ });
- return locations;
- }();
+ 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())
+ const auto graphs = [&graphCoreSegment]()
+ {
+ std::map<armem::MemoryID, core::Graph> graphs;
+ graphCoreSegment.doLocked(
+ [&]()
+ {
+ graphCoreSegment.forEachEntity(
+ [&](const armarx::armem::server::wm::Entity& entity) -> bool
{
- navigation::core::arondto::Graph aron;
- aron.fromAron(instance->data());
- fromAron(aron, graph);
- }
+ 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 true;
+ });
+ });
- return graphs;
- }();
+ return graphs;
+ }();
- // store on disk
+ // store on disk
- const std::filesystem::path baseDirectory = armarx::PackagePath(packagePath).toSystemPath();
+ const std::filesystem::path baseDirectory = armarx::PackagePath(packagePath).toSystemPath();
- store(locations, baseDirectory);
- store(graphs, baseDirectory);
+ store(locations, baseDirectory);
+ store(graphs, baseDirectory);
- return true;
-}
+ return true;
+ }
} // namespace armarx::navigation
--
GitLab
From 005f1c6fa9d8827c73d288be9b2f2484448d5334 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 10 Nov 2022 11:12:09 +0100
Subject: [PATCH 8/8] improved ui
---
.../LocationGraphEditorWidget.ui | 66 +++++++++++--------
1 file changed, 39 insertions(+), 27 deletions(-)
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
index 7173033c..d13d0c05 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
@@ -125,50 +125,62 @@
<property name="minimumSize">
<size>
<width>0</width>
- <height>200</height>
+ <height>130</height>
</size>
</property>
<property name="title">
<string>Export</string>
</property>
- <widget class="QLineEdit" name="packageNameEdit">
- <property name="geometry">
- <rect>
- <x>10</x>
- <y>30</y>
- <width>113</width>
- <height>28</height>
- </rect>
- </property>
- <property name="text">
- <string>PriorKnowledgeData</string>
- </property>
- </widget>
- <widget class="QLineEdit" name="packageDirectoryEdit">
+ <widget class="QPushButton" name="exportButton">
<property name="geometry">
<rect>
- <x>10</x>
- <y>80</y>
- <width>113</width>
+ <x>30</x>
+ <y>100</y>
+ <width>84</width>
<height>28</height>
</rect>
</property>
<property name="text">
- <string>navigation-graphs</string>
+ <string>Export</string>
</property>
</widget>
- <widget class="QPushButton" name="exportButton">
+ <widget class="QWidget" name="horizontalLayoutWidget">
<property name="geometry">
<rect>
- <x>20</x>
- <y>140</y>
- <width>84</width>
- <height>28</height>
+ <x>0</x>
+ <y>30</y>
+ <width>308</width>
+ <height>61</height>
</rect>
</property>
- <property name="text">
- <string>Export</string>
- </property>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QLineEdit" name="packageNameEdit">
+ <property name="minimumSize">
+ <size>
+ <width>150</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>PriorKnowledgeData</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="packageDirectoryEdit">
+ <property name="minimumSize">
+ <size>
+ <width>150</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>navigation-graphs</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
</widget>
</widget>
</item>
--
GitLab