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 
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 = 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 
238 void
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 
249 void
251 {
252 }
fawkes::NavGraphNode
Definition: navgraph_node.h:40
fawkes::NavGraphPolygonConstraint::Polygon
std::vector< Point > Polygon
A vector of points makes a polygon.
Definition: polygon_constraint.h:62
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:73
fawkes::NavGraphStaticListEdgeConstraint::add_edge
void add_edge(const fawkes::NavGraphEdge &edge)
Add a single edge to constraint list.
Definition: static_list_edge_constraint.cpp:84
fawkes::NavGraphPolygonConstraint::Point_
Simple point representation for polygon.
Definition: polygon_constraint.h:47
fawkes::NavGraphEdge
Definition: navgraph_edge.h:42
fawkes::NavGraphPolygonNodeConstraint
Definition: polygon_node_constraint.h:38
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
NavGraphStaticConstraintsThread::finalize
virtual void finalize()
Finalize the thread.
Definition: navgraph_stconstr_thread.cpp:239
NavGraphStaticConstraintsThread::~NavGraphStaticConstraintsThread
virtual ~NavGraphStaticConstraintsThread()
Destructor.
Definition: navgraph_stconstr_thread.cpp:44
fawkes::NavGraphPolygonEdgeConstraint
Definition: polygon_edge_constraint.h:38
fawkes::NavGraphNodeConstraint::name
std::string name()
Get name of constraint.
Definition: node_constraint.cpp:75
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
fawkes::NavGraphStaticListEdgeCostConstraint
Definition: static_list_edge_cost_constraint.h:40
NavGraphStaticConstraintsThread::init
virtual void init()
Initialize the thread.
Definition: navgraph_stconstr_thread.cpp:49
fawkes
fawkes::NavGraphStaticListNodeConstraint::add_node
void add_node(const fawkes::NavGraphNode &node)
Add a single node to constraint list.
Definition: static_list_node_constraint.cpp:83
fawkes::Configuration::get_strings
virtual std::vector< std::string > get_strings(const char *path)=0
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:53
fawkes::NavGraphStaticListNodeConstraint
Definition: static_list_node_constraint.h:41
fawkes::NavGraphEdgeCostConstraint::name
std::string name()
Get name of constraint.
Definition: edge_cost_constraint.cpp:84
fawkes::Thread
Definition: thread.h:45
fawkes::NavGraphAspect::navgraph
fawkes::LockPtr< NavGraph > navgraph
Definition: navgraph.h:50
NavGraphStaticConstraintsThread::NavGraphStaticConstraintsThread
NavGraphStaticConstraintsThread()
Constructor.
Definition: navgraph_stconstr_thread.cpp:38
NavGraphStaticConstraintsThread::loop
virtual void loop()
Code to execute in the thread.
Definition: navgraph_stconstr_thread.cpp:250
fawkes::NavGraphPolygonConstraint::add_polygon
PolygonHandle add_polygon(const Polygon &polygon)
Add a polygon to constraint list.
Definition: polygon_constraint.cpp:64
fawkes::NavGraphStaticListEdgeConstraint
Definition: static_list_edge_constraint.h:41
fawkes::NavGraphEdgeConstraint::name
std::string name()
Get name of constraint.
Definition: edge_constraint.cpp:83
fawkes::Exception
Definition: exception.h:41