Update navigation with visited path and unreachable target flagged

app.py

@@ -170,62 +170,59 @@ def highlight_water_mask_on_frame(frame, binary_mask, color=(255, 0, 0), alpha=0
 
 # ── A* and KNN over binary water grid ─────────────────────────────────
 def astar(start, goal, occ):
-    h   = lambda a,b: abs(a[0]-b[0])+abs(a[1]-b[1])
-    N8  = [(-1,-1),(-1,0),(-1,1),(0,-1),(0,1),(1,-1),(1,0),(1,1)]
-    openq=[(0,start)]; g={start:0}; came={}
+    h = lambda a, b: abs(a[0] - b[0]) + abs(a[1] - b[1])
+    N8 = [(-1,-1), (-1,0), (-1,1), (0,-1), (0,1), (1,-1), (1,0), (1,1)]
+    openq = [(0, start)]
+    g = {start: 0}
+    came = {}
+    visited = set()
+    H, W = occ.shape
     while openq:
-        _,cur=heapq.heappop(openq)
-        if cur==goal:
-            p=[cur];                   # reconstruct
-            while cur in came: cur=came[cur]; p.append(cur)
+        _, cur = heapq.heappop(openq)
+        if cur == goal:
+            p = [cur]
+            while cur in came:
+                cur = came[cur]
+                p.append(cur)
             return p[::-1]
-        for dx,dy in N8:
-            nx,ny=cur[0]+dx,cur[1]+dy
-            # out-of-bounds / blocked
-            if not (0<=nx<640 and 0<=ny<640) or occ[ny,nx]==0: continue
-            # if diagonal, ensure both orthogonals are free
-            if abs(dx)==1 and abs(dy)==1:
-                if occ[cur[1]+dy, cur[0]]==0 or occ[cur[1], cur[0]+dx]==0:
+        if cur in visited:
+            continue
+        visited.add(cur)
+        for dx, dy in N8:
+            nx, ny = cur[0] + dx, cur[1] + dy
+            if not (0 <= nx < W and 0 <= ny < H): continue
+            if occ[ny, nx] == 0: continue
+            # prevent corner-cutting on diagonals
+            if abs(dx) == 1 and abs(dy) == 1:
+                if occ[cur[1] + dy, cur[0]] == 0 or occ[cur[1], cur[0] + dx] == 0:
                     continue
-            ng=g[cur]+1
-            if (nx,ny) not in g or ng<g[(nx,ny)]:
-                g[(nx,ny)]=ng
-                f=ng+h((nx,ny),goal)
-                heapq.heappush(openq,(f,(nx,ny)))
-                came[(nx,ny)]=cur
+            next_node = (nx, ny)
+            ng = g[cur] + 1
+            if next_node not in g or ng < g[next_node]:
+                g[next_node] = ng
+                f = ng + h(next_node, goal)
+                heapq.heappush(openq, (f, next_node))
+                came[next_node] = cur
+    print(f"[A*] No path from {start} to {goal}")
     return []
+
 # KNN fit optimal path
 def knn_path(start, targets, occ):
     todo = targets[:]; path=[]
     cur  = tuple(start)
-    while todo:
-        # KNN follow a Greedy approach, which may not guarantee shortest path, hence only use A*
-        # nbrs = NearestNeighbors(n_neighbors=1).fit(todo)
-        # _,idx = nbrs.kneighbors([cur]); nxt=tuple(todo[idx[0][0]])
-        # seg  = astar(cur, nxt, occ)
-        # if seg:
-        #     if path and seg[0]==path[-1]: seg=seg[1:]
-        #     path.extend(seg)
-        # cur  = nxt; todo.remove(list(nxt))
-        best = None
-        best_len = float('inf')
-        best_seg = []
-        # Try A* to each target, find shortest actual path
-        for t in todo:
-            seg = astar(cur, tuple(t), occ)
-            if seg and len(seg) < best_len:
-                best = tuple(t)
-                best_len = len(seg)
-                best_seg = seg
-        if not best:
-            print("⚠️ Some garbage unreachable")
-            break  # stop if no reachable targets left
-        if path and path[-1] == best_seg[0]:
-            best_seg = best_seg[1:]  # avoid duplicate point
-        path.extend(best_seg)
-        cur = best
-        todo.remove(list(best))
-    return path
+    reachable = []; unreachable = []
+    for t in todo:
+        seg = astar(cur, tuple(t), occ)
+        if seg:
+            if path and path[-1] == seg[0]:
+                seg = seg[1:]
+            path.extend(seg)
+            reachable.append(t)
+            cur = tuple(t)
+        else:
+            unreachable.append(t)
+            print(f"⚠️ Unreachable garbage at {t}")
+    return path, reachable
 
 
 # ── Robot sprite/class -──────────────────────────────────────────────────
@@ -487,7 +484,10 @@ def _pipeline(uid,img_path):
     robot = Robot(SPRITE)
     # Robot will be spawn on the closest movable mask to top-left
     robot.pos = [spawn_x, spawn_y]
-    path  = knn_path(robot.pos, centres, movable_mask)
+    path, unreachable_targets = knn_path(robot.pos, centres, movable_mask)
+    objs = [{"pos": p, "col": False, "unreachable": False} for p in centres if p not in unreachable_targets]
+    objs += [{"pos": p, "col": False, "unreachable": True} for p in unreachable_targets]
+
 
     # 4- Video synthesis
     out_tmp=f"{OUTPUT_DIR}/{uid}_tmp.mp4"