-
Notifications
You must be signed in to change notification settings - Fork 28
/
a_star_todo.cpp
153 lines (129 loc) · 6.52 KB
/
a_star_todo.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
#include "a_star.h"
#include <cmath>
#include <unordered_set>
using namespace std;
template<typename Graph, typename State>
void AStar<Graph, State>::setGraph(std::shared_ptr<Graph> graph){
graph_ = graph;
}
template<typename Graph, typename State>
void AStar<Graph, State>::breadthFirstSearch(const Vec3f &start_pt, const Vec3f &end_pt,
std::function<huristics_cost_t( typename State::Ptr a, typename State::Ptr b)> calculate_huristics) {
ros::Time start_time = ros::Time::now();
Vec3i start_idx = graph_->coord2gridIndex(start_pt);
Vec3i end_idx = graph_->coord2gridIndex(end_pt);
typename State::Ptr current_node_ptr = nullptr;
typename State::Ptr neighbor_node_ptr = nullptr;
open_set_.clear();
typename State::Ptr end_node_ptr = new State(end_idx, end_pt);
end_node_ptr->id_ = State::ROBOT_NODE_STATUS::WILL_BE;
typename State::Ptr start_node_ptr = new State(start_idx, start_pt);
start_node_ptr->id_ = State::ROBOT_NODE_STATUS::WILL_BE;
// TODO insert start_node_ptr into the open_set_
start_node_ptr->g_score_ = 0;
open_set_.insert(std::make_pair(0, start_node_ptr));
std::vector< typename State::Ptr> neighbors_ptr;
while (!open_set_.empty()) {
current_node_ptr = open_set_.begin()->second;
current_node_ptr->id_ = State::ROBOT_NODE_STATUS::WAS_THERE;
// TODO remove current_node_ptr from the open_set_
open_set_.erase(open_set_.begin());
double dist = calculate_euclidean_dis<RobotNode::Ptr>(current_node_ptr, end_node_ptr);
if (dist < graph_->resolution_) {
terminate_ptr_ = current_node_ptr;
ros::Duration use_time = ros::Time::now() - start_time;
ROS_INFO("\033[1;32m Breadth first search uses time: %f (ms)\033[0m", use_time.toSec() * 1000);
return;
}
graph_->getNeighbors(current_node_ptr, neighbors_ptr);
for (unsigned int i = 0; i < neighbors_ptr.size(); ++i) {
neighbor_node_ptr = neighbors_ptr[i];
if (neighbor_node_ptr->id_ == State::ROBOT_NODE_STATUS::WOULD_LIKE) {
neighbor_node_ptr->parent_node_ = current_node_ptr;
neighbor_node_ptr->id_ = State::ROBOT_NODE_STATUS::WILL_BE;
open_set_.insert(std::make_pair(0, neighbor_node_ptr));
continue;
}
}
}
ROS_WARN_STREAM(" Breadth first search failed to search path!");
}
template<typename Graph, typename State>
void AStar<Graph, State>::dijkstraSearchPath(const Vec3f &start_pt, const Vec3f &end_pt,
std::function<huristics_cost_t( typename State::Ptr a, typename State::Ptr b)> calculate_huristics) {
ros::Time start_time = ros::Time::now();
Vec3i start_idx = graph_->coord2gridIndex(start_pt);
Vec3i end_idx = graph_->coord2gridIndex(end_pt);
typename State::Ptr current_node_ptr = nullptr;
typename State::Ptr neighbor_node_ptr = nullptr;
open_set_.clear();
typename State::Ptr end_node_ptr = new State(end_idx, end_pt);
end_node_ptr->g_score_ = 0.0;
end_node_ptr->id_ = State::ROBOT_NODE_STATUS::WILL_BE;
typename State::Ptr start_node_ptr = new State(start_idx, start_pt);
start_node_ptr->g_score_ = 0.0;
start_node_ptr->id_ = State::ROBOT_NODE_STATUS::WILL_BE;
open_set_.insert(std::make_pair(start_node_ptr->g_score_, start_node_ptr));
std::vector< typename State::Ptr> neighbors_ptr;
while (!open_set_.empty()) {
current_node_ptr = open_set_.begin()->second;
current_node_ptr->id_ = State::ROBOT_NODE_STATUS::WAS_THERE;
open_set_.erase(open_set_.begin());
double dist = calculate_euclidean_dis<RobotNode::Ptr>(current_node_ptr, end_node_ptr);
if (dist < graph_->resolution_) {
terminate_ptr_ = current_node_ptr;
ros::Duration use_time = ros::Time::now() - start_time;
ROS_INFO("\033[1;32m Dijkstra uses time: %f (ms)\033[0m", use_time.toSec() * 1000);
return;
}
graph_->getNeighbors(current_node_ptr, neighbors_ptr);
for (unsigned int i = 0; i < neighbors_ptr.size(); ++i) {
neighbor_node_ptr = neighbors_ptr[i];
double delta_score = calculate_huristics(neighbor_node_ptr, current_node_ptr);
if (neighbor_node_ptr->id_ == State::ROBOT_NODE_STATUS::WOULD_LIKE) {
neighbor_node_ptr->g_score_ = current_node_ptr->g_score_ + delta_score;
neighbor_node_ptr->parent_node_ = current_node_ptr;
neighbor_node_ptr->id_ = State::ROBOT_NODE_STATUS::WILL_BE;
open_set_.insert(std::make_pair(neighbor_node_ptr->g_score_, neighbor_node_ptr));
continue;
} else if (neighbor_node_ptr->id_ == State::ROBOT_NODE_STATUS::WILL_BE) {
if (neighbor_node_ptr->g_score_ > current_node_ptr->g_score_ + delta_score) {
neighbor_node_ptr->g_score_ = current_node_ptr->g_score_ + delta_score;
neighbor_node_ptr->parent_node_ = current_node_ptr;
typename std::multimap<double, typename State::Ptr>::iterator map_iter = open_set_.begin();
for (; map_iter != open_set_.end(); map_iter++) {
if (map_iter->second->robot_grid_index_ == neighbor_node_ptr->robot_grid_index_) {
open_set_.erase(map_iter);
open_set_.insert(std::make_pair(neighbor_node_ptr->g_score_, neighbor_node_ptr));
break;
}
}
}
continue;
} else {
continue;
}
}
}
ROS_WARN_STREAM(" Dijkstra failed to search path!");
}
template<typename Graph, typename State>
void AStar<Graph, State>::searchPath(const Vec3f &start_pt, const Vec3f &end_pt
, std::function<huristics_cost_t( typename State::Ptr a, typename State::Ptr b)> calculate_huristics) {
}
template<typename Graph, typename State>
std::vector<Vec3f> AStar<Graph, State>::getPath() {
std::vector<Vec3f> path;
std::vector< typename State::Ptr> grid_path;
typename State::Ptr grid_node_ptr = terminate_ptr_;
while (grid_node_ptr != nullptr) {
grid_path.emplace_back(grid_node_ptr);
grid_node_ptr = grid_node_ptr->parent_node_;
}
for (auto &ptr: grid_path) {
path.emplace_back(ptr->robot_state_);
}
reverse(path.begin(), path.end());
return path;
}
template class AStar<GridGraph3D, RobotNode>;