System update 13/5. 1. Spawn in valid zone. 2. Keep body offset (20x20). 3. Prevent skip-jump over obstacle. 4. Enhance garbage overlay. 5. Truncate KNN to focus on A*.

app.py

@@ -134,7 +134,7 @@ def build_masks(seg):
     return water_mask, garbage_mask, movable_mask
 
 # Garbage mask can be highlighted in red
-def highlight_chunk_masks_on_frame(frame, labels, objs, color_uncollected=(0, 0, 128), color_collected=(0, 128, 0), alpha=0.3):
+def highlight_chunk_masks_on_frame(frame, labels, objs, color_uncollected=(0, 0, 128), color_collected=(0, 128, 0), alpha=0.8):
     """
     Overlays semi-transparent colored regions for garbage chunks on the frame.
     `objs` must have 'pos' and 'col' keys. The collection status changes the overlay color.
@@ -148,6 +148,7 @@ def highlight_chunk_masks_on_frame(frame, labels, objs, color_uncollected=(0, 0,
         mask = (labels == lab).astype(np.uint8)
         contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
         color = color_collected if obj["col"] else color_uncollected
+        # drawContours on overlay
         cv2.drawContours(overlay, contours, -1, color, thickness=cv2.FILLED)
     # Blend overlay with original frame using alpha
     return cv2.addWeighted(overlay, alpha, frame, 1 - alpha, 0)
@@ -159,6 +160,7 @@ def highlight_water_mask_on_frame(frame, binary_mask, color=(255, 0, 0), alpha=0
     overlay = frame.copy()
     mask = binary_mask.astype(np.uint8) * 255
     contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+    # drawContours on overlay
     cv2.drawContours(overlay, contours, -1, color, thickness=cv2.FILLED)
     return cv2.addWeighted(overlay, alpha, frame, 1 - alpha, 0)
 
@@ -175,8 +177,12 @@ def astar(start, goal, occ):
             return p[::-1]
         for dx,dy in N8:
             nx,ny=cur[0]+dx,cur[1]+dy
-            if not (0<=nx<640 and 0<=ny<640): continue
-            if occ[ny,nx]==0: continue
+            # 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:
+                    continue
             ng=g[cur]+1
             if (nx,ny) not in g or ng<g[(nx,ny)]:
                 g[(nx,ny)]=ng
@@ -184,19 +190,37 @@ def astar(start, goal, occ):
                 heapq.heappush(openq,(f,(nx,ny)))
                 came[(nx,ny)]=cur
     return []
-
-# KNN fit
+# KNN fit optimal path
 def knn_path(start, targets, occ):
     todo = targets[:]; path=[]
     cur  = tuple(start)
     while todo:
-        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))
+        # 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
 
 # ── Robot sprite/class -──────────────────────────────────────────────────
@@ -207,16 +231,23 @@ class Robot:
         if self.png.shape[-1] != 4:
             raise ValueError("Sprite image must have 4 channels (RGBA)")
         self.png = np.array(Image.open(sprite).convert("RGBA").resize((40,40)))
-        self.pos = [20,20]; self.speed=speed
+        self.speed = speed
+        self.pos = [20, 20]  # Fallback spawn with body offset at top-left
     def step(self, path):
         while path:
             dx, dy = path[0][0] - self.pos[0], path[0][1] - self.pos[1]
             dist = (dx * dx + dy * dy) ** 0.5
             if dist <= self.speed:
                 self.pos = list(path.pop(0))
-            else:
+            else: # If valid path within
                 r = self.speed / dist
-                self.pos = [int(self.pos[0] + dx * r), int(self.pos[1] + dy * r)]
+                new_x = self.pos[0] + dx * r
+                new_y = self.pos[1] + dy * r
+                # Clip to valid region with 20px margin (for body offset)
+                self.pos = [
+                    int(np.clip(new_x, 20, 640 - 20)),
+                    int(np.clip(new_y, 20, 640 - 20))
+                ]
             # Break after one logical move to avoid overshooting
             break
 
@@ -461,8 +492,23 @@ def _pipeline(uid,img_path):
     else: # Garbage within valid travelable zone
         print(f"🧠 {len(centres)} garbage objects on water selected from {len(detections)} detections")
 
-    # 3- Global route
+    # 3- Robot initialization, position and navigation
+    # find all (y,x) within movable_mask
+    ys, xs = np.where(movable_mask)
+    if len(ys)==0:
+        # no travelable zone → bail out
+        print(f"❌ [{uid}] no water to spawn on")
+        video_ready[uid] = True
+        return
+    # sort by y, then x
+    idx = np.lexsort((xs, ys))
+    spawn_y, spawn_x = int(ys[idx[0]]), int(xs[idx[0]])
+    # enforce 20px margin so sprite never pokes out
+    spawn_x = np.clip(spawn_x,   20, 640-20)
+    spawn_y = np.clip(spawn_y,   20, 640-20)
     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)
 
     # 4- Video synthesis