Graph.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
 * astd::int64_t with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @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 "Graph.h"
 
#include <algorithm>
#include <cstdint>
#include <set>
#include <sstream>
#include <string>
#include <vector>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/libraries/armem_locations/aron/Location.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/framed.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/basic_types.h>
 
#include <SemanticObjectRelations/RelationGraph/RelationGraph.h>
#include <SemanticObjectRelations/Shapes/Shape.h>
 
namespace armarx::navigation::core
{
 
    std::string
    VertexAttribs::getLocationName() const
    {
        if (not hasLocationName())
        {
            return "anonymous";
        }
 
        return aron.locationID.entityName;
 
        // return aron.locationID.providerSegmentName + "/" + aron.locationID.entityName;
    }
 
    std::string
    VertexAttribs::getProviderName() const
    {
        return aron.locationID.providerSegmentName;
 
        // return aron.locationID.providerSegmentName + "/" + aron.locationID.entityName;
    }
 
    bool
    VertexAttribs::hasLocationName() const
    {
        return not(aron.locationID.providerSegmentName.empty() and
                   aron.locationID.entityName.empty());
    }
 
    bool
    VertexAttribs::hasPose() const
    {
        return _pose.has_value();
    }
 
    FramedPose
    VertexAttribs::getPose() const
    {
        ARMARX_CHECK(hasPose()) << "Pose for vertex " << QUOTED(getLocationName())  << " not set.";
        return _pose.value();
    }
 
    void
    VertexAttribs::setPose(const FramedPose& pose)
    {
        this->_pose = pose;
    }
 
    Pose
    VertexAttribs::requireGlobal() const
    {
        ARMARX_CHECK(hasPose()) << "Pose for vertex " << QUOTED(getLocationName())  << " not set.";
        ARMARX_CHECK_EMPTY(_pose->agent)
            << "Pose for vertex " << QUOTED(getLocationName())  << " must be in the global frame. Agent is '"
            << _pose->agent << "'.";
        ARMARX_CHECK_EQUAL(_pose->frame, armarx::GlobalFrame)
            << "Pose for vertex " << QUOTED(getLocationName())  << " must be in the global frame. Frame is '"
            << _pose->frame << "'.";
        return Pose(_pose->toEigen());
    }
 
    std::string
    Graph::strEdge(ConstEdge edge) const
    {
        std::stringstream ss;
        ss << semrel::RelationGraph<VertexAttribs, EdgeAttribs, GraphAttribs>::strEdge(edge);
 
        ss << " cost " << edge.attrib().cost()
           << ", type: " << static_cast<int>(edge.attrib().strategy) << " , has traj "
           << edge.attrib().trajectory.has_value();
        //    << ", aron: " << nlohmann::json(edge.attrib().aron);
 
        return ss.str();
    }
 
    std::string
    VertexAttribs::getFullName() const
    {
        return getProviderName() + "::" + getLocationName();
    }
} // namespace armarx::navigation::core
 
namespace armarx::navigation
{
 
 
    namespace detail
    {
 
        std::vector<core::GraphPath>
        findPathsTo(core::Graph::ConstVertex vertex,
                    const core::Graph& graph,
                    std::set<core::Graph::VertexDescriptor> visited)
        {
            using namespace core;
 
            // Stop recursion on loops.
            if (visited.count(vertex.descriptor()) > 0)
            {
                return {};
            }
            // Mark vertex as visited for this branch.
            visited.insert(vertex.descriptor());
 
            std::vector<GraphPath> paths;
            // In any case, vertex is reachable from itself.
            paths.push_back({vertex}); // Add [C]
 
            /**
            * If not a leaf, e.g:
            * A \
            *    C == vertex
            * B /
            * => [[C], [A, C], [B, C]}
            */
            for (const auto edge : vertex.inEdges())
            {
                std::vector<GraphPath> pathsToSource = findPathsTo(edge.source(), graph, visited);
                for (auto path : pathsToSource)
                {
                    // As we mark vertices as visited, it must not occur that we already have
                    // vertex in the path.
                    ARMARX_CHECK(std::find(path.begin(), path.end(), vertex) == path.end());
 
                    // e.g., path = [A]  => expand to [A, C]
                    path.push_back(vertex);
                    // Add to result paths
                    paths.push_back(path);
                }
            }
 
            return paths;
        }
    } // namespace detail
 
