try to fix code smell

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

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<weight> weighted_astar{MotionConstraintType::CARDINAL_MOTION};
+  EXPECT_FALSE(weighted_astar.SetStartAndDestination({0, 0}, {4, 4}));
+}
+
 TEST(WeightedAstar, FailedToFindPath) {
   WeightedAstar<weight> weighted_astar{MotionConstraintType::CARDINAL_MOTION};
 
@@ -156,7 +161,8 @@ TEST(WeightedAstar, GetPathAfterReset) {
 }
 
 TEST(WeightedAstar, GetPath8Dir) {
-  WeightedAstar<weight> weighted_astar{MotionConstraintType::CARDINAL_ORDINAL_MOTION};
+  WeightedAstar<weight> weighted_astar{
+      MotionConstraintType::CARDINAL_ORDINAL_MOTION};
 
   /**
    * s = start, e = end, x = occupied
@@ -201,7 +207,8 @@ TEST(WeightedAstar, GetPathWithRevisit) {
 }
 
 TEST(WeightedAstar, GetPath8DirWithRevisit) {
-  WeightedAstar<weight> weighted_astar{MotionConstraintType::CARDINAL_ORDINAL_MOTION};
+  WeightedAstar<weight> weighted_astar{
+      MotionConstraintType::CARDINAL_ORDINAL_MOTION};
 
   /**
    * s = start, e = end, x = occupied

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<std::vector<Coordinate>> 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<Coordinate> 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<MapStorage::CostDataType>::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<Coordinate> 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<MapStorage::CostDataType>::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;

src/robotics/best_first_search/test/best_first_search_test.cpp

@@ -1,20 +1,18 @@
-/* 
+/*
  * 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"
 
 #include "gtest/gtest.h"
 
 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<Coordinate> expected = {{0, 2}, {0, 1}, {0, 0}, {1, 0}, {2, 1}, {2, 2}};
+  std::vector<Coordinate> 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

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