i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-09 14:27:02 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat: monitor side camera streams

Browse Source
This commit is contained in:
slz
2026-09-10 10:45:25 +08:00
parent b76e062fe5
commit a3a7231302
9 changed files with 290 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/scripts/common_benchmark_node.py
+19 -27
View File
@@ -321,11 +321,24 @@ class CameraMonitorNode(Node):
camera["prev_online"] = msg.device_online
# update stats from DeviceStatus message fields
self.update_stats(camera["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(camera["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(camera["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(camera["stats"], "depth_delay", msg.depth_delay_ms_cur, msg.depth_delay_ms_min, msg.depth_delay_ms_max, msg.depth_delay_ms_avg)
# 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:
@@ -338,17 +351,7 @@ class CameraMonitorNode(Node):
if self.ideal_fps and self.ideal_fps > 0.0
else camera["stats"][f"{stream}_fps"]["avg"]
)
stamp, observed_fps = tracker.on_msg(msg.header, fps_to_use)
# DeviceStatus currently reports detailed values only for the main color/depth streams.
# Derive equivalent statistics from image timestamps for the side streams.
if stream not in ("color", "depth"):
if observed_fps is not None:
self.update_sample_stat(camera["stats"], f"{stream}_fps", observed_fps)
delay_ms = (self.get_clock().now().nanoseconds * 1e-9 - stamp) * 1000.0
# Device-domain timestamps are not comparable with the ROS clock.
if 0.0 <= delay_ms <= 60000.0:
self.update_sample_stat(camera["stats"], f"{stream}_delay", delay_ms)
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
@@ -361,17 +364,6 @@ class CameraMonitorNode(Node):
s["min"] = min(s["min"], min_val)
s["max"] = max(s["max"], max_val)
def update_sample_stat(self, stats, key, value):
if value is None or value < 0.0:
return
s = stats[key]
s["cur"] = value
s["count"] += 1
s["sum"] += value
s["avg"] = s["sum"] / s["count"]
s["min"] = min(s["min"], value)
s["max"] = max(s["max"], value)
def update_sys_stat(self, stat_dict, value, online=True):
stat_dict["cur"] = value
if value is None or value <= 0.0 or not online: