GraphBuilder.cpp

Go to the documentation of this file.

#include "GraphBuilder.h"
 
#include <algorithm>
#include <cstddef>
#include <optional>
#include <vector>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <armarx/navigation/client/types.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/basic_types.h>
 
#include <SemanticObjectRelations/Shapes/Shape.h>
 
namespace armarx::navigation::server
{
    const core::arondto::Edge ARON_UNSET_EDGE_VALUES = []()
    {
        core::arondto::Edge edge;
        edge.sourceVertexID = -1;
        edge.targetVertexID = -1;
        return edge;
    }();
 
    void
    GraphBuilder::initialize(const core::Pose& pose)
    {
        core::VertexAttribs attribs;
        attribs.setPose(FramedPose(pose.matrix(), armarx::GlobalFrame, ""));
 
        const semrel::ShapeID shapeId(++vertexIdCnt);
        graph.addVertex(shapeId, attribs);
 
        startVertex = graph.vertex(shapeId);
 
        // pedantic check
        ARMARX_CHECK(startVertex->attrib().hasPose()) << "Start vertex must have a pose!";
 
        activeVertices_ = {shapeId};
    }
 
    // const std::vector<core::Graph::VertexDescriptor>&
    // GraphBuilder::activeVertices() const
    // {
    //     return activeVertices_;
    // }
    void
    GraphBuilder::connect(const core::Pose& pose, const client::GlobalPlanningStrategy& strategy)
    {
        ARMARX_TRACE;
        getGraph(); // FIXME remove
 
        core::VertexAttribs attribs;
        attribs.setPose(FramedPose(pose.matrix(), armarx::GlobalFrame, ""));
 
        const semrel::ShapeID shapeId(++vertexIdCnt); // TODO implement newShapeId()
        graph.addVertex(shapeId, attribs);
 
 
        // pedantic check
        ARMARX_CHECK(graph.vertex(shapeId).attrib().hasPose()) << "Vertex pose must be set!";
 
        ARMARX_TRACE;
        getGraph(); // FIXME remove
 
 
        for (const semrel::ShapeID& activeVertexId : activeVertices_)
        {
            graph.addEdge(activeVertexId,
                          shapeId,
                          core::Graph::EdgeAttrib{.aron = ARON_UNSET_EDGE_VALUES,
                                                  .strategy = strategy,
                                                  .trajectory = std::nullopt});
        }
 
        activeVertices_ = {shapeId};
    }
 
    void
    GraphBuilder::connect(const std::vector<core::Graph::VertexDescriptor>& routes,
                          const client::GlobalPlanningStrategy& strategy)
    {
        // for (const auto& vertex : activeVertices_)
        // {
        //     graph.addEdge()
        // }
    }
 
    semrel::ShapeID
    GraphBuilder::addOrGetVertex(const core::Graph::VertexAttrib& attributes)
    {
        const auto vertex = getVertexByAttributes(attributes);
        if (vertex.has_value())
        {
            return vertex.value();
        }
 
        const semrel::ShapeID shapeId(++vertexIdCnt);
        graph.addVertex(shapeId, attributes);
 
        return shapeId;
    }
 
    bool
    operator==(const core::Graph::VertexAttrib& lhs, const core::Graph::VertexAttrib& rhs)
    {
        if (lhs.hasLocationName() and rhs.hasLocationName())
        {
            return lhs.getLocationName() == rhs.getLocationName();
        };
 
        // otherwise:
        // (1) only one of both has a name => not equal
        // (2) both are 'anonymous' => not equal (by definition)
        return false;
    }
 
    std::optional<semrel::ShapeID>
    GraphBuilder::getVertexByAttributes(const core::Graph::VertexAttrib& attributes)
    {
        auto vertices = graph.vertices();
        const auto vIt = std::find_if(vertices.begin(),
                                      vertices.end(),
                                      [&attributes](const core::Graph::ConstVertex& vertex)
                                      { return vertex.attrib() == attributes; });
 
        if (vIt != vertices.end()) // vertex exists
        {
            ARMARX_DEBUG << "Found matching vertex " << QUOTED(vIt->attrib().getLocationName());
            return vIt->objectID();
        }
 
        return std::nullopt;
    }
 
