From a9e3342d2c3f0d9c2cac507c6c51973739ce5ae6 Mon Sep 17 00:00:00 2001 From: Joe Dong Date: Tue, 7 Jun 2022 21:21:43 +0800 Subject: [PATCH] add get device info srv --- .../include/orbbec_camera/ob_camera_node.h | 10 +++++-- orbbec_camera/src/device_ctrl_service.cpp | 29 +++++++++++++++++++ orbbec_camera/src/ob_camera_node_factory.cpp | 14 +++++---- orbbec_camera_msgs/CMakeLists.txt | 2 +- orbbec_camera_msgs/msg/DeviceInfo.msg | 3 ++ orbbec_camera_msgs/srv/GetDeviceInfo.srv | 4 +++ orbbec_camera_msgs/srv/GetDeviceList.srv | 2 -- 7 files changed, 52 insertions(+), 12 deletions(-) create mode 100644 orbbec_camera_msgs/srv/GetDeviceInfo.srv delete mode 100644 orbbec_camera_msgs/srv/GetDeviceList.srv diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index cf7190c2..5929a0f0 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/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& request, std::shared_ptr& response); + void getDeviceInfoCallback(const std::shared_ptr& request_header, + const std::shared_ptr& request, + std::shared_ptr& response); + void publishPointCloud(std::shared_ptr frame_set, const rclcpp::Time& t); void publishDepthPointCloud(std::shared_ptr frame_set, const rclcpp::Time& t); @@ -249,7 +253,7 @@ class OBCameraNode { OBD2CTransform extrinsics_; rclcpp::Service::SharedPtr toggle_sensors_srv_; orbbec_camera_msgs::msg::DeviceInfo device_info_; - rclcpp::Service::SharedPtr get_device_list_srv; + rclcpp::Service::SharedPtr get_device_srv_; rclcpp::Service::SharedPtr set_laser_enable_srv_; rclcpp::Service::SharedPtr set_ldp_enable_srv_; rclcpp::Service::SharedPtr set_floor_enable_srv_; diff --git a/orbbec_camera/src/device_ctrl_service.cpp b/orbbec_camera/src/device_ctrl_service.cpp index 352126c6..c79f6c2a 100644 --- a/orbbec_camera/src/device_ctrl_service.cpp +++ b/orbbec_camera/src/device_ctrl_service.cpp @@ -93,6 +93,12 @@ void OBCameraNode::setupCameraCtrlServices() { std::shared_ptr response) { setWhiteBalanceCallback(request_header, request, response); }); + get_device_srv_ = node_->create_service( + "get_device_info", [this](const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) { + getDeviceInfoCallback(request_header, request, response); + }); } void OBCameraNode::setExposureCallback(const std::shared_ptr& request, @@ -364,4 +370,27 @@ void OBCameraNode::getExposureCallback(const std::shared_ptr& response->message = "unknown error"; } } +void OBCameraNode::getDeviceInfoCallback(const std::shared_ptr& request_header, + const std::shared_ptr& request, + std::shared_ptr& 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 diff --git a/orbbec_camera/src/ob_camera_node_factory.cpp b/orbbec_camera/src/ob_camera_node_factory.cpp index 1b84c033..26305643 100644 --- a/orbbec_camera/src/ob_camera_node_factory.cpp +++ b/orbbec_camera/src/ob_camera_node_factory.cpp @@ -94,13 +94,15 @@ void OBCameraNodeFactory::getDevice(const std::shared_ptr &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_, "get device pid " << dev->getDeviceInfo()->pid()); - RCLCPP_INFO_STREAM(logger_, "get device vid " << dev->getDeviceInfo()->vid()); + RCLCPP_INFO_STREAM(logger_, "device name " << dev->getDeviceInfo()->name()); + RCLCPP_INFO_STREAM(logger_, "device pid " << dev->getDeviceInfo()->pid()); + 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()); - RCLCPP_INFO_STREAM(logger_, - "get device firmware version " << dev->getDeviceInfo()->firmwareVersion()); + "device firmware version " << dev->getDeviceInfo()->firmwareVersion()); + RCLCPP_INFO_STREAM(logger_, "device supported min sdk version " + << dev->getDeviceInfo()->supportedMinSdkVersion()); + RCLCPP_INFO_STREAM(logger_, "device hardware version " << dev->getDeviceInfo()->hardwareVersion()); } } } diff --git a/orbbec_camera_msgs/CMakeLists.txt b/orbbec_camera_msgs/CMakeLists.txt index 42af66e7..b9945d8c 100644 --- a/orbbec_camera_msgs/CMakeLists.txt +++ b/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" diff --git a/orbbec_camera_msgs/msg/DeviceInfo.msg b/orbbec_camera_msgs/msg/DeviceInfo.msg index fd93f554..ac1bbe5f 100644 --- a/orbbec_camera_msgs/msg/DeviceInfo.msg +++ b/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 diff --git a/orbbec_camera_msgs/srv/GetDeviceInfo.srv b/orbbec_camera_msgs/srv/GetDeviceInfo.srv new file mode 100644 index 00000000..be08a553 --- /dev/null +++ b/orbbec_camera_msgs/srv/GetDeviceInfo.srv @@ -0,0 +1,4 @@ +--- +DeviceInfo info +bool success +string message \ No newline at end of file diff --git a/orbbec_camera_msgs/srv/GetDeviceList.srv b/orbbec_camera_msgs/srv/GetDeviceList.srv deleted file mode 100644 index 6c67e83b..00000000 --- a/orbbec_camera_msgs/srv/GetDeviceList.srv +++ /dev/null @@ -1,2 +0,0 @@ ---- -DeviceInfo[] dev_infos \ No newline at end of file