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