Fawkes API
Fawkes Development Version
|
23 #include <core/exceptions/software.h>
24 #include <navgraph/navgraph.h>
25 #include <navgraph/navgraph_path.h>
26 #include <utils/misc/string_split.h>
61 : graph_(graph), nodes_(nodes), cost_(cost)
77 if (cost_ < 0 && p.cost_ < 0)
79 return cost_ < p.cost_;
95 if (nodes_.size() != p.nodes_.size())
98 for (
size_t i = 0; i < nodes_.size(); ++i) {
99 if (nodes_[i] != p.nodes_[i])
103 if (cost_ >= 0 && p.cost_ >= 0 && fabs(cost_ - p.cost_) <= 0.00001)
120 nodes_.push_back(node);
121 if (cost_from_end > 0) {
122 cost_ += cost_from_end;
147 return nodes_.empty();
156 return nodes_.size();
176 return (std::find(nodes_.begin(), nodes_.end(), node) != nodes_.end());
186 if (nodes_.empty()) {
187 throw Exception(
"No nodes in plan, cannot retrieve goal");
190 return nodes_[nodes_.size() - 1];
259 NavGraphPath::Traversal::assert_initialized()
const
273 assert_initialized();
274 if (current_ >= 0 && current_ < (ssize_t)path_->nodes_.size()) {
275 return path_->nodes_[current_];
292 assert_initialized();
293 if (current_ >= 0 && current_ < (ssize_t)path_->nodes_.size() - 1) {
294 return path_->nodes_[current_ + 1];
306 return (current_ >= 0 && current_ < (ssize_t)path_->nodes_.size());
317 assert_initialized();
318 if (current_ >= 0 && current_ < (ssize_t)path_->nodes_.size()) {
331 assert_initialized();
332 if (current_ < (ssize_t)path_->nodes_.size())
335 return (current_ < (ssize_t)path_->nodes_.size());
345 assert_initialized();
346 return (current_ >= 0 && (
size_t)current_ == (path_->nodes_.size() - 1));
357 assert_initialized();
359 return path_->nodes_.size();
360 return path_->nodes_.size() - (size_t)current_;
371 assert_initialized();
372 if (!path_->graph_) {
377 return path_->cost();
380 for (ssize_t i = current_; i < (ssize_t)path_->nodes_.size() - 1; ++i) {
381 cost += path_->graph_->cost(path_->nodes_[i], path_->nodes_[i + 1]);
404 assert_initialized();
405 if (new_current >= path_->nodes_.size()) {
408 current_ = new_current;
424 std::vector<std::string>
427 std::vector<std::string>
nodes(nodes_.size());
428 for (
size_t i = 0; i < nodes_.size(); ++i) {
429 nodes[i] = nodes_[i].name();
NavGraphPath()
Default constructor.
const NavGraphNode & current() const
Get current node in path.
bool next()
Move on traversal to next node.
void set_nodes(const std::vector< NavGraphNode > &nodes, float cost=-1)
Set nodes erasing the current path.
size_t remaining() const
Get the number of remaining nodes in path traversal.
std::string get_path_as_string(const char delim=':') const
Get string representation of path.
bool operator<(const NavGraphPath &p) const
Check if this path is cheaper than the other path.
std::vector< std::string > get_node_names() const
Get names of nodes in path.
void reset()
Reset an ongoing traversal.
void add_node(const NavGraphNode &node, float cost_from_end=0)
Add a node to the path.
bool last() const
Check if the current node is the last node in the path.
float remaining_cost() const
Get the remaining cost to the goal.
float cost() const
Get cost of path from start to to end.
Fawkes library namespace.
const NavGraphNode & peek_next() const
Peek on the next node.
void clear()
Clear all nodes on this path.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the path.
bool running() const
Check if traversal is currently runnung.
void set_current(size_t new_current)
Set the current node.
size_t current_index() const
Get index of current node in path.
const NavGraph & graph() const
Get graph this path is based on.
bool contains(const NavGraphNode &node) const
Check if the path contains a given node.
static std::string str_join(const std::vector< std::string > &v, char delim='/')
Join vector of strings string using given delimiter.
A NULL pointer was supplied where not allowed.
Class representing a path for a NavGraph.
const NavGraphNode & goal() const
Get goal of path.
bool empty() const
Check if path is empty.
Sub-class representing a navgraph path traversal.
bool operator==(const NavGraphPath &p) const
Check if two paths are the same.
void invalidate()
Invalidate this traversal.
Traversal traversal() const
Get a new path traversal handle.
Base class for exceptions in Fawkes.
size_t size() const
Get size of path.