From e365ec692a5875562800bab6121a0186dcdc8e83 Mon Sep 17 00:00:00 2001 From: Pin Loon Lee Date: Sat, 30 Aug 2025 16:03:25 +0800 Subject: [PATCH 1/2] src: robotics: ...astar_variant_base.cpp: fix code smell --- .../astar_variant/src/astar_variant_base.cpp | 163 +++++++++--------- 1 file changed, 86 insertions(+), 77 deletions(-) diff --git a/src/robotics/astar_variant/src/astar_variant_base.cpp b/src/robotics/astar_variant/src/astar_variant_base.cpp index 6d9a303..b74dba0 100644 --- a/src/robotics/astar_variant/src/astar_variant_base.cpp +++ b/src/robotics/astar_variant/src/astar_variant_base.cpp @@ -36,96 +36,105 @@ bool AstarVariantBase::SetOccupiedGrid( bool AstarVariantBase::SetStartAndDestination(const Coordinate &start, const Coordinate &dest) { - if (map_storage_) { - if (map_storage_->Contains(start) && map_storage_->Contains(dest)) { - // new start / destination - if (reset_called_ || !(start_ == start && dest_ == dest)) { - start_ = start; - dest_ = dest; - - if (reset_called_) { - reset_called_ = false; - } else { - // clear possible historical search - ResetFunc(true); - } - - auto &map = map_storage_->GetMap(); - traverse_path_.insert( - std::make_pair(0, std::make_pair(start.x, start.y))); - map[start.x][start.y].parent_coordinate = start; - map[start.x][start.y].f = 0; - map[start.x][start.y].g = 0; - map[start.x][start.y].h = 0; - } - start_and_end_set_ = true; - return true; + if (!map_storage_) { + return false; + } + + if (map_storage_->Contains(start) && map_storage_->Contains(dest)) { + bool require_update = false; + + if (reset_called_) { + reset_called_ = false; + require_update = true; + } else if (!(start_ == start && dest_ == dest)) // new start / destination + { + // clear possible historical search + ResetFunc(true); + require_update = true; } + + if (require_update) { + start_ = start; + dest_ = dest; + + auto &map = map_storage_->GetMap(); + traverse_path_.insert( + std::make_pair(0, std::make_pair(start.x, start.y))); + map[start.x][start.y].parent_coordinate = start; + map[start.x][start.y].f = 0; + map[start.x][start.y].g = 0; + map[start.x][start.y].h = 0; + } + start_and_end_set_ = true; + return true; } + return false; } std::optional> AstarVariantBase::StepOverPathFinding() { - if (found_path_) return std::nullopt; - if (start_and_end_set_) { - if (!traverse_path_.empty()) { - const auto travelled_path = traverse_path_.begin(); - traverse_path_.erase(traverse_path_.begin()); + if (found_path_) { + return std::nullopt; + } - const auto [i, j] = travelled_path->second; + if ((!start_and_end_set_) || traverse_path_.empty()) { + return std::nullopt; + } - // mark node as visited - visited_map_[i][j] = true; + const auto travelled_path = traverse_path_.begin(); + traverse_path_.erase(traverse_path_.begin()); - auto &map = map_storage_->GetMap(); + const auto [i, j] = travelled_path->second; + + // mark node as visited + visited_map_[i][j] = true; + + auto &map = map_storage_->GetMap(); + + std::vector expanded_nodes; + + for (size_t k = 0; k < motion_constraint_.size(); ++k) { + Coordinate coordinate; + coordinate.x = i + motion_constraint_.dx[k]; + coordinate.y = j + motion_constraint_.dy[k]; + if (!map_storage_->Contains(coordinate)) continue; + + if (coordinate == dest_) { + map[coordinate.x][coordinate.y].parent_coordinate = {i, j}; + found_path_ = true; + return std::nullopt; + } + + // not occupied + if (!map[coordinate.x][coordinate.y].occupied && + !visited_map_[coordinate.x][coordinate.y]) { + const auto dist_travelled = + sqrt(abs(motion_constraint_.dx[k]) + abs(motion_constraint_.dy[k])); + + const auto astar_variant_spec = GetAstarVariantSpec(); + + const auto new_g = map[i][j].g + dist_travelled; + const auto new_h = astar_variant_spec.heuristic_func( + coordinate.x - dest_.x, coordinate.y - dest_.y); + const auto new_f = + astar_variant_spec.w1 * new_g + astar_variant_spec.w2 * new_h; + + if (map[coordinate.x][coordinate.y].f == + std::numeric_limits::max() || + map[coordinate.x][coordinate.y].f >= new_f) { + map[coordinate.x][coordinate.y].f = new_f; + map[coordinate.x][coordinate.y].g = new_g; + map[coordinate.x][coordinate.y].h = new_h; + map[coordinate.x][coordinate.y].parent_coordinate = {i, j}; + + traverse_path_.insert( + std::make_pair(new_f, std::make_pair(coordinate.x, coordinate.y))); - std::vector expanded_nodes; - - for (size_t k = 0; k < motion_constraint_.size(); ++k) { - Coordinate coordinate; - coordinate.x = i + motion_constraint_.dx[k]; - coordinate.y = j + motion_constraint_.dy[k]; - if (!map_storage_->Contains(coordinate)) continue; - - if (coordinate == dest_) { - map[coordinate.x][coordinate.y].parent_coordinate = {i, j}; - found_path_ = true; - return std::nullopt; - } - - // not occupied - if (!map[coordinate.x][coordinate.y].occupied && - !visited_map_[coordinate.x][coordinate.y]) { - const auto dist_travelled = sqrt(abs(motion_constraint_.dx[k]) + - abs(motion_constraint_.dy[k])); - - const auto astar_variant_spec = GetAstarVariantSpec(); - - const auto new_g = map[i][j].g + dist_travelled; - const auto new_h = astar_variant_spec.heuristic_func( - coordinate.x - dest_.x, coordinate.y - dest_.y); - const auto new_f = - astar_variant_spec.w1 * new_g + astar_variant_spec.w2 * new_h; - - if (map[coordinate.x][coordinate.y].f == - std::numeric_limits::max() || - map[coordinate.x][coordinate.y].f >= new_f) { - map[coordinate.x][coordinate.y].f = new_f; - map[coordinate.x][coordinate.y].g = new_g; - map[coordinate.x][coordinate.y].h = new_h; - map[coordinate.x][coordinate.y].parent_coordinate = {i, j}; - - traverse_path_.insert(std::make_pair( - new_f, std::make_pair(coordinate.x, coordinate.y))); - - expanded_nodes.push_back(coordinate); - } - } + expanded_nodes.push_back(coordinate); } - return expanded_nodes; } } - return std::nullopt; + return expanded_nodes; } bool AstarVariantBase::FindPath() { if (!start_and_end_set_) return false; From e96744b7564c0185b382d6c2ea504a3914eb4be2 Mon Sep 17 00:00:00 2001 From: Pin Loon Lee Date: Sat, 30 Aug 2025 16:04:07 +0800 Subject: [PATCH 2/2] src: robotics: astart, bfs, dijikstra: fixed missing test coverage --- src/robotics/astar/test/astar_test.cpp | 5 ++++ .../astar/test/weighted_astar_test.cpp | 21 ++++++++++----- .../test/best_first_search_test.cpp | 26 ++++++++++++------- .../dijikstra/test/dijikstra_test.cpp | 5 ++++ 4 files changed, 40 insertions(+), 17 deletions(-) diff --git a/src/robotics/astar/test/astar_test.cpp b/src/robotics/astar/test/astar_test.cpp index d896ce5..c5f3270 100644 --- a/src/robotics/astar/test/astar_test.cpp +++ b/src/robotics/astar/test/astar_test.cpp @@ -26,6 +26,11 @@ TEST(Astar, ValidSetOccupanciedGrid) { EXPECT_TRUE(astar.SetOccupiedGrid({{1, 6}})); } +TEST(Astar, InvalidSetStartAndDestination) { + Astar astar{MotionConstraintType::CARDINAL_MOTION}; + EXPECT_FALSE(astar.SetStartAndDestination({0, 0}, {4, 4})); +} + TEST(Astar, FailedToFindPath) { Astar astar{MotionConstraintType::CARDINAL_MOTION}; diff --git a/src/robotics/astar/test/weighted_astar_test.cpp b/src/robotics/astar/test/weighted_astar_test.cpp index ef88c38..5eb70f5 100644 --- a/src/robotics/astar/test/weighted_astar_test.cpp +++ b/src/robotics/astar/test/weighted_astar_test.cpp @@ -1,11 +1,11 @@ -/* +/* * weighted_astar_test.cpp - * + * * Created on: Jan 02, 2023 11:49 - * Description: - * + * Description: + * * Copyright (c) 2023 Pin Loon Lee (pllee4) - */ + */ #include "algorithm/robotics/astar/weighted_astar.hpp" @@ -28,6 +28,11 @@ TEST(WeightedAstar, ValidSetOccupanciedGrid) { EXPECT_TRUE(weighted_astar.SetOccupiedGrid({{1, 6}})); } +TEST(WeightedAstar, InvalidSetStartAndDestination) { + WeightedAstar weighted_astar{MotionConstraintType::CARDINAL_MOTION}; + EXPECT_FALSE(weighted_astar.SetStartAndDestination({0, 0}, {4, 4})); +} + TEST(WeightedAstar, FailedToFindPath) { WeightedAstar weighted_astar{MotionConstraintType::CARDINAL_MOTION}; @@ -156,7 +161,8 @@ TEST(WeightedAstar, GetPathAfterReset) { } TEST(WeightedAstar, GetPath8Dir) { - WeightedAstar weighted_astar{MotionConstraintType::CARDINAL_ORDINAL_MOTION}; + WeightedAstar weighted_astar{ + MotionConstraintType::CARDINAL_ORDINAL_MOTION}; /** * s = start, e = end, x = occupied @@ -201,7 +207,8 @@ TEST(WeightedAstar, GetPathWithRevisit) { } TEST(WeightedAstar, GetPath8DirWithRevisit) { - WeightedAstar weighted_astar{MotionConstraintType::CARDINAL_ORDINAL_MOTION}; + WeightedAstar weighted_astar{ + MotionConstraintType::CARDINAL_ORDINAL_MOTION}; /** * s = start, e = end, x = occupied diff --git a/src/robotics/best_first_search/test/best_first_search_test.cpp b/src/robotics/best_first_search/test/best_first_search_test.cpp index 7034682..dbe5431 100644 --- a/src/robotics/best_first_search/test/best_first_search_test.cpp +++ b/src/robotics/best_first_search/test/best_first_search_test.cpp @@ -1,11 +1,11 @@ -/* +/* * best_first_search_test.cpp - * + * * Created on: Jan 04, 2023 21:26 - * Description: - * + * Description: + * * Copyright (c) 2023 Pin Loon Lee (pllee4) - */ + */ #include "algorithm/robotics/best_first_search/best_first_search.hpp" @@ -13,8 +13,6 @@ using namespace pllee4::graph; -static constexpr uint8_t weight = 10; - TEST(BestFirstSearch, InvalidSetOccupiedGrid) { BestFirstSearch best_first_search{MotionConstraintType::CARDINAL_MOTION}; EXPECT_FALSE(best_first_search.SetOccupiedGrid({{1, 6}})); @@ -28,6 +26,11 @@ TEST(BestFirstSearch, ValidSetOccupanciedGrid) { EXPECT_TRUE(best_first_search.SetOccupiedGrid({{1, 6}})); } +TEST(BestFirstSearch, InvalidSetStartAndDestination) { + BestFirstSearch best_first_search{MotionConstraintType::CARDINAL_MOTION}; + EXPECT_FALSE(best_first_search.SetStartAndDestination({0, 0}, {4, 4})); +} + TEST(BestFirstSearch, FailedToFindPath) { BestFirstSearch best_first_search{MotionConstraintType::CARDINAL_MOTION}; @@ -156,7 +159,8 @@ TEST(BestFirstSearch, GetPathAfterReset) { } TEST(BestFirstSearch, GetPath8Dir) { - BestFirstSearch best_first_search{MotionConstraintType::CARDINAL_ORDINAL_MOTION}; + BestFirstSearch best_first_search{ + MotionConstraintType::CARDINAL_ORDINAL_MOTION}; /** * s = start, e = end, x = occupied @@ -172,7 +176,8 @@ TEST(BestFirstSearch, GetPath8Dir) { EXPECT_TRUE(best_first_search.GetPath().has_value()); const auto path = best_first_search.GetPath().value(); - std::vector expected = {{0, 2}, {0, 1}, {0, 0}, {1, 0}, {2, 1}, {2, 2}}; + std::vector expected = {{0, 2}, {0, 1}, {0, 0}, + {1, 0}, {2, 1}, {2, 2}}; EXPECT_TRUE(std::equal(std::begin(path), std::end(path), std::begin(expected), std::end(expected))); } @@ -201,7 +206,8 @@ TEST(BestFirstSearch, GetPathWithRevisit) { } TEST(BestFirstSearch, GetPath8DirWithRevisit) { - BestFirstSearch best_first_search{MotionConstraintType::CARDINAL_ORDINAL_MOTION}; + BestFirstSearch best_first_search{ + MotionConstraintType::CARDINAL_ORDINAL_MOTION}; /** * s = start, e = end, x = occupied diff --git a/src/robotics/dijikstra/test/dijikstra_test.cpp b/src/robotics/dijikstra/test/dijikstra_test.cpp index 662cd30..39f478b 100644 --- a/src/robotics/dijikstra/test/dijikstra_test.cpp +++ b/src/robotics/dijikstra/test/dijikstra_test.cpp @@ -26,6 +26,11 @@ TEST(Dijikstra, ValidSetOccupanciedGrid) { EXPECT_TRUE(dijikstra.SetOccupiedGrid({{1, 6}})); } +TEST(Dijikstra, InvalidSetStartAndDestination) { + Dijikstra dijikstra{MotionConstraintType::CARDINAL_MOTION}; + EXPECT_FALSE(dijikstra.SetStartAndDestination({0, 0}, {4, 4})); +} + TEST(Dijikstra, FailedToFindPath) { Dijikstra dijikstra{MotionConstraintType::CARDINAL_MOTION};