add file test cam intel D435i

include/robot_depth_image_proc/point_cloud_xyz.h

@@ -10,7 +10,8 @@ namespace depth_image_proc
 
 robot_sensor_msgs::PointCloud2 convertDepthToPointCloud(
     const robot_sensor_msgs::Image& depth_msg,
-    const robot_sensor_msgs::CameraInfo& info_msg);
+    const robot_sensor_msgs::CameraInfo& info_msg,
+    double range_max = 4.0);
 
 }  // namespace depth_image_proc
 

launch/depth_image_proc_gazebo.launch

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Use with Gazebo T800: depth topics come from URDF depth camera plugin -->
+  <arg name="multi_cam" default="true"/>
+  <arg name="depth_topic" default="/camera/depth/image_raw"/>
+  <arg name="camera_info_topic" default="/camera/depth/camera_info"/>
+  <arg name="cloud_topic" default="/camera/depth/points_proc"/>
+  <arg name="fixed_frame" default="odom"/>
+  <!-- <arg name="rviz" default="true"/> -->
+
+  <node pkg="robot_depth_image_proc"
+        type="depth_image_proc_node"
+        name="depth_image_proc"
+        output="screen">
+    <param name="multi_cam" value="$(arg multi_cam)"/>
+    <param name="depth_topic" value="$(arg depth_topic)"/>
+    <param name="camera_info_topic" value="$(arg camera_info_topic)"/>
+    <param name="cloud_topic" value="$(arg cloud_topic)"/>
+    <param name="fixed_frame" value="$(arg fixed_frame)"/>
+    <param name="publish_tf" value="false"/>
+  </node>
+
+  <!-- <node if="$(arg rviz)"
+        pkg="rviz"
+        type="rviz"
+        name="rviz"
+        args="-d $(find robot_depth_image_proc)/rviz/depth_image_proc_gazebo.rviz"/> -->
+</launch>

rviz/depth_image_proc_gazebo.rviz

@@ -0,0 +1,72 @@
+Panels:
+  - Class: rviz/Displays
+    Name: Displays
+  - Class: rviz/Views
+    Name: Views
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      Class: rviz/Grid
+      Enabled: true
+      Name: Grid
+      Reference Frame: odom
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Enabled: true
+      Name: T800
+      Robot Description: robot_description
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 5
+        Min Value: 0
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 0
+      Enabled: true
+      Name: depth_image_proc Cloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 2
+      Style: Points
+      Topic: /camera/depth/points_proc
+      Use Fixed Frame: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /camera/depth/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Depth Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+    - Class: rviz/MoveCamera
+    - Class: rviz/FocusCamera
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 4
+      Focal Point:
+        X: 0.5
+        Y: 0
+        Z: 0.5
+      Name: Current View
+      Pitch: 0.4
+      Yaw: 0.8
+Window Geometry:
+  Height: 800
+  Width: 1200

src/depth_image_proc_node.cpp

@@ -1,43 +1,49 @@
+#include <memory>
 #include <mutex>
 #include <string>
+#include <vector>
 
 #include <ros/ros.h>
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <tf2_ros/static_transform_broadcaster.h>
+#include <XmlRpcValue.h>
 
 #include <robot_depth_image_proc/point_cloud_xyz.h>
 #include <robot_depth_image_proc/ros_message_conversions.h>
 
