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];
208 return Traversal(
this);
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();