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/static_list_node_constraint.h>
24 #include <navgraph/constraints/static_list_edge_constraint.h>
25 #include <navgraph/constraints/static_list_edge_cost_constraint.h>
26 #include <navgraph/constraints/polygon_node_constraint.h>
27 #include <navgraph/constraints/polygon_edge_constraint.h>
28 #include <utils/misc/string_split.h>
29 
30 using 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 
48 void
50 {
51  std::vector<std::string> nodes =
52  config->get_strings("/navgraph/static-constraints/nodes");
53 
54  std::vector<std::string> c_edges =
55  config->get_strings("/navgraph/static-constraints/edges");
56 
57  std::vector<std::string> c_edge_costs =
58  config->get_strings("/navgraph/static-constraints/edge-costs");
59 
60  std::vector<std::string> c_polygons =
61  config->get_strings("/navgraph/static-constraints/polygons");
62 
63  std::vector<std::pair<std::string, std::string>> edges;
64  for (std::string & ce : c_edges) {
65  std::vector<std::string> node_names = str_split(ce, "--");
66  if (node_names.size() == 2) {
67  edges.push_back(std::make_pair(node_names[0], node_names[1]));
68  }
69  }
70 
71  std::vector<std::tuple<std::string, std::string, float>> edge_costs;
72  for (const std::string & cec : c_edge_costs) {
73  std::vector<std::string> nodes_cost = str_split(cec, ":");
74  if (nodes_cost.size() != 2) {
75  throw Exception("Invalid edge costs (colon): %s", cec.c_str());
76  }
77  std::vector<std::string> node_names = str_split(nodes_cost[0], "--");
78  if (node_names.size() != 2) {
79  throw Exception("Invalid edge costs (node names): %s", cec.c_str());
80  }
81 
82  edge_costs.push_back(std::make_tuple(node_names[0], node_names[1],
83  StringConversions::to_float(nodes_cost[1])));
84  }
85 
86  std::vector<NavGraphPolygonConstraint::Polygon> polygons;
87  for (std::string & ce : c_polygons) {
88  std::vector<std::string> points = str_split(ce);
89  if (points.size() < 2) {
90  throw Exception("Invalid polygon, must have at least two nodes");
91  }
93  for (const std::string &p : points) {
94  std::vector<std::string> coord = str_split(p, ":");
95  if (coord.size() != 2) {
96  throw Exception("Polygon constraint with invalid point %s", p.c_str());
97  }
99  polpoint(StringConversions::to_float(coord[0]),
100  StringConversions::to_float(coord[1]));
101  polygon.push_back(polpoint);
102  }
103  if (polygon.front().x != polygon.back().x || polygon.front().y != polygon.back().y) {
104  logger->log_info(name(), "Auto-circling constraint polygon %s",
105  ce.c_str());
106  polygon.push_back(NavGraphPolygonConstraint::Point(polygon.front().x,
107  polygon.front().y));
108  }
109  polygons.push_back(polygon);
110  }
111 
112  node_constraint_ = new NavGraphStaticListNodeConstraint("static-nodes");
113  edge_constraint_ = new NavGraphStaticListEdgeConstraint("static-edges");
114  edge_cost_constraint_ = new NavGraphStaticListEdgeCostConstraint("static-edge-cost");
115  node_poly_constraint_ = new NavGraphPolygonNodeConstraint("static-node-polygon");
116  edge_poly_constraint_ = new NavGraphPolygonEdgeConstraint("static-edge-polygon");
117 
118  const std::vector<NavGraphNode> &graph_nodes = navgraph->nodes();
119 
120  std::list<std::string> missing_nodes;
121  for (std::string node_name : nodes) {
122  bool found = false;
123  for (const NavGraphNode &gnode : graph_nodes) {
124  if (gnode.name() == node_name) {
125  node_constraint_->add_node(gnode);
126  found = true;
127  break;
128  }
129  }
130 
131  if (!found) {
132  missing_nodes.push_back(node_name);
133  }
134  }
135 
136  if (! missing_nodes.empty()) {
137  std::list<std::string>::iterator n = missing_nodes.begin();
138  std::string err_str = *n++;
139  for (;n != missing_nodes.end(); ++n) {
140  err_str += ", " + *n;
141  }
142 
143  delete node_constraint_;
144  delete edge_constraint_;
145  delete edge_cost_constraint_;
146  throw Exception("Some block nodes are not in graph: %s", err_str.c_str());
147  }
148 
149  const std::vector<NavGraphEdge> &graph_edges = navgraph->edges();
150 
151 
152  std::list<std::pair<std::string, std::string>> missing_edges;
153  for (std::pair<std::string, std::string> edge : edges) {
154  bool found = false;
155  for (const NavGraphEdge &gedge : graph_edges) {
156  if ((edge.first == gedge.from() && edge.second == gedge.to()) ||
157  (edge.first == gedge.to() && edge.second == gedge.from()))
158  {
159  edge_constraint_->add_edge(gedge);
160  found = true;
161  break;
162  }
163  }
164 
165  if (!found) {
166  missing_edges.push_back(edge);
167  }
168  }
169 
170  if (! missing_edges.empty()) {
171  std::list<std::pair<std::string, std::string>>::iterator n = missing_edges.begin();
172  std::string err_str = n->first + "--" + n->second;
173  for (++n ; n != missing_edges.end(); ++n) {
174  err_str += ", " + n->first + "--" + n->second;
175  }
176 
177  delete node_constraint_;
178  delete edge_constraint_;
179  delete edge_cost_constraint_;
180  throw Exception("Some blocked edges are not in graph: %s", err_str.c_str());
181  }
182 
183  missing_edges.clear();
184  for (std::tuple<std::string, std::string, float> edge : edge_costs) {
185  bool found = false;
186  for (const NavGraphEdge &gedge : graph_edges) {
187  if ((std::get<0>(edge) == gedge.from() && std::get<1>(edge) == gedge.to()) ||
188  (std::get<0>(edge) == gedge.to() && std::get<1>(edge) == gedge.from()))
189  {
190  edge_cost_constraint_->add_edge(gedge, std::get<2>(edge));
191  found = true;
192  break;
193  }
194  }
195 
196  if (!found) {
197  missing_edges.push_back(std::make_pair(std::get<0>(edge), std::get<1>(edge)));
198  }
199  }
200 
201  if (! missing_edges.empty()) {
202  std::list<std::pair<std::string, std::string>>::iterator n = missing_edges.begin();
203  std::string err_str = n->first + "--" + n->second;
204  for (++n ; n != missing_edges.end(); ++n) {
205  err_str += ", " + n->first + "--" + n->second;
206  }
207 
208  delete node_constraint_;
209  delete edge_constraint_;
210  delete edge_cost_constraint_;
211  throw Exception("Some edges for cost factors are not in graph: %s", err_str.c_str());
212  }
213 
214 
215  for (const NavGraphPolygonConstraint::Polygon &p : polygons) {
216  node_poly_constraint_->add_polygon(p);
217  edge_poly_constraint_->add_polygon(p);
218  }
219 
220 
221  /*
222  NavGraphPolygonNodeConstraint *pc = new NavGraphPolygonNodeConstraint("Poly");
223  NavGraphPolygonNodeConstraint::Polygon p;
224  p.push_back(NavGraphPolygonNodeConstraint::Point(0.0, 0.0));
225  p.push_back(NavGraphPolygonNodeConstraint::Point(1.0, 0.0));
226  p.push_back(NavGraphPolygonNodeConstraint::Point(1.0, 1.11));
227  p.push_back(NavGraphPolygonNodeConstraint::Point(0.0, 1.11));
228  p.push_back(NavGraphPolygonNodeConstraint::Point(0.0, 0.0));
229  pc->add_polygon(p);
230 
231  NavGraphPolygonEdgeConstraint *pc = new NavGraphPolygonEdgeConstraint("Poly");
232  NavGraphPolygonEdgeConstraint::Polygon p;
233  p.push_back(NavGraphPolygonConstraint::Point(0.0, 0.0));
234  p.push_back(NavGraphPolygonConstraint::Point(1.0, 0.0));
235  p.push_back(NavGraphPolygonConstraint::Point(1.0, 1.11));
236  p.push_back(NavGraphPolygonConstraint::Point(0.0, 1.11));
237  p.push_back(NavGraphPolygonConstraint::Point(0.0, 0.0));
238  pc->add_polygon(p);
239  */
240 
241  navgraph->constraint_repo()->register_constraint(node_constraint_);
242  navgraph->constraint_repo()->register_constraint(edge_constraint_);
243  navgraph->constraint_repo()->register_constraint(edge_cost_constraint_);
244  navgraph->constraint_repo()->register_constraint(node_poly_constraint_);
245  navgraph->constraint_repo()->register_constraint(edge_poly_constraint_);
246 }
247 
248 void
250 {
251  navgraph->constraint_repo()->unregister_constraint(node_constraint_->name());
252  navgraph->constraint_repo()->unregister_constraint(edge_constraint_->name());
253  navgraph->constraint_repo()->unregister_constraint(edge_cost_constraint_->name());
254  delete node_constraint_;
255  delete edge_constraint_;
256  delete edge_cost_constraint_;
257 }
258 
259 void
261 {
262 }
virtual void init()
Initialize the thread.
void add_edge(const fawkes::NavGraphEdge &edge, const float cost_factor)
Add a single edge to constraint list.
PolygonHandle add_polygon(const Polygon &polygon)
Add a polygon to constraint list.
Constraint that blocks nodes inside a polygon.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
Definition: navgraph.h:46
Simple point representation for polygon.
Fawkes library namespace.
std::string name()
Get name of constraint.
Constraint that hold cost factors for a static list of edges.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Constraint that blocks nodes within and edges touching a polygon.
void add_node(const fawkes::NavGraphNode &node)
Add a single node to constraint list.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Base class for exceptions in Fawkes.
Definition: exception.h:36
std::string name()
Get name of constraint.
const char * name() const
Get name of thread.
Definition: thread.h:95
Topological graph edge.
Definition: navgraph_edge.h:39
Topological graph node.
Definition: navgraph_node.h:38
virtual std::vector< std::string > get_strings(const char *path)=0
Get list of values from configuration which is of type string.
Constraint that holds a list of edges to block.
Constraint that holds a list of nodes to block.
virtual void finalize()
Finalize the thread.
std::vector< Point > Polygon
A vector of points makes a polygon.
void add_edge(const fawkes::NavGraphEdge &edge)
Add a single edge to constraint list.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual ~NavGraphStaticConstraintsThread()
Destructor.
virtual void loop()
Code to execute in the thread.
std::string name()
Get name of constraint.