-class DepthImageProcNode
+struct CameraConfig
 {
-public:
-  DepthImageProcNode(ros::NodeHandle& nh, ros::NodeHandle& pnh)
-  {
+  std::string name;
   std::string depth_topic;
   std::string camera_info_topic;
   std::string cloud_topic;
+};
 
-    pnh.param("depth_topic", depth_topic, std::string("/camera/depth/image_raw"));
-    pnh.param("camera_info_topic", camera_info_topic, std::string("/camera/depth/camera_info"));
-    pnh.param("cloud_topic", cloud_topic, std::string("/camera/depth/points"));
-    pnh.param("fixed_frame", fixed_frame_, std::string("map"));
-    pnh.param("publish_tf", publish_tf_, true);
+class DepthCameraPipeline
+{
+public:
+  DepthCameraPipeline(
+      ros::NodeHandle& nh,
+      const CameraConfig& config,
+      const std::string& fixed_frame,
+      bool publish_tf)
+      : config_(config),
+        fixed_frame_(fixed_frame),
+        publish_tf_(publish_tf)
+  {
+    cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>(config_.cloud_topic, 1);
 
-    cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>(cloud_topic, 1);
-
-    camera_info_sub_ = nh.subscribe(
-        camera_info_topic,
+    camera_info_sub_ = nh.subscribe<sensor_msgs::CameraInfo>(
+        config_.camera_info_topic,
         1,
-        &DepthImageProcNode::cameraInfoCallback,
-        this);
+        [this](const sensor_msgs::CameraInfoConstPtr& msg) { cameraInfoCallback(msg); });
 
-    depth_sub_ = nh.subscribe(
-        depth_topic,
+    depth_sub_ = nh.subscribe<sensor_msgs::Image>(
+        config_.depth_topic,
         1,
-        &DepthImageProcNode::depthCallback,
-        this);
+        [this](const sensor_msgs::ImageConstPtr& msg) { depthCallback(msg); });
 
     if (publish_tf_)
     {
@@ -45,10 +51,11 @@ public:
     }
 
     ROS_INFO(
-        "depth_image_proc listening on [%s] + [%s], publishing [%s]",
-        depth_topic.c_str(),
-        camera_info_topic.c_str(),
-        cloud_topic.c_str());
+        "[%s] depth_image_proc listening on [%s] + [%s], publishing [%s]",
+        config_.name.c_str(),
+        config_.depth_topic.c_str(),
+        config_.camera_info_topic.c_str(),
+        config_.cloud_topic.c_str());
   }
 
 private:
@@ -76,7 +83,10 @@ private:
       std::lock_guard<std::mutex> lock(mutex_);
       if (!has_camera_info_)
       {
-        ROS_WARN_THROTTLE(5.0, "Waiting for camera_info before converting depth image");
+        ROS_WARN_THROTTLE(
+            5.0,
+            "[%s] Waiting for camera_info before converting depth image",
+            config_.name.c_str());
         return;
       }
       camera_info = camera_info_;
@@ -86,7 +96,8 @@ private:
     {
       ROS_ERROR_THROTTLE(
           5.0,
-          "Unsupported depth encoding [%s], expected 16UC1/mono16/32FC1",
+          "[%s] Unsupported depth encoding [%s], expected 16UC1/mono16/32FC1",
+          config_.name.c_str(),
           msg->encoding.c_str());
       return;
     }
@@ -97,7 +108,10 @@ private:
 
     if (cloud.width == 0 || cloud.height == 0)
     {
-      ROS_ERROR_THROTTLE(5.0, "depth_image_proc conversion returned an empty point cloud");
+      ROS_ERROR_THROTTLE(
+          5.0,
+          "[%s] depth_image_proc conversion returned an empty point cloud",
+          config_.name.c_str());
       return;
     }
 
@@ -111,7 +125,7 @@ private:
   {
     if (frame_id_.empty())
     {
-      frame_id_ = "camera_depth_optical_frame";
+      frame_id_ = config_.name + "_depth_optical_frame";
     }
 
     geometry_msgs::TransformStamped transform;
@@ -122,13 +136,14 @@ private:
     static_broadcaster_.sendTransform(transform);
   }
 
+  const CameraConfig config_;
+  const std::string fixed_frame_;
+  const bool publish_tf_;
+
   std::mutex mutex_;
   robot_sensor_msgs::CameraInfo camera_info_;
   bool has_camera_info_{false};
-
-  std::string fixed_frame_;
   std::string frame_id_;
-  bool publish_tf_{true};
 
   ros::Subscriber depth_sub_;
   ros::Subscriber camera_info_sub_;
@@ -136,6 +151,128 @@ private:
   tf2_ros::StaticTransformBroadcaster static_broadcaster_;
 };
 
