diff --git a/orbbec_camera/scripts/common_benchmark_node.py b/orbbec_camera/scripts/common_benchmark_node.py index 040b78bc..15c59b05 100644 --- a/orbbec_camera/scripts/common_benchmark_node.py +++ b/orbbec_camera/scripts/common_benchmark_node.py @@ -4,11 +4,14 @@ 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. + subscriber-side frame rates, end-to-end delays, CPU and RAM usage, + and estimated frame loss statistics. usage: ros2 run orbbec_camera common_benchmark_node.py --run_time 20 --csv_file /tmp/cam_log.csv You can also pass an ideal frame rate for drop detection: --ideal_fps 30 Monitor multiple cameras: --camera_names camera,camera01 + Select topics explicitly: --topics color,depth + Compressed images and point clouds require full topic names. """ import argparse @@ -21,7 +24,7 @@ import csv import os from collections import defaultdict from orbbec_camera_msgs.msg import DeviceStatus -from sensor_msgs.msg import Image +from sensor_msgs.msg import CompressedImage, Image, PointCloud2 from tabulate import tabulate @@ -29,11 +32,14 @@ CAMERA_NODE_NAMES = ["component_container", "orbbec_camera_node", "nodelet"] MONITORED_STREAMS = ( "color", "depth", + "ir", "left_ir", "right_ir", "left_color", "right_color", ) +DISCOVERY_INTERVAL_SECONDS = 0.1 +DISCOVERY_DURATION_SECONDS = 1.0 DOCUMENTATION_URL = ( "https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/" "6_benchmark/benchmark_tools.html" @@ -107,29 +113,62 @@ def estimate_dropped_frames(dt, expected_interval): # ---------------------------------------------- class TopicTracker: - def __init__(self, logger=None): + def __init__(self, logger=None, sample_start_time=None): self.received = 0 - - self.last_time = None + self.sample_start_time = sample_start_time or time.monotonic() + self.last_sample_time = self.sample_start_time + self.last_sample_received = 0 + self.last_header_stamp = None + self.estimated_interval = None self.drop_frames = 0 self.logger = logger - def on_msg(self, header, avg_fps): - stamp = header.stamp.sec + header.stamp.nanosec * 1e-9 + def on_msg(self, header, ros_receive_time, ideal_fps): + """Record one received message and return its age in milliseconds.""" + header_stamp = header.stamp.sec + header.stamp.nanosec * 1e-9 self.received += 1 - observed_fps = None + delay_ms = None - if self.last_time is not None: - dt = stamp - self.last_time - if dt > 0: - observed_fps = 1.0 / dt - if avg_fps > 0: - expected_interval = 1.0 / avg_fps - self.drop_frames += estimate_dropped_frames(dt, expected_interval) + if header_stamp > 0: + delay_ms = (ros_receive_time - header_stamp) * 1000.0 + if self.last_header_stamp is not None: + header_dt = header_stamp - self.last_header_stamp + if header_dt > 0: + expected_interval = ( + 1.0 / ideal_fps + if ideal_fps and ideal_fps > 0.0 + else self.estimated_interval + ) + if expected_interval is not None: + self.drop_frames += estimate_dropped_frames( + header_dt, expected_interval + ) + self.update_estimated_interval(header_dt) + self.last_header_stamp = header_stamp - self.last_time = stamp - return stamp, observed_fps + return delay_ms + + def sample_fps(self, sample_time): + """Calculate current and average subscriber throughput.""" + window_elapsed = sample_time - self.last_sample_time + total_elapsed = sample_time - self.sample_start_time + if window_elapsed <= 0.0 or total_elapsed <= 0.0: + return None + + window_received = self.received - self.last_sample_received + current_fps = window_received / window_elapsed + average_fps = self.received / total_elapsed + self.last_sample_time = sample_time + self.last_sample_received = self.received + return current_fps, average_fps + + def update_estimated_interval(self, interval): + """Learn the nominal source interval while excluding likely frame gaps.""" + if self.estimated_interval is None or interval < 0.75 * self.estimated_interval: + self.estimated_interval = interval + elif interval <= 1.5 * self.estimated_interval: + self.estimated_interval = 0.9 * self.estimated_interval + 0.1 * interval def frames_loss_rate(self): total = self.received + self.drop_frames @@ -142,17 +181,27 @@ class TopicTracker: class CameraMonitorNode(Node): - def __init__(self, run_time, csv_file="camera_monitor_log.csv", ideal_fps: float = 0.0, camera_names=None): + def __init__( + self, + run_time, + csv_file="camera_monitor_log.csv", + ideal_fps: float = 0.0, + camera_names=None, + topics=None, + ): super().__init__("camera_monitor_node") self.run_time = run_time - self.start_time = time.time() + self.discovery_start_time = time.time() + self.discovery_start_monotonic = None + self.start_time = None self.process = psutil.Process(os.getpid()) self.first_data_collected = False self.camera_names = parse_camera_names(camera_names) self.node_names = {camera_name: "Not Found" for camera_name in self.camera_names} self.total_node_name = "Not Found" - # If > 0, use this ideal fps value for drop-frame detection instead of the reported average + # If > 0, use this ideal FPS for drop detection instead of learning the + # nominal interval from received image timestamps. self.ideal_fps = float(ideal_fps) if ideal_fps is not None else 0.0 self.finished = False @@ -166,22 +215,26 @@ class CameraMonitorNode(Node): "stats": defaultdict(make_stat), "cpu_stats": make_stat(), "ram_stats": make_stat(), - "trackers": { - stream: TopicTracker(logger=self.get_logger()) - for stream in MONITORED_STREAMS - } + "trackers": {}, } self.total_cpu_stats = make_stat() self.total_ram_stats = make_stat() + self.topic_subscriptions = {} + self.topic_configs = {} + self.discovered_streams = {} + self.discovery_complete = False + self.discovery_timer = None + self.timer = None + self.requested_streams = self.parse_requested_topics(topics) # 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(self.build_csv_header()) - # subscriptions + # Device status subscriptions are always present. Data subscriptions + # are created once after automatic discovery or explicit selection. for camera_name in self.camera_names: ns = self.camera_namespace(camera_name) self.create_subscription( @@ -190,26 +243,26 @@ class CameraMonitorNode(Node): lambda msg, name=camera_name: self.status_callback(msg, name), 5 ) - for stream in MONITORED_STREAMS: - self.create_subscription( - Image, - f"{ns}/{stream}/image_raw", - lambda msg, name=camera_name, stream_name=stream: self.image_callback( - msg, name, stream_name - ), - 5, - ) - # timer runs every 1s to update system stats, log csv and print status - self.timer = self.create_timer(1.0, self.timer_callback) + if self.requested_streams: + self.finish_topic_discovery(self.requested_streams) + else: + self.discovery_start_monotonic = time.monotonic() + self.discovery_timer = self.create_timer( + DISCOVERY_INTERVAL_SECONDS, self.update_topic_discovery + ) def timer_callback(self): + if not self.discovery_complete: + return + elapsed = time.time() - self.start_time if elapsed > self.run_time: self.finish() rclpy.shutdown() return + self.update_topic_fps_stats() camera_sys_stats, total_cpu, total_ram, self.total_node_name = self.get_camera_stats() for camera_name in self.camera_names: camera = self.cameras[camera_name] @@ -230,7 +283,8 @@ class CameraMonitorNode(Node): return self.finished = True - elapsed = time.time() - self.start_time + timer_start = self.start_time or self.discovery_start_time + elapsed = time.time() - timer_start try: self.csv_fh.close() except Exception: @@ -241,6 +295,150 @@ class CameraMonitorNode(Node): def camera_namespace(self, camera_name): return "/" + camera_name.strip("/") + def image_topic_name(self, camera_name, stream): + return f"{self.camera_namespace(camera_name)}/{stream}/image_raw" + + def make_raw_image_config(self, camera_name, stream): + return { + "camera_name": camera_name, + "topic_id": stream, + "topic_name": self.image_topic_name(camera_name, stream), + "msg_type": Image, + } + + def parse_full_topic(self, topic_name): + normalized_topic = "/" + topic_name.strip("/") + for camera_name in self.camera_names: + namespace_prefix = self.camera_namespace(camera_name) + "/" + if not normalized_topic.startswith(namespace_prefix): + continue + + relative_name = normalized_topic[len(namespace_prefix):] + for stream in MONITORED_STREAMS: + raw_name = f"{stream}/image_raw" + if relative_name == raw_name: + return self.make_raw_image_config(camera_name, stream) + if relative_name == f"{raw_name}/compressed": + return { + "camera_name": camera_name, + "topic_id": f"{stream}_compressed", + "topic_name": normalized_topic, + "msg_type": CompressedImage, + } + if relative_name == f"{raw_name}/compressedDepth": + return { + "camera_name": camera_name, + "topic_id": f"{stream}_compressed_depth", + "topic_name": normalized_topic, + "msg_type": CompressedImage, + } + + point_cloud_ids = { + "depth/points": "depth_points", + "depth_registered/points": "depth_registered_points", + } + if relative_name in point_cloud_ids: + return { + "camera_name": camera_name, + "topic_id": point_cloud_ids[relative_name], + "topic_name": normalized_topic, + "msg_type": PointCloud2, + } + return None + + def parse_requested_topics(self, topics): + if not topics: + return {} + + requested = str(topics).replace(";", ",").split(",") + selected = {} + for value in requested: + value = value.strip() + if not value: + continue + if value in MONITORED_STREAMS: + for camera_name in self.camera_names: + config = self.make_raw_image_config(camera_name, value) + selected[(camera_name, config["topic_id"])] = config + continue + + config = self.parse_full_topic(value) + if config is None: + raise ValueError( + f"Unsupported topic '{value}'. Specify a raw or compressed image topic, " + "or a depth/points or depth_registered/points topic under a configured " + "camera namespace." + ) + selected[(config["camera_name"], config["topic_id"])] = config + return selected + + def find_published_image_streams(self): + published_streams = {} + for camera_name in self.camera_names: + for stream in MONITORED_STREAMS: + config = self.make_raw_image_config(camera_name, stream) + key = (camera_name, config["topic_id"]) + if self.count_publishers(config["topic_name"]) > 0: + published_streams[key] = config + return published_streams + + def update_topic_discovery(self): + if self.discovery_complete: + return + + self.discovered_streams.update(self.find_published_image_streams()) + elapsed = time.monotonic() - self.discovery_start_monotonic + if elapsed >= DISCOVERY_DURATION_SECONDS: + self.finish_topic_discovery(self.discovered_streams) + + def finish_topic_discovery(self, selected_streams): + self.topic_configs = dict(selected_streams) + sample_start_time = time.monotonic() + for key, config in self.topic_configs.items(): + camera_name = config["camera_name"] + topic_id = config["topic_id"] + self.cameras[camera_name]["trackers"][topic_id] = TopicTracker( + logger=self.get_logger(), sample_start_time=sample_start_time + ) + self.topic_subscriptions[key] = self.create_subscription( + config["msg_type"], + config["topic_name"], + lambda msg, name=camera_name, selected_topic_id=topic_id: self.topic_callback( + msg, name, selected_topic_id + ), + 5, + ) + + self.csv_writer.writerow(self.build_csv_header()) + self.csv_fh.flush() + self.start_time = time.time() + self.discovery_complete = True + self.timer = self.create_timer(1.0, self.timer_callback) + if self.discovery_timer is not None: + self.discovery_timer.cancel() + + def topics_for_camera(self, camera_name): + return [ + config + for config in self.topic_configs.values() + if config["camera_name"] == camera_name + ] + + def update_topic_fps_stats(self): + sample_time = time.monotonic() + for config in self.topic_configs.values(): + camera = self.cameras[config["camera_name"]] + topic_id = config["topic_id"] + fps_sample = camera["trackers"][topic_id].sample_fps(sample_time) + if fps_sample is None: + continue + current_fps, average_fps = fps_sample + self.update_sample_stat( + camera["stats"][f"{topic_id}_fps"], + current_fps, + average=average_fps, + ) + def cmdline_has_camera_namespace(self, cmdline_args, camera_name): ns = self.camera_namespace(camera_name) candidates = [ @@ -321,48 +519,30 @@ class CameraMonitorNode(Node): camera["prev_online"] = msg.device_online - # Update every image stream from the native DeviceStatus fields. - for stream in MONITORED_STREAMS: - self.update_stats( - camera["stats"], - f"{stream}_fps", - getattr(msg, f"{stream}_frame_rate_cur"), - getattr(msg, f"{stream}_frame_rate_min"), - getattr(msg, f"{stream}_frame_rate_max"), - getattr(msg, f"{stream}_frame_rate_avg"), - ) - self.update_stats( - camera["stats"], - f"{stream}_delay", - getattr(msg, f"{stream}_delay_ms_cur"), - getattr(msg, f"{stream}_delay_ms_min"), - getattr(msg, f"{stream}_delay_ms_max"), - getattr(msg, f"{stream}_delay_ms_avg"), - ) - - def image_callback(self, msg: Image, camera_name: str, stream: str): - if stream not in MONITORED_STREAMS: - return + def topic_callback(self, msg, camera_name: str, topic_id: str): + self.first_data_collected = True camera = self.cameras[camera_name] - tracker = camera["trackers"][stream] - # Prefer a user-specified ideal fps for drop detection when provided. - fps_to_use = ( - self.ideal_fps - if self.ideal_fps and self.ideal_fps > 0.0 - else camera["stats"][f"{stream}_fps"]["avg"] + camera["data_collected"] = True + tracker = camera["trackers"][topic_id] + ros_receive_time = self.get_clock().now().nanoseconds * 1e-9 + delay_ms = tracker.on_msg( + msg.header, + ros_receive_time, + self.ideal_fps, ) - tracker.on_msg(msg.header, fps_to_use) - def update_stats(self, stats, key, cur, min_val, max_val, avg_val): - if min_val <= 1e-3 or avg_val < 0: # ignore invalid data - return - s = 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) + if delay_ms is not None: + self.update_sample_stat( + camera["stats"][f"{topic_id}_delay"], delay_ms + ) + + def update_sample_stat(self, stat, value, average=None): + stat["cur"] = value + stat["count"] += 1 + stat["sum"] += value + stat["avg"] = average if average is not None else stat["sum"] / stat["count"] + stat["min"] = min(stat["min"], value) + stat["max"] = max(stat["max"], value) def update_sys_stat(self, stat_dict, value, online=True): stat_dict["cur"] = value @@ -379,7 +559,7 @@ class CameraMonitorNode(Node): row = [round(elapsed, 2)] for camera_name in self.camera_names: camera = self.cameras[camera_name] - row.extend(self.build_camera_csv_values(camera)) + row.extend(self.build_camera_csv_values(camera_name, camera)) row.extend([ round(self.total_cpu_stats["cur"], 2), round(self.total_cpu_stats["avg"], 2), @@ -391,30 +571,42 @@ class CameraMonitorNode(Node): def build_csv_header(self): header = ["time(s)"] - camera_fields = ["connection_type", "status_online", "disconnects"] - for stream in MONITORED_STREAMS: - camera_fields.extend( + for camera_name in self.camera_names: + header.extend( [ - f"{stream}_fps_cur", - f"{stream}_fps_avg", - f"{stream}_fps_min", - f"{stream}_fps_max", - f"{stream}_delay_cur", - f"{stream}_delay_avg", - f"{stream}_delay_min", - f"{stream}_delay_max", - f"{stream}_frames_loss", - f"{stream}_frames_loss_rate(%)", + f"{camera_name}_connection_type", + f"{camera_name}_status_online", + f"{camera_name}_disconnects", + ] + ) + for config in self.topics_for_camera(camera_name): + topic_id = config["topic_id"] + header.extend( + [ + f"{camera_name}_{topic_id}_fps_cur", + f"{camera_name}_{topic_id}_fps_avg", + f"{camera_name}_{topic_id}_fps_min", + f"{camera_name}_{topic_id}_fps_max", + f"{camera_name}_{topic_id}_delay_cur", + f"{camera_name}_{topic_id}_delay_avg", + f"{camera_name}_{topic_id}_delay_min", + f"{camera_name}_{topic_id}_delay_max", + f"{camera_name}_{topic_id}_sub_lost_count", + f"{camera_name}_{topic_id}_sub_lost_rate(%)", + ] + ) + header.extend( + [ + f"{camera_name}_cpu_cur", + f"{camera_name}_cpu_avg", + f"{camera_name}_cpu_min", + f"{camera_name}_cpu_max", + f"{camera_name}_ram_cur", + f"{camera_name}_ram_avg", + f"{camera_name}_ram_min", + f"{camera_name}_ram_max", ] ) - camera_fields.extend( - [ - "cpu_cur", "cpu_avg", "cpu_min", "cpu_max", - "ram_cur", "ram_avg", "ram_min", "ram_max", - ] - ) - for camera_name in self.camera_names: - header.extend([f"{camera_name}_{field}" for field in camera_fields]) header.extend([ "total_cpu_cur", "total_cpu_avg", "total_cpu_min", "total_cpu_max", @@ -422,7 +614,7 @@ class CameraMonitorNode(Node): ]) return header - def build_camera_csv_values(self, camera): + def build_camera_csv_values(self, camera_name, camera): def safe(k): v = camera["stats"].get(k, {}) return ( @@ -437,11 +629,12 @@ class CameraMonitorNode(Node): camera["prev_online"], camera["disconnect_count"], ] - for stream in MONITORED_STREAMS: - tracker = camera["trackers"][stream] + for config in self.topics_for_camera(camera_name): + topic_id = config["topic_id"] + tracker = camera["trackers"][topic_id] if camera["prev_online"]: - values.extend(safe(f"{stream}_fps")) - values.extend(safe(f"{stream}_delay")) + values.extend(safe(f"{topic_id}_fps")) + values.extend(safe(f"{topic_id}_delay")) else: values.extend(["N/A"] * 8) values.extend( @@ -487,25 +680,30 @@ class CameraMonitorNode(Node): rows = [] for camera_name in self.camera_names: camera = self.cameras[camera_name] - for stream in MONITORED_STREAMS: - fps_key = f"{stream}_fps" - delay_key = f"{stream}_delay" - topic_name = f"/{camera_name}/{stream}/image_raw" + for config in self.topics_for_camera(camera_name): + topic_id = config["topic_id"] + fps_key = f"{topic_id}_fps" + delay_key = f"{topic_id}_delay" + topic_name = config["topic_name"] if not camera["prev_online"]: rows.append([camera_name, topic_name, *["N/A"] * 10]) else: fps_vals = format_stats(camera["stats"][fps_key]) delay_vals = format_stats(camera["stats"][delay_key]) - tracker = camera["trackers"][stream] + tracker = camera["trackers"][topic_id] frames_loss = tracker.drop_frames frames_loss_rate = round(tracker.frames_loss_rate() * 100.0, 3) rows.append([camera_name, topic_name, *fps_vals, *delay_vals, frames_loss, frames_loss_rate]) - header_bottom = ["Camera", "Topic", "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(%)"] + header_bottom = [ + "Camera", "Topic", "fps_cur", "fps_avg", "fps_min", "fps_max", + "delay_cur(ms)", "delay_avg(ms)", "delay_min(ms)", "delay_max(ms)", + "Sub_lost_count", "Sub_lost_rate(%)", + ] os.system("clear") - print("Orbbec Camera Benchmark\n") + print("Orbbec Camera Subscriber Benchmark\n") print(tabulate([header_bottom] + rows, tablefmt="fancy_grid")) sys_rows = [] @@ -542,13 +740,36 @@ def main(argv=None): ) 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") - parser.add_argument("--ideal_fps", type=float, default=0.0, help="Optional ideal frame rate to use for drop detection (overrides reported avg).") + parser.add_argument( + "--ideal_fps", + type=float, + default=0.0, + help=( + "Optional ideal frame rate for subscriber-side drop detection; " + "otherwise it is learned from image timestamps." + ), + ) parser.add_argument("--camera_names", type=str, default="camera", help="Comma-separated camera namespaces, e.g., camera,camera01,camera02.") + parser.add_argument( + "--topics", + type=str, + default="", + help=( + "Comma-separated raw stream names or full raw/compressed image and point cloud " + "topics. Automatic discovery only selects raw image topics." + ), + ) 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, ideal_fps=cli_args.ideal_fps, camera_names=cli_args.camera_names) + node = CameraMonitorNode( + run_time, + cli_args.csv_file, + ideal_fps=cli_args.ideal_fps, + camera_names=cli_args.camera_names, + topics=cli_args.topics, + ) try: rclpy.spin(node)