Add D555 PID

IntelRealSense · Oct 7, 2024 · e732d32 · e732d32
1 parent d2e9f15
commit e732d32
Show file tree

Hide file tree

Showing 5 changed files with 44 additions and 16 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -101,30 +101,38 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list)
                 }
                 auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
                 ROS_INFO_STREAM("Device with serial number " << sn << " was found."<<std::endl);
+
                 std::string pn = dev.get_info(RS2_CAMERA_INFO_PHYSICAL_PORT);
                 std::string name = dev.get_info(RS2_CAMERA_INFO_NAME);
                 ROS_INFO_STREAM("Device with physical ID " << pn << " was found.");
                 std::vector<std::string> results;
                 ROS_INFO_STREAM("Device with name " << name << " was found.");
                 std::string port_id = parseUsbPort(pn);
-                if (port_id.empty())
+
+                std::string pid_str(dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID));
+                if(pid_str != "DDS")
                 {
-                    std::stringstream msg;
-                    msg << "Error extracting usb port from device with physical ID: " << pn << std::endl << "Please report on github issue at https://github.com/IntelRealSense/realsense-ros";
-                    if (_usb_port_id.empty())
+                    if (port_id.empty())
                     {
-                        ROS_WARN_STREAM(msg.str());
+                        std::stringstream msg;
+                        msg << "Error extracting usb port from device with physical ID: " << pn << std::endl
+                            << "Please report on github issue at https://github.com/IntelRealSense/realsense-ros";
+                        if (_usb_port_id.empty())
+                        {
+                            ROS_WARN_STREAM(msg.str());
+                        }
+                        else
+                        {
+                            ROS_ERROR_STREAM(msg.str());
+                            ROS_ERROR_STREAM("Please use serial number instead of usb port.");
+                        }
                     }
                     else
                     {
-                        ROS_ERROR_STREAM(msg.str());
-                        ROS_ERROR_STREAM("Please use serial number instead of usb port.");
+                        ROS_INFO_STREAM("Device with port number " << port_id << " was found.");
                     }
                 }
-                else
-                {
-                    ROS_INFO_STREAM("Device with port number " << port_id << " was found.");                    
-                }
+
                 bool found_device_type(true);
                 if (!_device_type.empty())
                 {
@@ -353,7 +361,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 == "DDS")
+    {
+        pid = RS555_DDS_FAKE_PID;
+    }
+    else
+    {
+        pid = std::stoi(pid_str, 0, 16);
+    }
     try
     {
         switch(pid)
@@ -374,9 +391,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();