From d331486bfd30ce9b0ed9deb1c7ad337fbae77961 Mon Sep 17 00:00:00 2001
From: obyalian <yalian@orbbec.com>
Date: Tue, 12 Aug 2025 17:31:23 +0800
Subject: [PATCH] Add laser and LDP protection status callbacks and services

---
 .../include/orbbec_camera/ob_camera_node.h    |  8 +++
 orbbec_camera/src/ros_service.cpp             | 54 ++++++++++++++++++-
 2 files changed, 60 insertions(+), 2 deletions(-)

diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
index 30b38645..84f319e6 100644
--- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h
+++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
@@ -274,6 +274,12 @@ class OBCameraNode {
   void getLdpStatusCallback(const std::shared_ptr<GetBool::Request>& request,
                             std::shared_ptr<GetBool::Response>& response);
 
+  void getLaserStatusCallback(const std::shared_ptr<GetBool::Request>& request,
+                              std::shared_ptr<GetBool::Response>& response);
+
+  void getLdpProtectionStatusCallback(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);
 
@@ -417,6 +423,8 @@ class OBCameraNode {
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_laser_enable_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ldp_enable_srv_;
   rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_ldp_status_srv_;
+  rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_ldp_protection_status_srv_;
+  rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_laser_status_srv_;
   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_;
diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp
index 6e20fb5d..cd8ef683 100644
--- a/orbbec_camera/src/ros_service.cpp
+++ b/orbbec_camera/src/ros_service.cpp
@@ -113,7 +113,20 @@ void OBCameraNode::setupCameraCtrlServices() {
         (void)request_header;
         getLdpStatusCallback(request, response);
       });
-
+  get_ldp_protection_status_srv_ = node_->create_service<GetBool>(
+      "get_ldp_protection_status", [this](const std::shared_ptr<rmw_request_id_t> request_header,
+                                          const std::shared_ptr<GetBool::Request> request,
+                                          std::shared_ptr<GetBool::Response> response) {
+        (void)request_header;
+        getLdpProtectionStatusCallback(request, response);
+      });
+  get_laser_status_srv_ = node_->create_service<GetBool>(
+      "get_laser_status", [this](const std::shared_ptr<rmw_request_id_t> request_header,
+                                 const std::shared_ptr<GetBool::Request> request,
+                                 std::shared_ptr<GetBool::Response> response) {
+        (void)request_header;
+        getLaserStatusCallback(request, response);
+      });
   get_white_balance_srv_ = node_->create_service<GetInt32>(
       "get_white_balance", [this](const std::shared_ptr<GetInt32::Request> request,
                                   std::shared_ptr<GetInt32::Response> response) {
@@ -635,6 +648,23 @@ void OBCameraNode::setMirrorCallback(const std::shared_ptr<SetBool::Request>& re
 void OBCameraNode::getLdpStatusCallback(const std::shared_ptr<GetBool::Request>& request,
                                         std::shared_ptr<GetBool::Response>& response) {
   (void)request;
+  try {
+    response->data = device_->getBoolProperty(OB_PROP_LDP_BOOL);
+    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::getLdpProtectionStatusCallback(const std::shared_ptr<GetBool::Request>& request,
+                                                  std::shared_ptr<GetBool::Response>& response) {
+  (void)request;
   try {
     response->data = device_->getBoolProperty(OB_PROP_LDP_STATUS_BOOL);
     response->success = true;
@@ -649,7 +679,27 @@ void OBCameraNode::getLdpStatusCallback(const std::shared_ptr<GetBool::Request>&
     response->success = false;
   }
 }
-
+void OBCameraNode::getLaserStatusCallback(const std::shared_ptr<GetBool::Request>& request,
+                                          std::shared_ptr<GetBool::Response>& response) {
+  (void)request;
+  try {
+    if (device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
+      response->data = device_->getBoolProperty(OB_PROP_LASER_CONTROL_INT);
+    } else if (device_->isPropertySupported(OB_PROP_LASER_BOOL, OB_PERMISSION_READ_WRITE)) {
+      response->data = device_->getBoolProperty(OB_PROP_LASER_BOOL);
+    }
+    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::getLdpMeasureDistanceCallback(const std::shared_ptr<GetInt32::Request>& request,
                                                  std::shared_ptr<GetInt32::Response>& response) {
   (void)request;