+class DepthImageProcNode
+{
+public:
+  DepthImageProcNode(ros::NodeHandle& nh, ros::NodeHandle& pnh)
+  {
+    pnh.param("fixed_frame", fixed_frame_, std::string("map"));
+    pnh.param("publish_tf", publish_tf_, true);
+
+    const std::vector<CameraConfig> configs = loadCameraConfigs(pnh);
+    if (configs.empty())
+    {
+      ROS_FATAL("depth_image_proc_node: no camera configuration found");
+      throw std::runtime_error("no camera configuration found");
+    }
+
+    pipelines_.reserve(configs.size());
+    for (const CameraConfig& config : configs)
+    {
+      pipelines_.push_back(std::make_unique<DepthCameraPipeline>(
+          nh, config, fixed_frame_, publish_tf_));
+    }
+
+    ROS_INFO("depth_image_proc_node started with %zu camera(s)", pipelines_.size());
+  }
+
+private:
+  static bool readCameraConfig(
+      const XmlRpc::XmlRpcValue& entry,
+      CameraConfig& config,
+      const std::string& fallback_name)
+  {
+    if (entry.getType() != XmlRpc::XmlRpcValue::TypeStruct)
+    {
+      ROS_ERROR("Each cameras[] entry must be a struct");
+      return false;
+    }
+
+    config.name = fallback_name;
+    if (entry.hasMember("name"))
+    {
+      config.name = static_cast<std::string>(entry["name"]);
+    }
+
+    if (!entry.hasMember("depth_topic") ||
+        !entry.hasMember("camera_info_topic") ||
+        !entry.hasMember("cloud_topic"))
+    {
+      ROS_ERROR(
+          "Camera [%s] must define depth_topic, camera_info_topic, cloud_topic",
+          config.name.c_str());
+      return false;
+    }
+
+    config.depth_topic = static_cast<std::string>(entry["depth_topic"]);
+    config.camera_info_topic = static_cast<std::string>(entry["camera_info_topic"]);
+    config.cloud_topic = static_cast<std::string>(entry["cloud_topic"]);
+    return true;
+  }
+
+  static std::vector<CameraConfig> defaultMultiCameraConfigs()
+  {
+    return {
+        {"camera",
+         "/camera/depth/image_raw",
+         "/camera/depth/camera_info",
+         "/camera/depth/points_proc"},
+        {"camera_right",
+         "/camera_right/depth/image_raw",
+         "/camera_right/depth/camera_info",
+         "/camera_right/depth/points_proc"},
+    };
+  }
+
+  static std::vector<CameraConfig> loadCameraConfigs(ros::NodeHandle& pnh)
+  {
+    XmlRpc::XmlRpcValue cameras_param;
+    if (pnh.getParam("cameras", cameras_param))
+    {
+      if (cameras_param.getType() != XmlRpc::XmlRpcValue::TypeArray)
+      {
+        ROS_ERROR("'cameras' param must be an array");
+        return {};
+      }
+
+      std::vector<CameraConfig> configs;
+      configs.reserve(static_cast<size_t>(cameras_param.size()));
+      for (int i = 0; i < cameras_param.size(); ++i)
+      {
+        CameraConfig config;
+        const std::string fallback_name = "camera_" + std::to_string(i);
+        if (!readCameraConfig(cameras_param[i], config, fallback_name))
+        {
+          return {};
+        }
+        configs.push_back(config);
+      }
+      return configs;
+    }
+
+    bool multi_cam = false;
+    pnh.param("multi_cam", multi_cam, false);
+    if (multi_cam)
+    {
+      return defaultMultiCameraConfigs();
+    }
+
+    CameraConfig config;
+    config.name = "camera";
+    pnh.param("depth_topic", config.depth_topic, std::string("/camera/depth/image_raw"));
+    pnh.param(
+        "camera_info_topic",
+        config.camera_info_topic,
+        std::string("/camera/depth/camera_info"));
+    pnh.param("cloud_topic", config.cloud_topic, std::string("/camera/depth/points"));
+    return {config};
+  }
+
+  std::string fixed_frame_;
+  bool publish_tf_{true};
+  std::vector<std::unique_ptr<DepthCameraPipeline>> pipelines_;
+};
+
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "depth_image_proc_node");

src/point_cloud_xyz.cpp

@@ -12,7 +12,8 @@ namespace enc = robot_sensor_msgs::image_encodings;
 
 robot_sensor_msgs::PointCloud2 convertDepthToPointCloud(
     const robot_sensor_msgs::Image& depth_msg,
-    const robot_sensor_msgs::CameraInfo& info_msg)
+    const robot_sensor_msgs::CameraInfo& info_msg,
+    double range_max)
 {
   robot_sensor_msgs::PointCloud2 cloud_msg;
   cloud_msg.header = depth_msg.header;
@@ -29,11 +30,11 @@ robot_sensor_msgs::PointCloud2 convertDepthToPointCloud(
 
   if (depth_msg.encoding == enc::TYPE_16UC1 || depth_msg.encoding == enc::MONO16)
   {
-    convert<uint16_t>(depth_msg, cloud_msg, model);
+    convert<uint16_t>(depth_msg, cloud_msg, model, range_max);
   }
   else if (depth_msg.encoding == enc::TYPE_32FC1)
   {
-    convert<float>(depth_msg, cloud_msg, model);
+    convert<float>(depth_msg, cloud_msg, model, range_max);
   }
   else
   {

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()