diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index a65d1b7b..942dd0f9 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -375,6 +375,8 @@ class OBCameraNode { std::shared_ptr& response); void getPointCloudDecimationCallback(const std::shared_ptr& request, std::shared_ptr& response); + void setDisparityRangeModeCallback(const std::shared_ptr& request, + std::shared_ptr& response); void setSYNCHostimeCallback(const std::shared_ptr& request, std::shared_ptr& response); void sendSoftwareTriggerCallback(const std::shared_ptr& request, @@ -602,6 +604,7 @@ class OBCameraNode { rclcpp::Service::SharedPtr set_filter_srv_; rclcpp::Service::SharedPtr set_point_cloud_decimation_srv_; rclcpp::Service::SharedPtr get_point_cloud_decimation_srv_; + rclcpp::Service::SharedPtr set_disparity_range_mode_srv_; rclcpp::Service::SharedPtr get_streams_enable_srv_; rclcpp::Service::SharedPtr set_streams_enable_srv_; rclcpp::Service::SharedPtr get_user_calib_params_srv_; diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index d4fe970a..478e8cbd 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -283,6 +283,11 @@ void OBCameraNode::setupCameraCtrlServices() { std::shared_ptr response) { getPointCloudDecimationCallback(request, response); }); + set_disparity_range_mode_srv_ = node_->create_service( + "set_disparity_range_mode", [this](const std::shared_ptr request, + std::shared_ptr response) { + setDisparityRangeModeCallback(request, response); + }); } void OBCameraNode::getPointCloudDecimationCallback( @@ -330,6 +335,55 @@ void OBCameraNode::setPointCloudDecimationCallback( } } +void OBCameraNode::setDisparityRangeModeCallback(const std::shared_ptr& request, + std::shared_ptr& response) { + if (!request) { + response->success = false; + response->message = "Invalid request"; + return; + } + + try { + if (!device_->isPropertySupported(OB_PROP_DISP_SEARCH_RANGE_MODE_INT, OB_PERMISSION_WRITE)) { + response->success = false; + response->message = "OB_PROP_DISP_SEARCH_RANGE_MODE_INT is not supported"; + return; + } + + const bool allow_set = isGemini435LePID(pid_) || enable_stream_[DEPTH]; + if (!allow_set) { + response->success = false; + response->message = "Disparity range mode can only be set when depth stream is enabled"; + return; + } + + auto range = device_->getIntPropertyRange(OB_PROP_DISP_SEARCH_RANGE_MODE_INT); + if (hw_mode_index < range.min || hw_mode_index > range.max) { + response->success = false; + response->message = + "Invalid disparity range mode. Allowed values:" + std::to_string(range.min) + " to " + + std::to_string(range.max); + return; + } + + device_->setIntProperty(OB_PROP_DISP_SEARCH_RANGE_MODE_INT, hw_mode_index); + disparity_range_mode_ = requested_mode_value; + + RCLCPP_INFO_STREAM(logger_, "Set disparity_range_mode to " << requested_mode_value); + response->success = true; + response->message = "disparity_range_mode updated"; + } catch (const ob::Error& e) { + response->success = false; + response->message = e.getMessage(); + } catch (const std::exception& e) { + response->success = false; + response->message = e.what(); + } catch (...) { + response->success = false; + response->message = "unknown error"; + } +} + void OBCameraNode::setStreamsEnableCallback( const std::shared_ptr request, std::shared_ptr response) {