From c4953a57c05673996212a125b7259a1597776df2 Mon Sep 17 00:00:00 2001 From: Pepijn Date: Fri, 9 Jan 2026 11:08:02 +0100 Subject: [PATCH] debug --- examples/openarms/evaluate_interpolation.py | 31 +++++++++++++------ .../robots/openarms/openarms_follower.py | 7 ++++- 2 files changed, 27 insertions(+), 11 deletions(-) diff --git a/examples/openarms/evaluate_interpolation.py b/examples/openarms/evaluate_interpolation.py index 77bb7c4fa..06484bf1f 100644 --- a/examples/openarms/evaluate_interpolation.py +++ b/examples/openarms/evaluate_interpolation.py @@ -270,16 +270,27 @@ def get_actions_thread( # Filter out non-feature keys (like _timing_breakdown) obs_for_frame = {k: v for k, v in obs_processed.items() if not k.startswith("_")} - # Debug: log keys on first iteration - if action_queue.qsize() == 0: - logger.info(f"[GET_ACTIONS] obs_for_frame keys: {list(obs_for_frame.keys())}") - logger.info(f"[GET_ACTIONS] hw_features keys: {list(hw_features.keys())}") - # Check expected vs actual image keys - expected_img_keys = [k.removeprefix("observation.images.") - for k in hw_features if "images" in k] - logger.info(f"[GET_ACTIONS] Expected image keys: {expected_img_keys}") - for k in expected_img_keys: - logger.info(f"[GET_ACTIONS] '{k}' in obs_for_frame: {k in obs_for_frame}") + # Check for missing camera keys and wait for them if needed + expected_img_keys = [k.removeprefix("observation.images.") + for k in hw_features if "images" in k] + missing_keys = [k for k in expected_img_keys if k not in obs_for_frame] + + if missing_keys: + logger.warning(f"[GET_ACTIONS] Missing camera keys: {missing_keys}, retrying...") + # Retry observation to get camera frames + for _ in range(5): + time.sleep(0.05) + obs = robot.get_observation() + obs_processed = robot_observation_processor(obs) + obs_for_frame = {k: v for k, v in obs_processed.items() if not k.startswith("_")} + missing_keys = [k for k in expected_img_keys if k not in obs_for_frame] + if not missing_keys: + break + + if missing_keys: + logger.error(f"[GET_ACTIONS] Still missing keys after retries: {missing_keys}") + logger.error(f"[GET_ACTIONS] Available keys: {list(obs_for_frame.keys())}") + continue # Skip this inference cycle obs_with_policy_features = build_dataset_frame( hw_features, obs_for_frame, prefix="observation" diff --git a/src/lerobot/robots/openarms/openarms_follower.py b/src/lerobot/robots/openarms/openarms_follower.py index e8c3fa08b..cdbe60012 100644 --- a/src/lerobot/robots/openarms/openarms_follower.py +++ b/src/lerobot/robots/openarms/openarms_follower.py @@ -344,10 +344,15 @@ class OpenArmsFollower(Robot): obs_dict[cam_key] = frame except TimeoutError: # If no new frame available, reuse last valid frame from cache - # This prevents blocking the entire control loop on slow USB reads if self.camera_frame_cache[cam_key] is not None: obs_dict[cam_key] = self.camera_frame_cache[cam_key] logger.debug(f"Camera {cam_key} timeout, reusing cached frame") + else: + # First frame not available yet - use blocking read to ensure we get a frame + logger.warning(f"Camera {cam_key} no cached frame, doing blocking read") + frame = cam.read() + self.camera_frame_cache[cam_key] = frame + obs_dict[cam_key] = frame # Store timing with padded name to align output (e.g. "left_wrist ") timings[f"{cam_key:14s}"] = (time.perf_counter() - t0) * 1000