Skip to content

Commit f40c0e9

Browse files
pllee4pllee4
authored
Merge pull request #16 from pllee4/improve-sonarcloud
Improve sonarcloud code smell complaints
2 parents 480763d + ce2aea0 commit f40c0e9

7 files changed

Lines changed: 155 additions & 101 deletions

File tree

‎src/robotics/astar/test/astar_test.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,11 @@ TEST(Astar, ValidSetOccupanciedGrid) {
2626
EXPECT_TRUE(astar.SetOccupiedGrid({{1, 6}}));
2727
}
2828

29+
TEST(Astar, InvalidSetStartAndDestination) {
30+
Astar astar{MotionConstraintType::CARDINAL_MOTION};
31+
EXPECT_FALSE(astar.SetStartAndDestination({0, 0}, {4, 4}));
32+
}
33+
2934
TEST(Astar, FailedToFindPath) {
3035
Astar astar{MotionConstraintType::CARDINAL_MOTION};
3136

‎src/robotics/astar/test/weighted_astar_test.cpp‎

Lines changed: 14 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,11 @@
1-
/*
1+
/*
22
* weighted_astar_test.cpp
3-
*
3+
*
44
* Created on: Jan 02, 2023 11:49
5-
* Description:
6-
*
5+
* Description:
6+
*
77
* Copyright (c) 2023 Pin Loon Lee (pllee4)
8-
*/
8+
*/
99

1010
#include "algorithm/robotics/astar/weighted_astar.hpp"
1111

@@ -28,6 +28,11 @@ TEST(WeightedAstar, ValidSetOccupanciedGrid) {
2828
EXPECT_TRUE(weighted_astar.SetOccupiedGrid({{1, 6}}));
2929
}
3030

31+
TEST(WeightedAstar, InvalidSetStartAndDestination) {
32+
WeightedAstar<weight> weighted_astar{MotionConstraintType::CARDINAL_MOTION};
33+
EXPECT_FALSE(weighted_astar.SetStartAndDestination({0, 0}, {4, 4}));
34+
}
35+
3136
TEST(WeightedAstar, FailedToFindPath) {
3237
WeightedAstar<weight> weighted_astar{MotionConstraintType::CARDINAL_MOTION};
3338

@@ -156,7 +161,8 @@ TEST(WeightedAstar, GetPathAfterReset) {
156161
}
157162

158163
TEST(WeightedAstar, GetPath8Dir) {
159-
WeightedAstar<weight> weighted_astar{MotionConstraintType::CARDINAL_ORDINAL_MOTION};
164+
WeightedAstar<weight> weighted_astar{
165+
MotionConstraintType::CARDINAL_ORDINAL_MOTION};
160166

161167
/**
162168
* s = start, e = end, x = occupied
@@ -201,7 +207,8 @@ TEST(WeightedAstar, GetPathWithRevisit) {
201207
}
202208

203209
TEST(WeightedAstar, GetPath8DirWithRevisit) {
204-
WeightedAstar<weight> weighted_astar{MotionConstraintType::CARDINAL_ORDINAL_MOTION};
210+
WeightedAstar<weight> weighted_astar{
211+
MotionConstraintType::CARDINAL_ORDINAL_MOTION};
205212

206213
/**
207214
* s = start, e = end, x = occupied

‎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;

‎src/robotics/best_first_search/test/best_first_search_test.cpp‎

Lines changed: 16 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,20 +1,18 @@
1-
/*
1+
/*
22
* best_first_search_test.cpp
3-
*
3+
*
44
* Created on: Jan 04, 2023 21:26
5-
* Description:
6-
*
5+
* Description:
6+
*
77
* Copyright (c) 2023 Pin Loon Lee (pllee4)
8-
*/
8+
*/
99

1010
#include "algorithm/robotics/best_first_search/best_first_search.hpp"
1111

1212
#include "gtest/gtest.h"
1313

1414
using namespace pllee4::graph;
1515

16-
static constexpr uint8_t weight = 10;
17-
1816
TEST(BestFirstSearch, InvalidSetOccupiedGrid) {
1917
BestFirstSearch best_first_search{MotionConstraintType::CARDINAL_MOTION};
2018
EXPECT_FALSE(best_first_search.SetOccupiedGrid({{1, 6}}));
@@ -28,6 +26,11 @@ TEST(BestFirstSearch, ValidSetOccupanciedGrid) {
2826
EXPECT_TRUE(best_first_search.SetOccupiedGrid({{1, 6}}));
2927
}
3028

29+
TEST(BestFirstSearch, InvalidSetStartAndDestination) {
30+
BestFirstSearch best_first_search{MotionConstraintType::CARDINAL_MOTION};
31+
EXPECT_FALSE(best_first_search.SetStartAndDestination({0, 0}, {4, 4}));
32+
}
33+
3134
TEST(BestFirstSearch, FailedToFindPath) {
3235
BestFirstSearch best_first_search{MotionConstraintType::CARDINAL_MOTION};
3336

@@ -156,7 +159,8 @@ TEST(BestFirstSearch, GetPathAfterReset) {
156159
}
157160

158161
TEST(BestFirstSearch, GetPath8Dir) {
159-
BestFirstSearch best_first_search{MotionConstraintType::CARDINAL_ORDINAL_MOTION};
162+
BestFirstSearch best_first_search{
163+
MotionConstraintType::CARDINAL_ORDINAL_MOTION};
160164

161165
/**
162166
* s = start, e = end, x = occupied
@@ -172,7 +176,8 @@ TEST(BestFirstSearch, GetPath8Dir) {
172176
EXPECT_TRUE(best_first_search.GetPath().has_value());
173177
const auto path = best_first_search.GetPath().value();
174178

175-
std::vector<Coordinate> expected = {{0, 2}, {0, 1}, {0, 0}, {1, 0}, {2, 1}, {2, 2}};
179+
std::vector<Coordinate> expected = {{0, 2}, {0, 1}, {0, 0},
180+
{1, 0}, {2, 1}, {2, 2}};
176181
EXPECT_TRUE(std::equal(std::begin(path), std::end(path), std::begin(expected),
177182
std::end(expected)));
178183
}
@@ -201,7 +206,8 @@ TEST(BestFirstSearch, GetPathWithRevisit) {
201206
}
202207

203208
TEST(BestFirstSearch, GetPath8DirWithRevisit) {
204-
BestFirstSearch best_first_search{MotionConstraintType::CARDINAL_ORDINAL_MOTION};
209+
BestFirstSearch best_first_search{
210+
MotionConstraintType::CARDINAL_ORDINAL_MOTION};
205211

206212
/**
207213
* s = start, e = end, x = occupied

‎src/robotics/bresenham/src/bresenham.cpp‎

Lines changed: 19 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -27,17 +27,29 @@ LineGenerator::LineGenerator(const Coordinate& start, const Coordinate& end) {
2727
while (true) {
2828
points_.emplace_back(Coordinate{x, y});
2929
if (start == end) break;
30+
3031
const auto e2 = 2 * error;
32+
bool should_break = false;
33+
3134
if (e2 >= dy) {
32-
if (x == end.x) break;
33-
error += dy;
34-
x += sx;
35+
if (x == end.x) {
36+
should_break = true;
37+
} else {
38+
error += dy;
39+
x += sx;
40+
}
3541
}
36-
if (e2 <= dx) {
37-
if (y == end.y) break;
38-
error += dx;
39-
y += sy;
42+
43+
if (!should_break && e2 <= dx) {
44+
if (y == end.y) {
45+
should_break = true;
46+
} else {
47+
error += dx;
48+
y += sy;
49+
}
4050
}
51+
52+
if (should_break) break;
4153
}
4254
}
4355

‎src/robotics/bresenham/test/bresenham_test.cpp‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,3 +36,13 @@ TEST(Bresenham, CheckLineFromPositiveTowardsRightUpwards) {
3636
EXPECT_TRUE(std::equal(std::begin(points), std::end(points),
3737
std::begin(expected), std::end(expected)));
3838
}
39+
40+
TEST(Bresenham, CheckVerticalLineReachesEndYFirst) {
41+
Coordinate start{0, 0};
42+
Coordinate end{1, 3};
43+
LineGenerator line(start, end);
44+
auto points = line.GetPoints();
45+
std::vector<Coordinate> expected = {{0, 0}, {0, 1}, {1, 2}, {1, 3}};
46+
EXPECT_TRUE(std::equal(std::begin(points), std::end(points),
47+
std::begin(expected), std::end(expected)));
48+
}

‎src/robotics/dijikstra/test/dijikstra_test.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,11 @@ TEST(Dijikstra, ValidSetOccupanciedGrid) {
2626
EXPECT_TRUE(dijikstra.SetOccupiedGrid({{1, 6}}));
2727
}
2828

29+
TEST(Dijikstra, InvalidSetStartAndDestination) {
30+
Dijikstra dijikstra{MotionConstraintType::CARDINAL_MOTION};
31+
EXPECT_FALSE(dijikstra.SetStartAndDestination({0, 0}, {4, 4}));
32+
}
33+
2934
TEST(Dijikstra, FailedToFindPath) {
3035
Dijikstra dijikstra{MotionConstraintType::CARDINAL_MOTION};
3136

0 commit comments

Comments
 (0)