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;