Merge pull request #9 from blankey1337/feature/simulated-perception

Feature/simulated perception

Author: blankey1337

software/dashboard/app.py

@@ -39,20 +39,45 @@ def zmq_worker(ip='127.0.0.1', port=5556):
 
 def generate_frames(camera_name):
     while True:
-        frame_data = None
+        frame_bytes = None
+        detections = []
+        
         with lock:
             if camera_name in latest_observation:
                 b64_str = latest_observation[camera_name]
                 if b64_str:
                     try:
-                        # It is already a base64 encoded JPG from the host
-                        frame_data = base64.b64decode(b64_str)
+                        frame_bytes = base64.b64decode(b64_str)
                     except Exception:
                         pass
+            
+            # Get detections
+            raw_dets = latest_observation.get("detections", {})
+            if isinstance(raw_dets, dict):
+                detections = raw_dets.get(camera_name, [])
 
-        if frame_data:
+        if frame_bytes:
+            # Decode to image to draw on it
+            nparr = np.frombuffer(frame_bytes, np.uint8)
+            img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
+            
+            if img is not None:
+                # Draw detections
+                for det in detections:
+                    box = det.get("box", [])
+                    label = det.get("label", "obj")
+                    if len(box) == 4:
+                        x1, y1, x2, y2 = map(int, box)
+                        cv2.rectangle(img, (x1, y1), (x2, y2), (0, 255, 0), 2)
+                        cv2.putText(img, label, (x1, y1 - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2)
+                
+                # Re-encode
+                ret, buffer = cv2.imencode('.jpg', img)
+                if ret:
+                    frame_bytes = buffer.tobytes()
+
             yield (b'--frame\r\n'
-                   b'Content-Type: image/jpeg\r\n\r\n' + frame_data + b'\r\n')
+                   b'Content-Type: image/jpeg\r\n\r\n' + frame_bytes + b'\r\n')
         else:
             # Return a blank or placeholder image if no data
             pass
@@ -71,8 +96,13 @@ def video_feed(camera_name):
 @app.route('/api/status')
 def get_status():
     with lock:
-        # Filter out large image data for status endpoint
-        status = {k: v for k, v in latest_observation.items() if not (isinstance(v, str) and len(v) > 1000)}
+        # Filter out large image data for status endpoint, but keep the key
+        status = {}
+        for k, v in latest_observation.items():
+            if isinstance(v, str) and len(v) > 1000:
+                status[k] = "__IMAGE_DATA__"
+            else:
+                status[k] = v
         status['connected'] = connected
     return jsonify(status)
 

software/dashboard/templates/index.html

@@ -46,23 +46,32 @@ <h3>Robot State</h3>
 
                     // Update Status Table
                     for (const [key, value] of Object.entries(data)) {
-                        // Skip camera data (if any leaks into status) and long strings
-                        if (typeof value === 'string' && value.length > 100) continue;
 
-                        // Check if this key might be a camera
-                        // If it's a camera key and not in activeCameras, add it
-                        if (knownCameras.includes(key) || key.includes('cam') || key.includes('wrist')) {
+                        // Check if this key is a camera placeholder
+                        if (value === "__IMAGE_DATA__") {
                            if (!activeCameras.has(key) && !document.getElementById('cam-' + key)) {
                                addCamera(key);
                                activeCameras.add(key);
                            }
                            continue; // Don't show camera keys in text table
                         }
+                        
+                        // Skip if it looks like a camera key but we didn't get image data marker
+                        // (This avoids treating wrist_roll.pos as a camera)
+                        if (knownCameras.includes(key)) {
+                             // It's a known camera but value isn't image data? 
+                             // Maybe it's empty string or something.
+                             // Let's just continue to show it as text if it's not __IMAGE_DATA__
+                        }
 
                         let displayValue = value;
-                        if (typeof value === 'number') displayValue = value.toFixed(2);
+                        if (typeof value === 'number') {
+                            displayValue = value.toFixed(2);
+                        } else if (typeof value === 'object') {
+                            displayValue = JSON.stringify(value, null, 2);
+                        }
 
-                        rows += `<tr><th>${key}</th><td class="value">${displayValue}</td></tr>`;
+                        rows += `<tr><th>${key}</th><td class="value" style="white-space: pre;">${displayValue}</td></tr>`;
                     }
                     table.innerHTML = rows;
                 })

software/examples/alohamini/chore_sequencer.py

