add file test cam intel D435i

2026-07-21 09:54:31 +07:00
parent fcd02da1a4
commit 4bf19c6a7b
6 changed files with 386 additions and 36 deletions
--- a/test/test_cam.py
+++ b/test/test_cam.py
@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+
+import rospy
+import pyrealsense2 as rs
+import numpy as np
+
+from sensor_msgs.msg import Image, CameraInfo
+from cv_bridge import CvBridge
+
+
+def main():
+    rospy.init_node("depth_publisher")
+
+    bridge = CvBridge()
+
+    depth_pub = rospy.Publisher(
+        "/camera/depth/image_raw",
+        Image,
+        queue_size=1
+    )
+
+    info_pub = rospy.Publisher(
+        "/camera/depth/camera_info",
+        CameraInfo,
+        queue_size=1
+    )
+
+    # RealSense pipeline
+    pipeline = rs.pipeline()
+    config = rs.config()
+
+    width = 848
+    height = 480
+    fps = 30
+
+    config.enable_stream(
+        rs.stream.depth,
+        width,
+        height,
+        rs.format.z16,
+        fps
+    )
+
+    profile = pipeline.start(config)
+
+    # Lấy intrinsic của camera
+    depth_stream = profile.get_stream(rs.stream.depth)
+    intr = depth_stream.as_video_stream_profile().get_intrinsics()
+
+    rospy.loginfo("Depth camera started.")
+
+    rate = rospy.Rate(fps)
+
+    while not rospy.is_shutdown():
+
+        frames = pipeline.wait_for_frames()
+        depth = frames.get_depth_frame()
+
+        if not depth:
+            continue
+
+        depth_image = np.asanyarray(depth.get_data())
+
+        # Image message
+        img_msg = bridge.cv2_to_imgmsg(depth_image, encoding="16UC1")
+        img_msg.header.stamp = rospy.Time.now()
+        img_msg.header.frame_id = "camera_depth_optical_frame"
+
+        # CameraInfo message
+        info_msg = CameraInfo()
+        info_msg.header = img_msg.header
+
+        info_msg.width = intr.width
+        info_msg.height = intr.height
+
+        info_msg.distortion_model = "plumb_bob"
+
+        # Thông số méo (D)
+        info_msg.D = list(intr.coeffs)
+
+        # Camera matrix (K)
+        info_msg.K = [
+            intr.fx, 0.0, intr.ppx,
+            0.0, intr.fy, intr.ppy,
+            0.0, 0.0, 1.0
+        ]
+
+        # Rectification matrix (R)
+        info_msg.R = [
+            1.0, 0.0, 0.0,
+            0.0, 1.0, 0.0,
+            0.0, 0.0, 1.0
+        ]
+
+        # Projection matrix (P)
+        info_msg.P = [
+            intr.fx, 0.0, intr.ppx, 0.0,
+            0.0, intr.fy, intr.ppy, 0.0,
+            0.0, 0.0, 1.0, 0.0
+        ]
+
+        depth_pub.publish(img_msg)
+        info_pub.publish(info_msg)
+
+        rate.sleep()
+
+    pipeline.stop()
+
+
+if __name__ == "__main__":
+    main()