| 179 | } |
| 180 | |
| 181 | void Astar::backtrack(const NodePtr& end_node, const Eigen::Vector3d& end) { |
| 182 | path_nodes_.push_back(end); |
| 183 | path_nodes_.push_back(end_node->position); |
| 184 | NodePtr cur_node = end_node; |
| 185 | while (cur_node->parent != NULL) { |
| 186 | cur_node = cur_node->parent; |
| 187 | path_nodes_.push_back(cur_node->position); |
| 188 | } |
| 189 | reverse(path_nodes_.begin(), path_nodes_.end()); |
| 190 | } |
| 191 | |
| 192 | std::vector<Eigen::Vector3d> Astar::getPath() { |
| 193 | return path_nodes_; |