add get device info srv

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -37,7 +37,7 @@
 #include "libobsensor/ObSensor.hpp"
 
 #include "orbbec_camera_msgs/msg/device_info.hpp"
-#include "orbbec_camera_msgs/srv/get_device_list.hpp"
+#include "orbbec_camera_msgs/srv/get_device_info.hpp"
 #include "orbbec_camera_msgs/msg/extrinsics.hpp"
 #include "orbbec_camera_msgs/msg/metadata.hpp"
 #include "orbbec_camera_msgs/srv/get_int32.hpp"
@@ -70,7 +70,7 @@
       .str()
 
 namespace orbbec_camera {
-using GetDeviceList = orbbec_camera_msgs::srv::GetDeviceList;
+using GetDeviceInfo = orbbec_camera_msgs::srv::GetDeviceInfo;
 using Extrinsics = orbbec_camera_msgs::msg::Extrinsics;
 using SetInt32 = orbbec_camera_msgs::srv::SetInt32;
 using GetInt32 = orbbec_camera_msgs::srv::GetInt32;
@@ -174,6 +174,10 @@ class OBCameraNode {
                           const std::shared_ptr<SetInt32::Request>& request,
                           std::shared_ptr<SetInt32::Response>& response);
 
+  void getDeviceInfoCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
+                             const std::shared_ptr<GetDeviceInfo::Request>& request,
+                             std::shared_ptr<GetDeviceInfo::Response>& response);
+
   void publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set, const rclcpp::Time& t);
 
   void publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_set, const rclcpp::Time& t);
@@ -249,7 +253,7 @@ class OBCameraNode {
   OBD2CTransform extrinsics_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_sensors_srv_;
   orbbec_camera_msgs::msg::DeviceInfo device_info_;
-  rclcpp::Service<GetDeviceList>::SharedPtr get_device_list_srv;
+  rclcpp::Service<GetDeviceInfo>::SharedPtr get_device_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_laser_enable_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ldp_enable_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_floor_enable_srv_;

orbbec_camera/src/device_ctrl_service.cpp

@@ -93,6 +93,12 @@ void OBCameraNode::setupCameraCtrlServices() {
                                   std::shared_ptr<SetInt32::Response> response) {
         setWhiteBalanceCallback(request_header, request, response);
       });
+  get_device_srv_ = node_->create_service<GetDeviceInfo>(
+      "get_device_info", [this](const std::shared_ptr<rmw_request_id_t> request_header,
+                                const std::shared_ptr<GetDeviceInfo::Request> request,
+                                std::shared_ptr<GetDeviceInfo::Response> response) {
+        getDeviceInfoCallback(request_header, request, response);
+      });
 }
 
 void OBCameraNode::setExposureCallback(const std::shared_ptr<SetInt32::Request>& request,
@@ -364,4 +370,27 @@ void OBCameraNode::getExposureCallback(const std::shared_ptr<GetInt32::Request>&
     response->message = "unknown error";
   }
 }
+void OBCameraNode::getDeviceInfoCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
+                                         const std::shared_ptr<GetDeviceInfo::Request>& request,
+                                         std::shared_ptr<GetDeviceInfo::Response>& response) {
+  try {
+    auto device_info = device_->getDeviceInfo();
+    response->info.name = device_info->name();
+    response->info.pid = device_info->pid();
+    response->info.vid = device_info->vid();
+    response->info.serial_number = device_info->serialNumber();
+    response->info.firmware_version = device_info->firmwareVersion();
+    response->info.supported_min_sdk_version = device_info->supportedMinSdkVersion();
+    response->success = true;
+  } 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";
+  }
+}
 }  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node_factory.cpp

@@ -94,13 +94,15 @@ void OBCameraNodeFactory::getDevice(const std::shared_ptr<ob::DeviceList> &list)
     auto dev = list->getDevice(i);
     if (dev != nullptr) {
       device_ = dev;
-      RCLCPP_INFO_STREAM(logger_, "get device name " << dev->getDeviceInfo()->name());
+      RCLCPP_INFO_STREAM(logger_, "device name " << dev->getDeviceInfo()->name());
-      RCLCPP_INFO_STREAM(logger_, "get device pid " << dev->getDeviceInfo()->pid());
+      RCLCPP_INFO_STREAM(logger_, "device pid " << dev->getDeviceInfo()->pid());
-      RCLCPP_INFO_STREAM(logger_, "get device vid " << dev->getDeviceInfo()->vid());
+      RCLCPP_INFO_STREAM(logger_, "device vid " << dev->getDeviceInfo()->vid());
+      RCLCPP_INFO_STREAM(logger_, "device serial_name " << dev->getDeviceInfo()->serialNumber());
       RCLCPP_INFO_STREAM(logger_,
-                         "get device serial_name " << dev->getDeviceInfo()->serialNumber());
+                         "device firmware version " << dev->getDeviceInfo()->firmwareVersion());
-      RCLCPP_INFO_STREAM(logger_,
+      RCLCPP_INFO_STREAM(logger_, "device supported min sdk version "
-                         "get device firmware version " << dev->getDeviceInfo()->firmwareVersion());
+                                      << dev->getDeviceInfo()->supportedMinSdkVersion());
+      RCLCPP_INFO_STREAM(logger_, "device hardware version " << dev->getDeviceInfo()->hardwareVersion());
     }
   }
 }

orbbec_camera_msgs/CMakeLists.txt

@@ -18,7 +18,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/DeviceInfo.msg"
   "msg/Extrinsics.msg"
   "msg/Metadata.msg"
-  "srv/GetDeviceList.srv"
+  "srv/GetDeviceInfo.srv"
   "srv/GetCameraInfo.srv"
   "srv/GetInt32.srv"
   "srv/SetInt32.srv"

orbbec_camera_msgs/msg/DeviceInfo.msg

@@ -3,3 +3,6 @@ string name
 int32 vid
 int32 pid
 string serial_number
+string firmware_version
+string supported_min_sdk_version
+string hardware_version

orbbec_camera_msgs/srv/GetDeviceInfo.srv

@@ -0,0 +1,4 @@
+---
+DeviceInfo info
+bool success
+string message

orbbec_camera_msgs/srv/GetDeviceList.srv

@@ -1,2 +0,0 @@
----
-DeviceInfo[] dev_infos