Fawkes API  Fawkes Development Version
constraint_repo.cpp
1 /***************************************************************************
2  * constraint_repo.cpp - navgraph constraint repository
3  *
4  * Created: Fr Mar 14 10:47:35 2014
5  * Copyright 2014 Sebastian Reuter
6  * 2014 Tim Niemueller
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 #include <navgraph/constraints/constraint_repo.h>
22 
23 #include <algorithm>
24 
25 using namespace std;
26 
27 namespace fawkes {
28 
29 /** @class NavGraphConstraintRepo <navgraph/constraints/constraint_repo.h>
30  * Constraint repository to maintain blocks on nodes.
31  * @author Sebastian Reuter
32  * @author Tim Niemueller
33  */
34 
35 /** Constructor. */
36 NavGraphConstraintRepo::NavGraphConstraintRepo()
37 {
38  modified_ = false;
39 }
40 
41 /** Destructor. */
42 NavGraphConstraintRepo::~NavGraphConstraintRepo()
43 {
44 }
45 
46 /** Register a constraint.
47  * @param constraint node constraint to register
48  */
49 void
50 NavGraphConstraintRepo::register_constraint(NavGraphNodeConstraint *constraint)
51 {
52  modified_ = true;
53  node_constraints_.push_back(constraint);
54 }
55 
56 /** Register a constraint.
57  * @param constraint edge constraint to register
58  */
59 void
60 NavGraphConstraintRepo::register_constraint(NavGraphEdgeConstraint *constraint)
61 {
62  modified_ = true;
63  edge_constraints_.push_back(constraint);
64 }
65 
66 /** Register an edge cost constraint.
67  * @param constraint edge cost constraint to register
68  */
69 void
70 NavGraphConstraintRepo::register_constraint(NavGraphEdgeCostConstraint *constraint)
71 {
72  modified_ = true;
73  edge_cost_constraints_.push_back(constraint);
74 }
75 
76 /** Unregister a constraint by name.
77  * @param name name of constraint to remove.
78  */
79 void
80 NavGraphConstraintRepo::unregister_constraint(std::string name)
81 {
82  modified_ = true;
83 
84  NodeConstraintList::iterator nc =
85  std::find_if(node_constraints_.begin(),
86  node_constraints_.end(),
87  [&name](const NavGraphNodeConstraint *c) { return *c == name; });
88  if (nc != node_constraints_.end()) {
89  node_constraints_.erase(nc);
90  }
91 
92  EdgeConstraintList::iterator ec =
93  std::find_if(edge_constraints_.begin(),
94  edge_constraints_.end(),
95  [&name](const NavGraphEdgeConstraint *c) { return *c == name; });
96  if (ec != edge_constraints_.end()) {
97  edge_constraints_.erase(ec);
98  }
99 
100  EdgeCostConstraintList::iterator ecc =
101  std::find_if(edge_cost_constraints_.begin(),
102  edge_cost_constraints_.end(),
103  [&name](const NavGraphEdgeCostConstraint *c) { return *c == name; });
104  if (ecc != edge_cost_constraints_.end()) {
105  edge_cost_constraints_.erase(ecc);
106  }
107 }
108 
109 /** Check by name if a constraint has been registered.
110  * @param name name of constraint to look for
111  * @return true if a constraint with the given name has been registered,
112  * false otherwise
113  */
114 bool
115 NavGraphConstraintRepo::has_constraint(std::string &name)
116 {
117  NodeConstraintList::iterator nc =
118  std::find_if(node_constraints_.begin(),
119  node_constraints_.end(),
120  [&name](const NavGraphNodeConstraint *c) { return *c == name; });
121  if (nc != node_constraints_.end())
122  return true;
123 
124  EdgeConstraintList::iterator ec =
125  std::find_if(edge_constraints_.begin(),
126  edge_constraints_.end(),
127  [&name](const NavGraphEdgeConstraint *c) { return *c == name; });
128  if (ec != edge_constraints_.end())
129  return true;
130 
131  EdgeCostConstraintList::iterator ecc =
132  std::find_if(edge_cost_constraints_.begin(),
133  edge_cost_constraints_.end(),
134  [&name](const NavGraphEdgeCostConstraint *c) { return *c == name; });
135  if (ecc != edge_cost_constraints_.end())
136  return true;
137 
138  return false;
139 }
140 
141 /** Get a node constraint by name.
142  * @param name name of constraint to retrieve
143  * @return if found returns a pointer to the node constraint, NULL if not found
144  */
146 NavGraphConstraintRepo::get_node_constraint(std::string &name)
147 {
148  NodeConstraintList::iterator it =
149  std::find_if(node_constraints_.begin(),
150  node_constraints_.end(),
151  [&name](const NavGraphNodeConstraint *c) { return *c == name; });
152  if (it != node_constraints_.end()) {
153  return *it;
154  }
155 
156  return NULL;
157 }
158 
159 /** Get an edge constraint by name.
160  * @param name name of constraint to retrieve
161  * @return if found returns a pointer to the edge constraint, NULL if not found
162  */
164 NavGraphConstraintRepo::get_edge_constraint(std::string &name)
165 {
166  EdgeConstraintList::iterator it =
167  std::find_if(edge_constraints_.begin(),
168  edge_constraints_.end(),
169  [&name](const NavGraphEdgeConstraint *c) { return *c == name; });
170  if (it != edge_constraints_.end()) {
171  return *it;
172  }
173 
174  return NULL;
175 }
176 
177 /** Get an edge cost constraint by name.
178  * @param name name of constraint to retrieve
179  * @return if found returns a pointer to the edge cost constraint, NULL if not found
180  */
182 NavGraphConstraintRepo::get_edge_cost_constraint(std::string &name)
183 {
184  EdgeCostConstraintList::iterator it =
185  std::find_if(edge_cost_constraints_.begin(),
186  edge_cost_constraints_.end(),
187  [&name](const NavGraphEdgeCostConstraint *c) { return *c == name; });
188  if (it != edge_cost_constraints_.end()) {
189  return *it;
190  }
191 
192  return NULL;
193 }
194 
195 /** Get a list of registered node constraints.
196  * @return list of node constraints
197  */
199 NavGraphConstraintRepo::node_constraints() const
200 {
201  return node_constraints_;
202 }
203 
204 /** Get a list of registered edge constraints.
205  * @return list of edge constraints
206  */
208 NavGraphConstraintRepo::edge_constraints() const
209 {
210  return edge_constraints_;
211 }
212 
213 /** Get a list of registered edge cost constraints.
214  * @return list of edge cost constraints
215  */
217 NavGraphConstraintRepo::edge_cost_constraints() const
218 {
219  return edge_cost_constraints_;
220 }
221 
222 /** Check if there are any constraints at all.
223  * @return true if constraints have been registered, false otherwise
224  */
225 bool
226 NavGraphConstraintRepo::has_constraints() const
227 {
228  return (
229  !(node_constraints_.empty() && edge_constraints_.empty() && edge_cost_constraints_.empty()));
230 }
231 
232 /** Call compute method on all registered constraints.
233  * @return true if any constraint reported a change, false otherwise
234  */
235 bool
236 NavGraphConstraintRepo::compute()
237 {
238  bool modified = false;
239  for (fawkes::NavGraphNodeConstraint *c : node_constraints_) {
240  if (c->compute())
241  modified = true;
242  }
243  for (fawkes::NavGraphEdgeConstraint *c : edge_constraints_) {
244  if (c->compute())
245  modified = true;
246  }
247  for (fawkes::NavGraphEdgeCostConstraint *c : edge_cost_constraints_) {
248  if (c->compute())
249  modified = true;
250  }
251 
252  return modified;
253 }
254 
255 /** Check if any constraint in the repo blocks the node.
256  * @param node Node to check for a block
257  * @return the (first) node constraint that blocked the node,
258  * NULL if the node is not blocked
259  */
261 NavGraphConstraintRepo::blocks(const fawkes::NavGraphNode &node)
262 {
263  for (fawkes::NavGraphNodeConstraint *c : node_constraints_) {
264  if (c->blocks(node)) {
265  return c;
266  }
267  }
268 
269  return NULL;
270 }
271 
272 /** Check if any constraint in the repo blocks (some) nodes.
273  * @param nodes vector of nodes to check for a block
274  * @return map of blocked nodes, first element is the node name,
275  * second element is the name of the constraint that blocks the node.
276  * Nodes from @p nodes that are not blocked will not appear in the map.
277  */
278 std::map<std::string, std::string>
279 NavGraphConstraintRepo::blocks(const std::vector<fawkes::NavGraphNode> &nodes)
280 {
281  std::map<std::string, std::string> rv;
282  for (const fawkes::NavGraphNode &n : nodes) {
283  for (fawkes::NavGraphNodeConstraint *c : node_constraints_) {
284  if (c->blocks(n)) {
285  rv[n.name()] = c->name();
286  }
287  }
288  }
289 
290  return rv;
291 }
292 
293 /** Check if any constraint in the repo blocks the edge.
294  * @param from node from which the edge originates
295  * @param to node to which the edge leads
296  * @return the (first) edge constraint that blocked the node,
297  * NULL if the node is not blocked
298  */
300 NavGraphConstraintRepo::blocks(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
301 {
302  for (fawkes::NavGraphEdgeConstraint *c : edge_constraints_) {
303  if (c->blocks(from, to)) {
304  return c;
305  }
306  }
307 
308  return NULL;
309 }
310 
311 /** Check if any constraint in the repo increases the cost of the edge.
312  * @param from node from which the edge originates
313  * @param to node to which the edge leads
314  * @return the (first) edge cost constraint that increases the cost of
315  * the node, i.e. that returns a cost factor >= 1.00001.
316  */
318 NavGraphConstraintRepo::increases_cost(const fawkes::NavGraphNode &from,
319  const fawkes::NavGraphNode &to)
320 {
321  for (fawkes::NavGraphEdgeCostConstraint *c : edge_cost_constraints_) {
322  if (c->cost_factor(from, to) >= 1.00001) {
323  return c;
324  }
325  }
326 
327  return NULL;
328 }
329 
330 /** Check if any constraint in the repo increases the cost of the edge.
331  * @param from node from which the edge originates
332  * @param to node to which the edge leads
333  * @param cost_factor upon return with a non-NULL edge cost constraints
334  * contains the cost increase.
335  * @return the edge cost constraint that returns the highest increase
336  * in cost of the node (and by a cost factor of at least >= 1.00001).
337  */
339 NavGraphConstraintRepo::increases_cost(const fawkes::NavGraphNode &from,
340  const fawkes::NavGraphNode &to,
341  float & cost_factor)
342 {
343  float max_cost = 1.0;
345  for (fawkes::NavGraphEdgeCostConstraint *c : edge_cost_constraints_) {
346  float edge_cost_factor = c->cost_factor(from, to);
347  if (edge_cost_factor > max_cost) {
348  max_cost = edge_cost_factor;
349  max_c = c;
350  }
351  }
352  if (max_cost >= 1.00001) {
353  cost_factor = max_cost;
354  return max_c;
355  }
356 
357  return NULL;
358 }
359 
360 /** Check if any constraint in the repo blocks (some) edges.
361  * @param edges vector of edges to check for a block
362  * @return map of blocked edges, first element is a pair of the node names,
363  * second element is the name of the constraint that blocks the edge.
364  * Edges from @p edges that are not blocked will not appear in the map.
365  */
366 std::map<std::pair<std::string, std::string>, std::string>
367 NavGraphConstraintRepo::blocks(const std::vector<fawkes::NavGraphEdge> &edges)
368 {
369  std::map<std::pair<std::string, std::string>, std::string> rv;
370  for (const fawkes::NavGraphEdge &e : edges) {
371  for (fawkes::NavGraphEdgeConstraint *c : edge_constraints_) {
372  if (c->blocks(e.from_node(), e.to_node())) {
373  rv[std::make_pair(e.from(), e.to())] = c->name();
374  }
375  }
376  }
377 
378  return rv;
379 }
380 
381 /** Get the highest increasing cost factor for an edge.
382  * This methods goes through all of the given edges and queries all
383  * edge cost constraints. If any constraint increases the cost of an
384  * edge (cost >= 1.00001), it adds a tuple of the start node name, end
385  * node name, constraint name, and cost factor of the constraint that
386  * returned the highest cost factor to the list.
387  *
388  * @param edges vector of edges to check for a block
389  * @return tuple of edges with increased costs consisting of start node name,
390  * target node name, name and cost factor of constraint returning the highest
391  * cost increase factor.
392  * Edges for which no increase has been indicated will not be returned in the
393  * list of tuples.
394  */
395 std::list<std::tuple<std::string, std::string, std::string, float>>
396 NavGraphConstraintRepo::cost_factor(const std::vector<fawkes::NavGraphEdge> &edges)
397 {
398  std::list<std::tuple<std::string, std::string, std::string, float>> rv;
399  for (const fawkes::NavGraphEdge &e : edges) {
400  float max_cost = 1.0;
402  for (fawkes::NavGraphEdgeCostConstraint *c : edge_cost_constraints_) {
403  float cost_factor = c->cost_factor(e.from_node(), e.to_node());
404  if (cost_factor > max_cost) {
405  max_cost = cost_factor;
406  max_c = c;
407  }
408  }
409  if (max_c && max_cost >= 1.00001) {
410  rv.push_back(std::make_tuple(e.from(), e.to(), max_c->name(), max_cost));
411  }
412  }
413 
414  return rv;
415 }
416 
417 /** Get the highest increasing cost factor for an edge.
418  * This methods goes through all of the given edges and queries all
419  * edge cost constraints. If any constraint increases the cost of an
420  * edge (cost >= 1.00001), it adds a tuple of the start node name, end
421  * node name, constraint name, and cost factor of the constraint that
422  * returned the highest cost factor to the list.
423  *
424  * @param from start node of the edge
425  * @param to destination node of edge
426  * @return highest cost factor denoted by any edge or 1.0 if no constraint
427  * has been specified.
428  */
429 float
430 NavGraphConstraintRepo::cost_factor(const fawkes::NavGraphNode &from,
431  const fawkes::NavGraphNode &to)
432 {
433  float max_cost = 1.0;
434  for (fawkes::NavGraphEdgeCostConstraint *c : edge_cost_constraints_) {
435  float cost_factor = c->cost_factor(from, to);
436  if (cost_factor > max_cost) {
437  max_cost = cost_factor;
438  }
439  }
440 
441  return max_cost;
442 }
443 
444 /** Check if the constraint repo has been modified.
445  * @param reset_modified true to reset the modified flag, false to leave it
446  * @return true if the constraint repo has been modified, false otherwise
447  */
448 bool
449 NavGraphConstraintRepo::modified(bool reset_modified)
450 {
451  if (reset_modified) {
452  bool rv = modified_;
453  modified_ = false;
454  return rv;
455  } else {
456  return modified_;
457  }
458 }
459 
460 } // namespace fawkes
fawkes::NavGraphNode
Definition: navgraph_node.h:38
fawkes::NavGraphEdge
Definition: navgraph_edge.h:40
fawkes::NavGraphConstraintRepo::NodeConstraintList
std::vector< fawkes::NavGraphNodeConstraint * > NodeConstraintList
List of navgraph node constraints.
Definition: constraint_repo.h:50
fawkes::NavGraphEdgeCostConstraint::cost_factor
virtual float cost_factor(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)=0
fawkes::NavGraphConstraintRepo::EdgeConstraintList
std::vector< fawkes::NavGraphEdgeConstraint * > EdgeConstraintList
List of navgraph edge constraints.
Definition: constraint_repo.h:52
fawkes::NavGraphNodeConstraint
Definition: node_constraint.h:38
fawkes::NavGraphEdgeConstraint
Definition: edge_constraint.h:38
fawkes::NavGraphEdgeCostConstraint
Definition: edge_cost_constraint.h:36
fawkes
fawkes::NavGraphEdgeCostConstraint::name
std::string name()
Get name of constraint.
Definition: edge_cost_constraint.cpp:82
fawkes::NavGraphConstraintRepo::EdgeCostConstraintList
std::vector< fawkes::NavGraphEdgeCostConstraint * > EdgeCostConstraintList
List of navgraph edge cost constraints.
Definition: constraint_repo.h:54