23 #include <navgraph/navgraph_path.h> 24 #include <navgraph/navgraph.h> 25 #include <core/exceptions/software.h> 26 #include <utils/misc/string_split.h> 66 std::vector<NavGraphNode> &
nodes,
float cost)
67 : graph_(graph), nodes_(nodes), cost_(cost)
83 if (cost_ < 0 && p.cost_ < 0)
return false;
84 return cost_ < p.cost_;
101 if (nodes_.size() != p.nodes_.size())
return false;
103 for (
size_t i = 0; i < nodes_.size(); ++i) {
104 if (nodes_[i] != p.nodes_[i])
return false;
107 if (cost_ >= 0 && p.cost_ >= 0 && fabs(cost_ - p.cost_) <= 0.00001)
return false;
124 nodes_.push_back(node);
125 if (cost_from_end > 0) {
126 cost_ += cost_from_end;
153 return nodes_.empty();
163 return nodes_.size();
185 return (std::find(nodes_.begin(), nodes_.end(), node) != nodes_.end());
196 if (nodes_.empty()) {
197 throw Exception(
"No nodes in plan, cannot retrieve goal");
200 return nodes_[nodes_.size() - 1];
249 : path_(NULL), current_(-1)
258 : path_(path), current_(-1)
276 NavGraphPath::Traversal::assert_initialized()
const 289 assert_initialized();
290 if (current_ >= 0 && current_ < (ssize_t)path_->nodes_.size()) {
291 return path_->nodes_[current_];
309 assert_initialized();
310 if (current_ >= 0 && current_ < (ssize_t)path_->nodes_.size() - 1) {
311 return path_->nodes_[current_ + 1];
324 return (current_ >= 0 && current_ < (ssize_t)path_->nodes_.size());
335 assert_initialized();
336 if (current_ >= 0 && current_ < (ssize_t)path_->nodes_.size()) {
349 assert_initialized();
350 if (current_ < (ssize_t)path_->nodes_.size()) current_ += 1;
352 return (current_ < (ssize_t)path_->nodes_.size());
363 assert_initialized();
364 return (current_ >= 0 && (
size_t)current_ == (path_->nodes_.size() - 1));
376 assert_initialized();
377 if (current_ < 0)
return path_->nodes_.size();
378 return path_->nodes_.size() - (size_t)current_;
390 assert_initialized();
391 if (! path_->graph_) {
395 if (current_ < 0)
return path_->
cost();
398 for (ssize_t i = current_; i < (ssize_t)path_->nodes_.size() - 1; ++i) {
399 cost += path_->graph_->
cost(path_->nodes_[i], path_->nodes_[i+1]);
424 assert_initialized();
425 if (new_current >= path_->nodes_.size()) {
428 current_ = new_current;
445 std::vector<std::string>
448 std::vector<std::string>
nodes(nodes_.size());
449 for (
size_t i = 0; i < nodes_.size(); ++i){
450 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.
const NavGraphPath & path() const
Get parent path the traversal belongs to.
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.
float cost(const NavGraphNode &from, const NavGraphNode &to) const
Calculate cost between two adjacent nodes.
const NavGraphNode & goal() const
Get goal of path.