    std::vector<core::GraphPath>
    core::findPathsTo(Graph::ConstVertex vertex, const Graph& graph)
    {
        std::set<Graph::VertexDescriptor> visited; // empty
        return detail::findPathsTo(vertex, graph, visited);
    }
 
    void
    core::toAron(arondto::Graph& dto, const Graph& bo)
    {
        dto = {};
        for (auto vertex : bo.vertices())
        {
            auto& v = dto.vertices.emplace_back(vertex.attrib().aron);
            v.vertexID = static_cast<std::int64_t>(vertex.objectID());
        }
        ARMARX_CHECK_EQUAL(dto.vertices.size(), bo.numVertices());
 
        for (auto edge : bo.edges())
        {
            auto& e = dto.edges.emplace_back(edge.attrib().aron);
            e.sourceVertexID = static_cast<std::int64_t>(edge.sourceObjectID());
            e.targetVertexID = static_cast<std::int64_t>(edge.targetObjectID());
        }
        ARMARX_CHECK_EQUAL(dto.edges.size(), bo.numEdges());
    }
 
    core::Graph::ConstVertex
    core::getVertexByName(const std::string& vertexName, const core::Graph& graph)
    {
        auto vertices = graph.vertices(); // begin() and end() are not const ...
        const auto vertexIt =
            std::find_if(vertices.begin(),
                         vertices.end(),
                         [&vertexName](const core::Graph::ConstVertex& vertex) -> bool
                         { return vertex.attrib().getLocationName() == vertexName; });
 
        ARMARX_CHECK(vertexIt != vertices.end())
            << "No vertex found with id " << QUOTED(vertexName);
        return *vertexIt;
    }
 
    bool
    core::hasVertex(const std::string& vertexName, const core::Graph& graph)
    {
        auto vertices = graph.vertices(); // begin() and end() are not const ...
        return std::any_of(vertices.begin(),
                           vertices.end(),
                           [&vertexName](const core::Graph::ConstVertex& vertex) -> bool
                           { return vertex.attrib().getLocationName() == vertexName; });
    }
 
    const core::Graph&
    core::getSubgraph(const std::string& vertexName, const core::Graphs& graphs)
    {
        auto graphIt = std::find_if(graphs.begin(),
                                    graphs.end(),
                                    [&vertexName](const core::Graph& graph) -> bool
                                    { return hasVertex(vertexName, graph); });
 
        ARMARX_CHECK(graphIt != graphs.end())
            << "No subgraph found for vertex " << QUOTED(vertexName);
        return *graphIt;
    }
 
    void
    core::fromAron(const arondto::Graph& dto, Graph& bo)
    {
        bo = {};
        for (const arondto::Vertex& vertex : dto.vertices)
        {
            auto v = bo.addVertex(semrel::ShapeID(vertex.vertexID));
            v.attrib().aron = vertex;
        }
        ARMARX_CHECK_EQUAL(bo.numVertices(), dto.vertices.size());
 
        for (const arondto::Edge& edge : dto.edges)
        {
            auto e = bo.addEdge(semrel::ShapeID(edge.sourceVertexID),
                                semrel::ShapeID(edge.targetVertexID));
            e.attrib().aron = edge;
        }
        ARMARX_CHECK_EQUAL(bo.numEdges(), dto.edges.size());
    }
 
    void
    core::resolveLocation(Graph::Vertex& vertex, const aron::data::DictPtr& locationData)
    {
        navigation::location::arondto::Location dto;
        dto.fromAron(locationData);
        FramedPose pose;
        fromAron(dto.framedPose, pose);
 
        vertex.attrib().setPose(pose);
    }
 
 
} // namespace armarx::navigation