graph.cpp

Go to the documentation of this file.

#include "graph.h"
 
#include <VirtualRobot/ManipulationObject.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
#include "../SimoxObjectShape.h"
 
namespace armarx
{
 
    semantic::data::Graph
    semantic::toIce(const semrel::AttributedGraph& input)
    {
        static const int INDENT = 2;
 
        data::Graph result;
        result.attributes = input.attrib().json.dump(INDENT);
 
        for (auto inputVertex : input.vertices())
        {
            data::GraphVertex& resultVertex = result.vertices.emplace_back();
            resultVertex.id = static_cast<long>(inputVertex.descriptor());
            resultVertex.attributes = inputVertex.attrib().json.dump(INDENT);
        }
 
        for (auto inputEdge : input.edges())
        {
            data::GraphEdge& resultEdge = result.edges.emplace_back();
            resultEdge.sourceID = static_cast<long>(inputEdge.sourceDescriptor());
            resultEdge.targetID = static_cast<long>(inputEdge.targetDescriptor());
            resultEdge.attributes = inputEdge.attrib().json.dump(INDENT);
        }
 
        return result;
    }
 
    semrel::AttributedGraph
    semantic::fromIce(const data::Graph& graph)
    {
        semrel::AttributedGraph result;
        result.attrib().json = nlohmann::json::parse(graph.attributes);
 
        std::map<long, semrel::AttributedGraph::VertexDescriptor> id2desc;
        for (const data::GraphVertex& vertex : graph.vertices)
        {
            semrel::AttributedGraph::Vertex resultVertex =
                result.addVertex(semrel::ShapeID(vertex.id));
            resultVertex.attrib().json = nlohmann::json::parse(vertex.attributes);
            id2desc.emplace(vertex.id, resultVertex.descriptor());
        }
 
        for (const data::GraphEdge& edge : graph.edges)
        {
            auto sourceDesc = id2desc.find(edge.sourceID);
            if (sourceDesc == id2desc.end())
            {
                throw std::runtime_error("Source ID not found: " + std::to_string(edge.sourceID));
            }
            auto targetDesc = id2desc.find(edge.targetID);
            if (targetDesc == id2desc.end())
            {
                throw std::runtime_error("Target ID not found: " + std::to_string(edge.targetID));
            }
 
            semrel::AttributedGraph::Edge resultEdge =
                result.addEdge(sourceDesc->second, targetDesc->second);
            resultEdge.attrib().json = nlohmann::json::parse(edge.attributes);
        }
 
        return result;
    }
 
} // namespace armarx