    void
    GraphBuilder::connect(const std::vector<core::GraphPath>& routes,
                          const client::GlobalPlanningStrategy& strategy)
    {
        ARMARX_CHECK(not routes.empty())
            << "Routes must exist. Otherwise, the graph will not have any active connections!";
 
        // all active vertices will be connected to all routes
        for (const auto& vertex : activeVertices_)
        {
            for (const auto& route : routes)
            {
                if (route.empty())
                {
                    continue;
                }
 
                // The shape ids of the routes passed to this function are not meaningful. They will be changed.
                std::vector<semrel::ShapeID> routeShapeIds;
 
                ARMARX_TRACE;
 
                // add all vertices to the graph
                for (const auto& vertex : route)
                {
                    ARMARX_CHECK(vertex.attrib().hasPose())
                        << "Vertex on the route must have a pose!";
 
                    const semrel::ShapeID shapeId = addOrGetVertex(vertex.attrib());
                    routeShapeIds.push_back(shapeId);
 
                    // pedantic check
                    ARMARX_CHECK(graph.vertex(shapeId).attrib().hasPose())
                        << "Vertex pose must be set!";
                }
 
                ARMARX_TRACE;
 
                // add all edges to the graph (between consecutive vertices)
                for (size_t i = 0; i < route.size() - 1; i++)
                {
                    graph.addEdge(routeShapeIds.at(i),
                                  routeShapeIds.at(i + 1),
                                  core::Graph::EdgeAttrib{
                                      .aron = ARON_UNSET_EDGE_VALUES,
                                      .strategy = client::GlobalPlanningStrategy::Point2Point,
                                      .trajectory = std::nullopt});
                }
 
                ARMARX_TRACE;
                // connect `vertex` an this path
                graph.addEdge(graph.vertex(vertex).objectID(),
                              routeShapeIds.front(),
                              core::Graph::EdgeAttrib{.aron = ARON_UNSET_EDGE_VALUES,
                                                      .strategy = strategy,
                                                      .trajectory = std::nullopt});
            }
        }
 
        // By definition, all routes share the same final vertex, which is the new active vertex ...
        const auto targetDescriptor = routes.front().back().descriptor();
        // ... this is ensured by the following check ...
        ARMARX_CHECK(std::none_of(routes.begin(),
                                  routes.end(),
                                  [&targetDescriptor](const auto& route) -> bool
                                  { return route.back().descriptor() != targetDescriptor; }));
 
        // ... and will be the new active vertex.
        const std::optional<semrel::ShapeID> shapeId =
            getVertexByAttributes(routes.front().back().attrib());
        ARMARX_CHECK(shapeId.has_value()) << "Vertex exists. This should not have happened";
 
 
        activeVertices_ = {shapeId.value()};
    }
 
    inline void
    GraphBuilder::validate(auto&& vertices) const
    {
        ARMARX_TRACE;
        std::for_each(vertices.begin(),
                      vertices.end(),
                      [](const core::Graph::ConstVertex& vertex)
                      {
                          // ARMARX_CHECK(graph.vertex(vertex.objectID()).attrib().hasPose())
                          //       << "Check 1: Vertex " << QUOTED(vertex.attrib().getName())
                          //       << " with ID " << vertex.attrib().objectID
                          //       << " does not have pose attribute!";
 
                          ARMARX_CHECK(vertex.attrib().hasPose())
                              << "Check 2: Vertex " << QUOTED(vertex.attrib().getLocationName())
                              << " with ID " << vertex.attrib().objectID
                              << " does not have pose attribute!";
                      });
    }
 
    const core::Graph&
    GraphBuilder::getGraph() const
    {
        validate(graph.vertices());
        return graph;
    }
 
    core::Graph::VertexDescriptor
    GraphBuilder::goalVertex() const
    {
        ARMARX_CHECK(activeVertices_.size() == 1) << "There should be only one goal vertex.";
        return activeVertices_.front();
    }
 
    core::Pose
    GraphBuilder::goalPose() const
    {
        return getGraph().vertex(goalVertex()).attrib().requireGlobal();
    }
 
    // void
    // GraphBuilder::addEdge(const std::vector<core::Graph::VertexDescriptor>& sources,
    //                       const std::vector<core::Graph::VertexDescriptor>& targets)
    // {
 
    //     for (const core::Graph::VertexDescriptor& source : sources)
    //     {
    //         for (const core::Graph::VertexDescriptor& target : targets)
    //         {
    //             // connect source and target
    //             // FIXME implement graph.addEdge(source.)
    //         }
    //     }
    // }
} // namespace armarx::navigation::server