From 162be38603b17f1683a9d4894f15b894b4dc4118 Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Tue, 15 Sep 2026 15:25:31 +0800 Subject: [PATCH 1/2] fix: reject mixed FPS for Gemini 301 series --- .../include/orbbec_camera/constants.h | 3 +- .../include/orbbec_camera/ob_camera_node.h | 3 + orbbec_camera/src/dynamic_params.cpp | 24 +++---- orbbec_camera/src/ob_camera_node.cpp | 66 +++++++++++++++++++ 4 files changed, 84 insertions(+), 12 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/constants.h b/orbbec_camera/include/orbbec_camera/constants.h index 3259d676..6b30374d 100644 --- a/orbbec_camera/include/orbbec_camera/constants.h +++ b/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) { diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 085b2e47..abe66fde 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -309,6 +309,9 @@ class OBCameraNode { void setupProfiles(); + bool validate301SeriesStreamFrameRates(const std::map& fps, + std::string& message) const; + std::shared_ptr selectVideoStreamProfile( const stream_index_pair& stream_index, int width, int height, int fps, OBFormat format); diff --git a/orbbec_camera/src/dynamic_params.cpp b/orbbec_camera/src/dynamic_params.cpp index 657b9a36..52bb4c2c 100644 --- a/orbbec_camera/src/dynamic_params.cpp +++ b/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 ¶meters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; for (const auto ¶meter : 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; }); } diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 17bcc06c..836a84db 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/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 &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 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(pending_profile.profile->getFps()); + } + if (!validate301SeriesStreamFrameRates(requested_fps, message)) { + return false; + } + if (!has_changes) { message = "requested stream profiles are already active"; return false; From 68e51f7b9c07a898cbaa9155ec910a9539cacdc8 Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Tue, 15 Sep 2026 15:51:42 +0800 Subject: [PATCH 2/2] refactor: rename Gemini 301 series PID helper --- .../include/orbbec_camera/constants.h | 2 +- orbbec_camera/src/ob_camera_node.cpp | 28 +++++++++---------- orbbec_camera/src/ob_camera_node_driver.cpp | 6 ++-- orbbec_camera/src/ros_service.cpp | 4 +-- orbbec_camera/tools/list_camera_profile.cpp | 4 +-- 5 files changed, 22 insertions(+), 22 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/constants.h b/orbbec_camera/include/orbbec_camera/constants.h index 6b30374d..fdd609be 100644 --- a/orbbec_camera/include/orbbec_camera/constants.h +++ b/orbbec_camera/include/orbbec_camera/constants.h @@ -160,7 +160,7 @@ inline bool isGemini330SeriesPID(uint32_t pid) { pid == GEMINI_331L_PID; } -inline bool isGemini305SeriesPID(uint32_t pid) { +inline bool isGemini301SeriesPID(uint32_t pid) { return pid == GEMINI_305_PID || pid == GEMINI_305_PID2 || pid == GEMINI_305G_PID || pid == GEMINI_301G_PID || pid == GEMINI_309G_PID; } diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 836a84db..34ca616c 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -3249,7 +3249,7 @@ void OBCameraNode::setupLeftIrPostProcessFilter() { } auto device_info = device_->getDeviceInfo(); CHECK_NOTNULL(device_info); - if (isGemini330SeriesPID(pid_) || isGemini305SeriesPID(pid_)) { + if (isGemini330SeriesPID(pid_) || isGemini301SeriesPID(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()) { @@ -3290,7 +3290,7 @@ void OBCameraNode::setupRightIrPostProcessFilter() { } auto device_info = device_->getDeviceInfo(); CHECK_NOTNULL(device_info); - if (isGemini330SeriesPID(pid_) || isGemini305SeriesPID(pid_)) { + if (isGemini330SeriesPID(pid_) || isGemini301SeriesPID(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()) { @@ -3620,19 +3620,19 @@ void OBCameraNode::setupProfiles() { format_[elem] == OB_FORMAT_UNKNOWN) { selected_profile = profiles->getProfile(0)->as(); } else { - if (isGemini305SeriesPID(pid_) && elem == DEPTH) { + if (isGemini301SeriesPID(pid_) && elem == DEPTH) { OBHardwareDecimationConfig conf; conf.originWidth = width_[elem]; conf.originHeight = height_[elem]; conf.factor = depth_decimation_factor_; selected_profile = profiles->getVideoStreamProfile(conf, format_[elem], fps_[elem]); - } else if (isGemini305SeriesPID(pid_) && elem == INFRA1) { + } else if (isGemini301SeriesPID(pid_) && elem == INFRA1) { OBHardwareDecimationConfig conf; conf.originWidth = width_[elem]; conf.originHeight = height_[elem]; conf.factor = left_ir_decimation_factor_; selected_profile = profiles->getVideoStreamProfile(conf, format_[elem], fps_[elem]); - } else if (isGemini305SeriesPID(pid_) && elem == INFRA2) { + } else if (isGemini301SeriesPID(pid_) && elem == INFRA2) { OBHardwareDecimationConfig conf; conf.originWidth = width_[elem]; conf.originHeight = height_[elem]; @@ -3761,7 +3761,7 @@ void OBCameraNode::setupProfiles() { bool OBCameraNode::validate301SeriesStreamFrameRates(const std::map &fps, std::string &message) const { - if (!isGemini305SeriesPID(pid_)) { + if (!isGemini301SeriesPID(pid_)) { return true; } @@ -3823,19 +3823,19 @@ std::shared_ptr OBCameraNode::selectVideoStreamProfile( std::shared_ptr selected_profile; if (width == 0 && height == 0 && fps == 0) { selected_profile = profiles->getProfile(0)->as(); - } else if (!is_playback_device_ && isGemini305SeriesPID(pid_) && stream_index == DEPTH) { + } else if (!is_playback_device_ && isGemini301SeriesPID(pid_) && stream_index == DEPTH) { OBHardwareDecimationConfig conf; conf.originWidth = width; conf.originHeight = height; conf.factor = depth_decimation_factor_; selected_profile = profiles->getVideoStreamProfile(conf, format, fps); - } else if (!is_playback_device_ && isGemini305SeriesPID(pid_) && stream_index == INFRA1) { + } else if (!is_playback_device_ && isGemini301SeriesPID(pid_) && stream_index == INFRA1) { OBHardwareDecimationConfig conf; conf.originWidth = width; conf.originHeight = height; conf.factor = left_ir_decimation_factor_; selected_profile = profiles->getVideoStreamProfile(conf, format, fps); - } else if (!is_playback_device_ && isGemini305SeriesPID(pid_) && stream_index == INFRA2) { + } else if (!is_playback_device_ && isGemini301SeriesPID(pid_) && stream_index == INFRA2) { OBHardwareDecimationConfig conf; conf.originWidth = width; conf.originHeight = height; @@ -6025,7 +6025,7 @@ cv::Mat OBCameraNode::colorizeDepthImage(const cv::Mat &depth_image, cv::Mat depth_16u; depth_image.convertTo(depth_16u, CV_16UC1); - const uint16_t min_depth = isGemini305SeriesPID(pid_) ? kViewerColorizerG305MinDistanceMm + const uint16_t min_depth = isGemini301SeriesPID(pid_) ? kViewerColorizerG305MinDistanceMm : kViewerColorizerDefaultMinDistanceMm; const uint16_t max_depth = kViewerColorizerMaxDistanceMm; const uint32_t value_range = static_cast(max_depth) - min_depth + 1; @@ -6488,7 +6488,7 @@ void OBCameraNode::setDepthAutoExposureROI() { if (depth_roi_has_run) { return; } - if (isGemini305SeriesPID(pid_) && ae_reference_stream_ == "color") { + if (isGemini301SeriesPID(pid_) && ae_reference_stream_ == "color") { RCLCPP_WARN_STREAM(logger_, "Skip setting depth AE ROI because AE Reference Stream is color"); depth_roi_has_run = true; return; @@ -6533,7 +6533,7 @@ void OBCameraNode::setColorAutoExposureROI() { if (color_roi_has_run) { return; } - if (isGemini305SeriesPID(pid_) && ae_reference_stream_ == "depth") { + if (isGemini301SeriesPID(pid_) && ae_reference_stream_ == "depth") { RCLCPP_WARN_STREAM(logger_, "Skip setting color AE ROI because AE Reference Stream is depth"); color_roi_has_run = true; return; @@ -8091,7 +8091,7 @@ bool OBCameraNode::setupFormatConvertType(OBFormat format, ob::FormatConvertFilt bool OBCameraNode::isGemini435LePID(uint32_t pid) { return pid == GEMINI_435Le_PID; } bool OBCameraNode::isPublishMetaData(uint32_t pid) { - return isGemini330SeriesPID(pid) || isGemini435LePID(pid) || isGemini305SeriesPID(pid); + return isGemini330SeriesPID(pid) || isGemini435LePID(pid) || isGemini301SeriesPID(pid); } bool OBCameraNode::isDabaiASeriesForHwD2C(uint32_t pid) { @@ -8101,7 +8101,7 @@ bool OBCameraNode::isDabaiASeriesForHwD2C(uint32_t pid) { bool OBCameraNode::isDepthWorkModeDevices(uint32_t pid) { return pid == GEMINI_435Le_PID; } -bool OBCameraNode::isnotLaserDevices(uint32_t pid) { return isGemini305SeriesPID(pid); } +bool OBCameraNode::isnotLaserDevices(uint32_t pid) { return isGemini301SeriesPID(pid); } orbbec_camera_msgs::msg::IMUInfo OBCameraNode::createIMUInfo( const stream_index_pair &stream_index) { diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index fca453df..db77106e 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -1372,7 +1372,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr &dev } const bool should_delay_stream_start = delay_stream_start_after_reconnect_.exchange(false) && - isGemini305SeriesPID(device_info_->getPid()); + isGemini301SeriesPID(device_info_->getPid()); if (should_delay_stream_start) { std::this_thread::sleep_for(kStreamStartDelayAfterReconnect); } @@ -1604,8 +1604,8 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr &list if (isGmslCameraPID(pid)) { ob_camera_node_->startGmslTrigger(); } - // if (isGemini305SeriesPID(pid)) { - // // Fixing 305 series hot-swap not outputting power + // if (isGemini301SeriesPID(pid)) { + // // Fixing 301 series hot-swap not outputting power // ob_camera_node_->startStreams(); // } } catch (ob::Error &e) { diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 5bfd9e82..26995f45 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -1128,13 +1128,13 @@ void OBCameraNode::setAeRoiCallback(const std::shared_ptr& std::shared_ptr& response, const stream_index_pair& stream_index) { auto stream = stream_index.first; - if (isGemini305SeriesPID(device_->getDeviceInfo()->getPid()) && + if (isGemini301SeriesPID(device_->getDeviceInfo()->getPid()) && (stream != OB_STREAM_COLOR && ae_reference_stream_ == "color")) { response->success = false; response->message = "AE Reference Stream is color, other sensors setting is not supported"; return; } - if (isGemini305SeriesPID(device_->getDeviceInfo()->getPid()) && + if (isGemini301SeriesPID(device_->getDeviceInfo()->getPid()) && (stream != OB_STREAM_DEPTH && ae_reference_stream_ == "depth")) { response->success = false; response->message = diff --git a/orbbec_camera/tools/list_camera_profile.cpp b/orbbec_camera/tools/list_camera_profile.cpp index 3c54d8c7..07e352e5 100644 --- a/orbbec_camera/tools/list_camera_profile.cpp +++ b/orbbec_camera/tools/list_camera_profile.cpp @@ -145,8 +145,8 @@ void listSensorProfiles(const std::shared_ptr& device) { auto origin_profile = profile_list->getProfile(j); if ((sensor->getType() == OB_SENSOR_DEPTH || sensor->getType() == OB_SENSOR_IR_LEFT || sensor->getType() == OB_SENSOR_IR_RIGHT) && - isGemini305SeriesPID(pid)) { - // Gemini 305 series + isGemini301SeriesPID(pid)) { + // Gemini 301 series auto profile = origin_profile->as(); std::cout << magic_enum::enum_name(sensor->getType()) << " profile: " << profile->getWidth() << "x" << profile->getHeight() << " " << profile->getFps() << "fps "