diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 6f6fa908..9899af92 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -1087,6 +1087,7 @@ class OBCameraNode { double lrm_obstacle_distance_publish_rate_ = 10.0; bool enable_heartbeat_ = false; bool enable_firmware_log_ = false; + int monitor_poll_interval_sec_ = -1; bool enable_fps_boost_ = false; std::map enable_undistortion_; std::shared_ptr hw_d2c_color_undistortion_filter_; diff --git a/orbbec_camera/launch/gemini_301_series.launch.py b/orbbec_camera/launch/gemini_301_series.launch.py index 06e9b248..583bba60 100644 --- a/orbbec_camera/launch/gemini_301_series.launch.py +++ b/orbbec_camera/launch/gemini_301_series.launch.py @@ -291,6 +291,7 @@ def generate_launch_description(): DeclareLaunchArgument('config_file_path', default_value=''), DeclareLaunchArgument('enable_heartbeat', default_value='false'), DeclareLaunchArgument('enable_firmware_log', default_value='false'), + DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'), DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'), DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'), DeclareLaunchArgument('disparity_range_mode', default_value='-1'), diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index 65b894eb..2bff99ba 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/orbbec_camera/launch/gemini_330_series.launch.py @@ -303,6 +303,7 @@ def generate_launch_description(): DeclareLaunchArgument('config_file_path', default_value=''), DeclareLaunchArgument('enable_heartbeat', default_value='false'), DeclareLaunchArgument('enable_firmware_log', default_value='false'), + DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'), DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'), DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'), DeclareLaunchArgument('disparity_range_mode', default_value='-1'), diff --git a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py index 8f071da2..90c06a9f 100644 --- a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py +++ b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py @@ -287,6 +287,7 @@ def generate_launch_description(): DeclareLaunchArgument('config_file_path', default_value=''), DeclareLaunchArgument('enable_heartbeat', default_value='false'), DeclareLaunchArgument('enable_firmware_log', default_value='false'), + DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'), DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'), DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'), DeclareLaunchArgument('disparity_range_mode', default_value='-1'), diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 69670685..e2e1bd78 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -1042,6 +1042,17 @@ void OBCameraNode::setupDevices() { OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT)); } } + if (monitor_poll_interval_sec_ != -1) { + try { + const auto interval_ms = static_cast(monitor_poll_interval_sec_) * 1000U; + device_->setMonitorPollInterval(interval_ms); + RCLCPP_INFO_STREAM( + logger_, "Current monitor poll interval: " << device_->getMonitorPollInterval() << " ms"); + } catch (const ob::Error &e) { + RCLCPP_WARN_STREAM(logger_, "Skipping monitor poll interval configuration: " + << orbbec_camera::formatObErrorWithStatus(e)); + } + } if (should_apply_launch_config("enable_heartbeat") && device_->isPropertySupported(OB_PROP_HEARTBEAT_BOOL, OB_PERMISSION_READ_WRITE)) { TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_); @@ -4763,6 +4774,16 @@ void OBCameraNode::getParameters() { setAndGetNodeParameter(max_depth_limit_, "max_depth_limit", 0); setAndGetNodeParameter(enable_heartbeat_, "enable_heartbeat", false); setAndGetNodeParameter(enable_firmware_log_, "enable_firmware_log", false); + setAndGetNodeParameter(monitor_poll_interval_sec_, "monitor_poll_interval_sec", -1); + if (monitor_poll_interval_sec_ != -1 && + (monitor_poll_interval_sec_ < 1 || monitor_poll_interval_sec_ > 10)) { + const auto requested_monitor_poll_interval_sec = monitor_poll_interval_sec_; + monitor_poll_interval_sec_ = std::clamp(monitor_poll_interval_sec_, 1, 10); + RCLCPP_WARN_STREAM(logger_, "monitor_poll_interval_sec value " + << requested_monitor_poll_interval_sec + << " is out of range [1, 10], clamped to " + << monitor_poll_interval_sec_); + } setAndGetNodeParameter(enable_fps_boost_, "enable_fps_boost", false); setAndGetNodeParameter(time_domain_, "time_domain", "global"); time_domain_ = normalizeClosedSetParameterValue(logger_, "time_domain", time_domain_,