Skip to content

Commit 502b9c1

Browse files
vss2snvss2sn
committed
Minor fixes
1 parent 8b75bc2 commit 502b9c1

18 files changed

+181
-147
lines changed

‎cmake/ProjectSpecificOptions.cmake

Lines changed: 0 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -10,15 +10,9 @@ cmake_dependent_option( RUN_TESTS "Build and run tests" OFF "NOT BUILD_INDIVIDUA
1010
# default OFF, change by user input if and only if condition allows (BUILD_INDIVIDUAL=OFF)
1111
cmake_dependent_option( CHECK_COVERAGE "Run code coverage check" OFF "RUN_TESTS" OFF)
1212
# default OFF, change by user input if and only if condition allows (RUN_TESTS=ON)
13-
cmake_dependent_option( DYNAMIC_ALGOS "Build and run algorithms with obstacle detection in real time (D* Lite)" ON "NOT RUN_TESTS" OFF)
14-
# default OFF, change by user input if and only if condition allows (RUN_TESTS=OFF)
1513
cmake_dependent_option( CUSTOM_DEBUG_HELPER_FUNCION "Build custom debug helper functions" OFF "NOT RUN_TESTS" OFF)
1614
# default OFF, change by user input if and only if condition allows (RUN_TESTS=OFF)
1715

1816
if(CUSTOM_DEBUG_HELPER_FUNCION)
1917
add_definitions(-DCUSTOM_DEBUG_HELPER_FUNCION)
2018
endif(CUSTOM_DEBUG_HELPER_FUNCION)
21-
22-
if(DYNAMIC_ALGOS)
23-
add_definitions(-DDYNAMIC_ALGOS)
24-
endif(DYNAMIC_ALGOS)

‎include/path_planning/d_star_lite.hpp

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -163,10 +163,9 @@ class DStarLite : public Planner {
163163
/**
164164
* @brief Create a square grid of size n and set each value to
165165
* std::numeric_limits<double>::max()
166-
* @param n length of side
167166
* @return grid created
168167
*/
169-
std::vector<std::vector<double>> CreateGrid(const int n);
168+
std::vector<std::vector<double>> CreateGrid();
170169

171170
std::vector<std::vector<double>> rhs_;
172171
std::vector<std::vector<double>> g_;

‎include/path_planning/genetic_algorithm.hpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ class GeneticAlgorithm : public Planner {
3636
* @return no return value
3737
*/
3838
void SetParams(const int generations = 10000, const int popsize = 30,
39-
const float c = 1.05, const bool shorten_chromosome = false,
39+
const double c = 1.05, const bool shorten_chromosome = false,
4040
const int path_length = 30);
4141

4242
/**
@@ -128,7 +128,7 @@ class GeneticAlgorithm : public Planner {
128128
int f_val = std::numeric_limits<int>::max();
129129
int generations_ = 10000;
130130
int popsize_ = 30;
131-
float c_ = 1.05;
131+
double c_ = 1.05;
132132
std::vector<std::vector<Node>> paths_;
133133
std::vector<std::vector<Node>> truepaths_;
134134
bool found_ = false;

‎include/path_planning/lpa_star.hpp

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -164,10 +164,9 @@ class LPAStar : public Planner {
164164
/**
165165
* @brief Create a square grid of size n and set each value to
166166
* std::numeric_limits<double>::max()
167-
* @param n length of side
168167
* @return grid created
169168
*/
170-
std::vector<std::vector<double>> CreateGrid(const int n);
169+
std::vector<std::vector<double>> CreateGrid();
171170

172171
void ClearPathDisplay(const std::vector<Node>& path);
173172
std::vector<Node> GetNewPath();

‎include/path_planning/rrt.hpp

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,9 @@ class RRT : public Planner {
2727
*/
2828
explicit RRT(std::vector<std::vector<int>> grid) : Planner(std::move(grid)) {}
2929

30+
void SetParams(const int threshold = 2, const int max_iter_x_factor = 20);
31+
32+
3033
/**
3134
* @brief RRT algorithm implementation
3235
* @param start start node

‎include/path_planning/rrt_star.hpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,8 @@ class RRTStar : public Planner {
2828
explicit RRTStar(std::vector<std::vector<int>> grid)
2929
: Planner(std::move(grid)) {}
3030

31+
void SetParams(const int threshold = 2, const int max_iter_x_factor = 20);
32+
3133
/**
3234
* @brief RRT* algorithm implementation
3335
* @param start start node

‎lib/utils/src/utils.cpp

Lines changed: 16 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -190,11 +190,13 @@ void LazyPQ::insert(const NodeKeyPair& t) {
190190

191191
void LazyPQ::pop() {
192192
while(!pq.empty()) {
193-
if (const auto it = s.find(pq.top()); it == s.end()) { // Element been removed from set
193+
if (const auto it = s.find(pq.top()); it == s.end() ||
194+
(it != s.end() && pq.top().key != it->key)) {
195+
// Element been removed from set OR
196+
// Element has been updated in set with new key, and inserted already into pq with new value
194197
pq.pop();
195-
} else if (it != s.end() && pq.top().key != it->key) { // Element has been updated in set with new key, and inserted already into pq with new value
196-
pq.pop();
197-
} else if (it != s.end() && pq.top().key == it->key) { // Found an elelment that is in set and priority queue
198+
} else if (it != s.end() && pq.top().key == it->key) {
199+
// Found an elelment that is in set and priority queue
198200
break;
199201
}
200202
}
@@ -206,11 +208,13 @@ void LazyPQ::pop() {
206208
// The loop below allows top() to be const without making the
207209
// std::priority_queue mutable
208210
while(!pq.empty()) {
209-
if (const auto it = s.find(pq.top()); it == s.end()) { // Element been removed from set
210-
pq.pop();
211-
} else if (it != s.end() && pq.top().key != it->key) { // Element has been updated in set with new key, and inserted already into pq with new value
211+
if (const auto it = s.find(pq.top()); it == s.end() ||
212+
(it != s.end() && pq.top().key != it->key)) {
213+
// Element been removed from set OR
214+
// Element has been updated in set with new key, and inserted already into pq with new value
212215
pq.pop();
213-
} else if (it != s.end() && pq.top().key == it->key) { // Found an elelment that is in set and priority queue
216+
} else if (it != s.end() && pq.top().key == it->key) {
217+
// Found an elelment that is in set and priority queue
214218
break;
215219
}
216220
}
@@ -238,9 +242,10 @@ void LazyPQ::remove(const NodeKeyPair& t) {
238242
}
239243
// Ensure top() is const
240244
while(!pq.empty()) {
241-
if (const auto it = s.find(pq.top()); it == s.end()) { // Element been removed from set
242-
pq.pop();
243-
} else if (it != s.end() && pq.top().key != it->key) { // Element has been updated in set with new key, and inserted already into pq with new value
245+
if (const auto it = s.find(pq.top()); it == s.end() ||
246+
(it != s.end() && pq.top().key != it->key)) {
247+
// Element been removed from set OR
248+
// Element has been updated in set with new key, and inserted already into pq with new value
244249
pq.pop();
245250
} else if (it != s.end() && pq.top().key == it->key) { // Found an elelment that is in set and priority queue
246251
break;

‎main/main.cpp

Lines changed: 15 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -49,10 +49,17 @@ int main() {
4949
// Variables for Ant Colony Optimization
5050
constexpr int n_ants = 10;
5151
constexpr int iterations = 50;
52-
constexpr float alpha = 1;
53-
constexpr float beta = 0.7;
54-
constexpr float evap_rate = 0.3;
55-
constexpr float Q = 10;
52+
constexpr double alpha = 1;
53+
constexpr double beta = 0.7;
54+
constexpr double evap_rate = 0.3;
55+
constexpr double Q = 10;
56+
57+
// Variables for Genetic Algorithm
58+
constexpr int generations = 10000;
59+
constexpr int popsize = 30;
60+
constexpr double c = 1.05;
61+
constexpr bool shorten_chromosome = true;
62+
constexpr int path_length_x_factor = 4;
5663

5764
// Resetting grid
5865
// Create object for the algorithm
@@ -117,6 +124,7 @@ int main() {
117124
// clang-format on
118125
grid = main_grid;
119126
RRT rrt(grid);
127+
rrt.SetParams(threshold, max_iter_x_factor);
120128
{
121129
const auto [path_found, path_vector] = rrt.Plan(start, goal);
122130
PrintPath(path_vector, start, goal, grid);
@@ -129,6 +137,7 @@ int main() {
129137
// clang-format on
130138
grid = main_grid;
131139
RRTStar rrt_star(grid);
140+
rrt_star.SetParams(threshold, max_iter_x_factor);
132141
{
133142
const auto [path_found, path_vector] = rrt_star.Plan(start, goal);
134143
PrintPath(path_vector, start, goal, grid);
@@ -168,7 +177,8 @@ int main() {
168177
// clang-format on
169178
grid = main_grid;
170179
GeneticAlgorithm genetic_algorithm(grid);
171-
genetic_algorithm.SetParams(10000, 30, 1.05, true, 4 * start.h_cost_);
180+
genetic_algorithm.SetParams(generations, popsize, c, shorten_chromosome,
181+
static_cast<int>(path_length_x_factor * start.h_cost_));
172182
{
173183
const auto [path_found, path_vector] = genetic_algorithm.Plan(start, goal);
174184
PrintPathInOrder(path_vector, start, goal, grid);

‎src/a_star.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -85,7 +85,7 @@ std::vector<Node> AStar::ConvertClosedListToPath(
8585
* @return 0
8686
*/
8787
int main() {
88-
int n = 11;
88+
constexpr int n = 11;
8989
std::vector<std::vector<int>> grid(n, std::vector<int>(n, 0));
9090
MakeGrid(grid);
9191

‎src/ant_colony.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -72,7 +72,7 @@ std::tuple<bool, std::vector<Node>> AntColony::Plan(const Node& start,
7272
}
7373

7474
// heuristically set max steps
75-
const int max_steps = std::pow(n_, 2) / 2 + n_;
75+
const int max_steps = n_ * n_ / 2 + n_;
7676

7777
std::random_device device;
7878
std::mt19937 engine(device());
@@ -213,7 +213,7 @@ std::tuple<bool, std::vector<Node>> AntColony::Plan(const Node& start,
213213
* @return 0
214214
*/
215215
int main() {
216-
int n = 11;
216+
constexpr int n = 11;
217217
std::vector<std::vector<int>> grid(n, std::vector<int>(n, 0));
218218
MakeGrid(grid);
219219

0 commit comments

Comments
 (0)