Fawkes API Fawkes Development Version
navgraph_stconstr_thread.cpp
1/***************************************************************************
2 * navgraph_stconstr_thread.cpp - static constraints for navgraph
3 *
4 * Created: Fri Jul 11 17:34:18 2014
5 * Copyright 2012-2014 Tim Niemueller [www.niemueller.de]
6 ****************************************************************************/
7
8/* This program is free software; you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License as published by
10 * the Free Software Foundation; either version 2 of the License, or
11 * (at your option) any later version.
12 *
13 * This program is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU Library General Public License for more details.
17 *
18 * Read the full text in the LICENSE.GPL file in the doc directory.
19 */
20
21#include "navgraph_stconstr_thread.h"
22
23#include <navgraph/constraints/polygon_edge_constraint.h>
24#include <navgraph/constraints/polygon_node_constraint.h>
25#include <navgraph/constraints/static_list_edge_constraint.h>
26#include <navgraph/constraints/static_list_edge_cost_constraint.h>
27#include <navgraph/constraints/static_list_node_constraint.h>
28#include <utils/misc/string_split.h>
29
30using namespace fawkes;
31
32/** @class NavGraphStaticConstraintsThread "navgraph_stconstr_thread.h"
33 * Thread to statically block certain nodes from config.
34 * @author Tim Niemueller
35 */
36
37/** Constructor. */
39: Thread("NavGraphStaticConstraintsThread", Thread::OPMODE_WAITFORWAKEUP)
40{
41}
42
43/** Destructor. */
45{
46}
47
48void
50{
51 std::vector<std::string> nodes = config->get_strings("/navgraph/static-constraints/nodes");
52
53 std::vector<std::string> c_edges = config->get_strings("/navgraph/static-constraints/edges");
54
55 std::vector<std::string> c_edge_costs =
56 config->get_strings("/navgraph/static-constraints/edge-costs");
57
58 std::vector<std::string> c_polygons =
59 config->get_strings("/navgraph/static-constraints/polygons");
60
61 std::vector<std::pair<std::string, std::string>> edges;
62 for (std::string &ce : c_edges) {
63 std::vector<std::string> node_names = str_split(ce, "--");
64 if (node_names.size() == 2) {
65 edges.push_back(std::make_pair(node_names[0], node_names[1]));
66 }
67 }
68
69 std::vector<std::tuple<std::string, std::string, float>> edge_costs;
70 for (const std::string &cec : c_edge_costs) {
71 std::vector<std::string> nodes_cost = str_split(cec, ":");
72 if (nodes_cost.size() != 2) {
73 throw Exception("Invalid edge costs (colon): %s", cec.c_str());
74 }
75 std::vector<std::string> node_names = str_split(nodes_cost[0], "--");
76 if (node_names.size() != 2) {
77 throw Exception("Invalid edge costs (node names): %s", cec.c_str());
78 }
79
80 edge_costs.push_back(
81 std::make_tuple(node_names[0], node_names[1], StringConversions::to_float(nodes_cost[1])));
82 }
83
84 std::vector<NavGraphPolygonConstraint::Polygon> polygons;
85 for (std::string &ce : c_polygons) {
86 std::vector<std::string> points = str_split(ce);
87 if (points.size() < 2) {
88 throw Exception("Invalid polygon, must have at least two nodes");
89 }
91 for (const std::string &p : points) {
92 std::vector<std::string> coord = str_split(p, ":");
93 if (coord.size() != 2) {
94 throw Exception("Polygon constraint with invalid point %s", p.c_str());
95 }
96 const NavGraphPolygonConstraint::Point polpoint(StringConversions::to_float(coord[0]),
97 StringConversions::to_float(coord[1]));
98 polygon.push_back(polpoint);
99 }
100 if (polygon.front().x != polygon.back().x || polygon.front().y != polygon.back().y) {
101 logger->log_info(name(), "Auto-circling constraint polygon %s", ce.c_str());
102 polygon.push_back(NavGraphPolygonConstraint::Point(polygon.front().x, polygon.front().y));
103 }
104 polygons.push_back(polygon);
105 }
106
107 node_constraint_ = new NavGraphStaticListNodeConstraint("static-nodes");
108 edge_constraint_ = new NavGraphStaticListEdgeConstraint("static-edges");
109 edge_cost_constraint_ = new NavGraphStaticListEdgeCostConstraint("static-edge-cost");
110 node_poly_constraint_ = new NavGraphPolygonNodeConstraint("static-node-polygon");
111 edge_poly_constraint_ = new NavGraphPolygonEdgeConstraint("static-edge-polygon");
112
113 const std::vector<NavGraphNode> &graph_nodes = navgraph->nodes();
114
115 std::list<std::string> missing_nodes;
116 for (std::string node_name : nodes) {
117 bool found = false;
118 for (const NavGraphNode &gnode : graph_nodes) {
119 if (gnode.name() == node_name) {
120 node_constraint_->add_node(gnode);
121 found = true;
122 break;
123 }
124 }
125
126 if (!found) {
127 missing_nodes.push_back(node_name);
128 }
129 }
130
131 if (!missing_nodes.empty()) {
132 std::list<std::string>::iterator n = missing_nodes.begin();
133 std::string err_str = *n++;
134 for (; n != missing_nodes.end(); ++n) {
135 err_str += ", " + *n;
136 }
137
138 delete node_constraint_;
139 delete edge_constraint_;
140 delete edge_cost_constraint_;
141 throw Exception("Some block nodes are not in graph: %s", err_str.c_str());
142 }
143
144 const std::vector<NavGraphEdge> &graph_edges = navgraph->edges();
145
146 std::list<std::pair<std::string, std::string>> missing_edges;
147 for (std::pair<std::string, std::string> edge : edges) {
148 bool found = false;
149 for (const NavGraphEdge &gedge : graph_edges) {
150 if ((edge.first == gedge.from() && edge.second == gedge.to())
151 || (edge.first == gedge.to() && edge.second == gedge.from())) {
152 edge_constraint_->add_edge(gedge);
153 found = true;
154 break;
155 }
156 }
157
158 if (!found) {
159 missing_edges.push_back(edge);
160 }
161 }
162
163 if (!missing_edges.empty()) {
164 std::list<std::pair<std::string, std::string>>::iterator n = missing_edges.begin();
165 std::string err_str = n->first + "--" + n->second;
166 for (++n; n != missing_edges.end(); ++n) {
167 err_str += ", " + n->first + "--" + n->second;
168 }
169
170 delete node_constraint_;
171 delete edge_constraint_;
172 delete edge_cost_constraint_;
173 throw Exception("Some blocked edges are not in graph: %s", err_str.c_str());
174 }
175
176 missing_edges.clear();
177 for (std::tuple<std::string, std::string, float> edge : edge_costs) {
178 bool found = false;
179 for (const NavGraphEdge &gedge : graph_edges) {
180 if ((std::get<0>(edge) == gedge.from() && std::get<1>(edge) == gedge.to())
181 || (std::get<0>(edge) == gedge.to() && std::get<1>(edge) == gedge.from())) {
182 edge_cost_constraint_->add_edge(gedge, std::get<2>(edge));
183 found = true;
184 break;
185 }
186 }
187
188 if (!found) {
189 missing_edges.push_back(std::make_pair(std::get<0>(edge), std::get<1>(edge)));
190 }
191 }
192
193 if (!missing_edges.empty()) {
194 std::list<std::pair<std::string, std::string>>::iterator n = missing_edges.begin();
195 std::string err_str = n->first + "--" + n->second;
196 for (++n; n != missing_edges.end(); ++n) {
197 err_str += ", " + n->first + "--" + n->second;
198 }
199
200 delete node_constraint_;
201 delete edge_constraint_;
202 delete edge_cost_constraint_;
203 throw Exception("Some edges for cost factors are not in graph: %s", err_str.c_str());
204 }
205
206 for (const NavGraphPolygonConstraint::Polygon &p : polygons) {
207 node_poly_constraint_->add_polygon(p);
208 edge_poly_constraint_->add_polygon(p);
209 }
210
211 /*
212 NavGraphPolygonNodeConstraint *pc = new NavGraphPolygonNodeConstraint("Poly");
213 NavGraphPolygonNodeConstraint::Polygon p;
214 p.push_back(NavGraphPolygonNodeConstraint::Point(0.0, 0.0));
215 p.push_back(NavGraphPolygonNodeConstraint::Point(1.0, 0.0));
216 p.push_back(NavGraphPolygonNodeConstraint::Point(1.0, 1.11));
217 p.push_back(NavGraphPolygonNodeConstraint::Point(0.0, 1.11));
218 p.push_back(NavGraphPolygonNodeConstraint::Point(0.0, 0.0));
219 pc->add_polygon(p);
220
221 NavGraphPolygonEdgeConstraint *pc = new NavGraphPolygonEdgeConstraint("Poly");
222 NavGraphPolygonEdgeConstraint::Polygon p;
223 p.push_back(NavGraphPolygonConstraint::Point(0.0, 0.0));
224 p.push_back(NavGraphPolygonConstraint::Point(1.0, 0.0));
225 p.push_back(NavGraphPolygonConstraint::Point(1.0, 1.11));
226 p.push_back(NavGraphPolygonConstraint::Point(0.0, 1.11));
227 p.push_back(NavGraphPolygonConstraint::Point(0.0, 0.0));
228 pc->add_polygon(p);
229 */
230
231 navgraph->constraint_repo()->register_constraint(node_constraint_);
232 navgraph->constraint_repo()->register_constraint(edge_constraint_);
233 navgraph->constraint_repo()->register_constraint(edge_cost_constraint_);
234 navgraph->constraint_repo()->register_constraint(node_poly_constraint_);
235 navgraph->constraint_repo()->register_constraint(edge_poly_constraint_);
236}
237
238void
240{
241 navgraph->constraint_repo()->unregister_constraint(node_constraint_->name());
242 navgraph->constraint_repo()->unregister_constraint(edge_constraint_->name());
243 navgraph->constraint_repo()->unregister_constraint(edge_cost_constraint_->name());
244 delete node_constraint_;
245 delete edge_constraint_;
246 delete edge_cost_constraint_;
247}
248
249void
251{
252}
virtual ~NavGraphStaticConstraintsThread()
Destructor.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual std::vector< std::string > get_strings(const char *path)=0
Get list of values from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
Definition: navgraph.h:44
std::string name()
Get name of constraint.
std::string name()
Get name of constraint.
Topological graph edge.
Definition: navgraph_edge.h:38
std::string name()
Get name of constraint.
Topological graph node.
Definition: navgraph_node.h:36
std::vector< Point > Polygon
A vector of points makes a polygon.
PolygonHandle add_polygon(const Polygon &polygon)
Add a polygon to constraint list.
Constraint that blocks nodes within and edges touching a polygon.
Constraint that blocks nodes inside a polygon.
Constraint that holds a list of edges to block.
void add_edge(const fawkes::NavGraphEdge &edge)
Add a single edge to constraint list.
Constraint that hold cost factors for a static list of edges.
void add_edge(const fawkes::NavGraphEdge &edge, const float cost_factor)
Add a single edge to constraint list.
Constraint that holds a list of nodes to block.
void add_node(const fawkes::NavGraphNode &node)
Add a single node to constraint list.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.
Simple point representation for polygon.