21 #include "navgraph_stconstr_thread.h" 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> 39 :
Thread(
"NavGraphStaticConstraintsThread",
Thread::OPMODE_WAITFORWAKEUP)
51 std::vector<std::string> nodes =
54 std::vector<std::string> c_edges =
57 std::vector<std::string> c_edge_costs =
60 std::vector<std::string> c_polygons =
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]));
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());
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());
82 edge_costs.push_back(std::make_tuple(node_names[0], node_names[1],
83 StringConversions::to_float(nodes_cost[1])));
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");
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());
99 polpoint(StringConversions::to_float(coord[0]),
100 StringConversions::to_float(coord[1]));
101 polygon.push_back(polpoint);
103 if (polygon.front().x != polygon.back().x || polygon.front().y != polygon.back().y) {
109 polygons.push_back(polygon);
118 const std::vector<NavGraphNode> &graph_nodes =
navgraph->nodes();
120 std::list<std::string> missing_nodes;
121 for (std::string node_name : nodes) {
124 if (gnode.name() == node_name) {
132 missing_nodes.push_back(node_name);
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;
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());
149 const std::vector<NavGraphEdge> &graph_edges =
navgraph->edges();
152 std::list<std::pair<std::string, std::string>> missing_edges;
153 for (std::pair<std::string, std::string> edge : edges) {
156 if ((edge.first == gedge.from() && edge.second == gedge.to()) ||
157 (edge.first == gedge.to() && edge.second == gedge.from()))
166 missing_edges.push_back(edge);
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;
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());
183 missing_edges.clear();
184 for (std::tuple<std::string, std::string, float> edge : edge_costs) {
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()))
190 edge_cost_constraint_->
add_edge(gedge, std::get<2>(edge));
197 missing_edges.push_back(std::make_pair(std::get<0>(edge), std::get<1>(edge)));
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;
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());
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_);
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_;
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.
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.
NavGraphStaticConstraintsThread()
Constructor.
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.
Base class for exceptions in Fawkes.
std::string name()
Get name of constraint.
const char * name() const
Get name of thread.
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.
virtual ~NavGraphStaticConstraintsThread()
Destructor.
virtual void loop()
Code to execute in the thread.
std::string name()
Get name of constraint.