feat: plot path in search

Author: guilyx

src/components/plan/elastic_bands/elastic_bands_path_planner.py

@@ -233,9 +233,10 @@ def __init__(self, start, goal, map_file, *,
         self.y_range = np.arange(y_min, y_max, self.resolution)
         self.rows, self.cols = self.grid.shape
 
-        # SDF: distance (in cells) to nearest obstacle for every free cell
-        obstacle_mask = (self.grid >= 0.99)
-        self.sdf = distance_transform_edt(~obstacle_mask)
+        # SDF: distance (in cells) to nearest impassable cell
+        # (obstacles at >= 0.99 AND clearance zones at ~0.75)
+        impassable_mask = (self.grid > 0)
+        self.sdf = distance_transform_edt(~impassable_mask)
 
         # A* initial path
         self.path = []
@@ -308,7 +309,7 @@ def _astar_search(self):
                 nb = (current[0] + dx, current[1] + dy)
                 if not (0 <= nb[0] < self.cols and 0 <= nb[1] < self.rows):
                     continue
-                if self.grid[nb[1], nb[0]] >= 0.99:
+                if self.grid[nb[1], nb[0]] > 0:
                     continue
                 step = 1.414 if (dx != 0 and dy != 0) else 1.0
                 nc = cost[current] + step
@@ -456,17 +457,27 @@ def update(i, ax):
             ax.plot(self.start[0], self.start[1], 'go', markersize=8, label="Start")
             ax.plot(self.goal[0], self.goal[1], 'ro', markersize=8, label="Goal")
 
-            # Draw bubbles during optimisation phase
+            # Draw bubbles + path trace during optimisation phases
             if phase >= 3:
                 snap = (history[opt_indices[min(local, len(opt_indices) - 1)]]
                         if phase == 3 else self._optimiser.bubbles)
+                # Bubble circles
                 for bub in snap:
                     wx = self.x_range[0] + bub.pos[0] * self.resolution
                     wy = self.y_range[0] + bub.pos[1] * self.resolution
                     r_world = bub.radius * self.resolution
                     circ = Circle((wx, wy), r_world, fill=False,
                                   color='green', alpha=0.25, linewidth=0.5)
                     ax.add_patch(circ)
+                # Path line connecting bubble centers
+                bx = [self.x_range[0] + b.pos[0] * self.resolution
+                      for b in snap]
+                by = [self.y_range[0] + b.pos[1] * self.resolution
+                      for b in snap]
+                ax.plot(bx, by, '-', color='#1565C0', linewidth=2.0,
+                        label="EB Path", zorder=5)
+                ax.plot(bx, by, 'o', color='#1565C0', markersize=4,
+                        zorder=6)
 
             ax.set_title(titles[phase](local), fontsize=14)
             ax.legend(loc='upper left')

src/simulations/path_planning/elastic_bands_path_planning/elastic_bands_navigate.gif

@@ -1 +1 @@
-[[0.0, 0.0], [9.0, -2.5], [20.0, -6.0], [29.0, -9.0], [34.0, -11.0], [37.0, -12.0], [38.0, -12.0], [40.0, -12.0], [41.0, -12.0], [43.5, -11.5], [50.0, -10.0]]
+[[0.0, 0.0], [4.0, -1.5], [13.5, -4.5], [18.5, -6.0], [27.5, -9.5], [32.0, -11.5], [35.0, -13.0], [36.5, -13.5], [38.5, -13.5], [40.5, -14.0], [41.5, -13.5], [44.0, -13.0], [45.0, -12.5], [50.0, -10.0]]