initial

tools/aubo_joint_position.py

@@ -0,0 +1,106 @@
+from pyaubo_sdk import RpcClient, RtdeClient
+import time
+import threading
+
+ROBOT_IP = "192.168.192.100"
+RPC_PORT = 30004
+RTDE_PORT = 30010
+USERNAME = "aubo"
+PASSWORD = "123456"
+PRINT_INTERVAL = 2.0
+
+latest_joint_positions = None
+latest_tcp_pose = None
+lock = threading.Lock()
+
+
+def main():
+    global latest_joint_positions, latest_tcp_pose
+
+    rpc = RpcClient()
+    rtde = RtdeClient()
+    topic_id = None
+
+    try:
+        # 1. RPC 连接
+        rpc.connect(ROBOT_IP, RPC_PORT)
+        rpc.login(USERNAME, PASSWORD)
+
+        robot_names = rpc.getRobotNames()
+        if not robot_names:
+            print("未找到机器人")
+            return
+
+        print(f"已连接机器人: {robot_names[0]}")
+
+        # 2. RTDE 连接 + 登录
+        rtde.connect(ROBOT_IP, RTDE_PORT)
+        rtde.login(USERNAME, PASSWORD)
+
+        # 3. 设置 RTDE 订阅项
+        names = ["R1_actual_q", "R1_actual_TCP_pose"]
+        topic_id = rtde.setTopic(False, names, 50, 0)
+
+        # 4. 回调函数
+        def callback(parser):
+            global latest_joint_positions, latest_tcp_pose
+
+            joint_positions = parser.popVectorDouble()
+            tcp_pose = parser.popVectorDouble()
+
+            with lock:
+                latest_joint_positions = joint_positions
+                latest_tcp_pose = tcp_pose
+
+        # 5. 订阅
+        rtde.subscribe(topic_id, callback)
+
+        print(f"开始监听（每 {PRINT_INTERVAL:.0f}s 输出一次）... Ctrl+C 退出")
+
+        while True:
+            time.sleep(PRINT_INTERVAL)
+
+            with lock:
+                joints = latest_joint_positions[:] if latest_joint_positions is not None else None
+                tcp = latest_tcp_pose[:] if latest_tcp_pose is not None else None
+
+            if joints is not None and tcp is not None:
+                joints_fmt = [round(j, 4) for j in joints]
+                tcp_fmt = [round(t, 4) for t in tcp]
+
+                print("\n" + "=" * 60)
+                print(f"关节角 (rad): {joints_fmt}")
+                print(f"末端位姿 : {tcp_fmt}")
+                print("=" * 60)
+            else:
+                print("尚未收到完整数据...")
+
+    except KeyboardInterrupt:
+        print("\n程序退出")
+    except Exception as e:
+        print(f"\n运行出错: {e}")
+    finally:
+        try:
+            if topic_id is not None:
+                rtde.removeTopic(False, topic_id)
+        except Exception:
+            pass
+
+        try:
+            rtde.disconnect()
+        except Exception:
+            pass
+
+        try:
+            rpc.logout()
+        except Exception:
+            pass
+
+        try:
+            rpc.disconnect()
+        except Exception:
+            pass
+
+
+if __name__ == "__main__":
+    main()

tools/realsense_record_video.py

