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 <algorithm>
#include <cstddef>
#include <cstdint>
#include <filesystem>
#include <fstream>
#include <iomanip>
#include <iterator>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <utility>
#include <vector>
 
#include <Ice/Current.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Primitive.h>
#include <VirtualRobot/Robot.h> // IWYU pragma: keep
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/eigen/ice_conversions.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/time/forward_declarations.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/interface/core/PackagePath.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
 
#include <RobotAPI/components/ArViz/Client/Layer.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/PriorKnowledge/navigation_graphs/NavigationGraphFinder.h>
#include <RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.h>
#include <RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h>
#include <RobotAPI/libraries/armem/core/Commit.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_locations/aron/Location.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h> // IWYU pragma: keep
#include <RobotAPI/libraries/aron/common/aron_conversions/names.h> // IWYU pragma: keep
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
 
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
#include <armarx/navigation/algorithms/aron/Room.aron.generated.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/aron/Trajectory.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/constants.h>
 
#include "Visu.h"
#include <nlohmann/json_fwd.hpp>
 
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].");
 
        // Core segments (max history size)
        {
            def->optional(
                properties.coreSeg.parameterization.maxHistorySize,
                "p.coreSeg.parameterization.maxHistorySize",
                "Max history size of the " +
                    QUOTED(navigation::memory::constants::ParameterizationCoreSegmentName) +
                    " core segment.");
 
            def->optional(properties.coreSeg.costmap.maxHistorySize,
                          "p.coreSeg.costmap.maxHistorySize",
                          "Max history size of the " +
                              QUOTED(navigation::memory::constants::CostmapCoreSegmentName) +
                              " core segment.");
 
            def->optional(
                properties.coreSeg.resultsGlobalPlanner.maxHistorySize,
                "p.coreSeg.resultsGlobalPlanner.maxHistorySize",
                "Max history size of the " +
                    QUOTED(navigation::memory::constants::GlobalPlannerResultCoreSegment) +
                    " core segment.");
 
            def->optional(properties.coreSeg.resultsLocalPlanner.maxHistorySize,
                          "p.coreSeg.resultsLocalPlanner.maxHistorySize",
                          "Max history size of the " +
                              QUOTED(navigation::memory::constants::LocalPlannerResultCoreSegment) +
                              " core segment.");
 
            def->optional(properties.coreSeg.events.maxHistorySize,
                          "p.coreSeg.events.maxHistorySize",
                          "Max history size of the " +
                              QUOTED(navigation::memory::constants::EventsCoreSegmentName) +
                              " core segment.");
 
            def->optional(properties.coreSeg.exceptions.maxHistorySize,
                          "p.coreSeg.exceptions.maxHistorySize",
                          "Max history size of the " +
                              QUOTED(navigation::memory::constants::ExceptionsCoreSegmentName) +
                              " core segment.");
 
            def->optional(properties.coreSeg.location.maxHistorySize,
                          "p.coreSeg.location.maxHistorySize",
                          "Max history size of the " +
                              QUOTED(navigation::location::coreSegmentID.coreSegmentName) +
                              " core segment.");
 
            def->optional(properties.coreSeg.graph.maxHistorySize,
                          "p.coreSeg.graph.maxHistorySize",
                          "Max history size of the " +
                              QUOTED(navigation::graph::coreSegmentID.coreSegmentName) +
                              " core segment.");
 
            def->optional(properties.coreSeg.human.maxHistorySize,
                          "p.coreSeg.human.maxHistorySize",
                          "Max history size of the " +
                              QUOTED(navigation::memory::constants::HumanCoreSegmentName) +
                              " core segment.");
 
            def->optional(
                properties.coreSeg.laserScannerFeatures.maxHistorySize,
                "p.coreSeg.laserScannerFeatures.maxHistorySize",
                "Max history size of the " +
                    QUOTED(navigation::memory::constants::LaserScannerFeaturesCoreSegment) +
                    " core segment.");
 
            def->optional(properties.coreSeg.rooms.maxHistorySize,
                          "p.coreSeg.rooms.maxHistorySize",
                          "Max history size of the " +
                              QUOTED(navigation::rooms::coreSegmentID.coreSegmentName) +
                              " core segment.");
        }
 
        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)
            .setMaxHistorySize(properties.coreSeg.parameterization.maxHistorySize);
 
        workingMemory()
            .addCoreSegment(navigation::memory::constants::CostmapCoreSegmentName,
                            algorithms::arondto::Costmap::ToAronType())
            .setMaxHistorySize(properties.coreSeg.costmap.maxHistorySize);
 
        workingMemory()
            .addCoreSegment(navigation::memory::constants::GlobalPlannerResultCoreSegment,
                            navigation::core::arondto::GlobalTrajectory::ToAronType())
            .setMaxHistorySize(properties.coreSeg.resultsGlobalPlanner.maxHistorySize);
 
        workingMemory()
            .addCoreSegment(navigation::memory::constants::LocalPlannerResultCoreSegment,
                            navigation::core::arondto::LocalTrajectory::ToAronType())
            .setMaxHistorySize(properties.coreSeg.resultsLocalPlanner.maxHistorySize);
        // workingMemory().addCoreSegment("Results_TrajectoryController",
        //                                navigation::core::arondto::Twist::ToAronType());
        // workingMemory().addCoreSegment("Results_SafetyGuard",
        //                                navigation::core::arondto::Twist::ToAronType());
 
        workingMemory()
            .addCoreSegment(navigation::memory::constants::EventsCoreSegmentName)
            .setMaxHistorySize(properties.coreSeg.events
                                   .maxHistorySize); //, armem::example::ExampleData::ToAronType());
 
        workingMemory()
            .addCoreSegment(navigation::memory::constants::ExceptionsCoreSegmentName)
            .setMaxHistorySize(properties.coreSeg.exceptions
                                   .maxHistorySize); //, armem::example::ExampleData::ToAronType());
 
        workingMemory()
            .addCoreSegment(navigation::location::coreSegmentID.coreSegmentName,
                            navigation::location::arondto::Location::ToAronType())
            .setMaxHistorySize(properties.coreSeg.location.maxHistorySize);
        workingMemory()
            .addCoreSegment(navigation::graph::coreSegmentID.coreSegmentName,
                            navigation::core::arondto::Graph::ToAronType())
            .setMaxHistorySize(properties.coreSeg.graph.maxHistorySize);
 
        workingMemory()
            .addCoreSegment(memory::constants::HumanCoreSegmentName,
                            navigation::human::arondto::Human::ToAronType())
            .setMaxHistorySize(properties.coreSeg.human.maxHistorySize);
 
        workingMemory()
            .addCoreSegment(memory::constants::LaserScannerFeaturesCoreSegment,
                            navigation::memory::arondto::LaserScannerFeatures::ToAronType())
            .setMaxHistorySize(properties.coreSeg.laserScannerFeatures.maxHistorySize);
 
        // workingMemory().addCoreSegment(memory::constants::HumanGroupCoreSegmentName,
        //                                navigation::human::arondto::Human::ToAronType());
        workingMemory()
            .addCoreSegment(navigation::rooms::coreSegmentID.coreSegmentName,
                            navigation::algorithms::arondto::Room::ToAronType())
            .setMaxHistorySize(properties.coreSeg.rooms.maxHistorySize);
 
        loadSnapshot();
    }
 
    void
    Component::onConnectComponent()
    {
        // connect readers if enabled
        // if (properties.locationGraph.visuRelativeLocations)
        {
            objClient = ObjectPoseClientPluginUser::getClient();
            loadSpecialRooms();
        }
 
        // 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 armarx::RemoteGui::Client::VBoxLayout;
        using armarx::RemoteGui::Client::VSpacer;
 
        // 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 armarx::RemoteGui::Client::Button;
        using armarx::RemoteGui::Client::GridLayout;
        using armarx::RemoteGui::Client::GroupBox;
        using armarx::RemoteGui::Client::Label;
 
        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 = row, .column = 0}, {.rows = 1, .columns = 2});
            ++row;
 
            grid.add(Label("Visualize Locations"), {.row = row, .column = 0})
                .add(visuLocations, {.row = row, .column = 1});
            ++row;
 
            grid.add(Label("Visualize Graph Edges"), {.row = row, .column = 0})
                .add(visuGraphEdges, {.row = row, .column = 1});
            ++row;
        }
 
        group = GroupBox({grid});
        group.setLabel("Locations");
    }
 
    void
    Component::RemoteGuiTab::LocationGraph::update(Component& owner) const
    {
        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
            if (properties.locationGraph.visuRelativeLocations)
            {
                ARMARX_TRACE;
                visu.drawLocations(objClient, layers, pp.visuLocations);
            }
            else
            {
                ARMARX_TRACE;
                visu.drawLocations(layers, pp.visuLocations);
            }
 
            // Graph Edges
            if (properties.locationGraph.visuRelativeLocations)
            {
                ARMARX_TRACE;
                visu.drawGraphs(objClient, layers, pp.visuGraphEdges);
            }
            else
            {
                ARMARX_TRACE;
                visu.drawGraphs(layers, pp.visuGraphEdges);
            }
 
            // Costmaps
            ARMARX_TRACE;
            visu.drawCostmaps(layers, p.visuCostmaps, p.zOffsetCostmap);
 
            // Humans
            ARMARX_TRACE;
            visu.drawHumans(layers,
                            p.visuHumans,
                            p.visuTransparent,
                            Duration::MilliSeconds(p.visuHumanMaxAgeMs));
            // Rooms
            ARMARX_TRACE;
            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;
        }
 
        const std::vector<std::string> snapshotsToLoad =
            simox::alg::split(properties.snapshotToLoad, ";", true);
 
        for (const auto& snapshotToLoad : snapshotsToLoad)
        {
 
            ARMARX_INFO << "Loading snapshots from snapshot '" << 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(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(snapshotToLoad);
                        up.instancesData = {g.toAron()};
 
                        ARMARX_DEBUG << "Sending empty graph `" << 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>()));
 
                                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::algorithms::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 '" << snapshotToLoad << "' --> graph: '"
                            << graphId << "'";
            }
            else
            {
                ARMARX_WARNING << "Could not load NavigationMemory '" << snapshotToLoad
                               << "'. Continue with empty memory.";
            }
        }
    }
 
    namespace
    {
 
        [[maybe_unused]]
        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.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;
        }
 
    } // namespace
 
    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);
 
        // The graph is no longer relevant.
        // store(graphs, baseDirectory);
 
        return true;
    }
 
    void
    Component::loadSpecialRooms()
    {
        armem::Commit c; // will be populated and sent to the memory at the end
 
        // special handling of object affordances that are actually rooms
        {
            const auto objectFinder = objClient.getObjectFinder();
            const auto objectPoseMap = objClient.fetchObjectPosesAsMap();
 
            for (const auto& [providerName, objectPose] : objectPoseMap)
            {
                const auto objectInfoOpt = objectFinder.findObject(objectPose.objectID);
                if (not objectInfoOpt)
                {
                    ARMARX_WARNING << "Could not find object with id '" << objectPose.objectID
                                   << "'.";
                    continue;
                }
 
                const auto& objectInfo = *objectInfoOpt;
 
                // TODO: handle non-articulated model similar to articulated model below
 
                // load the articulated model and extract the room affordances (type: containable, name: room)
 
                const auto articulatedModelPathOpt = objectInfo.getArticulatedModel();
                if (not articulatedModelPathOpt)
                {
                    ARMARX_VERBOSE << "Object '" << objectPose.objectID
                                   << "' is not an articulated model.";
                    continue;
                }
 
                const auto& articulatedModelPath = *articulatedModelPathOpt;
                if (not std::filesystem::is_regular_file(articulatedModelPath.absolutePath))
                {
                    ARMARX_VERBOSE << "Object '" << objectPose.objectID
                                   << "' has no articulated model.";
                    continue;
                }
 
                // load the model
                VirtualRobot::RobotPtr articulatedObject = VirtualRobot::RobotIO::loadRobot(
                    articulatedModelPath.absolutePath.string(), VirtualRobot::RobotIO::eStructure);
                ARMARX_CHECK_NOT_NULL(articulatedObject)
                    << QUOTED(articulatedModelPath.absolutePath.string());
 
                // synchronize state
                articulatedObject->setJointValues(objectPose.objectJointValues);
                articulatedObject->setGlobalPose(objectPose.objectPoseGlobal);
 
                // extract the room affordances
                const auto affordances = articulatedObject->getAffordances();
                for (const auto& affordanceLocation : affordances)
                {
                    for (const auto affordance : affordanceLocation.affordances)
                    {
                        if (simox::alg::to_lower(affordance.type) == "containable")
                        {
                            // check if the affordance is a room
                            // if (simox::alg::to_lower(affordanceLocation.name) == "room")
                            {
                                for (const std::shared_ptr<VirtualRobot::Primitive::Primitive>&
                                         primitive : affordance.primitiveRepresentation)
                                {
                                    if (primitive->type == VirtualRobot::Primitive::Box::TYPE)
                                    {
                                        VirtualRobot::Primitive::Box* box =
                                            std::dynamic_pointer_cast<VirtualRobot::Primitive::Box>(
                                                primitive)
                                                .get();
 
                                        ARMARX_CHECK_NOT_NULL(box);
 
                                        const Eigen::Isometry3f& root_T_prim =
                                            affordanceLocation.pose.pose;
                                        const Eigen::Isometry3f global_T_prim =
                                            Eigen::Isometry3f{articulatedObject->getGlobalPose()} *
                                            root_T_prim;
 
                                        const Eigen::Isometry3f prim_T_affordance{
                                            primitive->transform};
                                        const Eigen::Isometry3f global_T_affordance =
                                            global_T_prim * prim_T_affordance;
 
                                        const Eigen::Vector3f boxSize{
                                            box->width, box->height, box->depth};
 
                                        // we want to extract the polygon in the xy-plane
 
                                        // Local corner points (clock-wise)
                                        const Eigen::Vector3f corner1 = Eigen::Vector3f{
                                            boxSize.x() / 2, boxSize.y() / 2, -boxSize.z() / 2};
                                        const Eigen::Vector3f corner2 = Eigen::Vector3f{
                                            -boxSize.x() / 2, boxSize.y() / 2, -boxSize.z() / 2};
                                        const Eigen::Vector3f corner3 = Eigen::Vector3f{
                                            -boxSize.x() / 2, -boxSize.y() / 2, -boxSize.z() / 2};
                                        const Eigen::Vector3f corner4 = Eigen::Vector3f{
                                            boxSize.x() / 2, -boxSize.y() / 2, -boxSize.z() / 2};
 
                                        // FIXME: strong assumption: the box is axis aligned. relax
                                        // obtain the corner points of the box in the xy-plane
                                        std::vector<Eigen::Vector3f> polygon;
                                        polygon.reserve(4);
                                        polygon.emplace_back(global_T_affordance * corner1);
                                        polygon.emplace_back(global_T_affordance * corner2);
                                        polygon.emplace_back(global_T_affordance * corner3);
                                        polygon.emplace_back(global_T_affordance * corner4);
 
                                        // create a room object
                                        navigation::algorithms::arondto::Room room;
                                        room.name = "inside";
                                        room.polygon = polygon;
                                        room.height = boxSize.z();
 
                                        // send commit to memory
                                        armem::MemoryID providerID = workingMemory().id();
                                        providerID.coreSegmentName =
                                            navigation::rooms::coreSegmentID.coreSegmentName;
                                        providerID.providerSegmentName =
                                            objectPose.objectID.dataset() +
                                            "::" + objectPose.objectID.className();
 
                                        const auto now = armem::Time::Now();
 
                                        auto& up = c.add();
                                        up.confidence = 1.0;
                                        up.referencedTime = now;
                                        up.sentTime = now;
                                        up.arrivedTime = now;
                                        up.entityID = providerID.withEntityName(room.name);
                                        up.instancesData = {room.toAron()};
                                    }
                                    else
                                    {
                                        ARMARX_INFO << "Not handling affordance of type "
                                                    << primitive->type;
                                        continue;
                                    }
                                }
                            }
                        }
                    }
                }
 
                // check whether a specific areas.json exists
                const auto areas = objectInfo.file(".json", "_areas", true);
                ARMARX_VERBOSE << "Checking for areas.json file at " << areas.absolutePath;
                if (std::filesystem::exists(areas.absolutePath))
                {
                    ARMARX_INFO << "Found areas.json file for object '" << objectPose.objectID
                                << "'.";
 
                    // load json
                    std::ifstream ifs(areas.absolutePath);
                    const auto json = nlohmann::json::parse(ifs);
 
                    const std::map<std::string, nlohmann::json> jAreas = json.at("areas");
                    ARMARX_INFO << "Found " << jAreas.size() << " areas.";
 
                    for (const auto& [areaName, jArea] : jAreas)
                    {
                        const std::vector<Eigen::Vector2f> polygon =
                            jArea.at("polygon").get<std::vector<Eigen::Vector2f>>();
                        const float height = jArea.at("height").get<float>();
 
                        // send commit to memory
                        armem::MemoryID providerID = workingMemory().id();
                        providerID.coreSegmentName =
                            navigation::rooms::coreSegmentID.coreSegmentName;
                        providerID.providerSegmentName =
                            objectPose.objectID.dataset() + "::" + objectPose.objectID.className();
 
                        const auto now = armem::Time::Now();
 
                        // create a room object
                        navigation::algorithms::arondto::Room room;
                        room.name = objectPose.objectID.getClassID().withInstanceName(areaName).str();
                        room.polygon = conv::to3D(polygon); // at this point relative to the object
                        room.height = height;
 
                        // change the room to the global coordinate system
                        const Eigen::Isometry3f global_T_object{objectPose.objectPoseGlobal};
                        for (auto& point : room.polygon)
                        {
                            // see comment below: point = global_T_object * point;
                            point = global_T_object * point;
                        }
 
                        // If you want to convert globalliy-defined rooms to local coordinates, 
                        // change the for loop above (comment it out) and uncomment the line below:
                        // point = global_T_object.inverse() * point;
                        // Then, comment in the lines below and read out the local cooredinates in the 
                        // ArmarX log.
 
                        // nlohmann::json jj;
                        // jj["polygon"] = conv::to2D(room.polygon);
                        // ARMARX_INFO << areaName << " " << jj.dump(4);
 
                        auto& up = c.add();
                        up.confidence = 1.0;
                        up.referencedTime = now;
                        up.sentTime = now;
                        up.arrivedTime = now;
                        up.entityID = providerID.withEntityName(areaName);
                        up.instancesData = {room.toAron()};
                    }
                }
            }
        }
 
        if (c.updates.empty())
        {
            ARMARX_INFO << "No special rooms found.";
            return;
        }
 
        ARMARX_INFO << "Sending " << c.updates.size() << " special rooms to memory";
        iceAdapter().commit(c);
    }
} // namespace armarx::navigation::components::navigation_memory