23 #include <navgraph/navgraph.h> 24 #include <navgraph/constraints/constraint_repo.h> 25 #include <navgraph/search_state.h> 26 #include <core/exception.h> 27 #include <utils/search/astar.h> 28 #include <utils/math/common.h> 38 #include <Eigen/Geometry> 46 const char *PROP_ORIENTATION =
"orientation";
70 NavGraph::NavGraph(
const std::string &graph_name)
72 graph_name_ = graph_name;
75 search_default_funcs_ =
true;
76 search_estimate_func_ = NavGraphSearchState::straight_line_estimate;
77 search_cost_func_ = NavGraphSearchState::euclidean_cost;
78 reachability_calced_ =
false;
79 notifications_enabled_ =
true;
100 graph_name_ = g.graph_name_;
115 NavGraph::name()
const 124 const std::vector<NavGraphNode> &
125 NavGraph::nodes()
const 134 const std::vector<NavGraphEdge> &
135 NavGraph::edges()
const 146 NavGraph::constraint_repo()
const 148 return constraint_repo_;
157 NavGraph::node(
const std::string &name)
const 159 std::vector<NavGraphNode>::const_iterator n =
160 std::find_if(nodes_.begin(), nodes_.end(),
162 return node.name() == name;
164 if (n != nodes_.end()) {
182 NavGraph::closest_node(
float pos_x,
float pos_y,
const std::string &property)
const 184 return closest_node(pos_x, pos_y,
false, property);
198 NavGraph::closest_node_with_unconnected(
float pos_x,
float pos_y,
199 const std::string &property)
const 201 return closest_node(pos_x, pos_y,
true, property);
214 NavGraph::closest_node_to(
const std::string &node_name,
215 const std::string &property)
const 217 return closest_node_to(node_name,
false, property);
230 NavGraph::closest_node_to_with_unconnected(
const std::string &node_name,
231 const std::string &property)
const 233 return closest_node_to(node_name,
true, property);
247 NavGraph::closest_node(
float pos_x,
float pos_y,
bool consider_unconnected,
248 const std::string &property)
const 250 std::vector<NavGraphNode> nodes = search_nodes(property);
252 float min_dist = std::numeric_limits<float>::max();
254 std::vector<NavGraphNode>::iterator i;
255 std::vector<NavGraphNode>::iterator elem = nodes.begin();
256 for (i = nodes.begin(); i != nodes.end(); ++i) {
257 if (! consider_unconnected && i->unconnected())
continue;
259 float dx = i->x() - pos_x;
260 float dy = i->y() - pos_y;
261 float dist = sqrtf(dx * dx + dy * dy);
262 if (sqrtf(dx * dx + dy * dy) < min_dist) {
268 if (elem == nodes.end()) {
286 NavGraph::closest_node_to(
const std::string &node_name,
bool consider_unconnected,
287 const std::string &property)
const 290 std::vector<NavGraphNode> nodes = search_nodes(property);
292 float min_dist = std::numeric_limits<float>::max();
294 std::vector<NavGraphNode>::iterator i;
295 std::vector<NavGraphNode>::iterator elem = nodes.begin();
296 for (i = nodes.begin(); i != nodes.end(); ++i) {
297 if (! consider_unconnected && i->unconnected())
continue;
299 float dx = i->x() - n.
x();
300 float dy = i->y() - n.
y();
301 float dist = sqrtf(dx * dx + dy * dy);
302 if ((sqrtf(dx * dx + dy * dy) < min_dist) && (i->name() != node_name)) {
308 if (elem == nodes.end()) {
324 NavGraph::edge(
const std::string &from,
const std::string &to)
const 326 std::vector<NavGraphEdge>::const_iterator e =
327 std::find_if(edges_.begin(), edges_.end(),
329 return (edge.from() == from && edge.to() == to) ||
330 (! edge.is_directed() && (edge.to() == from && edge.from() == to));
332 if (e != edges_.end()) {
350 NavGraph::closest_edge(
float pos_x,
float pos_y)
const 352 float min_dist = std::numeric_limits<float>::max();
356 Eigen::Vector2f point(pos_x, pos_y);
358 const Eigen::Vector2f origin(edge.from_node().x(), edge.from_node().y());
359 const Eigen::Vector2f target(edge.to_node().x(), edge.to_node().y());
360 const Eigen::Vector2f direction(target - origin);
361 const Eigen::Vector2f direction_norm = direction.normalized();
362 const Eigen::Vector2f diff = point - origin;
363 const float t = direction.dot(diff) / direction.squaredNorm();
365 if (t >= 0.0 && t <= 1.0) {
367 float distance = (diff - direction_norm.dot(diff) * direction_norm).norm();
382 std::vector<NavGraphNode>
383 NavGraph::search_nodes(
const std::string &property)
const 385 if (property ==
"") {
388 std::vector<NavGraphNode> rv;
390 std::vector<NavGraphNode>::const_iterator i;
391 for (i = nodes_.begin(); i != nodes_.end(); ++i) {
392 if ( i->has_property(property) ) rv.push_back(*i);
407 std::vector<NavGraphNode>::const_iterator n =
408 std::find(nodes_.begin(), nodes_.end(), node);
409 return (n != nodes_.end());
418 NavGraph::node_exists(
const std::string &name)
const 420 std::vector<NavGraphNode>::const_iterator n =
421 std::find_if(nodes_.begin(), nodes_.end(),
423 return node.name() == name;
425 return (n != nodes_.end());
435 std::vector<NavGraphEdge>::const_iterator e =
436 std::find(edges_.begin(), edges_.end(), edge);
437 return (e != edges_.end());
446 NavGraph::edge_exists(
const std::string &from,
const std::string &to)
const 448 std::vector<NavGraphEdge>::const_iterator e =
449 std::find_if(edges_.begin(), edges_.end(),
451 return (from == e.from() && to == e.to()) ||
452 (! e.is_directed() && (from == e.to() && to == e.from()));
454 return (e != edges_.end());
464 if (node_exists(node)) {
465 throw Exception(
"Node with name %s already exists", node.
name().c_str());
467 nodes_.push_back(node);
468 apply_default_properties(nodes_.back());
469 reachability_calced_ =
false;
476 typename std::enable_if<!std::numeric_limits<T>::is_integer,
bool>::type
477 almost_equal(T x, T y,
int ulp)
481 return (std::abs(x-y) < std::numeric_limits<T>::epsilon() * std::abs(x+y) * ulp)
483 || std::abs(x-y) < std::numeric_limits<T>::min();
506 connect_node_to_closest_node(node);
510 connect_node_to_closest_edge(node);
513 case CLOSEST_EDGE_OR_NODE:
515 connect_node_to_closest_edge(node);
517 connect_node_to_closest_node(node);
532 new_edge.set_property(
"generated",
true);
533 add_edge(new_edge, EDGE_SPLIT_INTERSECTION);
548 if (almost_equal(closest_conn.
distance(p.x, p.y), 0.f, 2)) {
551 cn =
NavGraphNode(NavGraph::format_name(
"C-%s", n.
name().c_str()), p.x, p.y);
559 new_edge.set_property(
"created-for", cn.
name() +
"--" + n.
name());
560 add_edge(new_edge, EDGE_SPLIT_INTERSECTION);
563 remove_edge(closest);
568 new_edge_2.set_properties(closest.
properties());
569 new_edge_3.set_property(
"created-for", cn.
name() +
"--" + n.
name());
570 new_edge_3.set_property(
"generated",
true);
572 if (! node_exists(cn)) add_node(cn);
573 add_edge(new_edge_1, EDGE_SPLIT_INTERSECTION);
574 add_edge(new_edge_2, EDGE_SPLIT_INTERSECTION);
575 add_edge(new_edge_3, EDGE_SPLIT_INTERSECTION);
595 if (edge_exists(edge)) {
596 if (allow_existing)
return;
597 else throw Exception(
"Edge from %s to %s already exists",
598 edge.
from().c_str(), edge.
to().c_str());
601 case EDGE_NO_INTERSECTION:
602 edge_add_no_intersection(edge);
605 case EDGE_SPLIT_INTERSECTION:
606 edge_add_split_intersection(edge);
610 edges_.push_back(edge);
611 edges_.back().set_nodes(node(edge.
from()), node(edge.
to()));
615 reachability_calced_ =
false;
627 nodes_.erase(std::remove(nodes_.begin(), nodes_.end(), node));
629 std::remove_if(edges_.begin(), edges_.end(),
631 return edge.from() == node.
name() || edge.to() == node.
name();
633 reachability_calced_ =
false;
641 NavGraph::remove_node(
const std::string &node_name)
644 std::remove_if(nodes_.begin(), nodes_.end(),
646 return node.name() == node_name;
649 std::remove_if(edges_.begin(), edges_.end(),
651 return edge.from() == node_name || edge.to() == node_name;
653 reachability_calced_ =
false;
664 std::remove_if(edges_.begin(), edges_.end(),
666 return (edge.
from() == e.from() && edge.
to() == e.to()) ||
667 (! e.is_directed() && (edge.
from() == e.to() && edge.
to() == e.from()));
669 reachability_calced_ =
false;
678 NavGraph::remove_edge(
const std::string &from,
const std::string &to)
681 std::remove_if(edges_.begin(), edges_.end(),
683 return (edge.from() == from && edge.to() == to) ||
684 (! edge.is_directed() && (edge.to() == from && edge.from() == to));
686 reachability_calced_ =
false;
699 std::vector<NavGraphNode>::iterator n =
700 std::find(nodes_.begin(), nodes_.end(), node);
701 if (n != nodes_.end()) {
704 throw Exception(
"No node with name %s known", node.
name().c_str());
717 std::vector<NavGraphEdge>::iterator e =
718 std::find(edges_.begin(), edges_.end(), edge);
719 if (e != edges_.end()) {
722 throw Exception(
"No edge from %s to %s is known",
723 edge.
from().c_str(), edge.
to().c_str());
736 default_properties_.clear();
743 const std::map<std::string, std::string> &
744 NavGraph::default_properties()
const 746 return default_properties_;
754 NavGraph::has_default_property(
const std::string &property)
const 756 return default_properties_.find(property) != default_properties_.end();
764 NavGraph::default_property(
const std::string &prop)
const 766 std::map<std::string, std::string>::const_iterator p;
767 if ((p = default_properties_.find(prop)) != default_properties_.end()) {
779 NavGraph::default_property_as_float(
const std::string &prop)
const 781 return StringConversions::to_float(default_property(prop));
789 NavGraph::default_property_as_int(
const std::string &prop)
const 791 return StringConversions::to_int(default_property(prop));
799 NavGraph::default_property_as_bool(
const std::string &prop)
const 801 return StringConversions::to_bool(default_property(prop));
809 NavGraph::set_default_property(
const std::string &property,
const std::string &value)
811 default_properties_[property] = value;
819 NavGraph::set_default_properties(
const std::map<std::string, std::string> &properties)
821 default_properties_ = properties;
830 NavGraph::set_default_property(
const std::string &property,
float value)
832 default_properties_[property] = StringConversions::to_string(value);
840 NavGraph::set_default_property(
const std::string &property,
int value)
842 default_properties_[property] = StringConversions::to_string(value);
850 NavGraph::set_default_property(
const std::string &property,
bool value)
852 default_properties_[property] = value ?
"true" :
"false";
864 for (
const auto &p : default_properties_) {
876 std::vector<std::string>
877 NavGraph::reachable_nodes(
const std::string &node_name)
const 879 std::vector<std::string> rv;
884 std::vector<NavGraphEdge>::const_iterator i;
885 for (i = edges_.begin(); i != edges_.end(); ++i) {
886 if (i->is_directed()) {
887 if (i->from() == node_name) {
888 rv.push_back(i->to());
891 if (i->from() == node_name) {
892 rv.push_back(i->to());
893 }
else if (i->to() == node_name) {
894 rv.push_back(i->from());
899 std::sort(rv.begin(), rv.end());
900 std::unique(rv.begin(), rv.end());
919 NavGraph::set_search_funcs(navgraph::EstimateFunction estimate_func,
920 navgraph::CostFunction cost_func)
922 if (! search_default_funcs_) {
923 throw Exception(
"Custom actual and estimated cost functions have already been set");
925 search_default_funcs_ =
false;
926 search_estimate_func_ = estimate_func;
927 search_cost_func_ = cost_func;
933 NavGraph::unset_search_funcs()
935 search_default_funcs_ =
true;
936 search_estimate_func_ = NavGraphSearchState::straight_line_estimate;
937 search_cost_func_ = NavGraphSearchState::euclidean_cost;
961 bool use_constraints,
bool compute_constraints)
963 return search_path(from, to,
964 search_estimate_func_, search_cost_func_,
965 use_constraints, compute_constraints);
988 NavGraph::search_path(
const std::string &from,
const std::string &to,
989 bool use_constraints,
bool compute_constraints)
991 return search_path(from, to,
992 search_estimate_func_, search_cost_func_,
993 use_constraints, compute_constraints);
1020 NavGraph::search_path(
const std::string &from,
const std::string &to,
1021 navgraph::EstimateFunction estimate_func,
1022 navgraph::CostFunction cost_func,
1023 bool use_constraints,
bool compute_constraints)
1027 return search_path(from_node, to_node, estimate_func, cost_func,
1028 use_constraints, compute_constraints);
1056 navgraph::EstimateFunction estimate_func,
1057 navgraph::CostFunction cost_func,
1058 bool use_constraints,
bool compute_constraints)
1060 if (! reachability_calced_) calc_reachability(
true);
1064 std::vector<AStarState *> a_star_solution;
1066 if (use_constraints) {
1067 constraint_repo_.lock();
1068 if (compute_constraints && constraint_repo_->has_constraints()) {
1069 constraint_repo_->compute();
1075 a_star_solution = astar.
solve(initial_state);
1076 constraint_repo_.unlock();
1080 a_star_solution = astar.
solve(initial_state);
1083 std::vector<fawkes::NavGraphNode> path(a_star_solution.size());
1085 for (
unsigned int i = 0; i < a_star_solution.size(); ++i ) {
1087 path[i] = solstate->node();
1091 (! a_star_solution.empty())
1092 ? a_star_solution[a_star_solution.size() - 1]->total_estimated_cost
1110 return search_cost_func_(from, to);
1116 NavGraph::assert_valid_edges()
1118 for (
size_t i = 0; i < edges_.size(); ++i) {
1119 if (! node_exists(edges_[i].from())) {
1120 throw Exception(
"Node '%s' for edge '%s' -> '%s' does not exist",
1121 edges_[i].from().c_str(), edges_[i].from().c_str(),
1122 edges_[i].to().c_str());
1125 if (! node_exists(edges_[i].to())) {
1126 throw Exception(
"Node '%s' for edge '%s' -> '%s' does not exist",
1127 edges_[i].to().c_str(), edges_[i].from().c_str(),
1128 edges_[i].to().c_str());
1135 NavGraph::edge_add_no_intersection(
const NavGraphEdge &edge)
1138 const NavGraphNode &n1 = node(edge.from());
1139 const NavGraphNode &n2 = node(edge.to());
1140 for (
const NavGraphEdge &ne : edges_) {
1141 if (edge.from() == ne.from() || edge.from() == ne.to() ||
1142 edge.to() == ne.to() || edge.to() == ne.from())
continue;
1144 if (ne.intersects(n1.x(), n1.y(), n2.x(), n2.y())) {
1145 throw Exception(
"Edge %s-%s%s not added: intersects with %s-%s%s",
1146 edge.from().c_str(), edge.is_directed() ?
">" :
"-", edge.to().c_str(),
1147 ne.from().c_str(), ne.is_directed() ?
">" :
"-", ne.to().c_str());
1150 add_edge(edge, EDGE_FORCE);
1151 }
catch (Exception &ex) {
1152 throw Exception(
"Failed to add edge %s-%s%s: %s",
1153 edge.from().c_str(), edge.is_directed() ?
">" :
"-", edge.to().c_str(),
1154 ex.what_no_backtrace());
1160 NavGraph::edge_add_split_intersection(
const NavGraphEdge &edge)
1162 std::list<std::pair<cart_coord_2d_t, NavGraphEdge>> intersections;
1163 const NavGraphNode &n1 = node(edge.from());
1164 const NavGraphNode &n2 = node(edge.to());
1168 for (
const NavGraphEdge &e : edges_) {
1170 if (e.intersection(n1.x(), n1.y(), n2.x(), n2.y(), ip)) {
1173 intersections.push_back(std::make_pair(ip, e));
1177 std::list<std::list<std::pair<cart_coord_2d_t, NavGraphEdge> >::iterator> deletions;
1179 for (
auto i1 = intersections.begin(); i1 != intersections.end(); ++i1) {
1180 const std::pair<cart_coord_2d_t, NavGraphEdge> &p1 = *i1;
1182 const NavGraphEdge &e1 = p1.second;
1184 const NavGraphNode &n1_from = node(e1.from());
1185 const NavGraphNode &n1_to = node(e1.to());
1187 for (
auto i2 = std::next(i1); i2 != intersections.end(); ++i2) {
1188 const std::pair<cart_coord_2d_t, NavGraphEdge> &p2 = *i2;
1190 const NavGraphEdge &e2 = p2.second;
1195 if (e1.from() == e2.from() || e1.from() == e2.to()) {
1196 d =
point_dist(n1_from.x(), n1_from.y(), c1.x, c1.y);
1197 }
else if (e1.to() == e2.to() || e1.to() == e2.from()) {
1198 d =
point_dist(n1_to.x(), n1_to.y(), c1.x, c1.y);
1203 deletions.push_back(i1);
1208 for (
auto d = deletions.rbegin(); d != deletions.rend(); ++d) {
1209 intersections.erase(*d);
1212 if (intersections.empty()) {
1213 NavGraphEdge e(edge);
1214 e.set_property(
"created-for", edge.from() +
"--" + edge.to());
1215 add_edge(e, EDGE_FORCE);
1217 Eigen::Vector2f e_origin(n1.x(), n1.y());
1218 Eigen::Vector2f e_target(n2.x(), n2.y());
1219 Eigen::Vector2f e_dir = (e_target - e_origin).normalized();
1221 intersections.sort([&e_origin, &e_dir](
const std::pair<cart_coord_2d_t, NavGraphEdge> &p1,
1222 const std::pair<cart_coord_2d_t, NavGraphEdge> &p2)
1224 const Eigen::Vector2f p1p(p1.first.x, p1.first.y);
1225 const Eigen::Vector2f p2p(p2.first.x, p2.first.y);
1226 const float k1 = e_dir.dot(p1p - e_origin);
1227 const float k2 = e_dir.dot(p2p - e_origin);
1231 std::string en_from = edge.from();
1233 std::string prev_to;
1234 for (
const auto &i : intersections) {
1236 const NavGraphEdge &e = i.second;
1239 NavGraphNode ip = closest_node(c.x, c.y);
1241 ip = NavGraphNode(gen_unique_name(), c.x, c.y);
1246 if (ip.name() != e.from() && ip.name() != e.to()) {
1247 NavGraphEdge e1(e.from(), ip.name(), e.is_directed());
1248 NavGraphEdge e2(ip.name(), e.to(), e.is_directed());
1250 e1.set_properties(e.properties());
1251 e2.set_properties(e.properties());
1252 add_edge(e1, EDGE_FORCE,
true);
1253 add_edge(e2, EDGE_FORCE,
true);
1263 if (en_from != ip.name() && prev_to != ip.name()) {
1264 NavGraphEdge e3(en_from, ip.name(), edge.is_directed());
1265 e3.set_property(
"created-for", en_from +
"--" + ip.name());
1266 add_edge(e3, EDGE_FORCE,
true);
1270 en_from = ip.name();
1274 if (en_from != edge.to()) {
1275 NavGraphEdge e3(en_from, edge.to(), edge.is_directed());
1276 e3.set_property(
"created-for", en_from +
"--" + edge.to());
1277 add_edge(e3, EDGE_FORCE,
true);
1281 }
catch (Exception &ex) {
1282 throw Exception(
"Failed to add edge %s-%s%s: %s",
1283 edge.from().c_str(), edge.is_directed() ?
">" :
"-", edge.to().c_str(),
1284 ex.what_no_backtrace());
1291 NavGraph::assert_connected()
1293 std::set<std::string> traversed;
1294 std::set<std::string> nodeset;
1295 std::queue<NavGraphNode> q;
1298 auto fcon = std::find_if_not(nodes_.begin(), nodes_.end(),
1299 [](
const NavGraphNode &n)
1300 {
return n.unconnected(); });
1301 if (fcon == nodes_.end()) {
1308 while (! q.empty()) {
1309 NavGraphNode &n = q.front();
1310 traversed.insert(n.name());
1312 const std::vector<std::string> & reachable = n.reachable_nodes();
1314 if (n.unconnected() && !reachable.empty()) {
1315 throw Exception(
"Node %s is marked unconnected but nodes are reachable from it",
1318 std::vector<std::string>::const_iterator r;
1319 for (r = reachable.begin(); r != reachable.end(); ++r) {
1320 NavGraphNode target(node(*r));
1321 if (target.unconnected()) {
1322 throw Exception(
"Node %s is marked unconnected but is reachable from node %s\n",
1323 target.name().c_str(), n.name().c_str());
1325 if (traversed.find(*r) == traversed.end()) q.push(node(*r));
1330 std::vector<NavGraphNode>::iterator n;
1331 for (n = nodes_.begin(); n != nodes_.end(); ++n) {
1332 nodeset.insert(n->name());
1335 if (traversed.size() != nodeset.size()) {
1336 std::set<std::string> nodediff;
1337 std::set_difference(nodeset.begin(), nodeset.end(),
1338 traversed.begin(), traversed.end(),
1339 std::inserter(nodediff, nodediff.begin()));
1345 std::set<std::string>::const_iterator ud = nodediff.begin();
1346 while (ud != nodediff.end()) {
1347 NavGraphNode udnode(node(*ud));
1348 if (udnode.unconnected()) {
1351 if (! udnode.reachable_nodes().empty()) {
1352 throw Exception(
"Node %s is marked unconnected but nodes are reachable from it",
1355 #if __cplusplus > 201100L || defined(__GXX_EXPERIMENTAL_CXX0X__) 1356 ud = nodediff.erase(ud);
1358 std::set<std::string>::const_iterator delit = ud;
1360 nodediff.erase(delit);
1367 if (! nodediff.empty()) {
1368 std::set<std::string>::iterator d = nodediff.begin();
1369 std::string disconnected = *d;
1370 for (++d; d != nodediff.end(); ++d) {
1371 disconnected +=
", " + *d;
1373 throw Exception(
"The graph is not fully connected, " 1374 "cannot reach (%s) from '%s' for example",
1375 disconnected.c_str(), fcon->name().c_str());
1388 NavGraph::calc_reachability(
bool allow_multi_graph)
1390 if (nodes_.empty())
return;
1392 assert_valid_edges();
1394 std::vector<NavGraphNode>::iterator i;
1395 for (i = nodes_.begin(); i != nodes_.end(); ++i) {
1396 i->set_reachable_nodes(reachable_nodes(i->name()));
1399 std::vector<NavGraphEdge>::iterator e;
1400 for (e = edges_.begin(); e != edges_.end(); ++e) {
1401 e->set_nodes(node(e->from()), node(e->to()));
1404 if (! allow_multi_graph) assert_connected();
1405 reachability_calced_ =
true;
1417 NavGraph::gen_unique_name(
const char *prefix)
1419 for (
unsigned int i = 0; i < std::numeric_limits<unsigned int>::max(); ++i) {
1420 std::string name = format_name(
"%s%i", prefix, i);
1425 throw Exception(
"Cannot generate unique name for given prefix, all taken");
1435 NavGraph::format_name(
const char *format, ...)
1438 va_start(arg, format);
1441 if (vasprintf(&tmp, format, arg) != -1) {
1455 change_listeners_.push_back(listener);
1464 change_listeners_.remove(listener);
1477 NavGraph::set_notifications_enabled(
bool enabled)
1479 notifications_enabled_ = enabled;
1484 NavGraph::notify_of_change() throw()
1486 if (! notifications_enabled_)
return;
1488 std::list<ChangeListener *> tmp_listeners = change_listeners_;
1490 std::list<ChangeListener *>::iterator l;
1491 for (l = tmp_listeners.begin(); l != tmp_listeners.end(); ++l) {
1492 (*l)->graph_changed();
1505 NavGraph::ChangeListener::~ChangeListener()
const std::string & from() const
Get edge originating node name.
void set_property(const std::string &property, const std::string &value)
Set property.
float distance(const NavGraphNode &n)
Get euclidean distance from this node to another.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
float point_dist(float x1, float y1, float x2, float y2)
Get distance of two points.
Cartesian coordinates (2D).
bool points_different(float x1, float y1, float x2, float y2, float threshold=1e-4)
Check if two points are different with regard to a given threshold.
Fawkes library namespace.
Graph-based path planner A* search state.
void set_properties(const std::map< std::string, std::string > &properties)
Overwrite properties with given ones.
std::vector< AStarState * > solve(AStarState *initialState)
Solves a situation given by the initial state with AStar, and returns a vector of AStarStates that so...
bool has_property(const std::string &property) const
Check if node has specified property.
Class representing a path for a NavGraph.
bool is_valid() const
Check if node is valid, i.e.
const std::map< std::string, std::string > & properties() const
Get all properties.
LockPtr<> is a reference-counting shared lockable smartpointer.
Constraint repository to maintain blocks on nodes.
EdgeMode
Mode to use to add edges.
fawkes::cart_coord_2d_t closest_point_on_edge(float x, float y) const
Get the point on edge closest to a given point.
Base class for exceptions in Fawkes.
const std::string & to() const
Get edge target node name.
void set_property(const std::string &property, const std::string &value)
Set property.
const std::string & name() const
Get name of node.
struct fawkes::cart_coord_2d_struct cart_coord_2d_t
Cartesian coordinates (2D).
float y() const
Get Y coordinate in global frame.
float x() const
Get X coordinate in global frame.
Topological graph change listener.
ConnectionMode
Connect mode enum for connect_node_* methods.