@@ -0,0 +1,258 @@
+from __future__ import annotations
+
+import argparse
+from dataclasses import dataclass
+from datetime import datetime
+from pathlib import Path
+
+import cv2
+import numpy as np
+import pyrealsense2 as rs
+
+
+SAVE_DIR = Path("d405_recordings")
+WINDOW_NAME = "RealSense D405 Video Recorder"
+PREFERRED_COLOR_FORMATS = (rs.format.bgr8, rs.format.rgb8)
+
+
+@dataclass(frozen=True)
+class ColorStreamConfig:
+    width: int
+    height: int
+    fps: int
+    stream_format: rs.format
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description="Record color video from an Intel RealSense D405 camera.")
+    parser.add_argument("--width", type=int, default=1280, help="Color stream width. Default: 1280")
+    parser.add_argument("--height", type=int, default=720, help="Color stream height. Default: 720")
+    parser.add_argument("--fps", type=int, default=30, help="Color stream FPS. Default: 30")
+    parser.add_argument(
+        "--list-profiles",
+        action="store_true",
+        help="List supported color video profiles for the connected camera and exit.",
+    )
+    return parser.parse_args()
+
+
+def get_color_stream_candidates() -> list[ColorStreamConfig]:
+    context = rs.context()
+    devices = context.query_devices()
+    if len(devices) == 0:
+        raise RuntimeError("No RealSense device detected.")
+
+    device = devices[0]
+    stream_candidates: list[ColorStreamConfig] = []
+
+    for sensor in device.sensors:
+        for profile in sensor.get_stream_profiles():
+            if profile.stream_type() != rs.stream.color:
+                continue
+            if profile.format() not in PREFERRED_COLOR_FORMATS:
+                continue
+
+            try:
+                video_profile = profile.as_video_stream_profile()
+            except RuntimeError:
+                continue
+
+            stream_candidates.append(
+                ColorStreamConfig(
+                    width=video_profile.width(),
+                    height=video_profile.height(),
+                    fps=profile.fps(),
+                    stream_format=profile.format(),
+                )
+            )
+
+    if not stream_candidates:
+        raise RuntimeError("No usable color stream profile found for the RealSense device.")
+
+    return stream_candidates
+
+
+def list_profiles() -> None:
+    try:
+        stream_candidates = get_color_stream_candidates()
+    except RuntimeError as exc:
+        print(exc)
+        return
+
+    print("Supported color stream profiles:")
+    for candidate in sorted(
+        set(stream_candidates),
+        key=lambda item: (item.width, item.height, item.fps, str(item.stream_format)),
+    ):
+        print(f"  {candidate.width}x{candidate.height}@{candidate.fps} {candidate.stream_format}")
+
+
+def select_color_stream(width: int, height: int, fps: int) -> ColorStreamConfig:
+    stream_candidates = get_color_stream_candidates()
+    matching_candidates = [
+        candidate
+        for candidate in stream_candidates
+        if candidate.width == width and candidate.height == height and candidate.fps == fps
+    ]
+
+    if not matching_candidates:
+        raise RuntimeError(
+            f"No supported color stream profile matches {width}x{height}@{fps}. "
+            "Run with --list-profiles to see available profiles."
+        )
+
+    def sort_key(candidate: ColorStreamConfig) -> int:
+        return 1 if candidate.stream_format == rs.format.bgr8 else 0
+
+    return max(matching_candidates, key=sort_key)
+
+
+def init_camera(stream_config: ColorStreamConfig) -> rs.pipeline:
+    pipeline = rs.pipeline()
+    config = rs.config()
+    config.enable_stream(
+        rs.stream.color,
+        stream_config.width,
+        stream_config.height,
+        stream_config.stream_format,
+        stream_config.fps,
+    )
+    pipeline.start(config)
+    return pipeline
+
+
+def build_video_writer(output_path: Path, stream_config: ColorStreamConfig) -> cv2.VideoWriter:
+    fourcc = cv2.VideoWriter_fourcc(*"mp4v")
+    writer = cv2.VideoWriter(
+        str(output_path),
+        fourcc,
+        stream_config.fps,
+        (stream_config.width, stream_config.height),
+    )
+    if not writer.isOpened():
+        raise RuntimeError(f"Failed to create video writer: {output_path}")
+    return writer
+
+
+def draw_status(frame: np.ndarray, is_recording: bool, output_path: Path | None) -> np.ndarray:
+    preview = frame.copy()
+    frame_height = preview.shape[0]
+    if is_recording:
+        cv2.circle(preview, (25, 30), 8, (0, 0, 255), -1)
+        cv2.putText(
+            preview,
+            "REC",
+            (40, 36),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.8,
+            (0, 0, 255),
+            2,
+        )
+    else:
+        cv2.putText(
+            preview,
+            "Press 's' to start recording",
+            (20, 36),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.7,
+            (0, 255, 255),
+            2,
+        )
+
+    cv2.putText(
+        preview,
+        "Press 'q' to stop and quit",
+        (20, 70),
+        cv2.FONT_HERSHEY_SIMPLEX,
+        0.7,
+        (255, 255, 255),
+        2,
+    )
+
+    if output_path is not None:
+        cv2.putText(
+            preview,
+            f"Saving: {output_path.name}",
+            (20, frame_height - 20),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.6,
+            (255, 255, 255),
+            2,
+        )
+
+    return preview
+
+
+def frame_to_bgr(color_frame: rs.video_frame, stream_config: ColorStreamConfig) -> np.ndarray:
+    color_image = np.asanyarray(color_frame.get_data())
+    if stream_config.stream_format == rs.format.rgb8:
+        return cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR)
+    return color_image
+
+
+def main() -> None:
+    args = parse_args()
+    if args.list_profiles:
+        list_profiles()
+        return
+
+    SAVE_DIR.mkdir(parents=True, exist_ok=True)
+    stream_config = select_color_stream(args.width, args.height, args.fps)
+    pipeline = init_camera(stream_config)
+    writer: cv2.VideoWriter | None = None
+    output_path: Path | None = None
+    is_recording = False
+
+    print("-" * 40)
+    print("RealSense D405 video recorder")
+    print(
+        "Selected color stream: "
+        f"{stream_config.width}x{stream_config.height} @ {stream_config.fps} FPS "
+        f"({stream_config.stream_format})"
+    )
+    print("Press 's' to start recording")
+    print("Press 'q' to stop recording and quit")
+    print(f"Video files will be saved to: {SAVE_DIR.resolve()}")
+    print("-" * 40)
+
+    try:
+        while True:
+            frames = pipeline.wait_for_frames(timeout_ms=2000)
+            color_frame = frames.get_color_frame()
+            if not color_frame:
+                continue
+
+            color_image = frame_to_bgr(color_frame, stream_config)
+
+            if is_recording and writer is not None:
+                writer.write(color_image)
+
+            preview = draw_status(color_image, is_recording, output_path)
+            cv2.imshow(WINDOW_NAME, preview)
+
+            key = cv2.waitKey(1) & 0xFF
+
+            if key == ord("s") and not is_recording:
+                timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+                output_path = SAVE_DIR / f"d405_record_{timestamp}.mp4"
+                writer = build_video_writer(output_path, stream_config)
+                is_recording = True
+                print(f"Recording started: {output_path}")
+
+            if key == ord("q"):
+                if is_recording:
+                    print("Recording stopped.")
+                else:
+                    print("Exit without recording.")
+                break
+    finally:
+        if writer is not None:
+            writer.release()
+        pipeline.stop()
+        cv2.destroyAllWindows()
+        if output_path is not None and output_path.exists():
+            print(f"Saved video: {output_path.resolve()}")
+
+
+if __name__ == "__main__":
+    main()

