Graph.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
17  * @date 2021
18  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
19  * GNU General Public License
20  */
21 
22 #pragma once
23 
24 #include <optional>
25 
26 #include <boost/graph/properties.hpp>
27 
31 
34 #include <armarx/navigation/core/aron/Graph.aron.generated.h>
37 
38 #include <SemanticObjectRelations/RelationGraph/RelationGraph.h>
39 
41 {
42 
43  struct VertexAttribs : public semrel::ShapeVertex
44  {
45  armarx::navigation::core::arondto::Vertex aron;
46 
47  std::string getName() const;
48  bool hasName() const;
49 
50  bool hasPose() const;
51  FramedPose getPose() const;
52  void setPose(const FramedPose& pose);
53 
54  Pose requireGlobal() const;
55 
56  private:
57  std::optional<FramedPose> _pose;
58  };
59 
60  struct EdgeAttribs
61  {
62  armarx::navigation::core::arondto::Edge aron;
63 
65 
66  // client::Config config;
67  float&
68  cost()
69  {
70  return m_value;
71  }
72 
73  const float&
74  cost() const
75  {
76  return m_value;
77  }
78 
79  std::optional<core::GlobalTrajectory> trajectory = std::nullopt;
80 
81  float m_value{0.F};
82  };
83 
84  struct GraphAttribs
85  {
86  };
87 
88  class Graph : public semrel::RelationGraph<VertexAttribs, EdgeAttribs, GraphAttribs>
89  {
90  public:
91  /**
92  * @brief Get a string representation of the given vertex for usage in `str()`.
93  * By default, returns `"[<vertex descriptor> (ID <object ID>)]"`.
94  */
95  // virtual std::string strVertex(ConstVertex v) const;
96  /**
97  * @brief Get a string representation of the given edge for usage in `str()`.
98  * By default, returns
99  * `"[<source descriptor> (ID <source ID>) -> <target descriptor> (ID <target ID>)"`.
100  */
101  std::string strEdge(ConstEdge edge) const override;
102  };
103 
104  using Graphs = std::vector<Graph>;
105 
106  using GraphPath = std::vector<Graph::ConstVertex>;
107  std::vector<GraphPath> findPathsTo(Graph::ConstVertex vertex, const Graph& graph);
108 
109  Graph::ConstVertex getVertexByName(const std::string& vertexName, const Graph& graph);
110 
111  bool hasVertex(const std::string& vertexName, const Graph& graph);
112  const core::Graph& getSubgraph(const std::string& vertexName, const Graphs& graphs);
113 
114  void toAron(arondto::Graph& dto, const Graph& bo);
115  void fromAron(const arondto::Graph& dto, Graph& bo);
116 
117 
118  // Location resolution
119 
120  void resolveLocation(Graph::Vertex& vertex, const aron::data::DictPtr& locationData);
121 
122  template <class MemoryContainerT>
123  bool
124  resolveLocation(Graph::Vertex& vertex, const MemoryContainerT& locationContainer)
125  {
126  armem::MemoryID locationID;
127  fromAron(vertex.attrib().aron.locationID, locationID);
128 
129  if (const auto* instance = locationContainer.findLatestInstance(locationID))
130  {
131  resolveLocation(vertex, instance->data());
132  return true;
133  }
134 
135  return false;
136  }
137 
138  template <class MemoryContainerT>
139  void
140  resolveLocations(Graph& graph, const MemoryContainerT& locationContainer)
141  {
142  for (auto vertex : graph.vertices())
143  {
144  resolveLocation(vertex, locationContainer);
145  }
146  }
147 
148 
149 } // namespace armarx::navigation::core
armarx::navigation::core::resolveLocations
void resolveLocations(Graph &graph, const MemoryContainerT &locationContainer)
Definition: Graph.h:140
armarx::navigation::core::resolveLocation
void resolveLocation(Graph::Vertex &vertex, const aron::data::DictPtr &locationData)
Definition: Graph.cpp:244
armarx::navigation::core::VertexAttribs::requireGlobal
Pose requireGlobal() const
Definition: Graph.cpp:78
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
types.h
basic_types.h
armarx::FramedPose
The FramedPose class.
Definition: FramedPose.h:258
armarx::navigation::client::GlobalPlanningStrategy::Free
@ Free
armarx::navigation::core::GraphAttribs
Definition: Graph.h:84
armarx::navigation::core::fromAron
void fromAron(const arondto::GlobalTrajectoryPoint &dto, GlobalTrajectoryPoint &bo)
Definition: aron_conversions.cpp:25
MemoryID.h
armarx::navigation::core::VertexAttribs::getName
std::string getName() const
Definition: Graph.cpp:41
armarx::navigation::core::EdgeAttribs::cost
const float & cost() const
Definition: Graph.h:74
forward_declarations.h
semrel::RelationGraph
Definition: forward_declarations.h:28
armarx::navigation::core::Graphs
std::vector< Graph > Graphs
Definition: Graph.h:104
armarx::navigation::core
This file is part of ArmarX.
Definition: aron_conversions.cpp:13
armarx::navigation::core::toAron
void toAron(arondto::GlobalTrajectoryPoint &dto, const GlobalTrajectoryPoint &bo)
Definition: aron_conversions.cpp:18
armarx::navigation::core::hasVertex
bool hasVertex(const std::string &vertexName, const Graph &graph)
Definition: Graph.cpp:201
armarx::navigation::core::Graph::strEdge
std::string strEdge(ConstEdge edge) const override
Get a string representation of the given vertex for usage in str().
Definition: Graph.cpp:91
armarx::navigation::core::VertexAttribs::hasPose
bool hasPose() const
Definition: Graph.cpp:59
armarx::navigation::core::EdgeAttribs::strategy
client::GlobalPlanningStrategy strategy
Definition: Graph.h:64
armarx::armem::MemoryID
A memory ID.
Definition: MemoryID.h:47
armarx::navigation::core::Graph
Definition: Graph.h:88
armarx::navigation::core::findPathsTo
std::vector< GraphPath > findPathsTo(Graph::ConstVertex vertex, const Graph &graph)
Definition: Graph.cpp:159
armarx::Graph
boost::subgraph< CloudGraph > Graph
Definition: Common.h:54
armarx::navigation::core::EdgeAttribs
Definition: Graph.h:60
armarx::navigation::core::VertexAttribs::getPose
FramedPose getPose() const
Definition: Graph.cpp:65
armarx::navigation::core::VertexAttribs::hasName
bool hasName() const
Definition: Graph.cpp:52
location.h
aron_conversions.h
armarx::navigation::core::EdgeAttribs::aron
armarx::navigation::core::arondto::Edge aron
Definition: Graph.h:62
armarx::aron::data::DictPtr
std::shared_ptr< Dict > DictPtr
Definition: Dict.h:41
armarx::navigation::core::VertexAttribs
Definition: Graph.h:43
armarx::navigation::client::GlobalPlanningStrategy
GlobalPlanningStrategy
Definition: types.h:27
armarx::navigation::core::GraphPath
std::vector< Graph::ConstVertex > GraphPath
Definition: Graph.h:106
armarx::navigation::core::EdgeAttribs::cost
float & cost()
Definition: Graph.h:68
Trajectory.h
armarx::navigation::core::EdgeAttribs::m_value
float m_value
Definition: Graph.h:81
armarx::navigation::core::VertexAttribs::aron
armarx::navigation::core::arondto::Vertex aron
Definition: Graph.h:45
armarx::navigation::core::VertexAttribs::setPose
void setPose(const FramedPose &pose)
Definition: Graph.cpp:72
armarx::aron::bo
const std::optional< BoT > & bo
Definition: aron_conversions.h:168
armarx::navigation::core::EdgeAttribs::trajectory
std::optional< core::GlobalTrajectory > trajectory
Definition: Graph.h:79
armarx::navigation::core::getSubgraph
const core::Graph & getSubgraph(const std::string &vertexName, const Graphs &graphs)
Definition: Graph.cpp:211
armarx::navigation::core::getVertexByName
Graph::ConstVertex getVertexByName(const std::string &vertexName, const Graph &graph)
Definition: Graph.cpp:186