21 #include "navgraph_stconstr_thread.h"
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>
39 :
Thread(
"NavGraphStaticConstraintsThread",
Thread::OPMODE_WAITFORWAKEUP)
51 std::vector<std::string> nodes =
config->
get_strings(
"/navgraph/static-constraints/nodes");
53 std::vector<std::string> c_edges =
config->
get_strings(
"/navgraph/static-constraints/edges");
55 std::vector<std::string> c_edge_costs =
58 std::vector<std::string> c_polygons =
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]));
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());
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());
81 std::make_tuple(node_names[0], node_names[1], StringConversions::to_float(nodes_cost[1])));
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");
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());
97 StringConversions::to_float(coord[1]));
98 polygon.push_back(polpoint);
100 if (polygon.front().x != polygon.back().x || polygon.front().y != polygon.back().y) {
104 polygons.push_back(polygon);
113 const std::vector<NavGraphNode> &graph_nodes =
navgraph->nodes();
115 std::list<std::string> missing_nodes;
116 for (std::string node_name : nodes) {
119 if (gnode.name() == node_name) {
127 missing_nodes.push_back(node_name);
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;
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());
144 const std::vector<NavGraphEdge> &graph_edges =
navgraph->edges();
146 std::list<std::pair<std::string, std::string>> missing_edges;
147 for (std::pair<std::string, std::string> edge : edges) {
150 if ((edge.first == gedge.from() && edge.second == gedge.to())
151 || (edge.first == gedge.to() && edge.second == gedge.from())) {
159 missing_edges.push_back(edge);
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;
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());
176 missing_edges.clear();
177 for (std::tuple<std::string, std::string, float> edge : edge_costs) {
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));
189 missing_edges.push_back(std::make_pair(std::get<0>(edge), std::get<1>(edge)));
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;
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());
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_);
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_;
NavGraphStaticConstraintsThread()
Constructor.
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.
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.
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.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
std::string name()
Get name of constraint.
std::string name()
Get name of constraint.
std::string name()
Get name of constraint.
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.
const char * name() const
Get name of thread.
Fawkes library namespace.
Simple point representation for polygon.