From 49ba570897081d6c68f70c9f97e2ca4cd1e5797d Mon Sep 17 00:00:00 2001
From: Joe Dong <jian.dong@pobox.com>
Date: Wed, 10 Jul 2024 19:57:17 +0800
Subject: [PATCH] add get_ldp_measure_distance

---
 .../include/orbbec_camera/ob_camera_node.h    |  4 ++++
 orbbec_camera/src/ros_service.cpp             | 23 +++++++++++++++++++
 2 files changed, 27 insertions(+)

diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
index f14ccb52..b24e3bcd 100644
--- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h
+++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
@@ -269,6 +269,9 @@ class OBCameraNode {
   void getLdpStatusCallback(const std::shared_ptr<GetBool::Request>& request,
                             std::shared_ptr<GetBool::Response>& response);
 
+  void getLdpMeasureDistanceCallback(const std::shared_ptr<GetInt32::Request>& request,
+                          std::shared_ptr<GetInt32::Response>& response);
+
   bool toggleSensor(const stream_index_pair& stream_index, bool enabled, std::string& msg);
 
   void saveImageCallback(const std::shared_ptr<std_srvs::srv::Empty::Request>& request,
@@ -408,6 +411,7 @@ class OBCameraNode {
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_floor_enable_srv_;
   rclcpp::Service<SetInt32>::SharedPtr set_fan_work_mode_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_sensors_srv_;
+  rclcpp::Service<GetInt32>::SharedPtr get_ldp_measure_distance_srv_;
 
   bool enable_sync_output_accel_gyro_ = false;
   bool publish_tf_ = false;
diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp
index dc0d12de..b2f835df 100644
--- a/orbbec_camera/src/ros_service.cpp
+++ b/orbbec_camera/src/ros_service.cpp
@@ -164,6 +164,11 @@ void OBCameraNode::setupCameraCtrlServices() {
                                      std::shared_ptr<SetBool::Response> response) {
         setIRLongExposureCallback(request, response);
       });
+  get_ldp_measure_distance_srv_ = node_->create_service<GetInt32>(
+      "get_ldp_measure_distance", [this](const std::shared_ptr<GetInt32::Request> request,
+                                         std::shared_ptr<GetInt32::Response> response) {
+        getLdpMeasureDistanceCallback(request, response);
+      });
 }
 
 void OBCameraNode::setExposureCallback(const std::shared_ptr<SetInt32::Request>& request,
@@ -628,6 +633,24 @@ void OBCameraNode::getLdpStatusCallback(const std::shared_ptr<GetBool::Request>&
   }
 }
 
+void OBCameraNode::getLdpMeasureDistanceCallback(const std::shared_ptr<GetInt32::Request>& request,
+                                                 std::shared_ptr<GetInt32::Response>& response) {
+  (void)request;
+  try {
+    response->data = device_->getIntProperty(OB_PROP_LDP_MEASURE_DISTANCE_INT);
+    response->success = true;
+  } catch (const ob::Error& e) {
+    response->message = e.getMessage();
+    response->success = false;
+  } catch (const std::exception& e) {
+    response->message = e.what();
+    response->success = false;
+  } catch (...) {
+    response->message = "unknown error";
+    response->success = false;
+  }
+}
+
 void OBCameraNode::toggleSensorCallback(const std::shared_ptr<SetBool::Request>& request,
                                         std::shared_ptr<SetBool::Response>& response,
                                         const stream_index_pair& stream_index) {