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
25using namespace std;
26
27namespace 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. */
37{
38 modified_ = false;
39}
40
41/** Destructor. */
43{
44}
45
46/** Register a constraint.
47 * @param constraint node constraint to register
48 */
49void
51{
52 modified_ = true;
53 node_constraints_.push_back(constraint);
54}
55
56/** Register a constraint.
57 * @param constraint edge constraint to register
58 */
59void
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 */
69void
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 */
79void
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 */
114bool
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 */
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 */
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 */
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 */
200{
201 return node_constraints_;
202}
203
204/** Get a list of registered edge constraints.
205 * @return list of edge constraints
206 */
209{
210 return edge_constraints_;
211}
212
213/** Get a list of registered edge cost constraints.
214 * @return list of edge cost constraints
215 */
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 */
225bool
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 */
235bool
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 */
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 */
278std::map<std::string, std::string>
279NavGraphConstraintRepo::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 */
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 */
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 */
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 */
366std::map<std::pair<std::string, std::string>, std::string>
367NavGraphConstraintRepo::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 */
395std::list<std::tuple<std::string, std::string, std::string, float>>
396NavGraphConstraintRepo::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 */
429float
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 */
448bool
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
std::vector< fawkes::NavGraphEdgeCostConstraint * > EdgeCostConstraintList
List of navgraph edge cost constraints.
fawkes::NavGraphNodeConstraint * get_node_constraint(std::string &name)
Get a node constraint by name.
fawkes::NavGraphEdgeConstraint * get_edge_constraint(std::string &name)
Get an edge constraint by name.
const EdgeConstraintList & edge_constraints() const
Get a list of registered edge constraints.
std::vector< fawkes::NavGraphNodeConstraint * > NodeConstraintList
List of navgraph node constraints.
std::vector< fawkes::NavGraphEdgeConstraint * > EdgeConstraintList
List of navgraph edge constraints.
void register_constraint(NavGraphNodeConstraint *constraint)
Register a constraint.
bool has_constraints() const
Check if there are any constraints at all.
void unregister_constraint(std::string name)
Unregister a constraint by name.
NavGraphEdgeCostConstraint * increases_cost(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Check if any constraint in the repo increases the cost of the edge.
fawkes::NavGraphEdgeCostConstraint * get_edge_cost_constraint(std::string &name)
Get an edge cost constraint by name.
bool modified(bool reset_modified=false)
Check if the constraint repo has been modified.
const NodeConstraintList & node_constraints() const
Get a list of registered node constraints.
float cost_factor(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Get the highest increasing cost factor for an edge.
NavGraphNodeConstraint * blocks(const fawkes::NavGraphNode &node)
Check if any constraint in the repo blocks the node.
bool compute()
Call compute method on all registered constraints.
const EdgeCostConstraintList & edge_cost_constraints() const
Get a list of registered edge cost constraints.
bool has_constraint(std::string &name)
Check by name if a constraint has been registered.
Constraint that can be queried to check if an edge is blocked.
Constraint that can be queried for an edge cost factor.
std::string name()
Get name of constraint.
virtual float cost_factor(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to) noexcept=0
Get cost factor for given edge.
Topological graph edge.
Definition: navgraph_edge.h:38
Constraint that can be queried to check if a node is blocked.
std::string name()
Get name of constraint.
Topological graph node.
Definition: navgraph_node.h:36
Fawkes library namespace.