23#include <core/exception.h>
24#include <navgraph/constraints/constraint_repo.h>
25#include <navgraph/navgraph.h>
26#include <navgraph/search_state.h>
27#include <utils/math/common.h>
28#include <utils/search/astar.h>
30#include <Eigen/Geometry>
42const char *PROP_ORIENTATION =
"orientation";
67 graph_name_ = graph_name;
70 search_default_funcs_ =
true;
73 reachability_calced_ =
false;
74 notifications_enabled_ =
true;
86 graph_name_ = g.graph_name_;
109 graph_name_ = g.graph_name_;
132const std::vector<NavGraphNode> &
141const std::vector<NavGraphEdge> &
154 return constraint_repo_;
165 std::vector<NavGraphNode>::const_iterator n =
167 return node.name() == name;
169 if (n != nodes_.end()) {
232 const std::string &property)
const
250 bool consider_unconnected,
251 const std::string &property)
const
255 float min_dist = std::numeric_limits<float>::max();
257 std::vector<NavGraphNode>::iterator i;
258 std::vector<NavGraphNode>::iterator elem =
nodes.end();
259 for (i =
nodes.begin(); i !=
nodes.end(); ++i) {
260 if (!consider_unconnected && i->unconnected())
263 float dx = i->x() - pos_x;
264 float dy = i->y() - pos_y;
265 float dist = sqrtf(dx * dx + dy * dy);
266 if (sqrtf(dx * dx + dy * dy) < min_dist) {
272 if (elem ==
nodes.end()) {
291 bool consider_unconnected,
292 const std::string &property)
const
297 float min_dist = std::numeric_limits<float>::max();
299 std::vector<NavGraphNode>::iterator i;
300 std::vector<NavGraphNode>::iterator elem =
nodes.begin();
301 for (i =
nodes.begin(); i !=
nodes.end(); ++i) {
302 if (!consider_unconnected && i->unconnected())
305 float dx = i->x() - n.
x();
306 float dy = i->y() - n.
y();
307 float dist = sqrtf(dx * dx + dy * dy);
308 if ((sqrtf(dx * dx + dy * dy) < min_dist) && (i->name() != node_name)) {
314 if (elem ==
nodes.end()) {
331 std::vector<NavGraphEdge>::const_iterator e =
332 std::find_if(edges_.begin(), edges_.end(), [&from, &to](
const NavGraphEdge &
edge) {
333 return (edge.from() == from && edge.to() == to)
334 || (!edge.is_directed() && (edge.to() == from && edge.from() == to));
336 if (e != edges_.end()) {
355 float min_dist = std::numeric_limits<float>::max();
359 Eigen::Vector2f point(pos_x, pos_y);
363 const Eigen::Vector2f direction(target - origin);
364 const Eigen::Vector2f direction_norm = direction.normalized();
365 const Eigen::Vector2f diff = point - origin;
366 const float t = direction.dot(diff) / direction.squaredNorm();
368 if (t >= 0.0 && t <= 1.0) {
370 float distance = (diff - direction_norm.dot(diff) * direction_norm).norm();
385std::vector<NavGraphNode>
388 if (property ==
"") {
391 std::vector<NavGraphNode> rv;
393 std::vector<NavGraphNode>::const_iterator i;
394 for (i = nodes_.begin(); i != nodes_.end(); ++i) {
395 if (i->has_property(property))
410 std::vector<NavGraphNode>::const_iterator n = std::find(nodes_.begin(), nodes_.end(),
node);
411 return (n != nodes_.end());
421 std::vector<NavGraphNode>::const_iterator n =
423 return node.name() == name;
425 return (n != nodes_.end());
435 std::vector<NavGraphEdge>::const_iterator e = std::find(edges_.begin(), edges_.end(),
edge);
436 return (e != edges_.end());
447 std::vector<NavGraphEdge>::const_iterator e =
448 std::find_if(edges_.begin(), edges_.end(), [&from, &to](
const NavGraphEdge &e) ->
bool {
449 return (from == e.from() && to == e.to())
450 || (!e.is_directed() && (from == e.to() && to == e.from()));
452 return (e != edges_.end());
465 nodes_.push_back(
node);
467 reachability_calced_ =
false;
474typename std::enable_if<!std::numeric_limits<T>::is_integer,
bool>::type
475almost_equal(T x, T y,
int ulp)
479 return (std::abs(x - y) < std::numeric_limits<T>::epsilon() * std::abs(x + y) * ulp)
481 || std::abs(x - y) < std::numeric_limits<T>::min();
539 if (almost_equal(closest_conn.
distance(p.
x, p.
y), 0.f, 2)) {
598 edges_.push_back(
edge);
603 reachability_calced_ =
false;
614 nodes_.erase(std::remove(nodes_.begin(), nodes_.end(),
node));
615 edges_.erase(std::remove_if(edges_.begin(),
618 return edge.from() == node.name() || edge.to() == node.name();
621 reachability_calced_ =
false;
631 nodes_.erase(std::remove_if(nodes_.begin(),
634 return node.name() == node_name;
637 edges_.erase(std::remove_if(edges_.begin(),
640 return edge.from() == node_name || edge.to() == node_name;
643 reachability_calced_ =
false;
654 std::list<std::string> to_remove;
661 if (e.from_node() == n || e.to_node() == n)
665 to_remove.push_back(n.name());
668 for (
const auto &nn : to_remove) {
679 edges_.erase(std::remove_if(edges_.begin(),
682 return (edge.from() == e.from() && edge.to() == e.to())
684 && (edge.from() == e.to() && edge.to() == e.from()));
687 reachability_calced_ =
false;
698 edges_.erase(std::remove_if(edges_.begin(),
701 return (edge.from() == from && edge.to() == to)
702 || (!edge.is_directed()
703 && (edge.to() == from && edge.from() == to));
706 reachability_calced_ =
false;
718 std::vector<NavGraphNode>::iterator n = std::find(nodes_.begin(), nodes_.end(),
node);
719 if (n != nodes_.end()) {
735 std::vector<NavGraphEdge>::iterator e = std::find(edges_.begin(), edges_.end(),
edge);
736 if (e != edges_.end()) {
751 default_properties_.clear();
758const std::map<std::string, std::string> &
761 return default_properties_;
771 return default_properties_.find(property) != default_properties_.end();
781 std::map<std::string, std::string>::const_iterator p;
782 if ((p = default_properties_.find(prop)) != default_properties_.end()) {
826 default_properties_[property] = value;
836 default_properties_ = properties;
866 default_properties_[property] = value ?
"true" :
"false";
877 for (
const auto &p : default_properties_) {
888std::vector<std::string>
891 std::vector<std::string> rv;
897 std::vector<NavGraphEdge>::const_iterator i;
898 for (i = edges_.begin(); i != edges_.end(); ++i) {
899 if (i->is_directed()) {
900 if (i->from() == node_name) {
901 rv.push_back(i->to());
904 if (i->from() == node_name) {
905 rv.push_back(i->to());
906 }
else if (i->to() == node_name) {
907 rv.push_back(i->from());
912 std::sort(rv.begin(), rv.end());
913 std::unique(rv.begin(), rv.end());
932 navgraph::CostFunction cost_func)
934 if (!search_default_funcs_) {
935 throw Exception(
"Custom actual and estimated cost functions have already been set");
937 search_default_funcs_ =
false;
938 search_estimate_func_ = estimate_func;
939 search_cost_func_ = cost_func;
946 search_default_funcs_ =
true;
973 bool use_constraints,
974 bool compute_constraints)
977 from, to, search_estimate_func_, search_cost_func_, use_constraints, compute_constraints);
1001 const std::string &to,
1002 bool use_constraints,
1003 bool compute_constraints)
1006 from, to, search_estimate_func_, search_cost_func_, use_constraints, compute_constraints);
1034 const std::string & to,
1035 navgraph::EstimateFunction estimate_func,
1036 navgraph::CostFunction cost_func,
1037 bool use_constraints,
1038 bool compute_constraints)
1043 from_node, to_node, estimate_func, cost_func, use_constraints, compute_constraints);
1072 navgraph::EstimateFunction estimate_func,
1073 navgraph::CostFunction cost_func,
1074 bool use_constraints,
1075 bool compute_constraints)
1077 if (!reachability_calced_)
1082 std::vector<AStarState *> a_star_solution;
1084 if (use_constraints) {
1085 constraint_repo_.lock();
1086 if (compute_constraints && constraint_repo_->has_constraints()) {
1087 constraint_repo_->compute();
1092 a_star_solution = astar.
solve(initial_state);
1093 constraint_repo_.unlock();
1097 a_star_solution = astar.
solve(initial_state);
1100 std::vector<fawkes::NavGraphNode> path(a_star_solution.size());
1102 for (
unsigned int i = 0; i < a_star_solution.size(); ++i) {
1104 path[i] = solstate->
node();
1107 float cost = (!a_star_solution.empty())
1108 ? a_star_solution[a_star_solution.size() - 1]->total_estimated_cost
1125 return search_cost_func_(from, to);
1130NavGraph::assert_valid_edges()
1132 for (
size_t i = 0; i < edges_.size(); ++i) {
1134 throw Exception(
"Node '%s' for edge '%s' -> '%s' does not exist",
1135 edges_[i].from().c_str(),
1136 edges_[i].from().c_str(),
1137 edges_[i].to().c_str());
1141 throw Exception(
"Node '%s' for edge '%s' -> '%s' does not exist",
1142 edges_[i].to().c_str(),
1143 edges_[i].from().c_str(),
1144 edges_[i].to().c_str());
1150NavGraph::edge_add_no_intersection(
const NavGraphEdge &edge)
1155 for (
const NavGraphEdge &ne : edges_) {
1157 ||
edge.
to() == ne.from())
1160 if (ne.intersects(n1.x(), n1.y(), n2.x(), n2.y())) {
1161 throw Exception(
"Edge %s-%s%s not added: intersects with %s-%s%s",
1166 ne.is_directed() ?
">" :
"-",
1171 }
catch (Exception &ex) {
1172 throw Exception(
"Failed to add edge %s-%s%s: %s",
1176 ex.what_no_backtrace());
1181NavGraph::edge_add_split_intersection(
const NavGraphEdge &edge)
1183 std::list<std::pair<cart_coord_2d_t, NavGraphEdge>> intersections;
1188 for (
const NavGraphEdge &e : edges_) {
1190 if (e.intersection(n1.x(), n1.y(), n2.x(), n2.y(), ip)) {
1193 intersections.push_back(std::make_pair(ip, e));
1197 std::list<std::list<std::pair<cart_coord_2d_t, NavGraphEdge>>::iterator> deletions;
1199 for (
auto i1 = intersections.begin(); i1 != intersections.end(); ++i1) {
1200 const std::pair<cart_coord_2d_t, NavGraphEdge> &p1 = *i1;
1202 const NavGraphEdge & e1 = p1.second;
1204 const NavGraphNode &n1_from =
node(e1.from());
1205 const NavGraphNode &n1_to =
node(e1.to());
1207 for (
auto i2 = std::next(i1); i2 != intersections.end(); ++i2) {
1208 const std::pair<cart_coord_2d_t, NavGraphEdge> &p2 = *i2;
1210 const NavGraphEdge & e2 = p2.second;
1216 if (e1.from() == e2.from() || e1.from() == e2.to()) {
1217 d =
point_dist(n1_from.x(), n1_from.y(), c1.x, c1.y);
1218 }
else if (e1.to() == e2.to() || e1.to() == e2.from()) {
1219 d =
point_dist(n1_to.x(), n1_to.y(), c1.x, c1.y);
1224 deletions.push_back(i1);
1229 for (
auto d = deletions.rbegin(); d != deletions.rend(); ++d) {
1230 intersections.erase(*d);
1233 if (intersections.empty()) {
1234 NavGraphEdge e(
edge);
1238 Eigen::Vector2f e_origin(n1.x(), n1.y());
1239 Eigen::Vector2f e_target(n2.x(), n2.y());
1240 Eigen::Vector2f e_dir = (e_target - e_origin).normalized();
1242 intersections.sort([&e_origin, &e_dir](
const std::pair<cart_coord_2d_t, NavGraphEdge> &p1,
1243 const std::pair<cart_coord_2d_t, NavGraphEdge> &p2) {
1244 const Eigen::Vector2f p1p(p1.first.x, p1.first.y);
1245 const Eigen::Vector2f p2p(p2.first.x, p2.first.y);
1246 const float k1 = e_dir.dot(p1p - e_origin);
1247 const float k2 = e_dir.dot(p2p - e_origin);
1253 std::string prev_to;
1254 for (
const auto &i : intersections) {
1256 const NavGraphEdge & e = i.second;
1266 if (ip.name() != e.from() && ip.name() != e.to()) {
1267 NavGraphEdge e1(e.from(), ip.name(), e.is_directed());
1268 NavGraphEdge e2(ip.name(), e.to(), e.is_directed());
1270 e1.set_properties(e.properties());
1271 e2.set_properties(e.properties());
1283 if (en_from != ip.name() && prev_to != ip.name()) {
1285 e3.set_property(
"created-for", en_from +
"--" + ip.name());
1289 en_from = ip.name();
1293 if (en_from !=
edge.
to()) {
1295 e3.set_property(
"created-for", en_from +
"--" +
edge.
to());
1300 }
catch (Exception &ex) {
1301 throw Exception(
"Failed to add edge %s-%s%s: %s",
1305 ex.what_no_backtrace());
1310NavGraph::assert_connected()
1312 std::set<std::string> traversed;
1313 std::set<std::string> nodeset;
1314 std::queue<NavGraphNode> q;
1317 auto fcon = std::find_if_not(nodes_.begin(), nodes_.end(), [](
const NavGraphNode &n) {
1318 return n.unconnected();
1320 if (fcon == nodes_.end()) {
1327 while (!q.empty()) {
1328 NavGraphNode &n = q.front();
1329 traversed.insert(n.name());
1331 const std::vector<std::string> &reachable = n.reachable_nodes();
1333 if (n.unconnected() && !reachable.empty()) {
1334 throw Exception(
"Node %s is marked unconnected but nodes are reachable from it",
1337 std::vector<std::string>::const_iterator r;
1338 for (r = reachable.begin(); r != reachable.end(); ++r) {
1339 NavGraphNode target(
node(*r));
1340 if (target.unconnected()) {
1341 throw Exception(
"Node %s is marked unconnected but is reachable from node %s\n",
1342 target.name().c_str(),
1345 if (traversed.find(*r) == traversed.end())
1351 std::vector<NavGraphNode>::iterator n;
1352 for (n = nodes_.begin(); n != nodes_.end(); ++n) {
1353 nodeset.insert(n->name());
1356 if (traversed.size() != nodeset.size()) {
1357 std::set<std::string> nodediff;
1358 std::set_difference(nodeset.begin(),
1362 std::inserter(nodediff, nodediff.begin()));
1368 std::set<std::string>::const_iterator ud = nodediff.begin();
1369 while (ud != nodediff.end()) {
1370 NavGraphNode udnode(
node(*ud));
1371 if (udnode.unconnected()) {
1374 if (!udnode.reachable_nodes().empty()) {
1375 throw Exception(
"Node %s is marked unconnected but nodes are reachable from it",
1378#if __cplusplus > 201100L || defined(__GXX_EXPERIMENTAL_CXX0X__)
1379 ud = nodediff.erase(ud);
1381 std::set<std::string>::const_iterator delit = ud;
1383 nodediff.erase(delit);
1390 if (!nodediff.empty()) {
1391 std::set<std::string>::iterator d = nodediff.begin();
1392 std::string disconnected = *d;
1393 for (++d; d != nodediff.end(); ++d) {
1394 disconnected +=
", " + *d;
1396 throw Exception(
"The graph is not fully connected, "
1397 "cannot reach (%s) from '%s' for example",
1398 disconnected.c_str(),
1399 fcon->name().c_str());
1415 assert_valid_edges();
1417 std::vector<NavGraphNode>::iterator i;
1418 for (i = nodes_.begin(); i != nodes_.end(); ++i) {
1422 std::vector<NavGraphEdge>::iterator e;
1423 for (e = edges_.begin(); e != edges_.end(); ++e) {
1424 e->set_nodes(
node(e->from()),
node(e->to()));
1427 if (!allow_multi_graph)
1429 reachability_calced_ =
true;
1442 for (
unsigned int i = 0; i < std::numeric_limits<unsigned int>::max(); ++i) {
1448 throw Exception(
"Cannot generate unique name for given prefix, all taken");
1460 va_start(arg, format);
1463 if (vasprintf(&tmp, format, arg) != -1) {
1477 change_listeners_.push_back(listener);
1486 change_listeners_.remove(listener);
1500 notifications_enabled_ = enabled;
1507 if (!notifications_enabled_)
1510 std::list<ChangeListener *> tmp_listeners = change_listeners_;
1512 std::list<ChangeListener *>::iterator l;
1513 for (l = tmp_listeners.begin(); l != tmp_listeners.end(); ++l) {
1514 (*l)->graph_changed();
This is an implementation of the A* search algorithm.
std::vector< AStarState * > solve(AStarState *initialState)
Solves a situation given by the initial state with AStar, and returns a vector of AStarStates that so...
Base class for exceptions in Fawkes.
LockPtr<> is a reference-counting shared lockable smartpointer.
Constraint repository to maintain blocks on nodes.
bool is_directed() const
Check if edge is directed.
void set_property(const std::string &property, const std::string &value)
Set property.
const std::string & to() const
Get edge target node name.
fawkes::cart_coord_2d_t closest_point_on_edge(float x, float y) const
Get the point on edge closest to a given point.
const std::map< std::string, std::string > & properties() const
Get all properties.
const std::string & from() const
Get edge originating node name.
void set_properties(const std::map< std::string, std::string > &properties)
Overwrite properties with given ones.
const NavGraphNode & from_node() const
Get edge originating node.
const NavGraphNode & to_node() const
Get edge target node.
float x() const
Get X coordinate in global frame.
void set_property(const std::string &property, const std::string &value)
Set property.
bool is_valid() const
Check if node is valid, i.e.
const std::string & name() const
Get name of node.
float distance(const NavGraphNode &n)
Get euclidean distance from this node to another.
bool has_property(const std::string &property) const
Check if node has specified property.
float y() const
Get Y coordinate in global frame.
Class representing a path for a NavGraph.
Graph-based path planner A* search state.
static float euclidean_cost(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Determine euclidean cost between two nodes.
fawkes::NavGraphNode & node()
Get graph node corresponding to this search state.
static float straight_line_estimate(const fawkes::NavGraphNode &node, const fawkes::NavGraphNode &goal)
Determine straight line estimate between two nodes.
Topological graph change listener.
virtual ~ChangeListener()
Virtual empty destructor.
ConnectionMode
Connect mode enum for connect_node_* methods.
@ CLOSEST_EDGE
Connect to closest edge.
@ CLOSEST_EDGE_OR_NODE
try to connect to closest edge, if that fails, connect to closest node
@ CLOSEST_NODE
Connect to closest node.
void update_node(const NavGraphNode &node)
Update a given node.
void remove_change_listener(ChangeListener *listener)
Remove a change listener.
NavGraph & operator=(const NavGraph &g)
Assign/copy structures from another graph.
std::vector< std::string > reachable_nodes(const std::string &node_name) const
Get nodes reachable from specified nodes.
void clear()
Remove all nodes and edges from navgraph.
NavGraphNode closest_node_to_with_unconnected(const std::string &node_name, const std::string &property="") const
Get node closest to another node with a certain property.
virtual ~NavGraph()
Virtual empty destructor.
void remove_orphan_nodes()
Remove orphan nodes.
NavGraphEdge edge(const std::string &from, const std::string &to) const
Get a specified edge.
void unset_search_funcs()
Reset actual and estimated cost function to defaults.
bool has_default_property(const std::string &property) const
Check if graph has specified default property.
NavGraphNode closest_node_with_unconnected(float pos_x, float pos_y, const std::string &property="") const
Get node closest to a specified point with a certain property.
void add_edge(const NavGraphEdge &edge, EdgeMode mode=EDGE_NO_INTERSECTION, bool allow_existing=false)
Add an edge.
bool default_property_as_bool(const std::string &prop) const
Get property converted to bol.
void remove_edge(const NavGraphEdge &edge)
Remove an edge.
void set_default_property(const std::string &property, const std::string &value)
Set property.
std::string default_property(const std::string &prop) const
Get specified default property as string.
const std::map< std::string, std::string > & default_properties() const
Get all default properties.
float cost(const NavGraphNode &from, const NavGraphNode &to) const
Calculate cost between two adjacent nodes.
NavGraphNode closest_node(float pos_x, float pos_y, const std::string &property="") const
Get node closest to a specified point with a certain property.
void update_edge(const NavGraphEdge &edge)
Update a given edge.
static std::string format_name(const char *format,...)
Create node name from a format string.
fawkes::LockPtr< NavGraphConstraintRepo > constraint_repo() const
Get locked pointer to constraint repository.
std::vector< NavGraphNode > search_nodes(const std::string &property) const
Search nodes for given property.
int default_property_as_int(const std::string &prop) const
Get property converted to int.
void add_node(const NavGraphNode &node)
Add a node.
void set_default_properties(const std::map< std::string, std::string > &properties)
Set default properties.
void calc_reachability(bool allow_multi_graph=false)
Calculate eachability relations.
bool node_exists(const NavGraphNode &node) const
Check if a certain node exists.
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
EdgeMode
Mode to use to add edges.
@ EDGE_SPLIT_INTERSECTION
Add the edge, but if it intersects with an existing edges add new points at the intersection points f...
@ EDGE_NO_INTERSECTION
Only add edge if it does not intersect.
@ EDGE_FORCE
add nodes no matter what (be careful)
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
void set_notifications_enabled(bool enabled)
Enable or disable notifications.
float default_property_as_float(const std::string &prop) const
Get property converted to float.
NavGraphEdge closest_edge(float pos_x, float pos_y) const
Get edge closest to a specified point.
void connect_node_to_closest_node(const NavGraphNode &n)
Connect node to closest node.
std::string gen_unique_name(const char *prefix="U-")
Generate a unique node name for the given prefix.
void remove_node(const NavGraphNode &node)
Remove a node.
void set_search_funcs(navgraph::EstimateFunction estimate_func, navgraph::CostFunction cost_func)
Set cost and cost estimation function for searching paths.
fawkes::NavGraphPath search_path(const std::string &from, const std::string &to, bool use_constraints=true, bool compute_constraints=true)
Search for a path between two nodes with default distance costs.
NavGraphNode node(const std::string &name) const
Get a specified node.
std::string name() const
Get graph name.
NavGraph(const std::string &graph_name)
Constructor.
void apply_default_properties(NavGraphNode &node)
Set default properties on node for which no local value exists.
bool edge_exists(const NavGraphEdge &edge) const
Check if a certain edge exists.
void add_change_listener(ChangeListener *listener)
Add a change listener.
void connect_node_to_closest_edge(const NavGraphNode &n)
Connect node to closest edge.
void add_node_and_connect(const NavGraphNode &node, ConnectionMode conn_mode)
Add a node and connect it to the graph.
void notify_of_change() noexcept
Notify all listeners of a change.
NavGraphNode closest_node_to(const std::string &node_name, const std::string &property="") const
Get node closest to another node with a certain property.
static float to_float(std::string s)
Convert string to a float value.
static bool to_bool(std::string s)
Convert string to a bool value.
static std::string to_string(unsigned int i)
Convert unsigned int value to a string.
static int to_int(std::string s)
Convert string to an int value.
Fawkes library namespace.
float point_dist(float x1, float y1, float x2, float y2)
Get distance of two points.
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.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
struct fawkes::cart_coord_2d_struct cart_coord_2d_t
Cartesian coordinates (2D).
Cartesian coordinates (2D).