initial commit

WATonomous · Jan 29, 2025 · 2710561 · 2710561
1 parent b573a4f
commit 2710561
Show file tree

Hide file tree

Showing 6 changed files with 34 additions and 1 deletion.
diff --git a/.DS_Store b/.DS_Store
diff --git a/.github/.DS_Store b/.github/.DS_Store
diff --git a/docker/.DS_Store b/docker/.DS_Store
diff --git a/docs/.DS_Store b/docs/.DS_Store
diff --git a/src/.DS_Store b/src/.DS_Store
diff --git a/src/world_modeling/occupancy/src/occupancy_core.cpp b/src/world_modeling/occupancy/src/occupancy_core.cpp
@@ -92,7 +92,40 @@ nav_msgs::msg::OccupancyGrid OccupancyCore::remove_z_dimension(
     data[index] = 0x32;
   }
 
-  output_costmap.data = std::move(data);
+  // Fill in surrounding cells
+  std::vector<int8_t> filled_data = data;
+  for (int y = 1; y < height - 1; y++) {
+    for (int x = 1; x < width - 1; x++) {
+      int current_index = y * width + x;
+
+      // Skip if current cell is already occupied
+      if (data[current_index] != -1) {
+        continue;
+      }
+
+      // Check all 8 surrounding cells
+      bool all_occupied = true;
+      for (int dy = -1; dy <= 1; dy++) {
+        for (int dx = -1; dx <= 1; dx++) {
+          if (dx == 0 && dy == 0) continue;
+
+          int neighbor_index = (y + dy) * width + (x + dx);
+          if (data[neighbor_index] == -1) {
+            all_occupied = false;
+            break;
+          }
+        }
+        if (!all_occupied) break;
+      }
+
+      // If all surrounding cells are occupied, fill in this cell
+      if (all_occupied) {
+        filled_data[current_index] = 0x32;
+      }
+    }
+  }
+
+  output_costmap.data = std::move(filled_data);
 
   return output_costmap;
 }