From e2884eecd16abfe06286933a5e8828c71b9f1f4a Mon Sep 17 00:00:00 2001
From: liangyuxuan <16126883+liangyuxuan123@user.noreply.gitee.com>
Date: Mon, 1 Dec 2025 15:39:10 +0800
Subject: [PATCH] add test

---
 tools/get_image.py | 46 ++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 46 insertions(+)
 create mode 100644 tools/get_image.py

diff --git a/tools/get_image.py b/tools/get_image.py
new file mode 100644
index 0000000..a73fc54
--- /dev/null
+++ b/tools/get_image.py
@@ -0,0 +1,46 @@
+import threading
+import json
+
+import cv2
+
+import rclpy
+from rclpy.node import Node
+from cv_bridge import CvBridge
+
+from interfaces.msg import ImgMsg
+
+
+lock = threading.Lock()
+cv_bridge = CvBridge()
+sign = True
+
+
+def main():
+    rclpy.init()
+    node = Node("get_image")
+    sub = node.create_subscription(ImgMsg, "/img_msg", sub_callback, 10)
+    while sign:
+        rclpy.spin_once(node)
+
+
+def sub_callback(msg):
+    global sign
+    camera_data = [
+        msg.image_color,
+        msg.image_depth,
+        msg.karr,
+        msg.darr
+    ]
+
+    cv2.imwrite("test/color_image.png", cv_bridge.imgmsg_to_cv2(camera_data[0]))
+    cv2.imwrite("test/depth_image.png", cv_bridge.imgmsg_to_cv2(camera_data[1]))
+    with open("test/K.json", "w") as f:
+        f.write(json.dumps(camera_data[2]))
+    with open("test/D.json", "w") as f:
+        f.write(json.dumps(camera_data[3]))
+
+    sign = False
+
+
+if __name__ == '__main__':
+    main()