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
27namespace 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
48bool
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 */
66void
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 */
81void
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 */
93void
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 */
113bool
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 */
130const std::vector<std::pair<fawkes::NavGraphEdge, float>> &
132{
133 return edge_cost_list_;
134}
135
136/** Remove all edges. */
137void
139{
140 if (!edge_cost_list_buffer_.empty()) {
141 modified_ = true;
142 edge_cost_list_buffer_.clear();
143 }
144}
145
146float
148 const fawkes::NavGraphNode &to) noexcept
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
Base class for exceptions in Fawkes.
Definition: exception.h:36
void push_back_locked(const Type &x)
Push element to vector at back with lock protection.
Definition: lock_vector.h:121
void erase_locked(typename std::vector< Type >::iterator pos)
Erase given element with lock protection.
Definition: lock_vector.h:143
Constraint that can be queried for an edge cost factor.
Topological graph edge.
Definition: navgraph_edge.h:38
Topological graph node.
Definition: navgraph_node.h:36
const std::vector< std::pair< fawkes::NavGraphEdge, float > > & edge_cost_list() const
Get list of blocked edges.
virtual float cost_factor(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to) noexcept
Get cost factor for given edge.
void add_edges(const std::vector< std::pair< fawkes::NavGraphEdge, float > > &edge_costs)
Add multiple edges to constraint list.
void add_edge(const fawkes::NavGraphEdge &edge, const float cost_factor)
Add a single edge to constraint list.
virtual bool compute(void) noexcept
Perform compuations before graph search and to indicate re-planning.
void remove_edge(const fawkes::NavGraphEdge &edge)
Remove a single edge from the constraint list.
bool has_edge(const fawkes::NavGraphEdge &edge)
Check if constraint has a specific edge.
virtual ~NavGraphStaticListEdgeCostConstraint()
Virtual empty destructor.
Fawkes library namespace.