tools/video_to_rgb_frames.py

@@ -0,0 +1,105 @@
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+import cv2
+
+
+DEFAULT_VIDEO_DIR = Path("d405_recordings")
+DEFAULT_OUTPUT_DIR = Path("d405_rgb_frames")
+DEFAULT_TARGET_FPS = 8.0
+
+
+def find_latest_video(video_dir: Path) -> Path:
+    candidates = sorted(video_dir.glob("*.mp4"), key=lambda path: path.stat().st_mtime, reverse=True)
+    if not candidates:
+        raise FileNotFoundError(f"No .mp4 videos found in {video_dir.resolve()}")
+    return candidates[0]
+
+
+def extract_frames(video_path: Path, output_dir: Path, target_fps: float) -> None:
+    cap = cv2.VideoCapture(str(video_path))
+    if not cap.isOpened():
+        raise RuntimeError(f"Failed to open video: {video_path}")
+
+    source_fps = cap.get(cv2.CAP_PROP_FPS)
+    if source_fps <= 0:
+        source_fps = 30.0
+
+    frame_interval = max(source_fps / target_fps, 1.0)
+    next_frame_to_save = 0.0
+    frame_index = 0
+    saved_count = 0
+
+    output_dir.mkdir(parents=True, exist_ok=True)
+
+    print("-" * 40)
+    print(f"Input video : {video_path.resolve()}")
+    print(f"Output dir  : {output_dir.resolve()}")
+    print(f"Source FPS  : {source_fps:.2f}")
+    print(f"Target FPS  : {target_fps:.2f}")
+    print("-" * 40)
+
+    try:
+        while True:
+            success, frame = cap.read()
+            if not success:
+                break
+
+            if frame_index + 1e-6 >= next_frame_to_save:
+                saved_count += 1
+                image_path = output_dir / f"{saved_count:06d}.jpg"
+                cv2.imwrite(str(image_path), frame)
+                next_frame_to_save += frame_interval
+
+            frame_index += 1
+    finally:
+        cap.release()
+
+    print(f"Saved {saved_count} RGB frames.")
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(
+        description="Extract RGB frames from a recorded D405 video at 8 FPS.",
+    )
+    parser.add_argument(
+        "--video",
+        type=Path,
+        default=None,
+        help="Path to the input video. If omitted, the latest video in d405_recordings is used.",
+    )
+    parser.add_argument(
+        "--output-dir",
+        type=Path,
+        default=None,
+        help="Directory to save extracted RGB images.",
+    )
+    parser.add_argument(
+        "--target-fps",
+        type=float,
+        default=DEFAULT_TARGET_FPS,
+        help="Frame extraction rate. Default is 8 FPS.",
+    )
+    return parser.parse_args()
+
+
+def main() -> None:
+    args = parse_args()
+    video_path = args.video if args.video is not None else find_latest_video(DEFAULT_VIDEO_DIR)
+    if not video_path.exists():
+        raise FileNotFoundError(f"Video file does not exist: {video_path.resolve()}")
+
+    output_dir = args.output_dir
+    if output_dir is None:
+        output_dir = DEFAULT_OUTPUT_DIR / video_path.stem
+
+    if args.target_fps <= 0:
+        raise ValueError("--target-fps must be greater than 0.")
+
+    extract_frames(video_path, output_dir, args.target_fps)
+
+
+if __name__ == "__main__":
+    main()