Skip to content

Commit 459d405

Browse files
pllee4pllee4
committed
fix code smell
1 parent db4d4ba commit 459d405

1 file changed

Lines changed: 86 additions & 77 deletions

File tree

‎src/robotics/astar_variant/src/astar_variant_base.cpp‎

Lines changed: 86 additions & 77 deletions
Original file line numberDiff line numberDiff line change
@@ -36,96 +36,105 @@ bool AstarVariantBase::SetOccupiedGrid(
3636

3737
bool AstarVariantBase::SetStartAndDestination(const Coordinate &start,
3838
const Coordinate &dest) {
39-
if (map_storage_) {
40-
if (map_storage_->Contains(start) && map_storage_->Contains(dest)) {
41-
// new start / destination
42-
if (reset_called_ || !(start_ == start && dest_ == dest)) {
43-
start_ = start;
44-
dest_ = dest;
45-
46-
if (reset_called_) {
47-
reset_called_ = false;
48-
} else {
49-
// clear possible historical search
50-
ResetFunc(true);
51-
}
52-
53-
auto &map = map_storage_->GetMap();
54-
traverse_path_.insert(
55-
std::make_pair(0, std::make_pair(start.x, start.y)));
56-
map[start.x][start.y].parent_coordinate = start;
57-
map[start.x][start.y].f = 0;
58-
map[start.x][start.y].g = 0;
59-
map[start.x][start.y].h = 0;
60-
}
61-
start_and_end_set_ = true;
62-
return true;
39+
if (!map_storage_) {
40+
return false;
41+
}
42+
43+
if (map_storage_->Contains(start) && map_storage_->Contains(dest)) {
44+
bool require_update = false;
45+
46+
if (reset_called_) {
47+
reset_called_ = false;
48+
require_update = true;
49+
} else if (!(start_ == start && dest_ == dest)) // new start / destination
50+
{
51+
// clear possible historical search
52+
ResetFunc(true);
53+
require_update = true;
6354
}
55+
56+
if (require_update) {
57+
start_ = start;
58+
dest_ = dest;
59+
60+
auto &map = map_storage_->GetMap();
61+
traverse_path_.insert(
62+
std::make_pair(0, std::make_pair(start.x, start.y)));
63+
map[start.x][start.y].parent_coordinate = start;
64+
map[start.x][start.y].f = 0;
65+
map[start.x][start.y].g = 0;
66+
map[start.x][start.y].h = 0;
67+
}
68+
start_and_end_set_ = true;
69+
return true;
6470
}
71+
6572
return false;
6673
}
6774

6875
std::optional<std::vector<Coordinate>> AstarVariantBase::StepOverPathFinding() {
69-
if (found_path_) return std::nullopt;
70-
if (start_and_end_set_) {
71-
if (!traverse_path_.empty()) {
72-
const auto travelled_path = traverse_path_.begin();
73-
traverse_path_.erase(traverse_path_.begin());
76+
if (found_path_) {
77+
return std::nullopt;
78+
}
7479

75-
const auto [i, j] = travelled_path->second;
80+
if ((!start_and_end_set_) || traverse_path_.empty()) {
81+
return std::nullopt;
82+
}
7683

77-
// mark node as visited
78-
visited_map_[i][j] = true;
84+
const auto travelled_path = traverse_path_.begin();
85+
traverse_path_.erase(traverse_path_.begin());
7986

80-
auto &map = map_storage_->GetMap();
87+
const auto [i, j] = travelled_path->second;
88+
89+
// mark node as visited
90+
visited_map_[i][j] = true;
91+
92+
auto &map = map_storage_->GetMap();
93+
94+
std::vector<Coordinate> expanded_nodes;
95+
96+
for (size_t k = 0; k < motion_constraint_.size(); ++k) {
97+
Coordinate coordinate;
98+
coordinate.x = i + motion_constraint_.dx[k];
99+
coordinate.y = j + motion_constraint_.dy[k];
100+
if (!map_storage_->Contains(coordinate)) continue;
101+
102+
if (coordinate == dest_) {
103+
map[coordinate.x][coordinate.y].parent_coordinate = {i, j};
104+
found_path_ = true;
105+
return std::nullopt;
106+
}
107+
108+
// not occupied
109+
if (!map[coordinate.x][coordinate.y].occupied &&
110+
!visited_map_[coordinate.x][coordinate.y]) {
111+
const auto dist_travelled =
112+
sqrt(abs(motion_constraint_.dx[k]) + abs(motion_constraint_.dy[k]));
113+
114+
const auto astar_variant_spec = GetAstarVariantSpec();
115+
116+
const auto new_g = map[i][j].g + dist_travelled;
117+
const auto new_h = astar_variant_spec.heuristic_func(
118+
coordinate.x - dest_.x, coordinate.y - dest_.y);
119+
const auto new_f =
120+
astar_variant_spec.w1 * new_g + astar_variant_spec.w2 * new_h;
121+
122+
if (map[coordinate.x][coordinate.y].f ==
123+
std::numeric_limits<MapStorage::CostDataType>::max() ||
124+
map[coordinate.x][coordinate.y].f >= new_f) {
125+
map[coordinate.x][coordinate.y].f = new_f;
126+
map[coordinate.x][coordinate.y].g = new_g;
127+
map[coordinate.x][coordinate.y].h = new_h;
128+
map[coordinate.x][coordinate.y].parent_coordinate = {i, j};
129+
130+
traverse_path_.insert(
131+
std::make_pair(new_f, std::make_pair(coordinate.x, coordinate.y)));
81132

82-
std::vector<Coordinate> expanded_nodes;
83-
84-
for (size_t k = 0; k < motion_constraint_.size(); ++k) {
85-
Coordinate coordinate;
86-
coordinate.x = i + motion_constraint_.dx[k];
87-
coordinate.y = j + motion_constraint_.dy[k];
88-
if (!map_storage_->Contains(coordinate)) continue;
89-
90-
if (coordinate == dest_) {
91-
map[coordinate.x][coordinate.y].parent_coordinate = {i, j};
92-
found_path_ = true;
93-
return std::nullopt;
94-
}
95-
96-
// not occupied
97-
if (!map[coordinate.x][coordinate.y].occupied &&
98-
!visited_map_[coordinate.x][coordinate.y]) {
99-
const auto dist_travelled = sqrt(abs(motion_constraint_.dx[k]) +
100-
abs(motion_constraint_.dy[k]));
101-
102-
const auto astar_variant_spec = GetAstarVariantSpec();
103-
104-
const auto new_g = map[i][j].g + dist_travelled;
105-
const auto new_h = astar_variant_spec.heuristic_func(
106-
coordinate.x - dest_.x, coordinate.y - dest_.y);
107-
const auto new_f =
108-
astar_variant_spec.w1 * new_g + astar_variant_spec.w2 * new_h;
109-
110-
if (map[coordinate.x][coordinate.y].f ==
111-
std::numeric_limits<MapStorage::CostDataType>::max() ||
112-
map[coordinate.x][coordinate.y].f >= new_f) {
113-
map[coordinate.x][coordinate.y].f = new_f;
114-
map[coordinate.x][coordinate.y].g = new_g;
115-
map[coordinate.x][coordinate.y].h = new_h;
116-
map[coordinate.x][coordinate.y].parent_coordinate = {i, j};
117-
118-
traverse_path_.insert(std::make_pair(
119-
new_f, std::make_pair(coordinate.x, coordinate.y)));
120-
121-
expanded_nodes.push_back(coordinate);
122-
}
123-
}
133+
expanded_nodes.push_back(coordinate);
124134
}
125-
return expanded_nodes;
126135
}
127136
}
128-
return std::nullopt;
137+
return expanded_nodes;
129138
}
130139
bool AstarVariantBase::FindPath() {
131140
if (!start_and_end_set_) return false;

0 commit comments

Comments
 (0)