Fawkes API  Fawkes Development Version
static_list_edge_cost_constraint.cpp
1 /***************************************************************************
2  * static_list_edge_cost_constraint.cpp - edge constraint that holds cost
3  * factors for edges in a static list
4  *
5  * Created: Fri Jul 18 15:39:56 2014 (Ouro Branco Hotel, Joao Pessoa, Brazil)
6  * Copyright 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 
22 #include <navgraph/constraints/static_list_edge_cost_constraint.h>
23 #include <core/exception.h>
24 
25 #include <algorithm>
26 
27 namespace fawkes{
28 #if 0 /* just to make Emacs auto-indent happy */
29 }
30 #endif
31 
32 /** @class NavGraphStaticListEdgeCostConstraint <navgraph/constraints/static_list_edge_cost_constraint.h>
33  * Constraint that hold cost factors for a static list of edges.
34  * @author Tim Niemueller
35  */
36 
37 
38 /** Constructor.
39  * @param name name of edge constraint
40  */
43 {
44  modified_ = false;
45 }
46 
47 
48 /** Virtual empty destructor. */
50 {
51 }
52 
53 
54 bool
56 {
57  if (modified_) {
58  modified_ = false;
59  edge_cost_list_buffer_.lock();
60  edge_cost_list_ = edge_cost_list_buffer_;
61  edge_cost_list_buffer_.unlock();
62  return true;
63  } else {
64  return false;
65  }
66 }
67 
68 
69 /** Add a single edge to constraint list.
70  * @param edge edge to add to constraint list
71  * @param cost_factor cost factor for this edge, must be >= 1.00001
72  */
73 void
75  float cost_factor)
76 {
77  if (cost_factor < 1.00001) {
78  throw Exception("Invalid cost factor %f, must be >= 1.00001", cost_factor);
79  }
80  if (! has_edge(edge)) {
81  modified_ = true;
82  edge_cost_list_buffer_.push_back_locked(std::make_pair(edge, cost_factor));
83  }
84 }
85 
86 /** Add multiple edges to constraint list.
87  * @param edges edges to add to constraint list
88  */
89 void
91  const std::vector<std::pair<fawkes::NavGraphEdge, float>> &edges)
92 {
93  for (const std::pair<NavGraphEdge, float> &ec : edges) {
94  add_edge(ec.first, ec.second);
95  }
96 }
97 
98 /** Remove a single edge from the constraint list.
99  * @param edge edge to remote
100  */
101 void
103 {
104  std::vector<std::pair<NavGraphEdge, float>>::iterator ec
105  = std::find_if(edge_cost_list_buffer_.begin(), edge_cost_list_buffer_.end(),
106  [&edge](const std::pair<fawkes::NavGraphEdge, float> &p) {
107  return p.first == edge;
108  });
109 
110  if (ec != edge_cost_list_buffer_.end()) {
111  modified_ = true;
112  edge_cost_list_buffer_.erase_locked(ec);
113  }
114 }
115 
116 /** Check if constraint has a specific edge.
117  * @param edge edge to check
118  * @return true if edge is in list, false otherwise
119  */
120 bool
122 {
123  return (std::find_if(edge_cost_list_buffer_.begin(), edge_cost_list_buffer_.end(),
124  [&edge](const std::pair<fawkes::NavGraphEdge, float> &p) {
125  return p.first == edge;
126  })
127  != edge_cost_list_buffer_.end());
128 }
129 
130 
131 /** Get list of blocked edges.
132  * Note that this is the list as it is currently used on queries.
133  * Any operations (adding/removing edges) that have been performed
134  * without compute() being called are not reflected.
135  * @return list of blocked edges
136  */
137 const std::vector<std::pair<fawkes::NavGraphEdge, float>> &
139 {
140  return edge_cost_list_;
141 }
142 
143 
144 /** Remove all edges. */
145 void
147 {
148  if (! edge_cost_list_buffer_.empty()) {
149  modified_ = true;
150  edge_cost_list_buffer_.clear();
151  }
152 }
153 
154 
155 float
157  const fawkes::NavGraphNode &to) throw()
158 {
159  for (std::pair<NavGraphEdge, float> &ec : edge_cost_list_) {
160  if ((ec.first.from() == from.name() && ec.first.to() == to.name()) ||
161  (ec.first.from() == to.name() && ec.first.to() == from.name()) )
162  {
163  return ec.second;
164  }
165  }
166  return 1.0;
167 }
168 
169 
170 } // end of namespace fawkes
void add_edge(const fawkes::NavGraphEdge &edge, const float cost_factor)
Add a single edge to constraint list.
void erase_locked(typename std::vector< Type >::iterator pos)
Erase given element with lock protection.
Definition: lock_vector.h:153
Fawkes library namespace.
bool has_edge(const fawkes::NavGraphEdge &edge)
Check if constraint has a specific edge.
virtual bool compute(void)
Perform compuations before graph search and to indicate re-planning.
virtual void lock() const
Lock vector.
Definition: lock_vector.h:98
Constraint that can be queried for an edge cost factor.
void push_back_locked(const Type &x)
Push element to vector at back with lock protection.
Definition: lock_vector.h:129
virtual float cost_factor(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Get cost factor for given edge.
virtual ~NavGraphStaticListEdgeCostConstraint()
Virtual empty destructor.
virtual void unlock() const
Unlock vector.
Definition: lock_vector.h:118
Base class for exceptions in Fawkes.
Definition: exception.h:36
const std::vector< std::pair< fawkes::NavGraphEdge, float > > & edge_cost_list() const
Get list of blocked edges.
void add_edges(const std::vector< std::pair< fawkes::NavGraphEdge, float >> &edge_costs)
Add multiple edges to constraint list.
Topological graph edge.
Definition: navgraph_edge.h:39
Topological graph node.
Definition: navgraph_node.h:38
void remove_edge(const fawkes::NavGraphEdge &edge)
Remove a single edge from the constraint list.