diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 9556f6b4..be07a7d1 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -365,6 +365,10 @@ class OBCameraNode { std::shared_ptr& response); void setFilterCallback(const std::shared_ptr& request, std::shared_ptr& response); + void setPointCloudDecimationCallback(const std::shared_ptr& request, + std::shared_ptr& response); + void getPointCloudDecimationCallback(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, @@ -572,6 +576,8 @@ class OBCameraNode { rclcpp::Service::SharedPtr set_sync_host_time_srv_; rclcpp::Service::SharedPtr send_software_trigger_srv_; 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 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 4e6feaa8..c45637fa 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -261,7 +261,63 @@ void OBCameraNode::setupCameraCtrlServices() { std::shared_ptr response) { getStreamsEnableCallback(request, response); }); + set_point_cloud_decimation_srv_ = node_->create_service( + "set_point_cloud_decimation", [this](const std::shared_ptr request, + std::shared_ptr response) { + setPointCloudDecimationCallback(request, response); + }); + get_point_cloud_decimation_srv_ = node_->create_service( + "get_point_cloud_decimation", [this](const std::shared_ptr request, + std::shared_ptr response) { + getPointCloudDecimationCallback(request, response); + }); } + +void OBCameraNode::getPointCloudDecimationCallback( + const std::shared_ptr& request, + std::shared_ptr& response) { + (void)request; + try { + response->data = point_cloud_decimation_filter_factor_; + response->success = true; + } catch (const std::exception& e) { + response->success = false; + response->message = e.what(); + } catch (...) { + response->success = false; + response->message = "unknown error"; + } +} + +void OBCameraNode::setPointCloudDecimationCallback( + const std::shared_ptr& request, + std::shared_ptr& response) { + if (!request) { + response->success = false; + response->message = "Invalid request"; + return; + } + + if (request->data <= 0 || request->data > 8) { + response->success = false; + response->message = "Decimation factor must be between 1 and 8"; + RCLCPP_WARN_STREAM(logger_, "Invalid decimation factor: " << request->data); + return; + } + + try { + point_cloud_decimation_filter_factor_ = request->data; + RCLCPP_INFO_STREAM(logger_, "Set point_cloud_decimation_filter_factor to " + << point_cloud_decimation_filter_factor_); + response->success = true; + response->message = "Point cloud decimation factor updated successfully"; + } catch (const std::exception &e) { + response->success = false; + response->message = std::string("Failed to set decimation factor: ") + e.what(); + RCLCPP_ERROR_STREAM(logger_, response->message); + } +} + void OBCameraNode::setStreamsEnableCallback( const std::shared_ptr request, std::shared_ptr response) {