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/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; 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/bresenham/src/bresenham.cpp b/src/robotics/bresenham/src/bresenham.cpp index aa69c47..557e83e 100644 --- a/src/robotics/bresenham/src/bresenham.cpp +++ b/src/robotics/bresenham/src/bresenham.cpp @@ -27,17 +27,29 @@ LineGenerator::LineGenerator(const Coordinate& start, const Coordinate& end) { while (true) { points_.emplace_back(Coordinate{x, y}); if (start == end) break; + const auto e2 = 2 * error; + bool should_break = false; + if (e2 >= dy) { - if (x == end.x) break; - error += dy; - x += sx; + if (x == end.x) { + should_break = true; + } else { + error += dy; + x += sx; + } } - if (e2 <= dx) { - if (y == end.y) break; - error += dx; - y += sy; + + if (!should_break && e2 <= dx) { + if (y == end.y) { + should_break = true; + } else { + error += dx; + y += sy; + } } + + if (should_break) break; } } diff --git a/src/robotics/bresenham/test/bresenham_test.cpp b/src/robotics/bresenham/test/bresenham_test.cpp index 992f3e9..fd006e4 100644 --- a/src/robotics/bresenham/test/bresenham_test.cpp +++ b/src/robotics/bresenham/test/bresenham_test.cpp @@ -36,3 +36,13 @@ TEST(Bresenham, CheckLineFromPositiveTowardsRightUpwards) { EXPECT_TRUE(std::equal(std::begin(points), std::end(points), std::begin(expected), std::end(expected))); } + +TEST(Bresenham, CheckVerticalLineReachesEndYFirst) { + Coordinate start{0, 0}; + Coordinate end{1, 3}; + LineGenerator line(start, end); + auto points = line.GetPoints(); + std::vector expected = {{0, 0}, {0, 1}, {1, 2}, {1, 3}}; + EXPECT_TRUE(std::equal(std::begin(points), std::end(points), + std::begin(expected), std::end(expected))); +} 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};