Files
a7l20-hug-sim/scripts/capture_start_evidence.py

125 lines
6.3 KiB
Python

"""Read-only arm subscriptions plus one RGB-D frame. NO command services.
Receive timestamps bound host-side alignment only; they do not certify device
clock synchronization or prove that the controller's state is not cached.
"""
import hashlib
import argparse
import json
from datetime import datetime, timezone
from pathlib import Path
import time
import cv2
import numpy as np
import pyrealsense2 as rs
import roslibpy
ROOT = Path(__file__).resolve().parents[1]
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--calibration', type=Path, default=ROOT / 'data/real_calib/camera.json')
calibration_file = parser.parse_args().calibration.resolve()
calibration = json.loads(calibration_file.read_text())
transform = np.asarray(calibration["T_world_cam_opencv"], dtype=float)
if transform.shape != (4, 4) or not np.isfinite(transform).all() or np.allclose(transform, np.eye(4)):
raise ValueError("A valid saved hand-eye transform is required")
stamp = datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%S%fZ")
out = ROOT / "data" / ("start_evidence_" + stamp)
out.mkdir(exist_ok=False)
ros = roslibpy.Ros(host="192.168.10.21", port=9090)
rows = {"left": [], "right": []}
topics = []
errors = {}
try:
try:
ros.run(timeout=8)
for side in rows:
topic = roslibpy.Topic(ros, f"/{side}/arm_state", "lbot_arm_interfaces/msg/ArmState")
def receive(message, side=side):
rows[side].append({"host_monotonic": time.monotonic(), "state": message})
rows[side][:] = rows[side][-200:]
topic.subscribe(receive)
topics.append(topic)
except Exception as exc:
errors["arm_connection"] = str(exc)
pipe = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.color, 848, 480, rs.format.bgr8, 30)
config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 30)
profile = pipe.start(config)
try:
scale = profile.get_device().first_depth_sensor().get_depth_scale()
align = rs.align(rs.stream.color)
for _ in range(30):
pipe.wait_for_frames(5000)
frames = align.process(pipe.wait_for_frames(5000))
frame_host = time.monotonic()
color, depth = frames.get_color_frame(), frames.get_depth_frame()
if not color or not depth:
raise RuntimeError("Missing aligned RGB/depth")
rgb = np.asanyarray(color.get_data()).copy()
dep = np.clip(np.rint(np.asanyarray(depth.get_data()).astype(float) * scale * 1000), 0, 65535).astype(np.uint16)
intr = color.profile.as_video_stream_profile().get_intrinsics()
camera = {"source": "realsense_d405", "width": rgb.shape[1], "height": rgb.shape[0],
"K": [[intr.fx, 0, intr.ppx], [0, intr.fy, intr.ppy], [0, 0, 1]],
"T_world_cam_opencv": transform.tolist(), "cam_pos_world": transform[:3, 3].tolist(),
"depth_unit": "mm(uint16)", "depth_scale_m": scale,
"calibration_file": str(calibration_file),
"calibration_sha256": hashlib.sha256(calibration_file.read_bytes()).hexdigest(),
"extrinsic_status": "saved_calibration_unchanged",
"capture_received_at_utc": datetime.now(timezone.utc).isoformat(),
"host_monotonic": frame_host,
"color_frame_number": color.get_frame_number(), "depth_frame_number": depth.get_frame_number(),
"device_color_timestamp_ms": color.get_timestamp(), "device_depth_timestamp_ms": depth.get_timestamp(),
"timing_scope": "host receive proximity, NOT synchronized controller/device clocks",
"motion_sent": False}
finally:
pipe.stop()
# Give messages on both sides of the camera receive instant a chance
# to arrive; no calls to FK/IK, enable or motion services are made.
deadline = time.monotonic() + 2.
while time.monotonic() < deadline:
if all(len(values) >= 5 and values[-1]["host_monotonic"] > frame_host + .15 for values in rows.values()):
break
time.sleep(.025)
finally:
for topic in topics:
topic.unsubscribe()
ros.terminate()
snapshot = {"states": {}, "motion_sent": False, "received_at_utc": stamp, "timing": {},
"source": "subscription_only_no_services", "errors": errors}
for side, samples in rows.items():
if not samples:
errors[side] = "No state received; no previous snapshot substituted"
continue
selected = min(samples, key=lambda row: abs(row["host_monotonic"] - frame_host))
state = selected["state"]
joints = np.asarray(state["joint_state"]["position"], float)
if joints.shape != (7,) or not np.isfinite(joints).all():
errors[side] = "Invalid received joint angles"
continue
snapshot["states"][side] = state
close = [row for row in samples if abs(row["host_monotonic"] - frame_host) < .5]
span = np.ptp([row["state"]["joint_state"]["position"] for row in close], axis=0) if close else None
headers = [row["state"]["joint_state"].get("header", {}).get("stamp") for row in close]
snapshot["timing"][side] = {"received_samples": len(samples),
"nearest_receive_delta_s": selected["host_monotonic"]-frame_host,
"samples_within_half_second": len(close),
"joint_span_rad_near_capture": None if span is None else span.tolist(),
"controller_stamp_changed": len({json.dumps(header, sort_keys=True) for header in headers}) > 1}
camera["arm_joints"] = {side: state["joint_state"]["position"] for side, state in snapshot["states"].items()}
cv2.imwrite(str(out / "rgb.png"), rgb)
cv2.imwrite(str(out / "depth.png"), dep)
(out / "intrinsics.txt").write_text(f"{intr.fx:.9f} {intr.fy:.9f} {intr.ppx:.9f} {intr.ppy:.9f}\n")
(out / "camera.json").write_text(json.dumps(camera, indent=2))
(out / "arm_snapshot.json").write_text(json.dumps(snapshot, indent=2))
print(out)
print(json.dumps({"timing": snapshot["timing"], "errors": errors, "motion_sent": False}, indent=2))
if __name__ == "__main__":
main()