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    MemoryX::ArmarXObjects::GraphImportExport
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "Component.h"
 
#include <iomanip>
#include <ios>
#include <map>
#include <string>
#include <vector>
 
#include <Ice/BuiltinSequences.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
 
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Writer.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_locations/aron/Location.aron.generated.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/graph/Visu.h>
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/location/constants.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h> // IWYU pragma: keep
#include <MemoryX/interface/memorytypes/MemoryEntities.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h> // IWYU pragma: keep
#include <SemanticObjectRelations/Shapes/Shape.h>
 
namespace armarx::navigation::components::graph_import_export
{
 
    Component::Component()
    {
        properties.locationCoreSegmentID = location::coreSegmentID;
        properties.graphCoreSegmentID = graph::coreSegmentID;
    }
 
    armarx::PropertyDefinitionsPtr
    Component::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        def->component(proxies.priorKnowledge);
        def->component(proxies.graphNodePoseResolver, "GraphNodePoseResolver");
 
        return def;
    }
 
    void
    Component::onInitComponent()
    {
        // Topics and properties defined above are automagically registered.
 
        // Keep debug observer data until calling `sendDebugObserverBatch()`.
        setDebugObserverBatchModeEnabled(true);
    }
 
    void
    Component::onConnectComponent()
    {
        objClient = ObjectPoseClientPluginUser::getClient();
 
        // Get proxies.
        if (proxies.priorKnowledge->hasGraphSegment())
        {
            proxies.graphSegment = proxies.priorKnowledge->getGraphSegment();
        }
        proxies.locationWriter = memoryNameSystem().useWriter(properties.locationCoreSegmentID);
        proxies.graphWriter = memoryNameSystem().useWriter(properties.graphCoreSegmentID);
 
 
        // Setup and start the remote GUI.
        refreshScenes();
        RemoteGui_startRunningTask();
    }
 
    void
    Component::onDisconnectComponent()
    {
    }
 
    void
    Component::onExitComponent()
    {
    }
 
    std::string
    Component::getDefaultName() const
    {
        return GetDefaultName();
    }
 
    std::string
    Component::GetDefaultName()
    {
        return "graph_import_export";
    }
 
    void
    Component::createRemoteGuiTab(const std::vector<std::string>& sceneNames)
    {
        using namespace armarx::RemoteGui::Client;
 
        tab.sceneComboBox.setOptions(sceneNames);
        tab.sceneRefreshButton.setLabel("Refresh");
 
        tab.providerSegmentLine.setValue(getName());
 
        tab.dryRun.setValue(false);
        tab.visuEnabled.setValue(true);
 
        tab.locationsMemoryxToArmemButton.setLabel("Locations MemoryX -> ArMem");
        tab.locationsArmemToMemoryxButton.setLabel("Locations ArMem -> MemoryX (WIP)");
        tab.locationsClearArMemButton.setLabel("Clear ArMem Locations");
 
        tab.graphMemoryxToArmemButton.setLabel("Graph MemoryX -> ArMem");
        tab.graphArmemToMemoryxButton.setLabel("Graph ArMem -> MemoryX (WIP)");
        tab.graphClearArMemButton.setLabel("Clear ArMem Graphs");
 
 
        GridLayout grid;
        int row = 0;
        {
            grid.add(Label("Scene:"), {row, 0})
                .add(tab.sceneComboBox, {row, 1})
                .add(tab.sceneRefreshButton, {row, 2});
            ++row;
 
            grid.add(Label("Provider Segment:"), {row, 0}).add(tab.providerSegmentLine, {row, 1});
            ++row;
 
            grid.add(Label("Dry Run:"), {row, 0}).add(tab.dryRun, {row, 1});
            ++row;
 
            grid.add(Label("Enable visu:"), {row, 0}).add(tab.visuEnabled, {row, 1});
            ++row;
 
            grid.add(tab.locationsMemoryxToArmemButton, {row, 0})
                .add(tab.locationsArmemToMemoryxButton, {row, 1})
                .add(tab.locationsClearArMemButton, {row, 2});
            ++row;
 
            grid.add(tab.graphMemoryxToArmemButton, {row, 0})
                .add(tab.graphArmemToMemoryxButton, {row, 1})
                .add(tab.graphClearArMemButton, {row, 2});
            ++row;
        }
 
        VBoxLayout root = {grid, VSpacer()};
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void
    Component::RemoteGui_update()
    {
        if (tab.sceneRefreshButton.wasClicked())
        {
            refreshScenes();
        }
 
        if (tab.locationsMemoryxToArmemButton.wasClicked())
        {
            locationsMemoryxToArmem(tab.sceneComboBox.getValue());
        }
        if (tab.locationsArmemToMemoryxButton.wasClicked())
        {
            locationsArmemToMemoryx(tab.sceneComboBox.getValue());
        }
        if (tab.graphMemoryxToArmemButton.wasClicked())
        {
            graphMemoryxToArmem(tab.sceneComboBox.getValue());
        }
        if (tab.graphArmemToMemoryxButton.wasClicked())
        {
            graphArmemToMemoryx(tab.sceneComboBox.getValue());
        }
 
        if (tab.locationsClearArMemButton.wasClicked())
        {
            clearArMemProviderSegment(proxies.locationWriter, getLocationProviderSegmentID());
        }
        if (tab.graphClearArMemButton.wasClicked())
        {
            clearArMemProviderSegment(proxies.graphWriter, getGraphProviderSegmentID());
        }
    }
 
    void
    Component::refreshScenes()
    {
        const ::Ice::StringSeq sceneNames = proxies.graphSegment->getScenes();
        createRemoteGuiTab(sceneNames);
    }
 
    void
    Component::locationsMemoryxToArmem(const std::string& sceneName)
    {
        const armem::Time time = armem::Time::Now();
        armem::Commit commit;
 
        memoryx::GraphNodeBaseList graphNodes = proxies.graphSegment->getNodesByScene(sceneName);
        for (memoryx::GraphNodeBasePtr& node : graphNodes)
        {
            ARMARX_CHECK_NOT_NULL(node);
            if (not node->isMetaEntity())
            {
                // ID is just some random MongoDB hash
                const std::string nodeId = node->getId();
                // This is the readable name entered in the GUI.
                const std::string name = node->getName();
 
                armarx::FramedPosePtr pose = armarx::FramedPosePtr::dynamicCast(node->getPose());
                ARMARX_CHECK_NOT_NULL(pose);
 
                FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(
                    proxies.graphNodePoseResolver->resolveToGlobalPose(node));
                ARMARX_CHECK_NOT_NULL(globalNodePose);
 
                // `pose` and `globalNodePose` seem to be identical. Is the last step necessary?
                // Maybe `pose` could be non-global.
 
                ARMARX_VERBOSE << std::setprecision(2) << std::fixed << "Processing node "
                               << (commit.updates.size() + 1) << "\n- ID: \t" << nodeId
                               << "\n- Name: \t" << name << "\n- Pose: \n"
                               << pose->toEigen() << "\n- Resolved global pose: \n"
                               << globalNodePose->toEigen();
 
                navigation::location::arondto::Location data;
                data.framedPose.header.frame = GlobalFrame; // TODO is that correct?
                data.framedPose.pose = globalNodePose->toEigen();
 
                armem::EntityUpdate& update = commit.add();
                update.entityID = getLocationProviderSegmentID().withEntityName(name);
                update.referencedTime = time;
                update.instancesData = {data.toAron()};
            }
        }
 
        if (not tab.dryRun.getValue())
        {
            armem::CommitResult result = proxies.locationWriter.commit(commit);
            if (result.allSuccess())
            {
                ARMARX_IMPORTANT << "Successfully exported " << result.results.size()
                                 << " locations from MemoryX to ArMem.";
            }
            else
            {
                ARMARX_WARNING << result.allErrorMessages();
            }
        }
        else
        {
            ARMARX_VERBOSE << "Dry Run - skipping commit.";
        }
    }
 
    void
    Component::locationsArmemToMemoryx(const std::string& sceneName)
    {
        ARMARX_IMPORTANT << "locationsArmemToMemoryx() is WIP!";
        (void)sceneName;
    }
 
    void
    Component::graphMemoryxToArmem(const std::string& sceneName)
    {
        memoryx::GraphNodeBaseList graphNodes = proxies.graphSegment->getNodesByScene(sceneName);
        navigation::core::Graph graph = toArmemGraph(graphNodes);
 
        if (tab.visuEnabled.getValue())
        {
            navigation::graph::GraphVisu visu;
            viz::Layer layer = arviz.layer("Graph '" + sceneName + "'");
 
            graph::visu::ObjectParserInfo info{objClient.fetchObjectPosesAsMap(),
                                               objClient.getObjectFinder().findAllObjects()};
            visu.draw(layer, graph, info);
 
            ARMARX_VERBOSE << "Visualize graph '" << sceneName << "' ...";
            arviz.commit(layer);
        }
 
        // Build ARON Graph
        navigation::core::arondto::Graph aron;
        toAron(aron, graph);
 
        // Build commit
        const armem::Time time = armem::Time::Now();
        armem::EntityUpdate update;
        update.entityID = getGraphProviderSegmentID().withEntityName(sceneName);
        update.referencedTime = time;
        update.instancesData = {aron.toAron()};
 
        if (not tab.dryRun.getValue())
        {
            armem::EntityUpdateResult result = proxies.graphWriter.commit(update);
            if (result.success)
            {
                ARMARX_IMPORTANT << "Successfully exported graph '" << sceneName
                                 << "' from MemoryX to ArMem.";
            }
            else
            {
                ARMARX_WARNING << result.errorMessage;
            }
        }
        else
        {
            ARMARX_VERBOSE << "Dry Run - skipping commit.";
        }
    }
 
    void
    Component::graphArmemToMemoryx(const std::string& sceneName)
    {
        ARMARX_IMPORTANT << "graphArmemToMemoryx() is WIP!";
        (void)sceneName;
    }
 
    void
    Component::clearArMemProviderSegment(armem::client::Writer& writer,
                                         const armem::MemoryID& providerSegmentID)
    {
        const bool clearWhenExists = true;
        auto result = writer.addSegment(providerSegmentID, clearWhenExists);
        if (result.success)
        {
            ARMARX_IMPORTANT << "Cleared ArMem provider segment " << providerSegmentID << ".";
        }
        else
        {
            ARMARX_WARNING << result.errorMessage;
        }
    }
 
    armem::MemoryID
    Component::getLocationProviderSegmentID()
    {
        return properties.locationCoreSegmentID.withProviderSegmentName(
            tab.providerSegmentLine.getValue());
    }
 
    armem::MemoryID
    Component::getGraphProviderSegmentID()
    {
        return properties.graphCoreSegmentID.withProviderSegmentName(
            tab.providerSegmentLine.getValue());
    }
 
    navigation::core::Graph
    Component::toArmemGraph(const memoryx::GraphNodeBaseList& graphNodes)
    {
        navigation::core::Graph graph;
        std::map<std::string, navigation::core::Graph::Vertex> vertexMap;
 
        // Add nodes
        semrel::ShapeID nextVertexID{0};
        for (memoryx::GraphNodeBasePtr node : graphNodes)
        {
            ARMARX_CHECK_NOT_NULL(node);
            if (not node->isMetaEntity())
            {
                // This is the readable name entered in the GUI.
                const std::string name = node->getName();
 
                FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(
                    proxies.graphNodePoseResolver->resolveToGlobalPose(node));
                ARMARX_CHECK_NOT_NULL(globalNodePose);
 
                ARMARX_VERBOSE << "\n- Adding node: \t" << name;
 
                navigation::core::Graph::Vertex vertex = graph.addVertex(nextVertexID);
                vertexMap.emplace(name, vertex);
 
                vertex.attrib().aron.vertexID = static_cast<long>(nextVertexID);
                toAron(vertex.attrib().aron.locationID,
                       getLocationProviderSegmentID().withEntityName(name));
                vertex.attrib().setPose(*globalNodePose);
 
                nextVertexID++;
            }
        }
        ARMARX_CHECK_EQUAL(graph.numVertices(), graphNodes.size());
        for (auto v : graph.vertices())
        {
            ARMARX_CHECK(v.attrib().hasName())
                << VAROUT(v.objectID()) << " | " << VAROUT(v.attrib().getName());
            ARMARX_CHECK(v.attrib().hasPose())
                << VAROUT(v.objectID()) << " | " << VAROUT(v.attrib().getName());
        }
 
        // Add edges
        for (memoryx::GraphNodeBasePtr node : graphNodes)
        {
            const auto& sourceVertex = vertexMap.at(node->getName());
            for (int i = 0; i < node->getOutdegree(); i++)
            {
                auto adjacent =
                    memoryx::GraphNodeBasePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
                ARMARX_CHECK_NOT_NULL(adjacent);
                const auto& targetVertex = vertexMap.at(adjacent->getName());
 
                ARMARX_VERBOSE << "\n- Adding edge: \t" << node->getName() << " \t-> "
                               << adjacent->getName();
                navigation::core::Graph::Edge edge = graph.addEdge(sourceVertex, targetVertex);
                edge.attrib().aron.sourceVertexID =
                    static_cast<long>(sourceVertex.attrib().aron.vertexID);
                edge.attrib().aron.targetVertexID =
                    static_cast<long>(targetVertex.attrib().aron.vertexID);
            }
        }
        ARMARX_CHECK_EQUAL(graph.numVertices(), graphNodes.size());
 
        return graph;
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
 
} // namespace armarx::navigation::components::graph_import_export