Component.cpp

Go to the documentation of this file.

/*
 * 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 version 2 as
 * published by the Free Software Foundation.
 *
 * 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 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    Navigation::ArmarXObjects::NavigationMemory
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "Component.h"
 
#include <filesystem>
#include <fstream>
#include <iostream>
#include <iterator>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/eigen/ice_conversions.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/time/forward_declarations.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <RobotAPI/libraries/PriorKnowledge/navigation_graphs/NavigationGraphFinder.h>
#include <RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.h>
#include <RobotAPI/libraries/armem/client/query.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/operations.h>
#include <RobotAPI/libraries/armem/core/wm/aron_conversions.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 <armarx/navigation/algorithms/Costmap.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>
#include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
#include <armarx/navigation/core/aron/Twist.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/human/aron/Human.aron.generated.h>
#include <armarx/navigation/location/constants.h>
#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/rooms/aron/Room.aron.generated.h>
#include <armarx/navigation/rooms/constants.h>
 
#include "Visu.h"
 
namespace armarx::navigation::components::navigation_memory
{
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
    armarx::PropertyDefinitionsPtr
    Component::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        // Publish to a topic (passing the TopicListenerPrx).
        // def->topic(myTopicListener);
 
        // Subscribe to a topic (passing the topic name).
        // def->topic<PlatformUnitListener>("MyTopic");
 
        // Use (and depend on) another component (passing the ComponentInterfacePrx).
        // def->component(myComponentProxy)
 
 
        def->optional(properties.snapshotToLoad,
                      "p.snapshotToLoad",
                      "Snapshot to load at start up \n"
                      "(e.g. 'R003' to load 'PriorKnowledgeData/navigation-graphs/R003').");
 
        def->optional(properties.packageToLoad,
                      "p.packageToLoad",
                      "Package to load snapshot at start up \n"
                      "(e.g. 'PriorKnowledgeData').");
 
        def->optional(properties.locationGraph.visuLocations,
                      "p.locationGraph.visuLocation",
                      "Enable visualization of locations.");
 
        def->optional(
            properties.locationGraph.visuRelativeLocations,
            "p.locationGraph.visuRelativeLocation",
            "Enable visualization of relative locations (currently supported: robots, objects).");
 
        def->optional(properties.locationGraph.visuGraphEdges,
                      "p.locationGraph.visuGraphEdges",
                      "Enable visualization of navigation graph edges.");
 
        def->optional(
            properties.visuCostmaps, "p.visuCostmaps", "Enable visualization of costmaps.");
        def->optional(properties.zOffsetCostmap,
                      "p.zOffsetCostmap",
                      "Adjust the height of the visualized costmap.");
 
        def->optional(properties.visuHumans, "p.visuHumans", "Enable visualization of humans.");
 
        def->optional(properties.visuTransparent,
                      "p.visuTransparent",
                      "Enable visualization of humans a bit transparent.");
 
        def->optional(properties.visuHumanMaxAgeMs,
                      "p.visuHumanMaxAgeMs",
                      "The maximum age of humans to be drawn in ms.");
 
        def->optional(properties.visuRooms, "p.visuRooms", "Enable visualization of rooms.");
 
        def->optional(properties.visuFrequency,
                      "p.visuFrequency",
                      "Visualization frequeny of locations and graph edges [Hz].");
 
        return def;
    }
 
    Component::Component()
    {
        workingMemory().setName("Navigation");
    }
 
    void
    Component::onInitComponent()
    {
        // Topics and properties defined above are automagically registered.
 
        // Keep debug observer data until calling `sendDebugObserverBatch()`.
        // (Requies the armarx::DebugObserverComponentPluginUser.)
        // setDebugObserverBatchModeEnabled(true);
 
        workingMemory().addCoreSegment(
            navigation::memory::constants::ParameterizationCoreSegmentName);
 
        workingMemory().addCoreSegment(navigation::memory::constants::CostmapCoreSegmentName,
                                       algorithms::arondto::Costmap::ToAronType());
 
        workingMemory().addCoreSegment("Results_GlobalPlanner",
                                       navigation::core::arondto::GlobalTrajectory::ToAronType());
        workingMemory().addCoreSegment("Results_LocalPlanner",
                                       navigation::core::arondto::LocalTrajectory::ToAronType());
        // workingMemory().addCoreSegment("Results_TrajectoryController",
        //                                navigation::core::arondto::Twist::ToAronType());
        // workingMemory().addCoreSegment("Results_SafetyController",
        //                                navigation::core::arondto::Twist::ToAronType());
 
        workingMemory().addCoreSegment("Events"); //, armem::example::ExampleData::ToAronType());
        workingMemory().addCoreSegment(
            "Exceptions"); //, armem::example::ExampleData::ToAronType());
 
 
        workingMemory().addCoreSegment(navigation::location::coreSegmentID.coreSegmentName,
                                       navigation::location::arondto::Location::ToAronType());
        workingMemory().addCoreSegment(navigation::graph::coreSegmentID.coreSegmentName,
                                       navigation::core::arondto::Graph::ToAronType());
 
        workingMemory().addCoreSegment(memory::constants::HumanCoreSegmentName,
                                       navigation::human::arondto::Human::ToAronType());
 
        workingMemory()
            .addCoreSegment(memory::constants::LaserScannerFeaturesCoreSegment,
                            navigation::memory::arondto::LaserScannerFeatures::ToAronType())
            .setMaxHistorySize(properties.laserScannerFeaturesMaxHistorySize);
 
        // workingMemory().addCoreSegment(memory::constants::HumanGroupCoreSegmentName,
        //                                navigation::human::arondto::Human::ToAronType());
        workingMemory().addCoreSegment(navigation::rooms::coreSegmentID.coreSegmentName,
                                       navigation::rooms::arondto::Room::ToAronType());
 
        loadSnapshot();
    }
 
    void
    Component::onConnectComponent()
    {
        // connect readers if enabled
        if (properties.locationGraph.visuRelativeLocations)
        {
            objClient = ObjectPoseClientPluginUser::getClient();
        }
 
        // Setup the remote GUI.
        {
            createRemoteGuiTab();
            RemoteGui_startRunningTask();
        }
 
        tasks.visuTask = new SimpleRunningTask<>([this]() { this->visuRun(); }, "Visualization");
        tasks.visuTask->start();
    }
 
    void
    Component::onDisconnectComponent()
    {
    }
 
    void
    Component::onExitComponent()
    {
    }
 
    std::string
    Component::getDefaultName() const
    {
        return GetDefaultName();
    }
 
    std::string
    Component::GetDefaultName()
    {
        return "navigation_memory";
    }
 
    void
    Component::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);
    }
 
    void
    Component::RemoteGui_update()
    {
        tab.locationGraph.update(*this);
    }
 
    void
    Component::RemoteGuiTab::LocationGraph::setup(Component& owner)
    {
        using namespace armarx::RemoteGui::Client;
        GridLayout grid;
        int row = 0;
        {
            reloadSnapshot = Button();
            reloadSnapshot.setLabel("Reload from Snapshot '" + owner.properties.snapshotToLoad +
                                    "'");
 
            visuLocations.setValue(owner.properties.locationGraph.visuLocations);
            visuGraphEdges.setValue(owner.properties.locationGraph.visuLocations);
 
            grid.add(reloadSnapshot, {row, 0}, {1, 2});
            ++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;
        }
 
        group = GroupBox({grid});
        group.setLabel("Locations");
    }
 
    void
    Component::RemoteGuiTab::LocationGraph::update(Component& owner)
    {
        if (reloadSnapshot.wasClicked())
        {
            owner.loadSnapshot();
        }
        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
    Component::visuRun()
    {
        memory::Visu visu{arviz,
                          workingMemory().getCoreSegment(navigation::location::coreSegmentID),
                          workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
                          workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
                          workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName),
                          workingMemory().getCoreSegment(navigation::rooms::coreSegmentID)};
 
        Properties::LocationGraph pp;
        Properties p;
        {
            std::scoped_lock lock(propertiesMutex);
            pp = properties.locationGraph;
            p = properties;
        }
 
        Metronome metronome(Frequency::HertzDouble(p.visuFrequency));
        while (tasks.visuTask and not tasks.visuTask->isStopped())
        {
            ARMARX_TRACE;
 
            {
                std::scoped_lock lock(propertiesMutex);
                pp = properties.locationGraph;
            }
 
            std::vector<viz::Layer> layers;
 
            // Locations
            // TODO: @fabian.peller this is ugly!
            if (properties.locationGraph.visuRelativeLocations)
            {
                visu.drawLocations(objClient, layers, pp.visuLocations);
            }
            else
            {
                visu.drawLocations(layers, pp.visuLocations);
            }
 
            // Graph Edges
            if (properties.locationGraph.visuRelativeLocations)
            {
                visu.drawGraphs(objClient, layers, pp.visuGraphEdges);
            }
            else
            {
                visu.drawGraphs(layers, pp.visuGraphEdges);
            }
 
            // Costmaps
            visu.drawCostmaps(layers, p.visuCostmaps, p.zOffsetCostmap);
 
            // Humans
            visu.drawHumans(layers,
                            p.visuHumans,
                            p.visuTransparent,
                            Duration::MilliSeconds(p.visuHumanMaxAgeMs));
            // Rooms
            visu.drawRooms(layers, p.visuRooms);
 
            arviz.commit(layers);
 
            metronome.waitForNextTick();
        }
    }
 
    void
    Component::loadSnapshot()
    {
        if (properties.snapshotToLoad.empty())
        {
            ARMARX_IMPORTANT
                << "Not loading any predefined locations or graphs as the params was empty ...";
            return;
        }
 
        ARMARX_INFO << "Loading snapshots from snapshot '" << properties.snapshotToLoad
                    << "' in package '" << properties.packageToLoad << "' ...";
 
        armarx::priorknowledge::navigation_graphs::NavigationGraphFinder f(
            properties.packageToLoad,
            armarx::priorknowledge::navigation_graphs::NavigationGraphFinder::DEFAULT_DIR_TO_IDS);
 
        if (auto graph = f.find(properties.snapshotToLoad); graph.has_value())
        {
            const std::string graphId = graph->getID();
            ARMARX_DEBUG << "Found graph " << graphId << " with path: " << graph->getFullPath();
 
            // Sometimes, we use the date before the snapshotname.
            // In this case a symlink to the real data should be used (e.g. R003 and 2022-03-01_R003)
 
            // This loads the location. All locations are stored in a single locations.json file
            // The file contains json with the framed poses (frame, agent, 4x4 matrix)
            {
                std::filesystem::path locationsFilePath = graph->getFullPath() / "locations.json";
                if (not std::filesystem::is_regular_file(locationsFilePath))
                {
                    ARMARX_WARNING << "Found no locations file for graph '" << graphId << "'.";
                }
                else
                {
 
                    ARMARX_DEBUG << "Found the locations file";
                    armem::Commit c;
                    armem::MemoryID providerID = workingMemory().id();
                    providerID.coreSegmentName = "Location";
                    providerID.providerSegmentName = graphId;
 
                    const auto now = armem::Time::Now();
                    ARMARX_DEBUG << "Loading " << locationsFilePath;
 
                    std::ifstream ifs(locationsFilePath);
                    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 js = nlohmann::json::parse(content);
 
                    auto locations =
                        armarx::priorknowledge::util::LocationLoader::LoadLocations(graphId, js);
 
                    for (const auto& location : locations)
                    {
                        // we assert that only framedLocations are allowed. Everything else will be rejected!
                        if (location->type !=
                            armarx::priorknowledge::util::LocationType::FRAMED_LOCATION)
                        {
                            ARMARX_WARNING
                                << "Navigation memory received location '" + location->id.name +
                                       "' which is not of type "
                                       "'armarx::priorknowledge::util::LocationType:"
                                       ":FRAMED_LOCATION' (e.g. you specified a "
                                       "FRAMED_BOXED_LOCATION?). Only FramedLocations (i.e. "
                                       "poses in 3D space) are navigatable. Ignoring this location "
                                       "but please check the location.json file.";
                            continue;
                        }
 
                        auto& framedLocation =
                            dynamic_cast<armarx::priorknowledge::util::FramedLocation&>(*location);
 
                        navigation::location::arondto::Location loc;
 
                        // set members of loc
                        loc.framedPose.header.frame = framedLocation.frame;
                        loc.framedPose.header.agent = framedLocation.agent;
                        loc.framedPose.pose = framedLocation.pose;
                        toAron(loc.names, framedLocation.names);
 
                        // send commit to memory
                        auto& up = c.add();
                        up.confidence = 1.0;
                        up.referencedTime = now;
                        up.sentTime = now;
                        up.arrivedTime = now;
                        up.entityID = providerID.withEntityName(framedLocation.id.name);
                        up.instancesData = {loc.toAron()};
                    }
 
                    ARMARX_DEBUG << "Sending locations to memory";
                    iceAdapter().commitLocking(c);
                }
            }
 
            {
                // 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).
                std::filesystem::path graphFilesPath = graph->getFullPath() / "Graph";
 
                if (not std::filesystem::is_directory(graphFilesPath))
                {
                    ARMARX_INFO << "Found no `Graph/` folder for graph '" << graphId << "'.";
                    ARMARX_INFO << "Creating empty graph.";
 
                    armem::Commit c;
                    armem::MemoryID providerID = workingMemory().id();
                    providerID.coreSegmentName = memory::constants::GraphCoreSegmentName;
                    providerID.providerSegmentName = graphId;
 
                    const auto now = armem::Time::Now();
 
                    navigation::core::arondto::Graph g;
 
                    auto& up = c.add();
                    up.confidence = 1.0;
                    up.referencedTime = now;
                    up.arrivedTime = now;
                    up.sentTime = now;
                    up.entityID = providerID.withEntityName(properties.snapshotToLoad);
                    up.instancesData = {g.toAron()};
 
                    ARMARX_DEBUG << "Sending empty graph `" << properties.snapshotToLoad
                                 << "` to memory.";
                    iceAdapter().commit(c);
                }
                else
                {
                    ARMARX_DEBUG << "Loading graphs from " << graphFilesPath;
 
                    armem::Commit c;
                    armem::MemoryID providerID = workingMemory().id();
                    providerID.coreSegmentName = memory::constants::GraphCoreSegmentName;
                    providerID.providerSegmentName = graphId;
 
                    const auto now = armem::Time::Now();
 
                    for (const auto& graphFile : std::filesystem::directory_iterator(
                             graphFilesPath)) // iterate over the different graphs (json files)
                    {
                        if (std::filesystem::is_regular_file(graphFile.path()) and
                            simox::alg::ends_with(graphFile.path().filename(),
                                                  ".json")) // assure that its a json file
                        {
                            const std::string graphName = graphFile.path().stem();
 
 
                            ARMARX_DEBUG << "Loading graph `" << graphName << " `from file `"
                                         << graphFile.path() << "`.";
 
                            navigation::core::arondto::Graph g;
 
                            std::ifstream ifs(graphFile.path());
                            const std::string content((std::istreambuf_iterator<char>(ifs)),
                                                      (std::istreambuf_iterator<char>()));
 
                            // TODO: Perhaps move to PriorKnowledge similar to locations
                            const 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 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.referencedTime = now;
                            up.arrivedTime = now;
                            up.sentTime = now;
                            up.entityID = providerID.withEntityName(graphName);
                            up.instancesData = {g.toAron()};
                        }
                    }
 
                    ARMARX_DEBUG << "Sending graphs to memory";
                    iceAdapter().commit(c);
                }
            }
 
            {
                // Next we load the rooms.
                std::filesystem::path roomsFilePath = graph->getFullPath() / "rooms.json";
                if (not std::filesystem::is_regular_file(roomsFilePath))
                {
                    ARMARX_INFO << "Found no rooms file for graph '" << graphId << "'.";
                }
                else
                {
                    ARMARX_DEBUG << "Found the rooms file";
 
                    armem::Commit c;
                    armem::MemoryID providerID = workingMemory().id();
                    providerID.coreSegmentName = navigation::rooms::coreSegmentID.coreSegmentName;
                    providerID.providerSegmentName = graphId;
 
                    const auto now = armem::Time::Now();
                    ARMARX_VERBOSE << "Loading " << roomsFilePath;
 
                    std::ifstream ifs(roomsFilePath);
                    const std::string content((std::istreambuf_iterator<char>(ifs)),
                                              (std::istreambuf_iterator<char>()));
 
                    // parse room. TODO: Move into PriorKnowledge similar to locations
                    const nlohmann::json js = nlohmann::json::parse(content);
 
                    const auto rooms = js.at("rooms").get<std::map<std::string, nlohmann::json>>();
                    ARMARX_INFO << "Found " << rooms.size() << " rooms.";
                    for (const auto& [roomId, j] : rooms)
                    {
                        auto roomMemoryId = providerID;
                        roomMemoryId.entityName = roomId;
 
                        const std::string roomMemoryIdStr = roomMemoryId.str();
 
                        if (j.find("polygon") == j.end())
                        {
                            ARMARX_WARNING << "The element '" << roomMemoryIdStr
                                           << "' has no 'polygon' member. Skipping "
                                              "this entity.";
                            continue;
                        }
 
                        if (j.find("height") == j.end())
                        {
                            ARMARX_WARNING << "The element '" << roomMemoryIdStr
                                           << "' has no 'height' member. "
                                              "Skipping this entity.";
                            continue;
                        }
 
                        navigation::rooms::arondto::Room room;
                        j.at("polygon").get_to(room.polygon);
                        j.at("height").get_to(room.height);
                        room.name = roomId;
 
                        // set the polygon to z=0
                        for (auto& point : room.polygon)
                        {
                            point.z() = 0;
                        }
 
                        // send commit to memory
                        auto& up = c.add();
                        up.confidence = 1.0;
                        up.referencedTime = now;
                        up.sentTime = now;
                        up.arrivedTime = now;
                        up.entityID = armarx::armem::MemoryID(roomMemoryIdStr);
                        up.instancesData = {room.toAron()};
                    }
 
                    ARMARX_DEBUG << "Sending rooms to memory";
                    iceAdapter().commit(c);
                }
            }
            ARMARX_INFO << "Loaded NavigationMemory '" << properties.snapshotToLoad
                        << "' --> graph: '" << graphId << "'";
        }
        else
        {
            ARMARX_WARNING << "Could not load NavigationMemory '" << properties.snapshotToLoad
                           << "'. Continue with empty memory.";
        }
    }
 
    bool
    store(const std::map<armem::MemoryID, core::Graph>& graphs,
          const std::filesystem::path& baseDirectory)
    {
        ARMARX_INFO << "Creating export directory `" << baseDirectory << "`.";
        std::filesystem::create_directories(baseDirectory);
 
        for (const auto& [memoryId, graph] : graphs)
        {
            const std::filesystem::path nestedSubDir =
                std::filesystem::path(memoryId.providerSegmentName) / memoryId.coreSegmentName;
            const std::filesystem::path dir = baseDirectory / nestedSubDir;
 
            std::filesystem::create_directories(dir);
 
            nlohmann::json j;
 
            // source -> target
            std::vector<std::pair<std::size_t, std::size_t>> outgoingEdges;
            std::transform(graph.m_edges.begin(),
                           graph.m_edges.end(),
                           std::back_inserter(outgoingEdges),
                           [](const auto& edge)
                           { return std::make_pair(edge.m_source, edge.m_target); });
 
            j["edges"] = outgoingEdges;
            j["vertices"] = {};
 
            for (const auto& vertex : graph.m_vertices)
            {
 
                armarx::armem::MemoryID locationId;
                fromAron(vertex.m_property.aron.locationID, locationId);
 
                nlohmann::json jVertex;
                jVertex["locationID"] = locationId.getEntityID().str();
                jVertex["vertexID"] = vertex.m_property.aron.vertexID;
 
                j["vertices"].push_back(jVertex);
            }
 
            // save to disk
            const std::filesystem::path filename = dir / (memoryId.entityName + ".json");
            ARMARX_VERBOSE << "Saving file `" << filename << "`.";
            std::ofstream ofs(filename);
            ofs << std::setw(4) << j;
        }
 
        return true;
    }
 
    bool
    store(const std::map<armem::MemoryID, location::arondto::Location>& locations,
          const std::filesystem::path& baseDirectory)
    {
        ARMARX_INFO << "Creating export directory `" << baseDirectory << "`.";
        std::filesystem::create_directories(baseDirectory);
 
        // key: provider id
        std::map<std::string, nlohmann::json> js;
 
        for (const auto& [memoryId, location] : locations)
        {
            auto& j = js[memoryId.providerSegmentName];
 
            if (j.count("locations") == 0)
            {
                j["locations"] = {};
            }
 
            nlohmann::json jLoc;
            nlohmann::json framedPose;
 
            framedPose["agent"] = location.framedPose.header.agent;
            framedPose["frame"] = location.framedPose.header.frame;
 
            // utilize ice conversion of eigen
            std::vector<std::vector<float>> poseAsVec;
            armarx::core::eigen::toIce(location.framedPose.pose, poseAsVec);
            framedPose["pose"] = poseAsVec;
 
            jLoc["framedPose"] = framedPose;
 
            auto entityId = memoryId.getEntityID();
            j["locations"][entityId.providerSegmentName + "/" + entityId.entityName] = jLoc;
        }
 
        // save to disk
        for (const auto& [providerId, j] : js)
        {
            const std::filesystem::path subDir = std::filesystem::path(providerId);
            const std::filesystem::path dir = baseDirectory / subDir;
 
            if (not std::filesystem::exists(dir))
            {
                std::filesystem::create_directories(dir);
            }
 
            const std::filesystem::path filename = dir / "locations.json";
            ARMARX_VERBOSE << "Saving file `" << filename << "`.";
            std::ofstream ofs(filename);
            ofs << std::setw(4) << j;
        }
 
 
        return true;
    }
 
    bool
    Component::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::components::navigation_memory