catch error when ctrl camera

orbbec_camera/include/orbbec_camera/constants.h

@@ -1,6 +1,6 @@
 /**************************************************************************/
 /*                                                                        */
-/* Copyright (c) 2013-2021 Orbbec 3D Technology, Inc                      */
+/* Copyright (c) 2013-2022 Orbbec 3D Technology, Inc                      */
 /*                                                                        */
 /* PROPRIETARY RIGHTS of Orbbec 3D Technology are involved in the         */
 /* subject matter of this material. All manufacturing, reproduction, use, */
@@ -29,13 +29,6 @@
 #define OB_ROS_VERSION_STR \
   (VAR_ARG_STRING(OB_ROS_MAJOR_VERSION.OB_ROS_MINOR_VERSION.OB_ROS_PATCH_VERSION))
 
-#define ROS_DEBUG(...) RCLCPP_DEBUG(_logger, __VA_ARGS__)
-#define ROS_INFO(...) RCLCPP_INFO(_logger, __VA_ARGS__)
-#define ROS_WARN(...) RCLCPP_WARN(_logger, __VA_ARGS__)
-#define ROS_ERROR(...) RCLCPP_ERROR(_logger, __VA_ARGS__)
-
-#
-
 namespace orbbec_camera {
 
 const bool ALIGN_DEPTH = false;

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -1,6 +1,6 @@
 /**************************************************************************/
 /*                                                                        */
-/* Copyright (c) 2013-2021 Orbbec 3D Technology, Inc                      */
+/* Copyright (c) 2013-2022 Orbbec 3D Technology, Inc                      */
 /*                                                                        */
 /* PROPRIETARY RIGHTS of Orbbec 3D Technology are involved in the         */
 /* subject matter of this material. All manufacturing, reproduction, use, */

orbbec_camera/include/orbbec_camera/ob_camera_node_factory.h

@@ -1,3 +1,14 @@
+/**************************************************************************/
+/*                                                                        */
+/* Copyright (c) 2013-2022 Orbbec 3D Technology, Inc                      */
+/*                                                                        */
+/* PROPRIETARY RIGHTS of Orbbec 3D Technology are involved in the         */
+/* subject matter of this material. All manufacturing, reproduction, use, */
+/* and sales rights pertaining to this subject matter are governed by the */
+/* license agreement. The recipient of this software implicitly accepts   */
+/* the terms of the license.                                              */
+/*                                                                        */
+/**************************************************************************/
 #pragma once
 #include <atomic>
 #include <thread>

orbbec_camera/scripts/echo_extrintisics.py

@@ -1,18 +1,14 @@
-import imp
 from orbbec_camera_msgs.msg import Extrinsics
 from rclpy.node import Node
 from rclpy.qos import qos_profile_system_default
 import rclpy
 
 
-class EchoExtrinsics(Node):
+class TestNode(Node):
     def __init__(self):
-        super().__init__("echo_extrinsics")
+        super().__init__("test_node")
         self.subscription = self.create_subscription(
-            Extrinsics,
+            Extrinsics, "/camera/extrinsics", self.callback, qos_profile_system_default
-            "/camera/extrinsics",
-            self.callback,
-            qos_profile_system_default
         )
 
     def callback(self, msg: Extrinsics):
@@ -26,7 +22,11 @@ class EchoExtrinsics(Node):
 
 
 def main(args=None):
-    rclpy.init()
+    rclpy.init(args=args)
+    test_node = TestNode()
+    rclpy.spin(test_node)
+    test_node.destry_node()
+    rclpy.shutdown()
 
 
 if __name__ == "__main__":

orbbec_camera/scripts/set_camera_exposure.py

@@ -0,0 +1,49 @@
+#!/usr/bin/python3
+
+from cgi import test
+import sys
+from urllib import response
+from orbbec_camera_msgs.srv import SetInt32
+from rclpy.node import Node
+from rclpy.qos import qos_profile_system_default
+import rclpy
+
+
+class TestNode(Node):
+    def __init__(self):
+        super().__init__("test_node")
+        sensor_name = str(sys.argv[1])
+        assert not sensor_name is None
+        self.cli = self.create_client(
+            SetInt32, "/camera/set/" + sensor_name + "/exposure"
+        )
+        while not self.cli.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("service not available, waiting again...")
+        self.req = SetInt32.Request()
+
+    def send_request(self):
+        self.req.data = int(sys.argv[2])
+        self.future = self.cli.call_async(self.req)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    test_node = TestNode()
+    test_node.send_request()
+    while rclpy.ok():
+        rclpy.spin_once(test_node)
+        if test_node.future.done():
+            try:
+                response = test_node.future.result()
+            except Exception as e:
+                test_node.get_logger().info("Service call failed %r" % (e,))
+        else:
+            test_node.get_logger().info("set camera exposure success")
+            break
+
+    test_node.destry_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

orbbec_camera/scripts/set_camera_gain.py

@@ -0,0 +1,49 @@
+#!/usr/bin/python3
+
+from cgi import test
+import sys
+from urllib import response
+from orbbec_camera_msgs.srv import SetInt32
+from rclpy.node import Node
+from rclpy.qos import qos_profile_system_default
+import rclpy
+
+
+class TestNode(Node):
+    def __init__(self):
+        super().__init__("test_node")
+        sensor_name = str(sys.argv[1])
+        assert not sensor_name is None
+        self.cli = self.create_client(
+            SetInt32, "/camera/set/" + sensor_name + "/gain"
+        )
+        while not self.cli.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("service not available, waiting again...")
+        self.req = SetInt32.Request()
+
+    def send_request(self):
+        self.req.data = int(sys.argv[2])
+        self.future = self.cli.call_async(self.req)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    test_node = TestNode()
+    test_node.send_request()
+    while rclpy.ok():
+        rclpy.spin_once(test_node)
+        if test_node.future.done():
+            try:
+                response = test_node.future.result()
+            except Exception as e:
+                test_node.get_logger().info("Service call failed %r" % (e,))
+        else:
+            test_node.get_logger().info("set camera gain success")
+            break
+
+    test_node.destry_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

orbbec_camera/src/device_ctrl_service.cpp

@@ -9,7 +9,7 @@ void OBCameraNode::setupCameraCtrlServices() {
   for (auto stream_index : IMAGE_STREAMS) {
     auto stream_name = stream_name_[stream_index.first];
     if (enable_[stream_index]) {
-      std::string service_name = "get/" + stream_name + "/exposure";
+      std::string service_name = "get_" + stream_name + "_exposure";
       get_exposure_srv_[stream_index] = node_->create_service<GetInt32>(
           service_name, [this, stream_index = stream_index](
                             const std::shared_ptr<rmw_request_id_t> request_header,
@@ -18,7 +18,7 @@ void OBCameraNode::setupCameraCtrlServices() {
             getExposureCallback(request, response, stream_index);
           });
 
-      service_name = "set/" + stream_name + "/exposure";
+      service_name = "set_" + stream_name + "_exposure";
       set_exposure_srv_[stream_index] = node_->create_service<SetInt32>(
           service_name, [this, stream_index = stream_index](
                             const std::shared_ptr<rmw_request_id_t> request_header,
@@ -26,7 +26,7 @@ void OBCameraNode::setupCameraCtrlServices() {
                             std::shared_ptr<SetInt32::Response> response) {
             setExposureCallback(request, response, stream_index);
           });
-      service_name = "get/" + stream_name + "/gain";
+      service_name = "get_" + stream_name + "_gain";
       get_gain_srv_[stream_index] = node_->create_service<GetInt32>(
           service_name, [this, stream_index = stream_index](
                             const std::shared_ptr<rmw_request_id_t> request_header,
@@ -35,7 +35,7 @@ void OBCameraNode::setupCameraCtrlServices() {
             getGainCallback(request, response, stream_index);
           });
 
-      service_name = "set/" + stream_name + "/gain";
+      service_name = "set_" + stream_name + "_gain";
       set_gain_srv_[stream_index] = node_->create_service<SetInt32>(
           service_name, [this, stream_index = stream_index](
                             const std::shared_ptr<rmw_request_id_t> request_header,
@@ -44,9 +44,7 @@ void OBCameraNode::setupCameraCtrlServices() {
             setGainCallback(request, response, stream_index);
           });
       if (stream_index.first == OB_STREAM_COLOR || stream_index.first == OB_STREAM_DEPTH) {
-        service_name = "set/" + stream_name +
+        service_name = "set_" + stream_name + "_auto_exposure";
-                       "/"
-                       "auto_exposure";
         set_auto_exposure_srv_[stream_index] = node_->create_service<SetBool>(
             service_name, [this, stream_index = stream_index](
                               const std::shared_ptr<rmw_request_id_t> request_header,
@@ -59,40 +57,40 @@ void OBCameraNode::setupCameraCtrlServices() {
   }
   set_fan_mode_srv_ = node_->create_service<SetInt32>(
       "set_fan_mode", [this](const std::shared_ptr<rmw_request_id_t> request_header,
-                              const std::shared_ptr<SetInt32::Request> request,
+                             const std::shared_ptr<SetInt32::Request> request,
-                              std::shared_ptr<SetInt32::Response> response) {
+                             std::shared_ptr<SetInt32::Response> response) {
         setFanModeCallback(request_header, request, response);
       });
   set_floor_enable_srv_ = node_->create_service<SetBool>(
       "set_floor_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
-                                  const std::shared_ptr<SetBool::Request> request,
+                                 const std::shared_ptr<SetBool::Request> request,
-                                  std::shared_ptr<SetBool::Response> response) {
+                                 std::shared_ptr<SetBool::Response> response) {
         setFloorEnableCallback(request_header, request, response);
       });
   set_laser_enable_srv_ = node_->create_service<SetBool>(
       "set_laser_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
-                                  const std::shared_ptr<SetBool::Request> request,
+                                 const std::shared_ptr<SetBool::Request> request,
-                                  std::shared_ptr<SetBool::Response> response) {
+                                 std::shared_ptr<SetBool::Response> response) {
         setLaserEnableCallback(request_header, request, response);
       });
   set_ldp_enable_srv_ = node_->create_service<SetBool>(
       "set_ldp_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
-                                const std::shared_ptr<SetBool::Request> request,
+                               const std::shared_ptr<SetBool::Request> request,
-                                std::shared_ptr<SetBool::Response> response) {
+                               std::shared_ptr<SetBool::Response> response) {
         setLdpEnableCallback(request_header, request, response);
       });
 
   get_white_balance_srv_ = node_->create_service<GetInt32>(
-      "get/white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
+      "get_white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
-                                   const std::shared_ptr<GetInt32::Request> request,
+                                  const std::shared_ptr<GetInt32::Request> request,
-                                   std::shared_ptr<GetInt32::Response> response) {
+                                  std::shared_ptr<GetInt32::Response> response) {
         getWhiteBalanceCallback(request_header, request, response);
       });
 
   set_white_balance_srv_ = node_->create_service<SetInt32>(
-      "set/white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
+      "set_white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
-                                   const std::shared_ptr<SetInt32::Request> request,
+                                  const std::shared_ptr<SetInt32::Request> request,
-                                   std::shared_ptr<SetInt32::Response> response) {
+                                  std::shared_ptr<SetInt32::Response> response) {
         setWhiteBalanceCallback(request_header, request, response);
       });
 }
@@ -101,19 +99,28 @@ void OBCameraNode::setExposureCallback(const std::shared_ptr<SetInt32::Request>&
                                        std::shared_ptr<SetInt32::Response>& response,
                                        const stream_index_pair& stream_index) {
   auto stream = stream_index.first;
-  switch (stream) {
+  try {
-    case OB_STREAM_IR:
+    switch (stream) {
-      device_->setIntProperty(OB_PROP_IR_EXPOSURE_INT, request->data);
+      case OB_STREAM_IR:
-      break;
+        device_->setIntProperty(OB_PROP_IR_EXPOSURE_INT, request->data);
-    case OB_STREAM_DEPTH:
+        break;
-      device_->setIntProperty(OB_PROP_DEPTH_EXPOSURE_INT, request->data);
+      case OB_STREAM_DEPTH:
-      break;
+        device_->setIntProperty(OB_PROP_DEPTH_EXPOSURE_INT, request->data);
-    case OB_STREAM_COLOR:
+        break;
-      device_->setIntProperty(OB_PROP_COLOR_EXPOSURE_INT, request->data);
+      case OB_STREAM_COLOR:
-      break;
+        device_->setIntProperty(OB_PROP_COLOR_EXPOSURE_INT, request->data);
-    default:
+        break;
-      RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
+      default:
-      break;
+        RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
+        break;
+    }
+  } catch (const ob::Error& e) {
+    response->message = e.getMessage();
+  } catch (const std::exception& e) {
+    response->message = e.what();
+  } catch (...) {
+    RCLCPP_ERROR(logger_, "%s unknown error %d", __FUNCTION__, __LINE__);
+    response->message = "unknown error";
   }
 }
 
@@ -121,19 +128,27 @@ void OBCameraNode::getGainCallback(const std::shared_ptr<GetInt32::Request>& req
                                    std::shared_ptr<GetInt32::Response>& response,
                                    const stream_index_pair& stream_index) {
   auto stream = stream_index.first;
-  switch (stream) {
+  try {
-    case OB_STREAM_IR:
+    switch (stream) {
-      response->data = device_->getIntProperty(OB_PROP_IR_GAIN_INT);
+      case OB_STREAM_IR:
-      break;
+        response->data = device_->getIntProperty(OB_PROP_IR_GAIN_INT);
-    case OB_STREAM_DEPTH:
+        break;
-      response->data = device_->getIntProperty(OB_PROP_DEPTH_GAIN_INT);
+      case OB_STREAM_DEPTH:
-      break;
+        response->data = device_->getIntProperty(OB_PROP_DEPTH_GAIN_INT);
-    case OB_STREAM_COLOR:
+        break;
-      response->data = device_->getIntProperty(OB_PROP_COLOR_GAIN_INT);
+      case OB_STREAM_COLOR:
-      break;
+        response->data = device_->getIntProperty(OB_PROP_COLOR_GAIN_INT);
-    default:
+        break;
-      RCLCPP_ERROR(logger_, " %s NOT a video stream", __FUNCTION__);
+      default:
-      break;
+        RCLCPP_ERROR(logger_, " %s NOT a video stream", __FUNCTION__);
+        break;
+    }
+  } catch (ob::Error& e) {
+    response->message = e.getMessage();
+  } catch (const std::exception& e) {
+    response->message = e.what();
+  } catch (...) {
+    response->message = "unknown error";
   }
 }
 
@@ -141,32 +156,56 @@ void OBCameraNode::setGainCallback(const std::shared_ptr<SetInt32 ::Request>& re
                                    std::shared_ptr<SetInt32::Response>& response,
                                    const stream_index_pair& stream_index) {
   auto stream = stream_index.first;
-  switch (stream) {
+  try {
-    case OB_STREAM_IR:
+    switch (stream) {
-      device_->setIntProperty(OB_PROP_IR_GAIN_INT, request->data);
+      case OB_STREAM_IR:
-      break;
+        device_->setIntProperty(OB_PROP_IR_GAIN_INT, request->data);
-    case OB_STREAM_DEPTH:
+        break;
-      device_->setIntProperty(OB_PROP_DEPTH_GAIN_INT, request->data);
+      case OB_STREAM_DEPTH:
-      break;
+        device_->setIntProperty(OB_PROP_DEPTH_GAIN_INT, request->data);
-    case OB_STREAM_COLOR:
+        break;
-      device_->setIntProperty(OB_PROP_COLOR_GAIN_INT, request->data);
+      case OB_STREAM_COLOR:
-      break;
+        device_->setIntProperty(OB_PROP_COLOR_GAIN_INT, request->data);
-    default:
+        break;
-      RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
+      default:
-      break;
+        RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
+        break;
+    }
+  } catch (const ob::Error& e) {
+    response->message = e.getMessage();
+  } catch (const std::exception& e) {
+    response->message = e.what();
+  } catch (...) {
+    response->message = "unknown error";
   }
 }
 
 void OBCameraNode::getWhiteBalanceCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
                                            const std::shared_ptr<GetInt32::Request>& request,
                                            std::shared_ptr<GetInt32::Response>& response) {
-  response->data = device_->getIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT);
+  try {
+    response->data = device_->getIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT);
+  } catch (const ob::Error& e) {
+    response->message = e.getMessage();
+  } catch (const std::exception& e) {
+    response->message = e.what();
+  } catch (...) {
+    response->message = "unknown error";
+  }
 }
 
 void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
                                            const std::shared_ptr<SetInt32 ::Request>& request,
                                            std::shared_ptr<SetInt32 ::Response>& response) {
-  device_->setIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT, request->data);
+  try {
+    device_->setIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT, request->data);
+  } catch (const ob::Error& e) {
+    response->message = e.getMessage();
+  } catch (const std::exception& e) {
+    response->message = e.what();
+  } catch (...) {
+    response->message = "unknown error";
+  }
 }
 
 void OBCameraNode::setAutoExposureCallback(
@@ -174,19 +213,27 @@ void OBCameraNode::setAutoExposureCallback(
     std::shared_ptr<std_srvs::srv::SetBool::Response>& response,
     const stream_index_pair& stream_index) {
   auto stream = stream_index.first;
-  switch (stream) {
+  try {
-    case OB_STREAM_IR:
+    switch (stream) {
-      response->message = "IR not support set auto exposure";
+      case OB_STREAM_IR:
-      break;
+        response->message = "IR not support set auto exposure";
-    case OB_STREAM_DEPTH:
+        break;
-      device_->setIntProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_BOOL, request->data);
+      case OB_STREAM_DEPTH:
-      break;
+        device_->setIntProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_BOOL, request->data);
-    case OB_STREAM_COLOR:
+        break;
-      device_->setIntProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, request->data);
+      case OB_STREAM_COLOR:
-      break;
+        device_->setIntProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, request->data);
-    default:
+        break;
-      RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
+      default:
-      break;
+        RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
+        break;
+    }
+  } catch (const ob::Error& e) {
+    response->message = e.getMessage();
+  } catch (const std::exception& e) {
+    response->message = e.what();
+  } catch (...) {
+    response->message = "unknown error";
   }
 }
 