@@ -0,0 +1,115 @@
+import time
+import json
+import threading
+import zmq
+from nav_service import NavigationService
+
+class ChoreExecutor:
+    def __init__(self):
+        self.nav = NavigationService()
+        self.nav.start()
+        
+        self.running = False
+        self.current_chore = None
+        self.chore_thread = None
+        
+        # Robot State
+        self.detections = {}
+        
+        # Subscribe to obs for detections
+        self.nav.sub_socket.setsockopt(zmq.SUBSCRIBE, b"")
+        # We need a way to read detections. NavigationService already reads obs.
+        # Let's piggyback or just read from nav service state?
+        # Nav service only stores pose and rooms.
+        # Let's subclass or monkeypatch NavService to expose detections, 
+        # OR just read from the socket in NavService and store it.
+        # Actually, let's just modify NavService to store full obs or detections.
+        
+    def start_chore(self, chore_name):
+        if self.running:
+            print("[CHORE] Already running a chore.")
+            return
+            
+        self.running = True
+        self.current_chore = chore_name
+        self.chore_thread = threading.Thread(target=self._run_chore, args=(chore_name,))
+        self.chore_thread.start()
+        
+    def stop_chore(self):
+        self.running = False
+        if self.chore_thread:
+            self.chore_thread.join()
+        self.nav.stop()
+        
+    def _run_chore(self, chore_name):
+        print(f"[CHORE] Starting: {chore_name}")
+        
+        if chore_name == "clean_up":
+            self._do_clean_up()
+        else:
+            print(f"[CHORE] Unknown chore: {chore_name}")
+            
+        self.running = False
+        print(f"[CHORE] Finished: {chore_name}")
+
+    def _do_clean_up(self):
+        # 1. Go to Kitchen
+        print("[CHORE] Step 1: Go to Kitchen")
+        self.nav.go_to_room("Kitchen")
+        self._wait_until_idle() 
+        
+        # 2. Look for Trash
+        print("[CHORE] Step 2: Scan for Trash")
+        trash = self._wait_for_detection("trash")
+        
+        if trash:
+            print(f"[CHORE] Found trash at {trash.get('box')}!")
+            # 3. Pick it up (Mock action)
+            print("[CHORE] Step 3: Picking up trash...")
+            time.sleep(2)
+            
+            # 4. Go to Bin (Let's say Bin is in Hallway)
+            print("[CHORE] Step 4: Go to Hallway (Trash Bin)")
+            self.nav.go_to_room("Hallway")
+            self._wait_until_idle()
+            
+            # 5. Drop it
+            print("[CHORE] Step 5: Dropping trash")
+            time.sleep(1)
+        else:
+            print("[CHORE] No trash found.")
+
+    def _wait_for_detection(self, label, timeout=5.0):
+        # Poll self.nav.detections
+        start = time.time()
+        while time.time() - start < timeout:
+            dets = self.nav.detections
+            for cam, items in dets.items():
+                for item in items:
+                    if item.get("label") == label:
+                        return item
+            time.sleep(0.1)
+        return None
+
+    def _wait_until_idle(self):
+        # Wait for nav service to report idle
+        # (It might briefly be idle before starting move, so wait a tiny bit first if needed)
+        time.sleep(0.5) 
+        while not self.nav.is_idle():
+            time.sleep(0.1)
+
+if __name__ == "__main__":
+    executor = ChoreExecutor()
+    
+    # Wait for connection
+    time.sleep(2)
+    
+    print("Available Rooms:", executor.nav.rooms.keys())
+    
+    executor.start_chore("clean_up")
+    
+    # Keep main thread alive
+    while executor.running:
+        time.sleep(1)
+        
+    executor.stop_chore()

software/examples/alohamini/nav_service.py

@@ -0,0 +1,106 @@
+import time
+import zmq
+import json
+import threading
+from navigation import NavigationController
+
+class NavigationService:
+    def __init__(self, zmq_obs_ip="127.0.0.1", zmq_obs_port=5556, zmq_cmd_port=5555):
+        self.controller = NavigationController()
+        
+        self.context = zmq.Context()
+        self.sub_socket = self.context.socket(zmq.SUB)
+        self.sub_socket.setsockopt(zmq.SUBSCRIBE, b"")
+        self.sub_socket.connect(f"tcp://{zmq_obs_ip}:{zmq_obs_port}")
+        self.sub_socket.setsockopt(zmq.CONFLATE, 1)
+        
+        self.pub_socket = self.context.socket(zmq.PUSH)
+        self.pub_socket.connect(f"tcp://{zmq_obs_ip}:{zmq_cmd_port}")
+        
+        self.running = False
+        self.current_pose = None
+        self.rooms = {}
+        self.detections = {}
+        self.nav_status = "idle" # idle, moving
+
+    def is_idle(self):
+        return self.nav_status == "idle"
+        
+    def start(self):
+        self.running = True
+        self.thread = threading.Thread(target=self._loop, daemon=True)
+        self.thread.start()
+        print("NavigationService started.")
+        
+    def stop(self):
+        self.running = False
+        if hasattr(self, 'thread'):
+            self.thread.join()
+            
+    def go_to_room(self, room_name):
+        if room_name not in self.rooms:
+            print(f"[NAV] Unknown room: {room_name}")
+            return False
+            
+        target = self.rooms[room_name]
+        self.controller.set_target(target["x"], target["y"])
+        self.nav_status = "moving"
+        return True
+        
+    def _loop(self):
+        while self.running:
+            try:
+                # 1. Get Observation
+                msg = self.sub_socket.recv_string()
+                obs = json.loads(msg)
+                
+                # Update World Knowledge
+                if "rooms" in obs:
+                    self.rooms = obs["rooms"]
+                
+                if "detections" in obs:
+                    self.detections = obs["detections"]
+                    
+                self.current_pose = {
+                    "x": obs.get("x_pos", 0.0),
+                    "y": obs.get("y_pos", 0.0),
+                    "theta": obs.get("theta_pos", 0.0)
+                }
+                
+                # 2. Compute Control
+                action = self.controller.get_action(self.current_pose)
+                
+                if action:
+                    # Check if action is zero (reached)
+                    if action["x.vel"] == 0 and action["theta.vel"] == 0:
+                        self.nav_status = "idle"
+                    else:
+                        self.nav_status = "moving"
+                        
+                    self.pub_socket.send_string(json.dumps(action))
+                else:
+                    self.nav_status = "idle"
+
+                    
+            except Exception as e:
+                print(f"[NAV] Error: {e}")
+                time.sleep(0.1)
+
+if __name__ == "__main__":
+    # Test Script
+    nav = NavigationService()
+    nav.start()
+    
+    print("Waiting for connection...")
+    time.sleep(2)
+    
+    rooms = ["Kitchen", "Bedroom", "Living Room"]
+    
+    for room in rooms:
+        print(f"Going to {room}...")
+        nav.go_to_room(room)
+        
+        # Wait for arrival (mock)
+        time.sleep(5)
+        
+    nav.stop()