MemoryIDResolver.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    VisionX::ArmarXObjects::MemoryGrapher
 * @author     [Author Name] ( [Author Email] )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "MemoryIDResolver.h"
 
#include <SimoxUtility/color/cmaps.h>
 
#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error/mns.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/aron/common/util/object_finders.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.h>
#include <RobotAPI/libraries/aron/core/type/variant/All.h>
 
#include "MemoryGraph.h"
#include <SemanticObjectRelations/Shapes/Shape.h>
 
namespace armarx::armem::id_graph
{
 
    MemoryIDResolver::MemoryIDResolver(armarx::armem::client::MemoryNameSystem& mns) : mns(mns)
    {
    }
 
    MemoryGraph
    MemoryIDResolver::resolveToGraph(const armarx::armem::MemoryID& memoryID)
    {
        int shapeIDCounter = 0;
        MemoryGraph graph;
        graph.attrib().initialMemoryID = memoryID;
 
        // Breadth-first search
        std::map<armarx::armem::MemoryID, int> shapeIDs;
        std::vector<std::tuple<int, int, std::pair<std::string, bool>>> edges;
        std::vector<armarx::armem::MemoryID> currentLevel{memoryID};
        std::vector<armarx::armem::MemoryID> nextLevel;
        int depth = 0;
 
        shapeIDs[memoryID] = shapeIDCounter++;
 
        while (!currentLevel.empty())
        {
            for (const armarx::armem::MemoryID& vertID : currentLevel)
            {
                MemoryVertex vert;
                vert.memoryID = vertID;
                vert.depth = depth;
                graph.addVertex(semrel::ShapeID{shapeIDs[vertID]}, vert);
 
                auto memory = armem::resolveID(mns, memoryID);
                if (!memory)
                {
                    continue;
                }
                auto objectAndType = armem::extractDataAndType(memory.value(), vertID);
                if (!objectAndType)
                {
                    ARMARX_WARNING << "Could not extract Aron data and type from memory object.";
                    continue;
                }
                auto [aronData, aronType] = objectAndType.value();
 
                auto ids = getMemoryIDs(aronData, aronType);
                for (const auto& ref : ids)
                {
                    if (!ref.second.hasMemoryName())
                    {
                        // Nowheres are duplicated to make the graph more legible.
                        MemoryVertex nowhere;
                        nowhere.memoryID = {};
                        nowhere.depth = depth + 1;
                        int nowhereID = shapeIDCounter++;
                        graph.addVertex(semrel::ShapeID{nowhereID}, nowhere);
                        edges.emplace_back(
                            shapeIDs[vertID], nowhereID, std::make_pair(ref.first, true));
                        continue;
                    }
 
                    if (shapeIDs.find(ref.second) == shapeIDs.end())
                    {
                        shapeIDs[ref.second] = shapeIDCounter++;
                        nextLevel.push_back(ref.second);
                    }
                    edges.emplace_back(
                        shapeIDs[vertID], shapeIDs[ref.second], std::make_pair(ref.first, false));
                }
            }
            currentLevel = std::move(nextLevel);
            nextLevel.clear();
            ++depth;
        }
 
        for (const auto& [vert1, vert2, edgeProps] : edges)
        {
            MemoryEdge memEdge;
            memEdge.referenceKey = edgeProps.first;
            memEdge.isNowhereLink = edgeProps.second;
            graph.addEdge(semrel::ShapeID{vert1}, semrel::ShapeID{vert2}, memEdge);
        }
 
        bool style = true;
        if (style)
        {
            int maxDepth = 1;
            for (auto vertex : graph.vertices())
            {
                maxDepth = std::max(maxDepth, vertex.attrib().depth);
            }
 
            simox::ColorMap cmap = simox::color::cmaps::GnBu();
            cmap.set_vlimits(0, maxDepth + 1);
            for (auto vertex : graph.vertices())
            {
                MemoryVertex& attrib = vertex.attrib();
                attrib.fillColor = cmap(attrib.depth);
            }
        }
 
        return graph;
    }
 
    std::map<std::string, armarx::armem::MemoryID>
    MemoryIDResolver::getMemoryIDs(const armarx::aron::data::DictPtr& obj,
                                   const armarx::aron::type::ObjectPtr& type)
    {
        aron::BOSubObjectFinder<armem::arondto::MemoryID, armem::MemoryID> finder;
        armarx::aron::data::visitRecursive(finder, obj, type);
        return finder.getFoundObjects();
    }
 
} // namespace armarx::armem::id_graph