@@ -196,7 +243,15 @@ void OBCameraNode::setFanModeCallback(const std::shared_ptr<rmw_request_id_t>& r
   (void)request_header;
   (void)response;
   bool fan_mode = request->data;
-  device_->setBoolProperty(OB_PROP_FAN_WORK_MODE_INT, fan_mode);
+  try {
+    device_->setBoolProperty(OB_PROP_FAN_WORK_MODE_INT, fan_mode);
+  } catch (const ob::Error& e) {
+    response->message = e.getMessage();
+  } catch (const std::exception& e) {
+    response->message = e.what();
+  } catch (...) {
+    response->message = "unknown error";
+  }
 }
 
 void OBCameraNode::setFloorEnableCallback(
@@ -206,7 +261,15 @@ void OBCameraNode::setFloorEnableCallback(
   (void)request_header;
   (void)response;
   bool floor_enable = request->data;
-  device_->setBoolProperty(OB_PROP_FLOOD_BOOL, floor_enable);
+  try {
+    device_->setBoolProperty(OB_PROP_FLOOD_BOOL, floor_enable);
+  } catch (const ob::Error& e) {
+    response->message = e.getMessage();
+  } catch (const std::exception& e) {
+    response->message = e.what();
+  } catch (...) {
+    response->message = "unknown error";
+  }
 }
 
 void OBCameraNode::setLaserEnableCallback(
@@ -216,7 +279,15 @@ void OBCameraNode::setLaserEnableCallback(
   (void)request_header;
   (void)response;
   bool laser_enable = request->data;
-  device_->setBoolProperty(OB_PROP_LASER_BOOL, laser_enable);
+  try {
+    device_->setBoolProperty(OB_PROP_LASER_BOOL, laser_enable);
+  } catch (const ob::Error& e) {
+    response->message = e.getMessage();
+  } catch (const std::exception& e) {
+    response->message = e.what();
+  } catch (...) {
+    response->message = "unknown error";
+  }
 }
 
 void OBCameraNode::setLdpEnableCallback(
@@ -226,25 +297,41 @@ void OBCameraNode::setLdpEnableCallback(
   (void)request_header;
   (void)response;
   bool ldp_enable = request->data;
-  device_->setBoolProperty(OB_PROP_LDP_BOOL, ldp_enable);
+  try {
+    device_->setBoolProperty(OB_PROP_LDP_BOOL, ldp_enable);
+  } catch (const ob::Error& e) {
+    response->message = e.getMessage();
+  } catch (const std::exception& e) {
+    response->message = e.what();
+  } catch (...) {
+    response->message = "unknown error";
+  }
 }
 void OBCameraNode::getExposureCallback(const std::shared_ptr<GetInt32::Request>& request,
                                        std::shared_ptr<GetInt32 ::Response>& response,
                                        const stream_index_pair& stream_index) {
   auto stream = stream_index.first;
-  switch (stream) {
+  try {
-    case OB_STREAM_IR:
+    switch (stream) {
-      response->data = device_->getIntProperty(OB_PROP_IR_EXPOSURE_INT);
+      case OB_STREAM_IR:
-      break;
+        response->data = device_->getIntProperty(OB_PROP_IR_EXPOSURE_INT);
-    case OB_STREAM_DEPTH:
+        break;
-      response->data = device_->getIntProperty(OB_PROP_DEPTH_EXPOSURE_INT);
+      case OB_STREAM_DEPTH:
-      break;
+        response->data = device_->getIntProperty(OB_PROP_DEPTH_EXPOSURE_INT);
-    case OB_STREAM_COLOR:
+        break;
-      response->data = device_->getIntProperty(OB_PROP_COLOR_EXPOSURE_INT);
+      case OB_STREAM_COLOR:
-      break;
+        response->data = device_->getIntProperty(OB_PROP_COLOR_EXPOSURE_INT);
-    default:
+        break;
-      RCLCPP_ERROR(logger_, " %s NOT a video stream", __FUNCTION__);
+      default:
-      break;
+        RCLCPP_ERROR(logger_, " %s NOT a video stream", __FUNCTION__);
+        break;
+    }
+  } catch (const ob::Error& e) {
+    response->message = e.getMessage();
+  } catch (const std::exception& e) {
+    response->message = e.what();
+  } catch (...) {
+    response->message = "unknown error";
   }
 }
 }  // namespace orbbec_camera