try to fix code smell

Author: pllee4

src/robotics/astar_variant/src/astar_variant_base.cpp

@@ -67,65 +67,64 @@ bool AstarVariantBase::SetStartAndDestination(const Coordinate &start,
 
 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 (start_and_end_set_ && !traverse_path_.empty()) {
+    const auto travelled_path = traverse_path_.begin();
+    traverse_path_.erase(traverse_path_.begin());
 
-      const auto [i, j] = travelled_path->second;
+    const auto [i, j] = travelled_path->second;
 
-      // mark node as visited
-      visited_map_[i][j] = true;
+    // mark node as visited
+    visited_map_[i][j] = true;
 
-      auto &map = map_storage_->GetMap();
+    auto &map = map_storage_->GetMap();
+
+    std::vector<Coordinate> expanded_nodes;
 
-      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;
 
-      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;
+      }
 
-        if (coordinate == dest_) {
+      // 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};
-          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);
-          }
+          traverse_path_.insert(std::make_pair(
+              new_f, std::make_pair(coordinate.x, coordinate.y)));
+
+          expanded_nodes.push_back(coordinate);
         }
       }
-      return expanded_nodes;
     }
+    return expanded_nodes;
   }
-  return std::nullopt;
+}
+return std::nullopt;
 }
 bool AstarVariantBase::FindPath() {
   if (!start_and_end_set_) return false;