Graph.h

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/>.
 *
 * @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
 */
 
#pragma once
 
#include <optional>
#include <string>
#include <vector>
 
#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/aron/core/data/variant/forward_declarations.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <armarx/navigation/client/types.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/basic_types.h>
 
#include <SemanticObjectRelations/RelationGraph/RelationGraph.h>
 
namespace armarx::navigation::core
{
 
    struct VertexAttribs : public semrel::ShapeVertex
    {
        armarx::navigation::core::arondto::Vertex aron;
 
        std::string getName() const;
        bool hasName() const;
 
        bool hasPose() const;
        FramedPose getPose() const;
        void setPose(const FramedPose& pose);
 
        Pose requireGlobal() const;
 
    private:
        std::optional<FramedPose> _pose;
    };
 
    struct EdgeAttribs
    {
        armarx::navigation::core::arondto::Edge aron;
 
        client::GlobalPlanningStrategy strategy = client::GlobalPlanningStrategy::Free;
 
        // client::Config config;
        float&
        cost()
        {
            return m_value;
        }
 
        const float&
        cost() const
        {
            return m_value;
        }
 
        std::optional<core::GlobalTrajectory> trajectory = std::nullopt;
 
        float m_value{0.F};
    };
 
    struct GraphAttribs
    {
    };
 
    class Graph : public semrel::RelationGraph<VertexAttribs, EdgeAttribs, GraphAttribs>
    {
    public:
        /**
     * @brief Get a string representation of the given vertex for usage in `str()`.
     * By default, returns `"[<vertex descriptor> (ID <object ID>)]"`.
     */
        // virtual std::string strVertex(ConstVertex v) const;
        /**
     * @brief Get a string representation of the given edge for usage in `str()`.
     * By default, returns
     * `"[<source descriptor> (ID <source ID>) -> <target descriptor> (ID <target ID>)"`.
     */
        std::string strEdge(ConstEdge edge) const override;
    };
 
    using Graphs = std::vector<Graph>;
 
    using GraphPath = std::vector<Graph::ConstVertex>;
    std::vector<GraphPath> findPathsTo(Graph::ConstVertex vertex, const Graph& graph);
 
    Graph::ConstVertex getVertexByName(const std::string& vertexName, const Graph& graph);
 
    bool hasVertex(const std::string& vertexName, const Graph& graph);
    const core::Graph& getSubgraph(const std::string& vertexName, const Graphs& graphs);
 
    void toAron(arondto::Graph& dto, const Graph& bo);
    void fromAron(const arondto::Graph& dto, Graph& bo);
 
 
    // Location resolution
 
    void resolveLocation(Graph::Vertex& vertex, const aron::data::DictPtr& locationData);
 
    template <class MemoryContainerT>
    bool
    resolveLocation(Graph::Vertex& vertex, const MemoryContainerT& locationContainer)
    {
        armem::MemoryID locationID;
        fromAron(vertex.attrib().aron.locationID, locationID);
 
        if (const auto* instance = locationContainer.findLatestInstance(locationID))
        {
            resolveLocation(vertex, instance->data());
            return true;
        }
 
        return false;
    }
 
    template <class MemoryContainerT>
    void
    resolveLocations(Graph& graph, const MemoryContainerT& locationContainer)
    {
        for (auto vertex : graph.vertices())
        {
            resolveLocation(vertex, locationContainer);
        }
    }
 
 
} // namespace armarx::navigation::core