Merge feature/435le_new_interface_example

docs/all_available_service_for_camera_control.md

@@ -352,3 +352,57 @@ ros2 service call /camera/set_sync_hosttime std_srvs/srv/SetBool '{data: true}'
 ```
 ros2 service call /camera/send_software_trigger std_srvs/srv/SetBool '{data: true}'
 ```
+
+- `/camera/set_streams_enable`
+
+```bash
+ros2 service call /camera/set_streams_enable std_srvs/srv/SetBool '{data: false}'
+```
+
+- `/camera/get_streams_enable`
+
+```bash
+ros2 service call /camera/get_streams_enable std_srvs/srv/GetBool '{}'
+```
+
+- `/camera/write_customer_data`
+
+```bash
+ros2 service call /camera/write_customer_data orbbec_camera_msgs/srv/SetString '{data: "string"}'
+```
+
+- `/camera/read_customer_data`
+
+```bash
+ros2 service call /camera/read_customer_data orbbec_camera_msgs/srv/GetString '{}'
+```
+
+- `/camera/set_user_calib_params`
+
+```bash
+ros2 service call /camera/set_user_calib_params orbbec_camera_msgs/srv/SetUserCalibParams \
+'{k: [614.9613647460938, 0.0, 634.91552734375,
+      0.0, 614.65771484375, 391.407470703125,
+      0.0, 0.0, 1.0],
+  d: [-0.03131488710641861,
+       0.032955970615148544,
+       9.096559369936585e-05,
+      -0.0003368517500348389,
+      -0.01115430984646082],
+  rotation: [0.9999880790710449,  0.0003024190664291382, -0.004874417092651129,
+            -0.0002965621242765337, 0.9999992251396179,   0.001202247804030776,
+             0.004874777048826218, -0.0012007878394797444, 0.9999874234199524],
+  translation: [-0.023897956848144532,
+                -9.439220279455185e-05,
+                -6.804073229432106e-06]}'
+```
+
+- `/camera/get_user_calib_params`
+
+```bash
+ros2 service call /camera/get_user_calib_params orbbec_camera_msgs/srv/GetUserCalibParams '{}'
+```
+
+> **Note**:
+>  The following four services (`/camera/write_customer_data`, `/camera/read_customer_data`, `/camera/set_user_calib_params`, `/camera/get_user_calib_params`) are currently supported only on the 435Le module.
+>  Each service can store only one set of data or string at a time.

docs/all_available_topics.md

