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 #if 0 /* just to make Emacs auto-indent happy */
29 }
30 #endif
31 
32 /** @class NavGraphConstraintRepo <navgraph/constraints/constraint_repo.h>
33  * Constraint repository to maintain blocks on nodes.
34  * @author Sebastian Reuter
35  * @author Tim Niemueller
36  */
37 
38 
39 /** Constructor. */
40 NavGraphConstraintRepo::NavGraphConstraintRepo()
41 {
42  modified_ = false;
43 }
44 
45 /** Destructor. */
46 NavGraphConstraintRepo::~NavGraphConstraintRepo()
47 {
48 }
49 
50 
51 /** Register a constraint.
52  * @param constraint node constraint to register
53  */
54 void
55 NavGraphConstraintRepo::register_constraint(NavGraphNodeConstraint* constraint)
56 {
57  modified_ = true;
58  node_constraints_.push_back(constraint);
59 }
60 
61 /** Register a constraint.
62  * @param constraint edge constraint to register
63  */
64 void
65 NavGraphConstraintRepo::register_constraint(NavGraphEdgeConstraint* constraint)
66 {
67  modified_ = true;
68  edge_constraints_.push_back(constraint);
69 }
70 
71 /** Register an edge cost constraint.
72  * @param constraint edge cost constraint to register
73  */
74 void
75 NavGraphConstraintRepo::register_constraint(NavGraphEdgeCostConstraint* constraint)
76 {
77  modified_ = true;
78  edge_cost_constraints_.push_back(constraint);
79 }
80 
81 
82 /** Unregister a constraint by name.
83  * @param name name of constraint to remove.
84  */
85 void
86 NavGraphConstraintRepo::unregister_constraint(std::string name)
87 {
88  modified_ = true;
89 
90  NodeConstraintList::iterator nc =
91  std::find_if(node_constraints_.begin(), node_constraints_.end(),
92  [&name](const NavGraphNodeConstraint *c) {
93  return *c == name;
94  });
95  if (nc != node_constraints_.end()) {
96  node_constraints_.erase(nc);
97  }
98 
99  EdgeConstraintList::iterator ec =
100  std::find_if(edge_constraints_.begin(), edge_constraints_.end(),
101  [&name](const NavGraphEdgeConstraint *c) {
102  return *c == name;
103  });
104  if (ec != edge_constraints_.end()) {
105  edge_constraints_.erase(ec);
106  }
107 
108  EdgeCostConstraintList::iterator ecc =
109  std::find_if(edge_cost_constraints_.begin(), edge_cost_constraints_.end(),
110  [&name](const NavGraphEdgeCostConstraint *c) {
111  return *c == name;
112  });
113  if (ecc != edge_cost_constraints_.end()) {
114  edge_cost_constraints_.erase(ecc);
115  }
116 }
117 
118 
119 /** Check by name if a constraint has been registered.
120  * @param name name of constraint to look for
121  * @return true if a constraint with the given name has been registered,
122  * false otherwise
123  */
124 bool
125 NavGraphConstraintRepo::has_constraint(std::string &name)
126 {
127  NodeConstraintList::iterator nc =
128  std::find_if(node_constraints_.begin(), node_constraints_.end(),
129  [&name](const NavGraphNodeConstraint *c) {
130  return *c == name;
131  });
132  if (nc != node_constraints_.end()) return true;
133 
134  EdgeConstraintList::iterator ec =
135  std::find_if(edge_constraints_.begin(), edge_constraints_.end(),
136  [&name](const NavGraphEdgeConstraint *c) {
137  return *c == name;
138  });
139  if (ec != edge_constraints_.end()) return true;
140 
141  EdgeCostConstraintList::iterator ecc =
142  std::find_if(edge_cost_constraints_.begin(), edge_cost_constraints_.end(),
143  [&name](const NavGraphEdgeCostConstraint *c) {
144  return *c == name;
145  });
146  if (ecc != edge_cost_constraints_.end()) return true;
147 
148  return false;
149 }
150 
151 
152 /** Get a node constraint by name.
153  * @param name name of constraint to retrieve
154  * @return if found returns a pointer to the node constraint, NULL if not found
155  */
157 NavGraphConstraintRepo::get_node_constraint(std::string &name)
158 {
159  NodeConstraintList::iterator it =
160  std::find_if(node_constraints_.begin(), node_constraints_.end(),
161  [&name](const NavGraphNodeConstraint *c) {
162  return *c == name;
163  });
164  if (it != node_constraints_.end()) {
165  return *it;
166  }
167 
168  return NULL;
169 }
170 
171 /** Get an edge constraint by name.
172  * @param name name of constraint to retrieve
173  * @return if found returns a pointer to the edge constraint, NULL if not found
174  */
176 NavGraphConstraintRepo::get_edge_constraint(std::string &name)
177 {
178  EdgeConstraintList::iterator it =
179  std::find_if(edge_constraints_.begin(), edge_constraints_.end(),
180  [&name](const NavGraphEdgeConstraint *c) {
181  return *c == name;
182  });
183  if (it != edge_constraints_.end()) {
184  return *it;
185  }
186 
187  return NULL;
188 }
189 
190 
191 /** Get an edge cost constraint by name.
192  * @param name name of constraint to retrieve
193  * @return if found returns a pointer to the edge cost constraint, NULL if not found
194  */
196 NavGraphConstraintRepo::get_edge_cost_constraint(std::string &name)
197 {
198  EdgeCostConstraintList::iterator it =
199  std::find_if(edge_cost_constraints_.begin(), edge_cost_constraints_.end(),
200  [&name](const NavGraphEdgeCostConstraint *c) {
201  return *c == name;
202  });
203  if (it != edge_cost_constraints_.end()) {
204  return *it;
205  }
206 
207  return NULL;
208 }
209 
210 
211 /** Get a list of registered node constraints.
212  * @return list of node constraints
213  */
215 NavGraphConstraintRepo::node_constraints() const
216 {
217  return node_constraints_;
218 }
219 
220 
221 /** Get a list of registered edge constraints.
222  * @return list of edge constraints
223  */
225 NavGraphConstraintRepo::edge_constraints() const
226 {
227  return edge_constraints_;
228 }
229 
230 /** Get a list of registered edge cost constraints.
231  * @return list of edge cost constraints
232  */
234 NavGraphConstraintRepo::edge_cost_constraints() const
235 {
236  return edge_cost_constraints_;
237 }
238 
239 
240 /** Check if there are any constraints at all.
241  * @return true if constraints have been registered, false otherwise
242  */
243 bool
244 NavGraphConstraintRepo::has_constraints() const
245 {
246  return (! (node_constraints_.empty() &&
247  edge_constraints_.empty() &&
248  edge_cost_constraints_.empty()));
249 }
250 
251 
252 /** Call compute method on all registered constraints.
253  * @return true if any constraint reported a change, false otherwise
254  */
255 bool
256 NavGraphConstraintRepo::compute()
257 {
258  bool modified = false;
259  for (fawkes::NavGraphNodeConstraint *c : node_constraints_) {
260  if (c->compute()) modified = true;
261  }
262  for (fawkes::NavGraphEdgeConstraint *c : edge_constraints_) {
263  if (c->compute()) modified = true;
264  }
265  for (fawkes::NavGraphEdgeCostConstraint *c : edge_cost_constraints_) {
266  if (c->compute()) modified = true;
267  }
268 
269  return modified;
270 }
271 
272 
273 /** Check if any constraint in the repo blocks the node.
274  * @param node Node to check for a block
275  * @return the (first) node constraint that blocked the node,
276  * NULL if the node is not blocked
277  */
279 NavGraphConstraintRepo::blocks(const fawkes::NavGraphNode &node)
280 {
281  for (fawkes::NavGraphNodeConstraint *c : node_constraints_) {
282  if (c->blocks(node)) {
283  return c;
284  }
285  }
286 
287  return NULL;
288 }
289 
290 
291 /** Check if any constraint in the repo blocks (some) nodes.
292  * @param nodes vector of nodes to check for a block
293  * @return map of blocked nodes, first element is the node name,
294  * second element is the name of the constraint that blocks the node.
295  * Nodes from @p nodes that are not blocked will not appear in the map.
296  */
297 std::map<std::string, std::string>
298 NavGraphConstraintRepo::blocks(const std::vector<fawkes::NavGraphNode> &nodes)
299 {
300  std::map<std::string, std::string> rv;
301  for (const fawkes::NavGraphNode &n : nodes) {
302  for (fawkes::NavGraphNodeConstraint *c : node_constraints_) {
303  if (c->blocks(n)) {
304  rv[n.name()] = c->name();
305  }
306  }
307  }
308 
309  return rv;
310 }
311 
312 
313 /** Check if any constraint in the repo blocks the edge.
314  * @param from node from which the edge originates
315  * @param to node to which the edge leads
316  * @return the (first) edge constraint that blocked the node,
317  * NULL if the node is not blocked
318  */
320 NavGraphConstraintRepo::blocks(const fawkes::NavGraphNode &from,
321  const fawkes::NavGraphNode &to)
322 {
323  for (fawkes::NavGraphEdgeConstraint *c : edge_constraints_) {
324  if (c->blocks(from, to)) {
325  return c;
326  }
327  }
328 
329  return NULL;
330 }
331 
332 /** Check if any constraint in the repo increases the cost of the edge.
333  * @param from node from which the edge originates
334  * @param to node to which the edge leads
335  * @return the (first) edge cost constraint that increases the cost of
336  * the node, i.e. that returns a cost factor >= 1.00001.
337  */
339 NavGraphConstraintRepo::increases_cost(const fawkes::NavGraphNode &from,
340  const fawkes::NavGraphNode &to)
341 {
342  for (fawkes::NavGraphEdgeCostConstraint *c : edge_cost_constraints_) {
343  if (c->cost_factor(from, to) >= 1.00001) {
344  return c;
345  }
346  }
347 
348  return NULL;
349 }
350 
351 
352 /** Check if any constraint in the repo increases the cost of the edge.
353  * @param from node from which the edge originates
354  * @param to node to which the edge leads
355  * @param cost_factor upon return with a non-NULL edge cost constraints
356  * contains the cost increase.
357  * @return the edge cost constraint that returns the highest increase
358  * in cost of the node (and by a cost factor of at least >= 1.00001).
359  */
361 NavGraphConstraintRepo::increases_cost(const fawkes::NavGraphNode &from,
362  const fawkes::NavGraphNode &to,
363  float & cost_factor)
364 {
365  float max_cost = 1.0;
367  for (fawkes::NavGraphEdgeCostConstraint *c : edge_cost_constraints_) {
368  float cost_factor = c->cost_factor(from, to);
369  if (cost_factor > max_cost) {
370  max_cost = cost_factor;
371  max_c = c;
372  }
373  }
374  if (max_cost >= 1.00001) {
375  cost_factor = max_cost;
376  return max_c;
377  }
378 
379  return NULL;
380 }
381 
382 
383 /** Check if any constraint in the repo blocks (some) edges.
384  * @param edges vector of edges to check for a block
385  * @return map of blocked edges, first element is a pair of the node names,
386  * second element is the name of the constraint that blocks the edge.
387  * Edges from @p edges that are not blocked will not appear in the map.
388  */
389 std::map<std::pair<std::string, std::string>, std::string>
390 NavGraphConstraintRepo::blocks(const std::vector<fawkes::NavGraphEdge> &edges)
391 {
392  std::map<std::pair<std::string, std::string>, std::string> rv;
393  for (const fawkes::NavGraphEdge &e : edges) {
394  for (fawkes::NavGraphEdgeConstraint *c : edge_constraints_) {
395  if (c->blocks(e.from_node(), e.to_node()) ){
396  rv[std::make_pair(e.from(), e.to())] = c->name();
397  }
398  }
399  }
400 
401  return rv;
402 }
403 
404 
405 
406 /** Get the highest increasing cost factor for an edge.
407  * This methods goes through all of the given edges and queries all
408  * edge cost constraints. If any constraint increases the cost of an
409  * edge (cost >= 1.00001), it adds a tuple of the start node name, end
410  * node name, constraint name, and cost factor of the constraint that
411  * returned the highest cost factor to the list.
412  *
413  * @param edges vector of edges to check for a block
414  * @return tuple of edges with increased costs consisting of start node name,
415  * target node name, name and cost factor of constraint returning the highest
416  * cost increase factor.
417  * Edges for which no increase has been indicated will not be returned in the
418  * list of tuples.
419  */
420 std::list<std::tuple<std::string, std::string, std::string, float>>
421 NavGraphConstraintRepo::cost_factor(const std::vector<fawkes::NavGraphEdge> &edges)
422 {
423  std::list<std::tuple<std::string, std::string, std::string, float>> rv;
424  for (const fawkes::NavGraphEdge &e : edges) {
425  float max_cost = 1.0;
427  for (fawkes::NavGraphEdgeCostConstraint *c : edge_cost_constraints_) {
428  float cost_factor = c->cost_factor(e.from_node(), e.to_node());
429  if (cost_factor > max_cost) {
430  max_cost = cost_factor;
431  max_c = c;
432  }
433  }
434  if (max_c && max_cost >= 1.00001) {
435  rv.push_back(std::make_tuple(e.from(), e.to(), max_c->name(), max_cost));
436  }
437  }
438 
439  return rv;
440 }
441 
442 
443 /** Get the highest increasing cost factor for an edge.
444  * This methods goes through all of the given edges and queries all
445  * edge cost constraints. If any constraint increases the cost of an
446  * edge (cost >= 1.00001), it adds a tuple of the start node name, end
447  * node name, constraint name, and cost factor of the constraint that
448  * returned the highest cost factor to the list.
449  *
450  * @param from start node of the edge
451  * @param to destination node of edge
452  * @return highest cost factor denoted by any edge or 1.0 if no constraint
453  * has been specified.
454  */
455 float
456 NavGraphConstraintRepo::cost_factor(const fawkes::NavGraphNode &from,
457  const fawkes::NavGraphNode &to)
458 {
459  float max_cost = 1.0;
460  for (fawkes::NavGraphEdgeCostConstraint *c : edge_cost_constraints_) {
461  float cost_factor = c->cost_factor(from, to);
462  if (cost_factor > max_cost) {
463  max_cost = cost_factor;
464  }
465  }
466 
467  return max_cost;
468 }
469 
470 
471 /** Check if the constraint repo has been modified.
472  * @param reset_modified true to reset the modified flag, false to leave it
473  * @return true if the constraint repo has been modified, false otherwise
474  */
475 bool
476 NavGraphConstraintRepo::modified(bool reset_modified)
477 {
478  if (reset_modified) {
479  bool rv = modified_;
480  modified_ =false;
481  return rv;
482  } else {
483  return modified_;
484  }
485 }
486 
487 } // namespace
std::vector< fawkes::NavGraphNodeConstraint * > NodeConstraintList
List of navgraph node constraints.
Constraint that can be queried to check if an edge is blocked.
Fawkes library namespace.
std::vector< fawkes::NavGraphEdgeConstraint * > EdgeConstraintList
List of navgraph edge constraints.
STL namespace.
Constraint that can be queried for an edge cost factor.
virtual float cost_factor(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)=0
Get cost factor for given edge.
Topological graph edge.
Definition: navgraph_edge.h:39
Topological graph node.
Definition: navgraph_node.h:38
Constraint that can be queried to check if a node is blocked.
std::string name()
Get name of constraint.
std::vector< fawkes::NavGraphEdgeCostConstraint * > EdgeCostConstraintList
List of navgraph edge cost constraints.