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> 42 const 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_;
132 const std::vector<NavGraphNode> &
141 const std::vector<NavGraphEdge> &
154 return constraint_repo_;
165 std::vector<NavGraphNode>::const_iterator n =
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) {
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();
385 std::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 =
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;
474 typename std::enable_if<!std::numeric_limits<T>::is_integer,
bool>::type
475 almost_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();
524 new_edge.set_property(
"generated",
true);
539 if (almost_equal(closest_conn.
distance(p.x, p.y), 0.f, 2)) {
550 new_edge.set_property(
"created-for", cn.
name() +
"--" + n.
name());
559 new_edge_2.set_properties(closest.
properties());
560 new_edge_3.set_property(
"created-for", cn.
name() +
"--" + n.
name());
561 new_edge_3.set_property(
"generated",
true);
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(),
621 reachability_calced_ =
false;
631 nodes_.erase(std::remove_if(nodes_.begin(),
637 edges_.erase(std::remove_if(edges_.begin(),
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(),
687 reachability_calced_ =
false;
698 edges_.erase(std::remove_if(edges_.begin(),
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();
758 const 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_) {
888 std::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) {
1103 solstate = dynamic_cast<NavGraphSearchState *>(a_star_solution[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);
1130 NavGraph::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());
1150 NavGraph::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());
1181 NavGraph::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());
1310 NavGraph::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();
void add_node_and_connect(const NavGraphNode &node, ConnectionMode conn_mode)
Add a node and connect it to the graph.
const std::string & from() const
Get edge originating node name.
NavGraphEdge closest_edge(float pos_x, float pos_y) const
Get edge closest to a specified point.
fawkes::LockPtr< NavGraphConstraintRepo > constraint_repo() const
Get locked pointer to constraint repository.
void remove_edge(const NavGraphEdge &edge)
Remove an edge.
float default_property_as_float(const std::string &prop) const
Get property converted to float.
void set_property(const std::string &property, const std::string &value)
Set property.
bool default_property_as_bool(const std::string &prop) const
Get property converted to bol.
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.
const std::map< std::string, std::string > & default_properties() const
Get all default properties.
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.
void set_default_property(const std::string &property, const std::string &value)
Set property.
void remove_node(const NavGraphNode &node)
Remove a node.
Graph-based path planner A* search state.
NavGraph & operator=(const NavGraph &g)
Assign/copy structures from another graph.
void set_properties(const std::map< std::string, std::string > &properties)
Overwrite properties with given ones.
void set_default_properties(const std::map< std::string, std::string > &properties)
Set default properties.
bool is_directed() const
Check if edge is directed.
static float straight_line_estimate(const fawkes::NavGraphNode &node, const fawkes::NavGraphNode &goal)
Determine straight line estimate between two nodes.
void add_node(const NavGraphNode &node)
Add a node.
static std::string format_name(const char *format,...)
Create node name from a format string.
bool has_property(const std::string &property) const
Check if node has specified property.
Class representing a path for a NavGraph.
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.
bool is_valid() const
Check if node is valid, i.e.
std::vector< NavGraphNode > search_nodes(const std::string &property) const
Search nodes for given property.
const NavGraphNode & from_node() const
Get edge originating node.
bool edge_exists(const NavGraphEdge &edge) const
Check if a certain edge exists.
const std::map< std::string, std::string > & properties() const
Get all properties.
virtual ~ChangeListener()
Virtual empty destructor.
try to connect to closest edge, if that fails, connect to closest node
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
void add_change_listener(ChangeListener *listener)
Add a change listener.
static int to_int(std::string s)
Convert string to an int value.
std::vector< std::string > reachable_nodes(const std::string &node_name) const
Get nodes reachable from specified nodes.
LockPtr<> is a reference-counting shared lockable smartpointer.
void update_edge(const NavGraphEdge &edge)
Update a given edge.
void set_search_funcs(navgraph::EstimateFunction estimate_func, navgraph::CostFunction cost_func)
Set cost and cost estimation function for searching paths.
void update_node(const NavGraphNode &node)
Update a given node.
std::string name() const
Get graph name.
Constraint repository to maintain blocks on nodes.
EdgeMode
Mode to use to add edges.
void unset_search_funcs()
Reset actual and estimated cost function to defaults.
std::string gen_unique_name(const char *prefix="U-")
Generate a unique node name for the given prefix.
NavGraph(const std::string &graph_name)
Constructor.
void add_edge(const NavGraphEdge &edge, EdgeMode mode=EDGE_NO_INTERSECTION, bool allow_existing=false)
Add an edge.
fawkes::cart_coord_2d_t closest_point_on_edge(float x, float y) const
Get the point on edge closest to a given point.
void set_notifications_enabled(bool enabled)
Enable or disable notifications.
Base class for exceptions in Fawkes.
const std::string & to() const
Get edge target node name.
add nodes no matter what (be careful)
void remove_orphan_nodes()
Remove orphan nodes.
std::string default_property(const std::string &prop) const
Get specified default property as string.
Add the edge, but if it intersects with an existing edges add new points at the intersection points f...
This is an implementation of the A* search algorithm.
void set_property(const std::string &property, const std::string &value)
Set property.
void remove_change_listener(ChangeListener *listener)
Remove a change listener.
void connect_node_to_closest_edge(const NavGraphNode &n)
Connect node to closest edge.
NavGraphEdge edge(const std::string &from, const std::string &to) const
Get a specified edge.
NavGraphNode node(const std::string &name) const
Get a specified node.
const std::string & name() const
Get name of node.
bool node_exists(const NavGraphNode &node) const
Check if a certain node exists.
void clear()
Remove all nodes and edges from navgraph.
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 apply_default_properties(NavGraphNode &node)
Set default properties on node for which no local value exists.
void notify_of_change()
Notify all listeners of a change.
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
struct fawkes::cart_coord_2d_struct cart_coord_2d_t
Cartesian coordinates (2D).
float y() const
Get Y coordinate in global frame.
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.
int default_property_as_int(const std::string &prop) const
Get property converted to int.
Only add edge if it does not intersect.
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 euclidean_cost(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Determine euclidean cost between two nodes.
virtual ~NavGraph()
Virtual empty destructor.
bool has_default_property(const std::string &property) const
Check if graph has specified default property.
float x() const
Get X coordinate in global frame.
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.
Topological graph change listener.
ConnectionMode
Connect mode enum for connect_node_* methods.
static std::string to_string(unsigned int i)
Convert unsigned int value to a string.
void calc_reachability(bool allow_multi_graph=false)
Calculate eachability relations.
void connect_node_to_closest_node(const NavGraphNode &n)
Connect node to closest node.
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.
float cost(const NavGraphNode &from, const NavGraphNode &to) const
Calculate cost between two adjacent nodes.
const NavGraphNode & to_node() const
Get edge target node.
std::vector< AStarState * > solve(AStarState *initialState)
Solves a situation given by the initial state with AStar, and returns a vector of AStarStates that so...