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();
void clear()
Clear all nodes on this path.
size_t size() const
Get size of path.
void reset()
Reset an ongoing traversal.
const NavGraphNode & current() const
Get current node in path.
std::string get_path_as_string(const char delim=':') const
Get string representation of path.
Fawkes library namespace.
bool operator<(const NavGraphPath &p) const
Check if this path is cheaper than the other path.
Class representing a path for a NavGraph.
A NULL pointer was supplied where not allowed.
void set_current(size_t new_current)
Set the current node.
void set_nodes(const std::vector< NavGraphNode > &nodes, float cost=-1)
Set nodes erasing the current path.
float cost() const
Get cost of path from start to to end.
bool last() const
Check if the current node is the last node in the path.
Base class for exceptions in Fawkes.
std::vector< std::string > get_node_names() const
Get names of nodes in path.
const NavGraphNode & peek_next() const
Peek on the next node.
size_t remaining() const
Get the number of remaining nodes in path traversal.
static std::string str_join(const std::vector< std::string > &v, char delim='/')
Join vector of strings string using given delimiter.
const NavGraph & graph() const
Get graph this path is based on.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the path.
Traversal traversal() const
Get a new path traversal handle.
Sub-class representing a navgraph path traversal.
bool running() const
Check if traversal is currently runnung.
bool empty() const
Check if path is empty.
size_t current_index() const
Get index of current node in path.
float remaining_cost() const
Get the remaining cost to the goal.
bool operator==(const NavGraphPath &p) const
Check if two paths are the same.
void invalidate()
Invalidate this traversal.
void add_node(const NavGraphNode &node, float cost_from_end=0)
Add a node to the path.
bool contains(const NavGraphNode &node) const
Check if the path contains a given node.
bool next()
Move on traversal to next node.
NavGraphPath()
Default constructor.
const NavGraphNode & goal() const
Get goal of path.