fix: reject mixed FPS for Gemini 301 series

orbbec_camera/include/orbbec_camera/constants.h

@@ -143,6 +143,7 @@ const int32_t GEMINI_435Le_PID = 0x815;                   // Gemini 435Le
 const int32_t GEMINI_305_PID = 0x0840;                    // Gemini 305
 const int32_t GEMINI_305_PID2 = 0x0841;                   // Gemini 305
 const int32_t GEMINI_305G_PID = 0x0842;                   // Gemini 305g
+const int32_t GEMINI_301G_PID = 0x0843;                   // Gemini 301g
 const int32_t GEMINI_309G_PID = 0x0845;                   // Gemini 309g
 const int32_t GEMINI_338LG_PID = 0x081A;                  // Gemini 338Lg
 const int32_t GEMINI_338LE_PID = 0x081B;                  // Gemini 338Le
@@ -161,7 +162,7 @@ inline bool isGemini330SeriesPID(uint32_t pid) {
 
 inline bool isGemini305SeriesPID(uint32_t pid) {
   return pid == GEMINI_305_PID || pid == GEMINI_305_PID2 || pid == GEMINI_305G_PID ||
-         pid == GEMINI_309G_PID;
+         pid == GEMINI_301G_PID || pid == GEMINI_309G_PID;
 }
 
 inline bool isGmslCameraPID(uint32_t pid) {

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -309,6 +309,9 @@ class OBCameraNode {
 
   void setupProfiles();
 
+  bool validate301SeriesStreamFrameRates(const std::map<stream_index_pair, int>& fps,
+                                         std::string& message) const;
+
   std::shared_ptr<ob::VideoStreamProfile> selectVideoStreamProfile(
       const stream_index_pair& stream_index, int width, int height, int fps, OBFormat format);
 

orbbec_camera/src/dynamic_params.cpp

@@ -21,21 +21,23 @@ Parameters::Parameters(rclcpp::Node *node)
     : node_(node), logger_(node_->get_logger()), params_backend_(node) {
   params_backend_.addOnSetParametersCallback(
       [this](const std::vector<rclcpp::Parameter> &parameters) {
+        rcl_interfaces::msg::SetParametersResult result;
+        result.successful = true;
         for (const auto &parameter : parameters) {
-          if (param_functions_.find(parameter.get_name()) != param_functions_.end()) {
-            auto functions = param_functions_[parameter.get_name()];
-            if (functions.empty()) {
-              RCLCPP_WARN_STREAM(logger_, "Parameter " << parameter.get_name()
-                                                       << " can not be changed in runtime.");
-            } else {
-              for (const auto &func : param_functions_[parameter.get_name()]) {
-                func(parameter);
-              }
+          const auto function_it = param_functions_.find(parameter.get_name());
+          if (function_it == param_functions_.end()) {
+            continue;
+          }
+          if (function_it->second.empty()) {
+            result.successful = false;
+            result.reason = "Parameter " + parameter.get_name() + " can not be changed in runtime.";
+            RCLCPP_WARN_STREAM(logger_, result.reason);
+          } else {
+            for (const auto &func : function_it->second) {
+              func(parameter);
             }
           }
         }
-        rcl_interfaces::msg::SetParametersResult result;
-        result.successful = true;
         return result;
       });
 }

orbbec_camera/src/ob_camera_node.cpp

@@ -3708,6 +3708,13 @@ void OBCameraNode::setupProfiles() {
       }
     }
   }
+
+  std::string stream_fps_message;
+  if (!validate301SeriesStreamFrameRates(fps_, stream_fps_message)) {
+    RCLCPP_ERROR_STREAM(logger_, stream_fps_message);
+    throw std::runtime_error(stream_fps_message);
+  }
+
   // IMU
   for (const auto &stream_index : HID_STREAMS) {
     if (!enable_stream_[stream_index]) {
@@ -3752,6 +3759,55 @@ void OBCameraNode::setupProfiles() {
   }
 }
 
+bool OBCameraNode::validate301SeriesStreamFrameRates(const std::map<stream_index_pair, int> &fps,
+                                                     std::string &message) const {
+  if (!isGemini305SeriesPID(pid_)) {
+    return true;
+  }
+
+  int active_fps = 0;
+  bool fps_mismatch = false;
+  std::string active_streams;
+  for (const auto &stream_index : IMAGE_STREAMS) {
+    if (stream_index.first == OB_STREAM_LIDAR) {
+      continue;
+    }
+    const auto enable_it = enable_stream_.find(stream_index);
+    const auto fps_it = fps.find(stream_index);
+    if (enable_it == enable_stream_.end() || !enable_it->second || fps_it == fps.end() ||
+        fps_it->second <= 0) {
+      continue;
+    }
+
+    if (!active_streams.empty()) {
+      active_streams += ", ";
+    }
+    const auto name_it = stream_name_.find(stream_index);
+    if (name_it != stream_name_.end()) {
+      active_streams += name_it->second;
+    } else {
+      active_streams += std::string(magic_enum::enum_name(stream_index.first));
+    }
+    active_streams += "=" + std::to_string(fps_it->second);
+
+    if (active_fps == 0) {
+      active_fps = fps_it->second;
+    } else if (active_fps != fps_it->second) {
+      fps_mismatch = true;
+    }
+  }
+
+  if (!fps_mismatch) {
+    return true;
+  }
+
+  message =
+      "Gemini 301 series requires the same FPS for all enabled image streams. "
+      "Active stream FPS: " +
+      active_streams + ". Set all enabled image streams to the same FPS or disable unused streams.";
+  return false;
+}
+
 std::shared_ptr<ob::VideoStreamProfile> OBCameraNode::selectVideoStreamProfile(
     const stream_index_pair &stream_index, int width, int height, int fps, OBFormat format) {
   auto sensor_it = sensors_.find(stream_index);
@@ -3906,6 +3962,16 @@ bool OBCameraNode::validateStreamProfileRequest(
       return false;
     }
   }
+
+  auto requested_fps = fps_;
+  for (const auto &pending_profile : pending_profiles) {
+    requested_fps[pending_profile.stream_index] =
+        static_cast<int>(pending_profile.profile->getFps());
+  }
+  if (!validate301SeriesStreamFrameRates(requested_fps, message)) {
+    return false;
+  }
+
   if (!has_changes) {
     message = "requested stream profiles are already active";
     return false;