125 lines
6.3 KiB
Python
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()
|