From af4c072aa6c523bc0ba1377b9082f8427c693b0b Mon Sep 17 00:00:00 2001
From: SamerKhshiboun <samer.khshiboun@intel.com>
Date: Sat, 5 Oct 2024 23:33:29 +0300
Subject: [PATCH] Add D555 PID

---
 realsense2_camera/CMakeLists.txt                 |  1 +
 realsense2_camera/include/base_realsense_node.h  |  6 ++++--
 realsense2_camera/include/constants.h            |  4 +++-
 realsense2_camera/src/base_realsense_node.cpp    |  4 +++-
 realsense2_camera/src/realsense_node_factory.cpp | 15 ++++++++++++++-
 5 files changed, 25 insertions(+), 5 deletions(-)

diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt
index 3453a665de..9a708ab5d8 100644
--- a/realsense2_camera/CMakeLists.txt
+++ b/realsense2_camera/CMakeLists.txt
@@ -118,6 +118,7 @@ find_package(tf2_ros REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(diagnostic_updater REQUIRED)
 find_package(OpenCV REQUIRED COMPONENTS core)
+find_package(fastrtps REQUIRED)
 
 find_package(realsense2 2.56.0)
 if (BUILD_ACCELERATE_GPU_WITH_GLSL)
diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h
index e6b4287a81..cce8273948 100755
--- a/realsense2_camera/include/base_realsense_node.h
+++ b/realsense2_camera/include/base_realsense_node.h
@@ -118,7 +118,8 @@ namespace realsense2_camera
         BaseRealSenseNode(rclcpp::Node& node,
                           rs2::device dev,
                           std::shared_ptr<Parameters> parameters,
-                          bool use_intra_process = false);
+                          bool use_intra_process = false,
+                          bool is_dds_device = false);
         ~BaseRealSenseNode();
         void publishTopics();
 
@@ -324,7 +325,8 @@ namespace realsense2_camera
         std::vector<geometry_msgs::msg::TransformStamped> _static_tf_msgs;
         std::shared_ptr<std::thread> _tf_t;
 
-        bool _use_intra_process;      
+        bool _use_intra_process;
+        bool _is_dds_device;
         std::map<stream_index_pair, std::shared_ptr<image_publisher>> _image_publishers;
         
         std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr> _imu_publishers;
diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h
index 7f5b28cee1..08f7b8609e 100644
--- a/realsense2_camera/include/constants.h
+++ b/realsense2_camera/include/constants.h
@@ -65,7 +65,9 @@ namespace realsense2_camera
     const uint16_t RS430i_PID       = 0x0b4b; // D430i
     const uint16_t RS405_PID        = 0x0B5B; // DS5U
     const uint16_t RS455_PID        = 0x0B5C; // D455
-    const uint16_t RS457_PID        = 0xABCD; // D457   
+    const uint16_t RS457_PID        = 0xABCD; // D457
+    const uint16_t RS555_USB_PID    = 0x0B56; // D555 USB
+    const uint16_t RS555_DDS_FAKE_PID = 0xFFFF; //D555 DDS (Dummy PID)
 
     const bool ALLOW_NO_TEXTURE_POINTS = false;
     const bool ORDERED_PC     = false;
diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp
index 4b88ec7df1..434d8b5bfd 100755
--- a/realsense2_camera/src/base_realsense_node.cpp
+++ b/realsense2_camera/src/base_realsense_node.cpp
@@ -91,7 +91,8 @@ size_t SyncedImuPublisher::getNumSubscribers()
 BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
                                      rs2::device dev,
                                      std::shared_ptr<Parameters> parameters,
-                                     bool use_intra_process) :
+                                     bool use_intra_process,
+                                     bool is_dds_device) :
     _is_running(true),
     _node(node),
     _logger(node.get_logger()),
@@ -107,6 +108,7 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
     _tf_publish_rate(TF_PUBLISH_RATE),
     _diagnostics_period(0),
     _use_intra_process(use_intra_process),
+    _is_dds_device(is_dds_device),
     _is_initialized_time_base(false),
     _camera_time_base(0),
     _sync_frames(SYNC_FRAMES),
diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp
index f7ea18bca7..b1f1119850 100644
--- a/realsense2_camera/src/realsense_node_factory.cpp
+++ b/realsense2_camera/src/realsense_node_factory.cpp
@@ -353,7 +353,16 @@ void RealSenseNodeFactory::startDevice()
 {
     if (_realSenseNode) _realSenseNode.reset();
     std::string pid_str(_device.get_info(RS2_CAMERA_INFO_PRODUCT_ID));
-    uint16_t pid = std::stoi(pid_str, 0, 16);
+    uint16_t pid;
+
+    if (pid_str == "0xDDS")
+    {
+        pid = RS555_DDS_FAKE_PID;
+    }
+    else
+    {
+        pid = std::stoi(pid_str, 0, 16);
+    }
     try
     {
         switch(pid)
@@ -374,9 +383,13 @@ void RealSenseNodeFactory::startDevice()
         case RS435i_RGB_PID:
         case RS455_PID:
         case RS457_PID:
+        case RS555_USB_PID:
         case RS_USB2_PID:
             _realSenseNode = std::unique_ptr<BaseRealSenseNode>(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms()));
             break;
+        case RS555_DDS_FAKE_PID: // D555 in ethernet mode (DDS)
+            _realSenseNode = std::unique_ptr<BaseRealSenseNode>(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms(), true));
+            break;
         default:
             ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str);
             rclcpp::shutdown();