Merge branch 'fix/enhance_filter_support' into v2/develop

orbbec_camera/include/orbbec_camera/constants.h

@@ -149,6 +149,16 @@ const int32_t GEMINI_338LE_PID = 0x081B;                  // Gemini 338Le
 const int32_t GEMINI_338L_PID = 0x081C;                   // Gemini 338L
 const int32_t GEMINI_331L_PID = 0x081D;                   // Gemini 331L
 
+inline bool isGemini330SeriesPID(uint32_t pid) {
+  return pid == GEMINI_335_PID || pid == GEMINI_330_PID || pid == GEMINI_336_PID ||
+         pid == GEMINI_335L_PID || pid == GEMINI_330L_PID || pid == GEMINI_336L_PID ||
+         pid == GEMINI_335LG_PID || pid == GEMINI_336LG_PID || pid == GEMINI_335LE_PID ||
+         pid == GEMINI_336LE_PID || pid == CUSTOM_ADVANTECH_GEMINI_336_PID ||
+         pid == CUSTOM_ADVANTECH_GEMINI_336L_PID || pid == GEMINI_338_PID ||
+         pid == GEMINI_338LG_PID || pid == GEMINI_338LE_PID || pid == GEMINI_338L_PID ||
+         pid == GEMINI_331L_PID;
+}
+
 inline bool isGemini305SeriesPID(uint32_t pid) {
   return pid == GEMINI_305_PID || pid == GEMINI_305_PID2 || pid == GEMINI_305G_PID ||
          pid == GEMINI_309G_PID;

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -666,8 +666,6 @@ class OBCameraNode {
 
   orbbec_camera_msgs::msg::IMUInfo createIMUInfo(const stream_index_pair& stream_index);
 
-  static bool isGemini335PID(uint32_t pid);
-
   static bool isGemini435LePID(uint32_t pid);
   static bool isPublishMetaData(uint32_t pid);
   static bool isDabaiASeriesForHwD2C(uint32_t pid);

orbbec_camera/src/ob_camera_node.cpp

@@ -650,7 +650,9 @@ void OBCameraNode::publishDepthFiltersStatus() {
   if (disp_outliers_filter_supported) {
     append_unique_filter_name("DispOutliersFilter");
   }
-  append_unique_filter_name("EnhancedDepthFilter");
+  if (isGemini330SeriesPID(pid_)) {
+    append_unique_filter_name("EnhancedDepthFilter");
+  }
 
   msg.filters.reserve(ordered_filter_names.size());
   for (const auto &filter_name : ordered_filter_names) {
@@ -3235,7 +3237,7 @@ void OBCameraNode::setupLeftIrPostProcessFilter() {
   }
   auto device_info = device_->getDeviceInfo();
   CHECK_NOTNULL(device_info);
-  if (isGemini335PID(pid_)) {
+  if (isGemini330SeriesPID(pid_)) {
     auto left_ir_sensor = device_->getSensor(OB_SENSOR_IR_LEFT);
     left_ir_filter_list_ = left_ir_sensor->createRecommendedFilters();
     if (left_ir_filter_list_.empty()) {
@@ -3276,7 +3278,7 @@ void OBCameraNode::setupRightIrPostProcessFilter() {
   }
   auto device_info = device_->getDeviceInfo();
   CHECK_NOTNULL(device_info);
-  if (isGemini335PID(pid_)) {
+  if (isGemini330SeriesPID(pid_)) {
     auto right_ir_sensor = device_->getSensor(OB_SENSOR_IR_RIGHT);
     right_ir_filter_list_ = right_ir_sensor->createRecommendedFilters();
     if (right_ir_filter_list_.empty()) {
@@ -5288,6 +5290,11 @@ bool OBCameraNode::validateEnhancedDepthFilterConfig(std::string &message) const
   constexpr char kEnhancedDepthSupportedTargetResolutions[] = "640x480/1280x720/1280x800";
   constexpr char kEnhancedDepthSupportedDepthFormats[] = "Y10/Y11/Y12/Y14/Y16/Z16";
 
+  if (!isGemini330SeriesPID(pid_)) {
+    message = "Enhanced depth filter is only supported by Gemini 330 series devices";
+    return false;
+  }
+
   if (!enable_stream_.count(COLOR) || !enable_stream_.at(COLOR) || !enable_stream_.count(DEPTH) ||
       !enable_stream_.at(DEPTH)) {
     message = "Enhanced depth filter requires color and depth streams";
@@ -7931,18 +7938,9 @@ bool OBCameraNode::setupFormatConvertType(OBFormat format, ob::FormatConvertFilt
   return true;
 }
 
-bool OBCameraNode::isGemini335PID(uint32_t pid) {
-  return pid == GEMINI_335_PID || pid == GEMINI_330_PID || pid == GEMINI_336_PID ||
-         pid == GEMINI_335L_PID || pid == GEMINI_330L_PID || pid == GEMINI_336L_PID ||
-         pid == GEMINI_335LG_PID || pid == GEMINI_336LG_PID || pid == GEMINI_335LE_PID ||
-         pid == GEMINI_336LE_PID || pid == CUSTOM_ADVANTECH_GEMINI_336_PID ||
-         pid == CUSTOM_ADVANTECH_GEMINI_336L_PID || pid == GEMINI_338_PID ||
-         pid == GEMINI_338L_PID || pid == GEMINI_338LE_PID || pid == GEMINI_338LG_PID;
-}
-
 bool OBCameraNode::isGemini435LePID(uint32_t pid) { return pid == GEMINI_435Le_PID; }
 bool OBCameraNode::isPublishMetaData(uint32_t pid) {
-  return isGemini335PID(pid) || isGemini435LePID(pid) || isGemini305SeriesPID(pid);
+  return isGemini330SeriesPID(pid) || isGemini435LePID(pid) || isGemini305SeriesPID(pid);
 }
 
 bool OBCameraNode::isDabaiASeriesForHwD2C(uint32_t pid) {
@@ -8279,6 +8277,11 @@ bool OBCameraNode::applyEnhancedDepthFilterConfig(
     bool enabled, const std::vector<float> &positional_params,
     const std::vector<orbbec_camera_msgs::msg::DepthFilterParam> &named_params,
     std::string &message) {
+  if (!isGemini330SeriesPID(pid_)) {
+    message = "Enhanced depth filter is only supported by Gemini 330 series devices";
+    return false;
+  }
+
   if (positional_params.size() > 1) {
     message = "EnhancedDepthFilter only supports one positional parameter";
     return false;

orbbec_camera/tools/multi_save_rgbir.cpp

@@ -46,7 +46,7 @@ class MultiCameraSubscriber : public rclcpp::Node {
         std::string uid = device_info->uid();
         auto usb_port = parseUsbPort(uid);
         serial_numbers_[usb_port] = serial;
-        is_gemini330_ = isGemini335PID(pid);
+        is_gemini330_ = isGemini330SeriesPID(pid);
       }
     } catch (ob::Error &e) {
       RCLCPP_ERROR_STREAM(get_logger(), orbbec_camera::formatObErrorWithStatus(e));
@@ -76,15 +76,6 @@ class MultiCameraSubscriber : public rclcpp::Node {
  private:
   std::mutex image_mutex_;
   std::mutex meta_mutex_;
-  bool isGemini335PID(uint32_t pid) {
-    return pid == GEMINI_335_PID || pid == GEMINI_330_PID || pid == GEMINI_336_PID ||
-           pid == GEMINI_335L_PID || pid == GEMINI_330L_PID || pid == GEMINI_336L_PID ||
-           pid == GEMINI_335LG_PID || pid == GEMINI_336LG_PID || pid == GEMINI_335LE_PID ||
-           pid == GEMINI_336LE_PID || pid == CUSTOM_ADVANTECH_GEMINI_336_PID ||
-           pid == CUSTOM_ADVANTECH_GEMINI_336L_PID || pid == GEMINI_338_PID ||
-           pid == GEMINI_338LG_PID || pid == GEMINI_338LE_PID || pid == GEMINI_338L_PID ||
-           pid == GEMINI_331L_PID;
-  }
   void params_init() {
     std::ifstream file(
         "install/orbbec_camera/share/orbbec_camera/config/tools/multisavergbir/"