diff --git a/include/robot_depth_image_proc/point_cloud_xyz.h b/include/robot_depth_image_proc/point_cloud_xyz.h
index a1e104e..a1b227d 100644
--- a/include/robot_depth_image_proc/point_cloud_xyz.h
+++ b/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
diff --git a/launch/depth_image_proc_gazebo.launch b/launch/depth_image_proc_gazebo.launch
new file mode 100644
index 0000000..53c62c6
--- /dev/null
+++ b/launch/depth_image_proc_gazebo.launch
@@ -0,0 +1,28 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/rviz/depth_image_proc_gazebo.rviz b/rviz/depth_image_proc_gazebo.rviz
new file mode 100644
index 0000000..d81da9d
--- /dev/null
+++ b/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
diff --git a/src/depth_image_proc_node.cpp b/src/depth_image_proc_node.cpp
index ce65a70..1077db8 100644
--- a/src/depth_image_proc_node.cpp
+++ b/src/depth_image_proc_node.cpp
@@ -1,43 +1,49 @@
+#include
#include
#include
+#include
#include
#include
#include
#include
#include
+#include
#include
#include
-class DepthImageProcNode
+struct CameraConfig
+{
+ std::string name;
+ std::string depth_topic;
+ std::string camera_info_topic;
+ std::string cloud_topic;
+};
+
+class DepthCameraPipeline
{
public:
- DepthImageProcNode(ros::NodeHandle& nh, ros::NodeHandle& pnh)
+ 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)
{
- std::string depth_topic;
- std::string camera_info_topic;
- std::string cloud_topic;
+ cloud_pub_ = nh.advertise(config_.cloud_topic, 1);
- 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);
-
- cloud_pub_ = nh.advertise(cloud_topic, 1);
-
- camera_info_sub_ = nh.subscribe(
- camera_info_topic,
+ camera_info_sub_ = nh.subscribe(
+ 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(
+ 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 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 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(
+ 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(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(entry["depth_topic"]);
+ config.camera_info_topic = static_cast(entry["camera_info_topic"]);
+ config.cloud_topic = static_cast(entry["cloud_topic"]);
+ return true;
+ }
+
+ static std::vector 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 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 configs;
+ configs.reserve(static_cast(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> pipelines_;
+};
+
int main(int argc, char** argv)
{
ros::init(argc, argv, "depth_image_proc_node");
diff --git a/src/point_cloud_xyz.cpp b/src/point_cloud_xyz.cpp
index f376803..420d0e2 100644
--- a/src/point_cloud_xyz.cpp
+++ b/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(depth_msg, cloud_msg, model);
+ convert(depth_msg, cloud_msg, model, range_max);
}
else if (depth_msg.encoding == enc::TYPE_32FC1)
{
- convert(depth_msg, cloud_msg, model);
+ convert(depth_msg, cloud_msg, model, range_max);
}
else
{
diff --git a/test/test_cam.py b/test/test_cam.py
new file mode 100755
index 0000000..651ede9
--- /dev/null
+++ 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()
\ No newline at end of file