@@ -19,7 +19,8 @@
 - `/camera/ir/metadata`: The IR stream firmware data.
 - `/camera/accel/sample`: Acceleration data stream `enable_sync_output_accel_gyro`turned off，`enable_accel`turned on
 - `/camera/gyro/sample`: Gyroscope data stream，enable_sync_output_accel_gyro `turned off，`enable_gyro`turned on
-- `camera/gyro_accel/sample`: Synchronized data stream of acceleration and gyroscope，`enable_sync_output_accel_gyro`
+- `/camera/gyro_accel/sample`: Synchronized data stream of acceleration and gyroscope，`enable_sync_output_accel_gyro`
   turned on
 - `/diagnostics`: The diagnostic information of the camera, Currently, the diagnostic information only includes the
   temperature of the camera.
+- `/camera/device_status`: The device status of the camera.

orbbec_camera/CMakeLists.txt

@@ -191,6 +191,7 @@ add_orbbec_executable(list_depth_work_mode_node tools/list_depth_work_mode.cpp)
 add_orbbec_executable(list_camera_profile_mode_node tools/list_camera_profile.cpp)
 add_orbbec_executable(topic_statistics_node tools/topic_statistics.cpp)
 add_orbbec_executable(ob_benchmark_node tools/ob_benchmark.cpp)
+add_orbbec_executable(435le_example_node examples/435le_new_interface/camera_example_node.cpp)
 
 add_library(frame_latency SHARED tools/frame_latency.cpp)
 target_include_directories(frame_latency PUBLIC ${COMMON_INCLUDE_DIRS})
@@ -238,8 +239,7 @@ if(DEFINED ENV{BUILDING_PACKAGE})
     install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/scripts/99-obsensor-libusb.rules DESTINATION /etc/udev/rules.d)
 endif()
 
-install(TARGETS list_devices_node list_depth_work_mode_node list_camera_profile_mode_node topic_statistics_node
-                ob_benchmark_node DESTINATION lib/${PROJECT_NAME}/
+install(TARGETS list_devices_node list_depth_work_mode_node list_camera_profile_mode_node topic_statistics_node ob_benchmark_node 435le_example_node DESTINATION lib/${PROJECT_NAME}/
 )
 
 if(BUILD_TESTING)

orbbec_camera/examples/435le_new_interface/README.MD

@@ -0,0 +1,189 @@
+# 435le\_example\_node
+
+This example node demonstrates how to interact with an Orbbec camera in ROS2 using the Orbbec SDK.
+Users can learn how to retrieve camera information, control camera streams, and read/write camera calibration parameters through this node.
+
+---
+
+## 1. How to Run
+
+After building the workspace, run the example node directly:
+
+```bash
+ros2 run orbbec_camera 435le_example_node
+```
+
+Once successfully started, the following menu will appear. Enter the corresponding number to select a feature:
+
+```bash
+========= Example Menu =========
+1. Write camera parameters
+2. Read camera parameters
+3. Enable camera streams
+4. Disable camera streams
+5. Get device info
+6. Show device status
+0. Exit
+```
+
+---
+
+## 2. Features
+
+### 2.1 Camera Parameters
+
+Used to manage camera calibration parameters, including:
+
+* **k**: Camera intrinsic matrix (3×3)
+* **d**: Distortion coefficients (radial/tangential)
+* **rotation**: Extrinsic rotation matrix (3×3)
+* **translation**: Extrinsic translation vector (3×1)
+
+#### Example Commands
+
+```bash
+# Write parameters
+ros2 service call /camera/set_user_calib_params orbbec_camera_msgs/srv/SetUserCalibParams \
+"{k: [614.9613647460938, 0.0, 634.91552734375,
+      0.0, 614.65771484375, 391.407470703125,
+      0.0, 0.0, 1.0],
+  d: [-0.03131488710641861,
+       0.032955970615148544,
+       9.096559369936585e-05,
+      -0.0003368517500348389,
+      -0.01115430984646082],
+  rotation: [0.9999880790710449,  0.0003024190664291382, -0.004874417092651129,
+            -0.0002965621242765337, 0.9999992251396179,   0.001202247804030776,
+             0.004874777048826218, -0.0012007878394797444, 0.9999874234199524],
+  translation: [-0.023897956848144532,
+                -9.439220279455185e-05,
+                -6.804073229432106e-06]}"
+```
+
+```bash
+# Read parameters
+ros2 service call /camera/get_user_calib_params orbbec_camera_msgs/srv/GetUserCalibParams '{}'
+```
+
+> MD5 verification is performed during both writing and reading to ensure data integrity.
+
+---
+
+### 2.2 Stream Control
+
+The master switch for camera streams allows enabling or disabling all streams during runtime.
+
+#### Example Commands
+
+* **Enable camera streams**:
+
+  ```bash
+  ros2 service call /camera/set_streams_enable std_srvs/srv/SetBool "{data: true}"
+  ```
+* **Disable camera streams**:
+
+  ```bash
+  ros2 service call /camera/set_streams_enable std_srvs/srv/SetBool "{data: false}"
+  ```
+
+- **Get streams status**
+
+  ```bash
+  ros2 service call /camera/get_streams_enable std_srvs/srv/GetBool '{}'
+  ```
+
+### 2.3 Device Info
+
+Get camera device information, including Name, Serial Number, Firmware Version, Supported Minimum SDK Version, Current SDK Version, and Hardware Version.
+
+#### Example Command
+
+```bash
+ros2 service call /camera/get_device_info orbbec_camera_msgs/srv/GetDeviceInfo "{}"
+```
+
+---
+
+### 2.4 Device Status
+
+`/camera/device_status` is a real-time topic provided by the Orbbec camera for monitoring device status and performance metrics.
+
+#### Example Command
+
+```bash
+ros2 topic echo /camera/device_status
+```
+
+---
+
+## 3. Additional Features
+
+### 3.1 Optimized Color Stream Latency
+
+In the previous version, enabling the color point cloud caused noticeable latency in color image publishing.
+This version optimizes data synchronization and publishing, allowing color images and point clouds to correspond more in real time and eliminating visual delays.
+
+---
+
+### 3.2 Added `time_sync_period`
+
+A new parameter **time\_sync\_period** is added to set the time synchronization interval between the camera module and the host.
+
+* Unit: **seconds**
+* Function: The camera aligns its internal timestamp with the host at the specified interval, ensuring consistent timestamps.
+
+---
+
+### 3.3 Added `camera_info_url` and `ir_info_url`
+
+Support specifying calibration file paths through parameters to load external camera intrinsic and extrinsic configurations:
+
+* **camera\_info\_url**: Path to the color camera calibration file
+* **ir\_info\_url**: Path to the IR camera calibration file
+
+> Note: This method does not write the file information to the device.
+
+#### Usage Example
+
+Specify the path in the launch file or when launching:
+
+```launch
+DeclareLaunchArgument('ir_info_url', default_value='file:///home/user/calibration/color.yaml'),
+DeclareLaunchArgument('color_info_url', default_value='file:///home/user/calibration/ir.yaml'),
+```
+
+#### Calibration File Example (YAML Format)
+
+```yaml
+image_width: 1280
+image_height: 800
+camera_name: orbbec_color
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [614.9613647460938, 0.0, 634.91552734375,
+         0.0, 614.65771484375, 391.407470703125,
+         0.0, 0.0, 1.0]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 8
+  data: [-0.03131488710641861,
+          0.032955970615148544,
+          9.096559369936585e-05,
+         -0.0003368517500348389,
+         -0.01115430984646082,
+          0.0, 0.0, 0.0]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.0, 0.0, 0.0,
+         0.0, 1.0, 0.0,
+         0.0, 0.0, 1.0]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [614.9613647460938, 0.0, 634.91552734375, 0.0,
+         0.0, 614.65771484375, 391.407470703125, 0.0,
+         0.0, 0.0, 1.0, 0.0]
+```

orbbec_camera/examples/435le_new_interface/camera_example_node.cpp

@@ -0,0 +1,227 @@
+#include <rclcpp/rclcpp.hpp>
+#include "orbbec_camera_msgs/srv/get_user_calib_params.hpp"
+#include "orbbec_camera_msgs/srv/set_user_calib_params.hpp"
+#include "orbbec_camera_msgs/msg/device_status.hpp"
+#include <orbbec_camera_msgs/srv/get_device_info.hpp>
+#include <std_srvs/srv/set_bool.hpp>
+#include <iostream>
+
+using namespace std::chrono_literals;
+
+class CameraExampleNode : public rclcpp::Node {
+ public:
+  CameraExampleNode() : Node("camera_example_node") {
+    set_params_client_ = this->create_client<orbbec_camera_msgs::srv::SetUserCalibParams>(
+        "/camera/set_user_calib_params");
+    get_params_client_ = this->create_client<orbbec_camera_msgs::srv::GetUserCalibParams>(
+        "/camera/get_user_calib_params");
+    set_streams_client_ = this->create_client<std_srvs::srv::SetBool>("/camera/set_streams_enable");
+    get_device_info_client_ =
+        this->create_client<orbbec_camera_msgs::srv::GetDeviceInfo>("/camera/get_device_info");
+  }
+
+  // Feature 1a: Write camera parameters
+  void exampleWriteParams() {
+    if (!set_params_client_->wait_for_service(2s)) {
+      RCLCPP_ERROR(this->get_logger(), "SetUserCalibParams service not available");
+      return;
+    }
+
+    auto req = std::make_shared<orbbec_camera_msgs::srv::SetUserCalibParams::Request>();
+    req->k = {525.0, 0.0, 319.5, 0.0, 525.0, 239.5, 0.0, 0.0, 1.0};
+    req->d = {0.01, -0.02, 0.0, 0.0, 0.0};
+    req->rotation = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
+    req->translation = {0.0, 0.0, 0.0};
+
+    auto result = set_params_client_->async_send_request(req);
+    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) !=
+        rclcpp::FutureReturnCode::SUCCESS) {
+      RCLCPP_ERROR(this->get_logger(), "Failed to call SetUserCalibParams");
+      return;
+    }
+    RCLCPP_INFO(this->get_logger(), "Set result: %s", result.get()->message.c_str());
+  }
+
+  // Feature 1b: Read camera parameters
+  void exampleReadParams() {
+    if (!get_params_client_->wait_for_service(2s)) {
+      RCLCPP_ERROR(this->get_logger(), "GetUserCalibParams service not available");
+      return;
+    }
+
+    auto req = std::make_shared<orbbec_camera_msgs::srv::GetUserCalibParams::Request>();
+    auto result = get_params_client_->async_send_request(req);
+    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) !=
+        rclcpp::FutureReturnCode::SUCCESS) {
+      RCLCPP_ERROR(this->get_logger(), "Failed to call GetUserCalibParams");
+      return;
+    }
+
+    auto res = result.get();
+    RCLCPP_INFO(this->get_logger(), "Get result: %s", res->message.c_str());
+
+    std::cout << "K: ";
+    for (auto v : res->k) std::cout << v << " ";
+    std::cout << std::endl;
+
+    std::cout << "D: ";
+    for (auto v : res->d) std::cout << v << " ";
+    std::cout << std::endl;
+
+    std::cout << "Rotation: ";
+    for (auto v : res->rotation) std::cout << v << " ";
+    std::cout << std::endl;
+
+    std::cout << "Translation: ";
+    for (auto v : res->translation) std::cout << v << " ";
+    std::cout << std::endl;
+  }
+
+  // Feature 2a: Enable camera streams
+  void exampleEnableStreams() { exampleSetStreamsEnable(true); }
+
+  // Feature 2b: Disable camera streams
+  void exampleDisableStreams() { exampleSetStreamsEnable(false); }
+
+  // Feature 3: Get device info
+  void exampleGetDeviceInfo() {
+    if (!get_device_info_client_->wait_for_service(2s)) {
+      RCLCPP_ERROR(this->get_logger(), "GetDeviceInfo service not available");
+      return;
+    }
+
+    auto req = std::make_shared<orbbec_camera_msgs::srv::GetDeviceInfo::Request>();
+    auto result = get_device_info_client_->async_send_request(req);
+    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) !=
+        rclcpp::FutureReturnCode::SUCCESS) {
+      RCLCPP_ERROR(this->get_logger(), "Failed to call GetDeviceInfo");
+      return;
+    }
+
+    auto res = result.get();
+    if (!res->success) {
+      RCLCPP_ERROR(this->get_logger(), "GetDeviceInfo failed: %s", res->message.c_str());
+      return;
+    }
+
+    std::cout << "Device Info:" << std::endl;
+    std::cout << "Name: " << res->info.name << std::endl;
+    std::cout << "Serial Number: " << res->info.serial_number << std::endl;
+    std::cout << "Firmware Version: " << res->info.firmware_version << std::endl;
+    std::cout << "Supported Min SDK Version: " << res->info.supported_min_sdk_version << std::endl;
+    std::cout << "Current SDK Version: " << res->info.current_sdk_version << std::endl;
+    std::cout << "Hardware Version: " << res->info.hardware_version << std::endl;
+  }
+
+  // Feature 4: Show device status
+  void exampleShowdeviceStatus() {
+    device_status_sub_ = this->create_subscription<orbbec_camera_msgs::msg::DeviceStatus>(
+        "/camera/device_status", 10,
+        std::bind(&CameraExampleNode::deviceStatusCallback, this, std::placeholders::_1));
+
+    while (rclcpp::ok()) {
+      rclcpp::spin_some(shared_from_this());
+    }
+  }
+
+ private:
+  void exampleSetStreamsEnable(bool enable) {
+    if (!set_streams_client_->wait_for_service(2s)) {
+      RCLCPP_ERROR(this->get_logger(), "SetStreamsEnable service not available");
+      return;
+    }
+
+    auto req = std::make_shared<std_srvs::srv::SetBool::Request>();
+    req->data = enable;
+
+    auto result = set_streams_client_->async_send_request(req);
+    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) !=
+        rclcpp::FutureReturnCode::SUCCESS) {
+      RCLCPP_ERROR(this->get_logger(), "Failed to call SetStreamsEnable");
+      return;
+    }
+
+    auto res = result.get();
+    RCLCPP_INFO(this->get_logger(), "SetStreamsEnable result: %s", res->message.c_str());
+  }
+
+  void deviceStatusCallback(const orbbec_camera_msgs::msg::DeviceStatus::SharedPtr msg) {
+    RCLCPP_INFO(this->get_logger(), "-------orbbec camera real-time status-------");
+    RCLCPP_INFO(this->get_logger(), "--------------------------------------------");
+    RCLCPP_INFO(this->get_logger(), "Color Frame Rate: Cur: %.2f, Avg: %.2f",
+                msg->color_frame_rate_cur, msg->color_frame_rate_avg);
+    RCLCPP_INFO(this->get_logger(), "Depth Frame Rate: Cur: %.2f, Avg: %.2f",
+                msg->depth_frame_rate_cur, msg->depth_frame_rate_avg);
+    RCLCPP_INFO(this->get_logger(), "Color Delay (ms): Cur: %.2f, Avg: %.2f",
+                msg->color_delay_ms_cur, msg->color_delay_ms_avg);
+    RCLCPP_INFO(this->get_logger(), "Depth Delay (ms): Cur: %.2f, Avg: %.2f",
+                msg->depth_delay_ms_cur, msg->depth_delay_ms_avg);
+
+    RCLCPP_INFO(this->get_logger(), "Device Online: %s", msg->device_online ? "True" : "False");
+    RCLCPP_INFO(this->get_logger(), "Connection Type: %s", msg->connection_type.c_str());
+    RCLCPP_INFO(this->get_logger(), "customer_calibration_ready: %s",
+                msg->customer_calibration_ready ? "True" : "False");
+    RCLCPP_INFO(this->get_logger(), "calibration_from_factory: %s",
+                msg->calibration_from_factory ? "True" : "False");
+    RCLCPP_INFO(this->get_logger(), "Calibration From Launch Param: %s",
+                msg->calibration_from_launch_param ? "True" : "False");
+    RCLCPP_INFO(this->get_logger(), "--------------------------------------------\n");
+  }
+
+  rclcpp::Client<orbbec_camera_msgs::srv::SetUserCalibParams>::SharedPtr set_params_client_;
+  rclcpp::Client<orbbec_camera_msgs::srv::GetUserCalibParams>::SharedPtr get_params_client_;
+  rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr set_streams_client_;
+  rclcpp::Client<orbbec_camera_msgs::srv::GetDeviceInfo>::SharedPtr get_device_info_client_;
+
+  rclcpp::Subscription<orbbec_camera_msgs::msg::DeviceStatus>::SharedPtr device_status_sub_;
+};
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<CameraExampleNode>();
+
+  while (rclcpp::ok()) {
+    std::cout << "\n========= Example Menu =========\n";
+    std::cout << "1. Write camera parameters\n";
+    std::cout << "2. Read camera parameters\n";
+    std::cout << "3. Enable camera streams\n";
+    std::cout << "4. Disable camera streams\n";
+    std::cout << "5. Get device info\n";
+    std::cout << "6. Show device status\n";
+    std::cout << "0. Exit\n";
+    std::cout << "Choose option: ";
+
+    int choice;
+    std::cin >> choice;
+
+    switch (choice) {
+      case 1:
+        node->exampleWriteParams();
+        break;
+      case 2:
+        node->exampleReadParams();
+        break;
+      case 3:
+        node->exampleEnableStreams();
+        break;
+      case 4:
+        node->exampleDisableStreams();
+        break;
+      case 5:
+        node->exampleGetDeviceInfo();
+        break;
+      case 6:
+        node->exampleShowdeviceStatus();
+        break;
+      case 0:
+        rclcpp::shutdown();
+        return 0;
+      default:
+        std::cout << "Invalid choice, try again.\n";
+        break;
+    }
+  }
+
+  rclcpp::shutdown();
+  return 0;
+}

orbbec_camera_msgs/CMakeLists.txt

@@ -22,6 +22,7 @@ rosidl_generate_interfaces(
     "msg/Metadata.msg"
     "msg/IMUInfo.msg"
     "msg/RGBD.msg"
+    "msg/DeviceStatus.msg"
     "srv/GetBool.srv"
     "srv/GetDeviceInfo.srv"
     "srv/GetCameraInfo.srv"

orbbec_camera_msgs/msg/DeviceStatus.msg

@@ -1,32 +1,32 @@
-std_msgs/Header header
-
-# --- Color stream ---
-float64 color_frame_rate_cur
-float64 color_frame_rate_avg
-float64 color_frame_rate_min
-float64 color_frame_rate_max
-
-float64 color_delay_ms_cur
-float64 color_delay_ms_avg
-float64 color_delay_ms_min
-float64 color_delay_ms_max
-
-# --- Depth stream ---
-float64 depth_frame_rate_cur
-float64 depth_frame_rate_avg
-float64 depth_frame_rate_min
-float64 depth_frame_rate_max
-
-float64 depth_delay_ms_cur
-float64 depth_delay_ms_avg
-float64 depth_delay_ms_min
-float64 depth_delay_ms_max
-
-# --- Device info ---
-bool device_online
-string connection_type   # e.g. "USB2.0", "USB3.0", "GigE"
-
-# --- Calibration status ---
-bool customer_calibration_ready
-bool calibration_from_factory
-bool calibration_from_launch_param
+std_msgs/Header header
+
+# --- Color stream ---
+float64 color_frame_rate_cur
+float64 color_frame_rate_avg
+float64 color_frame_rate_min
+float64 color_frame_rate_max
+
+float64 color_delay_ms_cur
+float64 color_delay_ms_avg
+float64 color_delay_ms_min
+float64 color_delay_ms_max
+
+# --- Depth stream ---
+float64 depth_frame_rate_cur
+float64 depth_frame_rate_avg
+float64 depth_frame_rate_min
+float64 depth_frame_rate_max
+
+float64 depth_delay_ms_cur
+float64 depth_delay_ms_avg
+float64 depth_delay_ms_min
+float64 depth_delay_ms_max
+
+# --- Device info ---
+bool device_online
+string connection_type   # e.g. "USB2.0", "USB3.0", "GigE"
+
+# --- Calibration status ---
+bool customer_calibration_ready
+bool calibration_from_factory
+bool calibration_from_launch_param

orbbec_camera_msgs/srv/GetUserCalibParams.srv

@@ -4,4 +4,4 @@ float64[8] d            #The distortion parameters
 float64[9] rotation     #Extrinsic rotation matrix
 float64[3] translation  #Extrinsic translation vector
 bool success
-string message
+string message