Add benchmarking nodes for camera service calls and performance monitoring

orbbec_camera/CMakeLists.txt

@@ -48,6 +48,7 @@ set(dependencies
     diagnostic_updater
     diagnostic_msgs
     statistics_msgs
+    yaml-cpp
 )
 
 foreach(dep IN LISTS dependencies)
@@ -113,6 +114,7 @@ set(COMMON_LIBRARIES
     Threads::Threads
     -lrt
     -ldw
+    yaml-cpp
 )
 if(USE_RK_HW_DECODER)
     list(APPEND COMMON_LIBRARIES ${RK_MPP_LIBRARIES} ${RGA_LIBRARIES})
@@ -192,6 +194,14 @@ add_orbbec_executable(list_camera_profile_mode_node tools/list_camera_profile.cp
 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_orbbec_executable(service_benchmark_node scripts/service_benchmark_node.cpp)
+
+install(
+  PROGRAMS
+    scripts/common_benchmark_node.py
+    scripts/service_benchmark_node.py
+  DESTINATION lib/${PROJECT_NAME}
+)
 
 add_library(frame_latency SHARED tools/frame_latency.cpp)
 target_include_directories(frame_latency PUBLIC ${COMMON_INCLUDE_DIRS})
@@ -239,7 +249,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 435le_example_node DESTINATION lib/${PROJECT_NAME}/
+install(TARGETS list_devices_node list_depth_work_mode_node list_camera_profile_mode_node topic_statistics_node  service_benchmark_node ob_benchmark_node 435le_example_node DESTINATION lib/${PROJECT_NAME}/
 )
 
 if(BUILD_TESTING)

orbbec_camera/scripts/common_benchmark_node.py

@@ -0,0 +1,354 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Support: ROS2
+name: common_benchmark_node.py
+function: A ROS2 node to monitor and log the performance of an Orbbec camera node:
+          frame rates, delays, CPU and RAM usage, packet/frame loss statistics.
+usage:
+  python3 common_benchmark_node.py --run_time 20 --csv_file /tmp/cam_log.csv
+"""
+
+import argparse
+import rclpy
+from rclpy.node import Node
+import psutil
+import time
+import csv
+import os
+from collections import defaultdict
+from orbbec_camera_msgs.msg import DeviceStatus
+from sensor_msgs.msg import Image
+import sys
+
+from tabulate import tabulate
+
+CAMERA_NODE_NAMES = ["component_container", "orbbec_camera_node", "nodelet"]
+
+# ----------------tool functions----------------
+def parse_duration(s):
+    # Parse duration strings like "10s", "5m", "1h", "2d" into seconds.
+    if isinstance(s, (int, float)):
+        return float(s)
+
+    s = str(s).strip().lower()
+    if s.endswith("s"):
+        return float(s[:-1])
+    elif s.endswith("m"):
+        return float(s[:-1]) * 60
+    elif s.endswith("h"):
+        return float(s[:-1]) * 3600
+    elif s.endswith("d"):
+        return float(s[:-1]) * 86400
+    else:
+        return float(s)
+
+def format_duration(seconds):
+    seconds = int(seconds)
+    days, seconds = divmod(seconds, 86400)
+    hours, seconds = divmod(seconds, 3600)
+    minutes, seconds = divmod(seconds, 60)
+
+    parts = []
+    if days > 0:
+        parts.append(f"{days}d")
+    if hours > 0:
+        parts.append(f"{hours}h")
+    if minutes > 0:
+        parts.append(f"{minutes}m")
+    if seconds > 0 or not parts:
+        parts.append(f"{seconds}s")
+    return " ".join(parts)
+# ----------------------------------------------
+
+class TopicTracker:
+    """
+    Tracks sequence/packet loss and frame-drop estimates for a topic stream.
+    Works when header.seq exists (ROS1 style) or when only timestamps exist (ROS2 common case).
+    """
+    def __init__(self, logger=None):
+        self.received = 0
+
+        self.last_time = None    # last seen stamp in seconds (float)
+        self.drop_frames = 0
+
+        self.logger = logger
+
+    def on_msg(self, header, avg_fps):
+        """
+        header: std_msgs/Header (ROS2)
+        avg_fps: float (the expected average FPS for determining drop frames)
+        """
+        stamp = header.stamp.sec + header.stamp.nanosec * 1e-9
+        self.received += 1
+
+        if self.last_time is not None and avg_fps > 0:
+            dt = stamp - self.last_time
+            expected_interval = 1.0 / avg_fps
+            if expected_interval > 0 and dt > 1.5 * expected_interval:
+                self.drop_frames += 1
+
+        self.last_time = stamp
+
+    def frames_loss_rate(self):
+        total = self.received + self.drop_frames
+        if total <= 0:
+            return 0.0
+        return float(self.drop_frames) / total
+
+    def reset(self):
+        self.__init__(logger=self.logger)
+
+
+class CameraMonitorNode(Node):
+    def __init__(self, run_time, csv_file="camera_monitor_log.csv"):
+        super().__init__("camera_monitor_node")
+
+        self.run_time = run_time
+        self.start_time = time.time()
+        self.process = psutil.Process(os.getpid())
+        self.first_data_collected = False
+        self.node_name = ""
+
+        self.connection_type = None
+        self.disconnect_count = 0
+        self.prev_online = True
+        self.finished = False
+
+        self.stats = defaultdict(lambda:
+                                 {"count": 0, "sum": 0.0, "cur": 0.0, "avg": 0.0, "min": float("inf"), "max": float("-inf")})
+
+        self.cpu_stats = {"cur": 0.0, "avg": 0.0, "min": float("inf"), "max": float("-inf"), "count": 0, "sum": 0.0}
+        self.ram_stats = {"cur": 0.0, "avg": 0.0, "min": float("inf"), "max": float("-inf"), "count": 0, "sum": 0.0}
+
+        # pass node logger into trackers for helpful warnings
+        self.trackers = {
+            "color": TopicTracker(logger=self.get_logger()),
+            "depth": TopicTracker(logger=self.get_logger())
+        }
+
+        # CSV
+        self.csv_file = csv_file
+        self.csv_fh = open(self.csv_file, "w", newline="")
+        self.csv_writer = csv.writer(self.csv_fh)
+        self.csv_writer.writerow([
+            "time(s)", "connection_type", "disconnects",
+            "color_fps_cur", "color_fps_avg", "color_fps_min", "color_fps_max",
+            "color_delay_cur", "color_delay_avg", "color_delay_min", "color_delay_max",
+            "depth_fps_cur", "depth_fps_avg", "depth_fps_min", "depth_fps_max",
+            "depth_delay_cur", "depth_delay_avg", "depth_delay_min", "depth_delay_max",
+            "cpu_cur", "cpu_avg", "cpu_min", "cpu_max",
+            "ram_cur", "ram_avg", "ram_min", "ram_max",
+            "color_frames_loss", "color_frames_loss_rate(%)",
+            "depth_frames_loss", "depth_frames_loss_rate(%)"
+        ])
+
+        # subscriptions
+        self.create_subscription(DeviceStatus, "/camera/device_status", self.status_callback, 5)
+
+        self.create_subscription(Image, "/camera/color/image_raw", lambda msg: self.image_callback(msg, "color"), 5)
+        self.create_subscription(Image, "/camera/depth/image_raw", lambda msg: self.image_callback(msg, "depth"), 5)
+
+        # timer runs every 1s to update system stats, log csv and print status
+        self.timer = self.create_timer(1.0, self.timer_callback)
+
+    def timer_callback(self):
+        elapsed = time.time() - self.start_time
+        if elapsed > self.run_time:
+            self.finish()
+            rclpy.shutdown()
+            return
+
+        cpu, ram, self.node_name = self.get_camera_stats()
+        self.update_sys_stat(self.cpu_stats, cpu)
+        self.update_sys_stat(self.ram_stats, ram)
+
+        if self.first_data_collected:
+            self.log_to_csv(elapsed)
+            self.print_status()
+
+    def finish(self):
+        if self.finished:
+            return
+        self.finished = True
+
+        elapsed = time.time() - self.start_time
+        try:
+            self.csv_fh.close()
+        except Exception:
+            pass
+        print(f"Monitoring finished, it takes time: {format_duration(elapsed)}")
+        print(f"CSV data is saved to: {self.csv_file}")
+
+    def find_camera_node(self):
+        for proc in psutil.process_iter(['pid', 'name', 'cmdline']):
+            try:
+                cmdline = " ".join(proc.info.get('cmdline') or [])
+                if any(name.lower() in cmdline.lower() for name in CAMERA_NODE_NAMES):
+                    return proc
+            except Exception:
+                continue
+        return None
+
+    def get_camera_stats(self):
+        proc = self.find_camera_node()
+        if not proc:
+            return 0.0, 0.0, "Not Found"
+        try:
+            procs = [proc] + proc.children(recursive=True)
+            cpu = sum((p.cpu_percent(interval=None) for p in procs)) / max(1, psutil.cpu_count())
+            mem_bytes = sum((p.memory_info().rss for p in procs))
+            mem_mb = mem_bytes / (1024 * 1024)
+            name = f"{proc.name()} + {len(procs)-1} child" if len(procs) > 1 else proc.name()
+            return cpu, mem_mb, name
+        except Exception:
+            return 0.0, 0.0, "Error"
+
+    def status_callback(self, msg: DeviceStatus):
+        if not self.first_data_collected:
+            self.first_data_collected = True
+
+        self.connection_type = msg.connection_type
+        if self.prev_online and not msg.device_online:
+            self.disconnect_count += 1
+            self.prev_online = msg.device_online
+            return
+
+        self.prev_online = msg.device_online
+
+        # update stats from DeviceStatus message fields
+        self.update_stats("color_fps", msg.color_frame_rate_cur, msg.color_frame_rate_min, msg.color_frame_rate_max, msg.color_frame_rate_avg)
+        self.update_stats("color_delay", msg.color_delay_ms_cur, msg.color_delay_ms_min, msg.color_delay_ms_max, msg.color_delay_ms_avg)
+        self.update_stats("depth_fps", msg.depth_frame_rate_cur, msg.depth_frame_rate_min, msg.depth_frame_rate_max, msg.depth_frame_rate_avg)
+        self.update_stats("depth_delay", msg.depth_delay_ms_cur, msg.depth_delay_ms_min, msg.depth_delay_ms_max, msg.depth_delay_ms_avg)
+
+    def image_callback(self, msg: Image, stream: str):
+        if stream not in ("color", "depth"):
+            return
+        header = msg.header
+        tracker = self.trackers[stream]
+        tracker.on_msg(header, self.stats[f"{stream}_fps"]["avg"])
+
+    def update_stats(self, key, cur, min_val, max_val, avg_val):
+        if min_val <= 1e-3 or avg_val < 0:  # ignore invalid data
+            return
+        s = self.stats[key]
+        s["cur"] = (cur)
+        s["count"] += 1
+        s["sum"] += avg_val
+        s["avg"] = s["sum"] / s["count"] if s["count"] > 0 else 0.0
+        s["min"] = min(s["min"], min_val)
+        s["max"] = max(s["max"], max_val)
+
+    def update_sys_stat(self, stat_dict, value):
+        stat_dict["cur"] = value
+        if value is None or value <= 0.0 or not self.prev_online:
+            return
+
+        stat_dict["count"] += 1
+        stat_dict["sum"] += value
+        stat_dict["avg"] = stat_dict["sum"] / stat_dict["count"] if stat_dict["count"] > 0 else 0.0
+        stat_dict["min"] = min(stat_dict["min"], value)
+        stat_dict["max"] = max(stat_dict["max"], value)
+
+    def log_to_csv(self, elapsed):
+        if not self.prev_online:
+            self.csv_writer.writerow([
+                round(elapsed, 2), self.connection_type, self.disconnect_count, *["N/A"] * 28
+            ])
+            return
+
+        color_tracker = self.trackers["color"]
+        depth_tracker = self.trackers["depth"]
+        color_frames_loss = color_tracker.drop_frames
+        color_frames_loss_rate = round(color_tracker.frames_loss_rate() * 100.0, 2)
+        depth_frames_loss = depth_tracker.drop_frames
+        depth_frames_loss_rate = round(depth_tracker.frames_loss_rate() * 100.0, 2)
+
+        # guard: if stats keys missing, use 0
+        def safe(k):
+            v = self.stats.get(k, {})
+            return round(v.get("cur", 0.0), 2), round(v.get("avg", 0.0), 2), round(v.get("min", 0.0), 2), round(v.get("max", 0.0), 2)
+
+        color_fps_cur, color_fps_avg, color_fps_min, color_fps_max = safe("color_fps")
+        color_delay_cur, color_delay_avg, color_delay_min, color_delay_max = safe("color_delay")
+        depth_fps_cur, depth_fps_avg, depth_fps_min, depth_fps_max = safe("depth_fps")
+        depth_delay_cur, depth_delay_avg, depth_delay_min, depth_delay_max = safe("depth_delay")
+
+        self.csv_writer.writerow([
+            round(elapsed, 2), self.connection_type, self.disconnect_count,
+            color_fps_cur, color_fps_avg, color_fps_min, color_fps_max,
+            color_delay_cur, color_delay_avg, color_delay_min, color_delay_max,
+            depth_fps_cur, depth_fps_avg, depth_fps_min, depth_fps_max,
+            depth_delay_cur, depth_delay_avg, depth_delay_min, depth_delay_max,
+            round(self.cpu_stats["cur"], 2), round(self.cpu_stats["avg"], 2), round(self.cpu_stats["min"], 2), round(self.cpu_stats["max"], 2),
+            round(self.ram_stats["cur"], 2), round(self.ram_stats["avg"], 2), round(self.ram_stats["min"], 2), round(self.ram_stats["max"], 2),
+            color_frames_loss, color_frames_loss_rate,
+            depth_frames_loss, depth_frames_loss_rate
+        ])
+
+    def print_status(self):
+        def format_stats(s):
+            return f"{s['cur']:.2f}", f"{s['avg']:.2f}", f"{s['min']:.2f}", f"{s['max']:.2f}"
+
+        rows = []
+        for stream in ["color", "depth"]:
+            fps_key = f"{stream}_fps"
+            delay_key = f"{stream}_delay"
+            topic_name = f"{stream}/image_raw"
+            if not self.prev_online:
+                rows.append([topic_name, *["N/A"] * 10])
+            else:
+                fps_vals = format_stats(self.stats[fps_key]) if self.stats[fps_key]["count"] > 0 else ("0.00","0.00","0.00","0.00")
+                delay_vals = format_stats(self.stats[delay_key]) if self.stats[delay_key]["count"] > 0 else ("0.00","0.00","0.00","0.00")
+                tracker = self.trackers[stream]
+
+                frames_loss = tracker.drop_frames
+                frames_loss_rate = round(tracker.frames_loss_rate() * 100.0, 2)
+                rows.append([topic_name, *fps_vals, *delay_vals, frames_loss, frames_loss_rate])
+
+        header_bottom = ["Option", "fps_cur", "fps_avg", "fps_min", "fps_max", "delay_cur(ms)", "delay_avg(ms)", "delay_min(ms)", "delay_max(ms)", "Pub_lost_count", "Pub_lost_rate(%)"]
+
+        os.system("clear")
+        print("Orbbec Camera Benchmark\n")
+        print(tabulate([header_bottom] + rows, tablefmt="fancy_grid"))
+
+        sys_rows = []
+        if not self.prev_online:
+            cpu_vals = (round(self.cpu_stats['cur'], 2), "N/A", "N/A", "N/A")
+            ram_vals = (round(self.ram_stats['cur'], 2), "N/A", "N/A", "N/A")
+        else:
+            cpu_vals = format_stats(self.cpu_stats)
+            ram_vals = format_stats(self.ram_stats)
+
+        sys_rows.append(["CPU Usage (%)", *cpu_vals])
+        sys_rows.append(["RAM Usage (MB)", *ram_vals])
+
+        print(f"\n\n(CPU & RAM) Camera Node: {self.node_name}\n")
+        print(tabulate(sys_rows, headers=["Option", "cur", "avg", "min", "max"], tablefmt="fancy_grid"))
+        print("\nconnection_type: %s\nstatus_online: %s\ndisconnect_count: %d" % (self.connection_type, self.prev_online, self.disconnect_count))
+
+
+def main(argv=None):
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--run_time", type=str, default="10s", help="Total run time for monitoring, e.g., 10s, 5m, 1h.")
+    parser.add_argument("--csv_file", type=str, default="camera_monitor_log.csv")
+    cli_args, _ = parser.parse_known_args(argv)
+
+    rclpy.init(args=argv)
+    run_time = parse_duration(cli_args.run_time)
+    node = CameraMonitorNode(run_time, cli_args.csv_file)
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.finish()
+    finally:
+        try:
+            node.csv_fh.close()
+        except Exception:
+            pass
+        node.destroy_node()
+
+if __name__ == "__main__":
+    main()

orbbec_camera/scripts/default_service.yaml

@@ -0,0 +1,188 @@
+default_count: 50
+
+services:
+# - name: /camera/set_filter
+#   type: orbbec_camera_msgs/srv/SetFilter
+#   request: {filter_name: DecimationFilter,filter_enable: false,filter_param: [5]}
+
+- name: /camera/set_depth_exposure
+  type: orbbec_camera_msgs/srv/SetInt32
+  request: {data: 3000}
+
+- name: /camera/get_depth_exposure
+  type: orbbec_camera_msgs/srv/GetInt32
+
+- name: /camera/set_depth_ae_roi
+  type: orbbec_camera_msgs/srv/SetArrays
+  request: {data_param: [0,1279,0,719]}
+
+# - name: /camera/reset_depth_exposure
+#   type: std_srvs/Empty
+
+- name: /camera/get_depth_gain
+  type: orbbec_camera_msgs/srv/GetInt32
+
+- name: /camera/set_depth_gain
+  type: orbbec_camera_msgs/srv/SetInt32
+  request: {data: 200}
+
+# - name: /camera/reset_depth_gain
+#   type: std_srvs/Empty
+
+- name: /camera/set_depth_mirror
+  type: std_srvs/SetBool
+  request: {data: false}
+
+- name: /camera/set_depth_flip
+  type: std_srvs/SetBool
+  request: {data: false}
+
+- name: /camera/set_depth_rotation
+  type: orbbec_camera_msgs/srv/SetInt32
+  request: {data: 90}
+
+- name: /camera/set_depth_auto_exposure
+  type: std_srvs/SetBool
+  request: {data: false}
+
+# - name: /camera/get_depth_auto_exposure
+#   type: orbbec_camera_msgs/srv/GetBool
+
+# - name: /camera/toggle_depth
+#   type: std_srvs/SetBool
+#   request: {data: true}
+
+# - name: /camera/get_depth_camera_info
+#   type: orbbec_camera_msgs/srv/GetCameraInfo
+
+- name: /camera/get_color_exposure
+  type: orbbec_camera_msgs/srv/GetInt32
+
+- name: /camera/set_color_exposure
+  type: orbbec_camera_msgs/srv/SetInt32
+  request: {data: 30}
+
+- name: /camera/set_color_ae_roi
+  type: orbbec_camera_msgs/srv/SetArrays
+  request: {data_param: [0,1279,0,719]}
+
+# - name: /camera/reset_color_exposure
+#   type: std_srvs/Empty
+
+- name: /camera/get_color_gain
+  type: orbbec_camera_msgs/srv/GetInt32
+
+- name: /camera/set_color_gain
+  type: orbbec_camera_msgs/srv/SetInt32
+  request: {data: 20}
+
+# - name: /camera/reset_color_gain
+#   type: std_srvs/Empty
+
+- name: /camera/set_color_mirror
+  type: std_srvs/SetBool
+  request: {data: false}
+
+- name: /camera/set_color_flip
+  type: std_srvs/SetBool
+  request: {data: false}
+
+- name: /camera/set_color_rotation
+  type: orbbec_camera_msgs/srv/SetInt32
+  request: {data: 90}
+
+- name: /camera/set_color_auto_exposure
+  type: std_srvs/SetBool
+  request: {data: false}
+
+# - name: /camera/get_color_auto_exposure
+#   type: orbbec_camera_msgs/srv/GetBool
+
+# - name: /camera/toggle_color
+#   type: std_srvs/SetBool
+#   request: {data: true}
+
+# - name: /camera/get_color_camera_info
+#   type: orbbec_camera_msgs/srv/GetCameraInfo
+
+- name: /camera/get_auto_white_balance
+  type: orbbec_camera_msgs/srv/GetInt32
+
+# - name: /camera/set_auto_white_balance
+#   type: orbbec_camera_msgs/srv/SetInt32
+#   request: {data: 0}
+
+- name: /camera/get_white_balance
+  type: orbbec_camera_msgs/srv/GetInt32
+
+# - name: /camera/set_white_balance
+#   type: orbbec_camera_msgs/srv/SetInt32
+#   request: {data: 3000}
+
+# - name: /camera/reset_white_balance
+#   type: std_srvs/Empty
+
+# - name: /camera/set_laser
+#   type: std_srvs/SetBool
+#   request: {data: true}
+
+# - name: /camera/set_ldp
+#   type: std_srvs/SetBool
+#   request: {data: false}
+
+- name: /camera/get_ldp_status
+  type: orbbec_camera_msgs/srv/GetBool
+
+- name: /camera/get_device_info
+  type: orbbec_camera_msgs/srv/GetDeviceInfo
+
+# - name: /camera/get_camera_params
+#   type: orbbec_camera_msgs/srv/GetCameraParams
+
+- name: /camera/get_sdk_version
+  type: orbbec_camera_msgs/srv/GetString
+
+- name: /camera/save_point_cloud
+  type: std_srvs/Empty
+
+- name: /camera/save_images
+  type: std_srvs/Empty
+
+- name: /camera/get_lrm_measure_distance
+  type: orbbec_camera_msgs/srv/GetInt32
+
+# - name: /camera/reboot_device
+#   type: std_srvs/Empty
+
+# - name: /camera/color/set_camera_info
+#   type: sensor_msgs/SetCameraInfo
+
+# - name: /camera/ir/set_camera_info
+#   type: sensor_msgs/SetCameraInfo
+
+# - name: /camera/set_ptp_config
+#   type: std_srvs/SetBool
+#   request: {data: false}
+
+# - name: /camera/get_ptp_config
+#   type: orbbec_camera_msgs/srv/GetBool
+
+# - name: /camera/set_fan_work_mode
+#   type: std_srvs/SetBool
+#   request: {data: false}
+
+# - name: /camera/set_flood
+#   type: std_srvs/SetBool
+#   request: {data: false}
+
+# - name: /camera/switch_ir_mode
+#   type: orbbec_camera_msgs/srv/SetInt32
+
+# - name: /camera/switch_ir
+#   type: orbbec_camera_msgs/srv/SetString
+
+# - name: /camera/set_write_customer_data
+#   type: orbbec_camera_msgs/srv/SetString
+
+# - name: /camera/set_read_customer_data
+#   type: orbbec_camera_msgs/srv/GetString

orbbec_camera/scripts/service_benchmark_node.cpp

@@ -0,0 +1,373 @@
+/**
+ * @file service_benchmark_node.cpp
+ * @brief A ROS2 node to benchmark Orbbec camera service calls.
+ *
+ * Features:
+ *   - Benchmark a single service call (latency, success rate)
+ *   - Benchmark multiple services defined in a YAML configuration file
+ *   - Optionally save results to a CSV file
+ *
+ * Usage:
+ *   ros2 run orbbec_camera service_benchmark_node --ros-args -p
+ *   yaml_file:=/path/to/default_service_cpp.yaml
+ */
+#include "orbbec_camera/ob_camera_node_driver.h"
+#include <algorithm>
+#include <chrono>
+#include <numeric>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include <vector>
+#include <yaml-cpp/yaml.h>
+#include <iomanip>
+#include <fstream>
+#include <functional>
+#include <map>
+
+class SingleServiceBenchmark {
+ public:
+  SingleServiceBenchmark(rclcpp::Node::SharedPtr nh, const std::string &service_name,
+                         const std::string &service_type, int count, const YAML::Node &request_data)
+      : nh_(nh),
+        service_name_(service_name),
+        service_type_(service_type),
+        count_(count),
+        request_data_(request_data) {
+    service_map_["orbbec_camera_msgs/srv/GetDeviceInfo"] = [this](std::vector<double> &durations,
+                                                                  int &success) {
+      runTyped<orbbec_camera::GetDeviceInfo>(durations, success);
+    };
+    service_map_["orbbec_camera_msgs/srv/GetString"] = [this](std::vector<double> &durations,
+                                                              int &success) {
+      runTyped<orbbec_camera::GetString>(durations, success);
+    };
+    service_map_["orbbec_camera_msgs/srv/GetBool"] = [this](std::vector<double> &durations,
+                                                            int &success) {
+      runTyped<orbbec_camera::GetBool>(durations, success);
+    };
+    service_map_["orbbec_camera_msgs/srv/GetInt32"] = [this](std::vector<double> &durations,
+                                                             int &success) {
+      runTyped<orbbec_camera::GetInt32>(durations, success);
+    };
+    service_map_["std_srvs/Empty"] = [this](std::vector<double> &durations, int &success) {
+      runTyped<std_srvs::srv::Empty>(durations, success);
+    };
+    service_map_["orbbec_camera_msgs/srv/SetInt32"] = [this](std::vector<double> &durations,
+                                                             int &success) {
+      runTyped<orbbec_camera::SetInt32>(durations, success);
+    };
+    service_map_["orbbec_camera_msgs/srv/SetArrays"] = [this](std::vector<double> &durations,
+                                                              int &success) {
+      runTyped<orbbec_camera::SetArrays>(durations, success);
+    };
+    service_map_["std_srvs/SetBool"] = [this](std::vector<double> &durations, int &success) {
+      runTyped<std_srvs::srv::SetBool>(durations, success);
+    };
+    service_map_["orbbec_camera_msgs/srv/SetFilter"] = [this](std::vector<double> &durations,
+                                                              int &success) {
+      runTyped<orbbec_camera::SetFilter>(durations, success);
+    };
+    service_map_["orbbec_camera_msgs/srv/SetString"] = [this](std::vector<double> &durations,
+                                                              int &success) {
+      runTyped<orbbec_camera::SetString>(durations, success);
+    };
+  }
+
+  template <typename ServiceT>
+  typename rclcpp::Client<ServiceT>::SharedPtr getClient() {
+    auto it = client_cache_.find(service_name_);
+    if (it != client_cache_.end()) {
+      return std::dynamic_pointer_cast<rclcpp::Client<ServiceT>>(it->second);
+    }
+    auto client = nh_->create_client<ServiceT>(service_name_);
+    client_cache_[service_name_] = client;
+    return client;
+  }
+
+  int run(std::vector<double> &durations_out) {
+    int success = 0;
+    auto it = service_map_.find(service_type_);
+    if (it != service_map_.end()) {
+      it->second(durations_out, success);
+    } else {
+      RCLCPP_ERROR(nh_->get_logger(), "Unsupported service type: %s", service_type_.c_str());
+    }
+    return success;
+  }
+
+ private:
+  rclcpp::Node::SharedPtr nh_;
+  std::string service_name_;
+  std::string service_type_;
+  int count_;
+  YAML::Node request_data_;
+  std::map<std::string, std::function<void(std::vector<double> &, int &)>> service_map_;
+  std::map<std::string, rclcpp::ClientBase::SharedPtr> client_cache_;
+
+  template <typename T, typename = void>
+  struct has_success : std::false_type {};
+  template <typename T>
+  struct has_success<T, std::void_t<decltype(std::declval<typename T::Response>().success)>>
+      : std::true_type {};
+
+  template <typename ServiceT>
+  void fillRequest(typename ServiceT::Request &request);
+
+  template <typename ServiceT>
+  void runTyped(std::vector<double> &durations_out, int &success_out) {
+    auto client = getClient<ServiceT>();
+    std::vector<double> durations;
+    int success = 0;
+
+    if (!client->wait_for_service(std::chrono::seconds(2))) {
+      RCLCPP_ERROR(nh_->get_logger(), "Service %s not available", service_name_.c_str());
+      durations_out.clear();
+      success_out = 0;
+      return;
+    }
+
+    for (int i = 0; i < count_; ++i) {
+      auto Request = std::make_shared<typename ServiceT::Request>();
+      fillRequest<ServiceT>(*Request);
+
+      auto start = std::chrono::steady_clock::now();
+      bool call_ok = false;
+      try {
+        auto result_future = client->async_send_request(Request);
+        auto status = rclcpp::spin_until_future_complete(nh_, result_future);
+        call_ok = (status == rclcpp::FutureReturnCode::SUCCESS);
+        if (call_ok) {
+          auto end = std::chrono::steady_clock::now();
+          double dt =
+              std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() / 1000.0;
+          durations.push_back(dt);
+
+          if constexpr (has_success<ServiceT>::value) {
+            if (result_future.get()->success) {
+              success++;
+            } else {
+              RCLCPP_WARN(nh_->get_logger(), "Call %s %d/%d responded with success=false",
+                          service_name_.c_str(), i + 1, count_);
+            }
+          } else {
+            success++;
+          }
+          RCLCPP_INFO(nh_->get_logger(), "Call %s %d/%d succeeded (cost: %.2f ms)",
+                      service_name_.c_str(), i + 1, count_, dt);
+        }
+      } catch (const std::exception &e) {
+        RCLCPP_ERROR(nh_->get_logger(), "Exception calling service %s: %s", service_name_.c_str(),
+                     e.what());
+      }
+
+      if (!call_ok) {
+        std::string request_str;
+        try {
+          if (request_data_ && !request_data_.IsNull()) {
+            request_str = YAML::Dump(request_data_);
+          } else {
+            request_str = "{error 6}";
+          }
+        } catch (...) {
+          request_str = "{error null}";
+        }
+        RCLCPP_WARN(nh_->get_logger(),
+                    "Call %d/%d failed for service '%s' (type: '%s') with request: %s", i + 1,
+                    count_, service_name_.c_str(), service_type_.c_str(), request_str.c_str());
+      }
+    }
+
+    if (success == 0 || durations.empty()) {
+      RCLCPP_WARN(nh_->get_logger(), "No successful calls for service %s (%s)",
+                  service_name_.c_str(), service_type_.c_str());
+      durations_out.clear();
+      success_out = 0;
+      return;
+    }
+
+    durations_out = durations;
+    success_out = success;
+
+    printSummary(durations, success);
+  }
+
+  void printSummary(const std::vector<double> &durations, int success) {
+    if (durations.empty()) {
+      RCLCPP_WARN(nh_->get_logger(), "No successful calls!");
+      return;
+    }
+
+    double avg = std::accumulate(durations.begin(), durations.end(), 0.0) / durations.size();
+    double minv = *std::min_element(durations.begin(), durations.end());
+    double maxv = *std::max_element(durations.begin(), durations.end());
+    double success_rate = 100.0 * success / count_;
+
+    std::cout << std::string(64, '=') << std::endl;
+    std::cout << std::setw(7) << "Calls" << std::setw(10) << "Success" << std::setw(11) << "Rate(%)"
+              << std::setw(12) << "Avg(ms)" << std::setw(12) << "Min(ms)" << std::setw(12)
+              << "Max(ms)" << std::endl;
+    std::cout << std::string(64, '-') << std::endl;
+
+    std::cout << std::setw(5) << count_ << std::setw(10) << success << std::setw(12) << std::fixed
+              << std::setprecision(2) << success_rate << std::setw(11) << std::fixed
+              << std::setprecision(2) << avg << std::setw(12) << std::fixed << std::setprecision(2)
+              << minv << std::setw(12) << std::fixed << std::setprecision(2) << maxv << std::endl;
+
+    std::cout << std::string(64, '=') << std::endl;
+  }
+};
+
+template <typename ServiceT>
+void SingleServiceBenchmark::fillRequest(typename ServiceT::Request &request) {
+  (void)request;
+}
+
+// SetInt32
+template <>
+void SingleServiceBenchmark::fillRequest<orbbec_camera::SetInt32>(
+    orbbec_camera::SetInt32::Request &request) {
+  if (request_data_ && request_data_["data"] && request_data_["data"].IsScalar()) {
+    request.data = request_data_["data"].as<int>();
+  }
+}
+
+// SetArrays
+template <>
+void SingleServiceBenchmark::fillRequest<orbbec_camera::SetArrays>(
+    orbbec_camera::SetArrays::Request &request) {
+  if (request_data_ && request_data_["data_param"] && request_data_["data_param"].IsSequence()) {
+    const YAML::Node &arr = request_data_["data_param"];
+    request.data_param.clear();
+    for (const auto &v : arr) request.data_param.push_back(v.as<int>());
+  }
+}
+
+// SetFilter
+template <>
+void SingleServiceBenchmark::fillRequest<orbbec_camera::SetFilter>(
+    orbbec_camera::SetFilter::Request &request) {
+  if (!request_data_ || request_data_.IsNull()) return;
+
+  if (request_data_["filter_name"] && request_data_["filter_name"].IsScalar())
+    request.filter_name = request_data_["filter_name"].as<std::string>();
+  if (request_data_["filter_enable"] && request_data_["filter_enable"].IsScalar())
+    request.filter_enable = request_data_["filter_enable"].as<bool>();
+  if (request_data_["filter_param"] && request_data_["filter_param"].IsSequence()) {
+    request.filter_param.clear();
+    for (const auto &v : request_data_["filter_param"]) request.filter_param.push_back(v.as<int>());
+  }
+}
+
+class MultiServiceBenchmark {
+ public:
+  MultiServiceBenchmark(rclcpp::Node::SharedPtr nh, const YAML::Node &services_config,
+                        int default_count, const std::string &csv_file)
+      : nh_(nh),
+        services_config_(services_config),
+        default_count_(default_count),
+        csv_file_(csv_file) {
+    if (!csv_file_.empty()) {
+      csv_stream_.open(csv_file_, std::ios::out);
+      if (csv_stream_.is_open()) {
+        csv_stream_ << "Service,Type,Calls,Success,Rate,Avg(ms),Min(ms),Max(ms)\n";
+      } else {
+        RCLCPP_WARN(nh_->get_logger(), "Failed to open CSV file: %s", csv_file_.c_str());
+      }
+    }
+  }
+
+  ~MultiServiceBenchmark() {
+    if (csv_stream_.is_open()) csv_stream_.close();
+    RCLCPP_INFO(nh_->get_logger(), "Benchmark results saved to CSV file: %s", csv_file_.c_str());
+  }
+
+  void run() {
+    if (!services_config_ || !services_config_.IsSequence()) {
+      RCLCPP_ERROR(nh_->get_logger(), "No services found in YAML config.");
+      return;
+    }
+
+    for (size_t i = 0; i < services_config_.size(); ++i) {
+      YAML::Node svc = services_config_[i];
+      std::string service_name = svc["name"] ? svc["name"].as<std::string>() : "";
+      std::string service_type = svc["type"] ? svc["type"].as<std::string>() : "";
+      YAML::Node request_data = svc["request"];
+      int count = svc["count"] ? svc["count"].as<int>() : default_count_;
+
+      RCLCPP_INFO(nh_->get_logger(), "Running benchmark for service %s (%s)", service_name.c_str(),
+                  service_type.c_str());
+
+      SingleServiceBenchmark bench(nh_, service_name, service_type, count, request_data);
+
+      std::vector<double> durations;
+      int success = bench.run(durations);
+
+      if (csv_stream_.is_open()) {
+        if (durations.empty() || success == 0) {
+          csv_stream_ << service_name << "," << service_type << "," << count << "," << success
+                      << ",0.00%,0.00,0.00,0.00\n";
+        } else {
+          double avg = std::accumulate(durations.begin(), durations.end(), 0.0) / durations.size();
+          double minv = *std::min_element(durations.begin(), durations.end());
+          double maxv = *std::max_element(durations.begin(), durations.end());
+          double success_rate = 100.0 * success / count;
+
+          csv_stream_ << service_name << "," << service_type << "," << count << "," << success
+                      << "," << std::fixed << std::setprecision(2) << success_rate << "%"
+                      << "," << avg << "," << minv << "," << maxv << "\n";
+        }
+      }
+    }
+  }
+
+ private:
+  rclcpp::Node::SharedPtr nh_;
+  YAML::Node services_config_;
+  int default_count_;
+  std::string csv_file_;
+  std::ofstream csv_stream_;
+};
+
+// ------------------- main -------------------
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  auto nh = std::make_shared<rclcpp::Node>("service_benchmark_node");
+
+  std::string service_name, service_type, request_str;
+  YAML::Node request_data;
+  int count = 10;
+  std::string yaml_file, csv_file;
+
+  nh->declare_parameter("yaml_file", "");
+  nh->declare_parameter("csv_file", "multi_service_results_log_cpp.csv");
+  nh->declare_parameter("service_name", "/camera/get_sdk_version");
+  nh->declare_parameter("service_type", "orbbec_camera/GetString");
+  nh->declare_parameter("request_data", "");
+  nh->declare_parameter("count", 10);
+
+  nh->get_parameter("yaml_file", yaml_file);
+  nh->get_parameter("csv_file", csv_file);
+  nh->get_parameter("service_name", service_name);
+  nh->get_parameter("service_type", service_type);
+  nh->get_parameter("request_data", request_str);
+  nh->get_parameter("count", count);
+
+  if (yaml_file.empty()) {
+    if (!request_str.empty()) {
+      request_data = YAML::Load(request_str);
+    }
+    SingleServiceBenchmark bench(nh, service_name, service_type, count, request_data);
+    std::vector<double> durations_out;
+    bench.run(durations_out);
+  } else {
+    YAML::Node config = YAML::LoadFile(yaml_file);
+    YAML::Node services = config["services"];
+    int global_count = config["default_count"] ? config["default_count"].as<int>() : 1;
+
+    MultiServiceBenchmark multi_bench(nh, services, global_count, csv_file);
+    multi_bench.run();
+  }
+
+  rclcpp::shutdown();
+  return 0;
+}

orbbec_camera/scripts/service_benchmark_node.py

@@ -0,0 +1,197 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@file service_benchmark_node.py
+@brief A ROS2 node to benchmark Orbbec camera service calls.
+
+Features:
+  Benchmark multiple services defined in a YAML configuration file
+
+Usage:
+  ros2 run orbbec_camera service_benchmark_node.py \
+    --ros-args --params-file /path/to/default_service.yaml
+"""
+
+import rclpy
+from rclpy.node import Node
+import argparse
+import time
+import yaml
+from statistics import mean
+from tabulate import tabulate
+import importlib
+import csv
+
+
+class ServiceBenchmark:
+    def __init__(self, node: Node, service_name, count, request_dict=None):
+        self.node = node
+        self.service_name = service_name
+        self.count = count
+        self.request_dict = request_dict or {}
+
+        # 动态导入服务类型
+        self.service_type = self.get_service_type(service_name)
+        if not self.service_type:
+            raise RuntimeError(f"Service {service_name} type not found")
+
+        self.ServiceClass = self.service_class(self.service_type)
+        self.client = self.node.create_client(self.ServiceClass, self.service_name)
+
+        if not self.client.wait_for_service(timeout_sec=5.0):
+            raise RuntimeError(f"Service {self.service_name} not available")
+
+    def get_service_type(self, service_name):
+        """获取服务类型 (用 CLI 方式调用 ros2 service type)"""
+        import subprocess
+        try:
+            output = subprocess.check_output(["ros2", "service", "type", service_name])
+            return output.decode("utf-8").strip()
+        except subprocess.CalledProcessError:
+            return None
+
+    def service_class(self, service_type):
+        package, srv = service_type.split('/')
+        module = importlib.import_module(f"{package}.srv")
+        return getattr(module, srv)
+
+    def service_class(self, service_type):
+      # ROS2 service type is like "orbbec_camera/srv/GetString"
+      package, _, srv = service_type.split('/')
+      module = importlib.import_module(f"{package}.srv")
+      return getattr(module, srv)
+
+
+    def run(self):
+        durations = []
+        success = 0
+
+        for i in range(self.count):
+            try:
+                self.node.get_logger().info(f"Running service {self.service_name} {i+1}/{self.count}")
+                start = time.time()
+                if self.request_dict:
+                    request = self.ServiceClass.Request(**self.request_dict)
+                else:
+                    request = self.ServiceClass.Request()
+
+                future = self.client.call_async(request)
+                rclpy.spin_until_future_complete(self.node, future)
+                if not future.result():
+                    self.node.get_logger().warn(f"Service {self.service_name} Call {i+1}/{self.count} failed (no response)")
+                    continue
+
+                response = future.result()
+                dt = (time.time() - start) * 1000.0
+                durations.append(dt)
+
+                if hasattr(response, "success"):
+                    if response.success:
+                        success += 1
+                    else:
+                        self.node.get_logger().warn(f"Service {self.service_name} Call {i+1}/{self.count} failed, success=False")
+                else:
+                    success += 1
+            except Exception as e:
+                self.node.get_logger().warn(f"Call {i+1}/{self.count} failed: {e}")
+
+        if durations:
+            avg_time = mean(durations)
+            min_time = min(durations)
+            max_time = max(durations)
+        else:
+            avg_time = min_time = max_time = 0.0
+
+        success_rate = (success / self.count) * 100.0
+
+        return {
+            "Service": self.service_name,
+            "Type": self.service_type,
+            "Calls": self.count,
+            "Success": success,
+            "Success Rate": f"{success_rate:.2f}%",
+            "Avg(ms)": f"{avg_time:.2f}",
+            "Min(ms)": f"{min_time:.2f}",
+            "Max(ms)": f"{max_time:.2f}"
+        }
+
+
+class BenchmarkRunner:
+    def __init__(self, node: Node, yaml_file=None, service=None, count=10):
+        self.node = node
+        self.yaml_file = yaml_file
+        self.service = service
+        self.count = count
+        self.results = []
+
+    def load_from_yaml(self):
+        with open(self.yaml_file, "r") as f:
+            config = yaml.safe_load(f)
+
+        for srv_cfg in config.get("services", []):
+            name = srv_cfg["name"]
+            count = srv_cfg.get("count", config.get("default_count", 10))
+            request = srv_cfg.get("request", None)
+
+            bench = ServiceBenchmark(self.node, name, count, request)
+            result = bench.run()
+            self.results.append(result)
+
+    def run_single(self):
+        if not self.service:
+            self.node.get_logger().error("Need --service or --yaml")
+            return
+        bench = ServiceBenchmark(self.node, self.service, self.count)
+        result = bench.run()
+        self.results.append(result)
+
+    def print_results(self):
+        if not self.results:
+            return
+        headers = self.results[0].keys()
+        rows = [r.values() for r in self.results]
+        print("\nService Benchmark Results")
+        print(tabulate(rows, headers, tablefmt="fancy_grid"))
+
+    def save_to_csv(self, file_path):
+        if not self.results:
+            self.node.get_logger().warn("No results to save")
+            return
+        headers = self.results[0].keys()
+        with open(file_path, mode="w", newline="") as f:
+            writer = csv.DictWriter(f, fieldnames=headers)
+            writer.writeheader()
+            for r in self.results:
+                writer.writerow(r)
+        self.node.get_logger().info(f"Results saved to {file_path}")
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--service", help="Service name")
+    parser.add_argument("--count", type=int, default=10, help="Number of calls")
+    parser.add_argument("--yaml_file", help="YAML config file for batch testing")
+    parser.add_argument("--csv_file", default="multi_service_results_log_py.csv", help="CSV file to save results")
+
+    args = parser.parse_args()
+
+    rclpy.init()
+    node = rclpy.create_node("service_benchmark_node")
+
+    runner = BenchmarkRunner(node, yaml_file=args.yaml_file, service=args.service, count=args.count)
+
+    if args.yaml_file:
+        runner.load_from_yaml()
+    else:
+        runner.run_single()
+
+    runner.print_results()
+    if args.csv_file:
+        runner.save_to_csv(args.csv_file)
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()