mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-09 14:27:02 +08:00
feat: monitor side camera streams
This commit is contained in:
slz
@@ -20,6 +20,7 @@
|
||||
#include "orbbec_camera_msgs/msg/device_status.hpp"
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <limits>
|
||||
#include <mutex>
|
||||
namespace orbbec_camera {
|
||||
class FpsDelayStatus {
|
||||
@@ -58,38 +59,57 @@ class FpsDelayStatus {
|
||||
}
|
||||
|
||||
void fillColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
msg.color_frame_rate_cur = last_fps_;
|
||||
msg.color_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
|
||||
msg.color_frame_rate_min = fps_min_;
|
||||
msg.color_frame_rate_max = fps_max_;
|
||||
fillStatus(msg.color_frame_rate_cur, msg.color_frame_rate_avg, msg.color_frame_rate_min,
|
||||
msg.color_frame_rate_max, msg.color_delay_ms_cur, msg.color_delay_ms_avg,
|
||||
msg.color_delay_ms_min, msg.color_delay_ms_max);
|
||||
}
|
||||
|
||||
msg.color_delay_ms_cur = last_delay_ms_;
|
||||
msg.color_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
|
||||
msg.color_delay_ms_min = delay_min_;
|
||||
msg.color_delay_ms_max = delay_max_;
|
||||
void fillLeftColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
|
||||
fillStatus(msg.left_color_frame_rate_cur, msg.left_color_frame_rate_avg,
|
||||
msg.left_color_frame_rate_min, msg.left_color_frame_rate_max,
|
||||
msg.left_color_delay_ms_cur, msg.left_color_delay_ms_avg,
|
||||
msg.left_color_delay_ms_min, msg.left_color_delay_ms_max);
|
||||
}
|
||||
|
||||
last_delay_ms_ = 0.0;
|
||||
last_fps_ = 0.0;
|
||||
frame_count_ = 0;
|
||||
fps_sum_ = delay_sum_ = 0.0;
|
||||
fps_max_ = delay_max_ = 0.0;
|
||||
fps_min_ = delay_min_ = 0.0;
|
||||
void fillRightColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
|
||||
fillStatus(msg.right_color_frame_rate_cur, msg.right_color_frame_rate_avg,
|
||||
msg.right_color_frame_rate_min, msg.right_color_frame_rate_max,
|
||||
msg.right_color_delay_ms_cur, msg.right_color_delay_ms_avg,
|
||||
msg.right_color_delay_ms_min, msg.right_color_delay_ms_max);
|
||||
}
|
||||
|
||||
void fillDepthStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
|
||||
fillStatus(msg.depth_frame_rate_cur, msg.depth_frame_rate_avg, msg.depth_frame_rate_min,
|
||||
msg.depth_frame_rate_max, msg.depth_delay_ms_cur, msg.depth_delay_ms_avg,
|
||||
msg.depth_delay_ms_min, msg.depth_delay_ms_max);
|
||||
}
|
||||
|
||||
void fillLeftIrStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
|
||||
fillStatus(msg.left_ir_frame_rate_cur, msg.left_ir_frame_rate_avg, msg.left_ir_frame_rate_min,
|
||||
msg.left_ir_frame_rate_max, msg.left_ir_delay_ms_cur, msg.left_ir_delay_ms_avg,
|
||||
msg.left_ir_delay_ms_min, msg.left_ir_delay_ms_max);
|
||||
}
|
||||
|
||||
void fillRightIrStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
|
||||
fillStatus(msg.right_ir_frame_rate_cur, msg.right_ir_frame_rate_avg,
|
||||
msg.right_ir_frame_rate_min, msg.right_ir_frame_rate_max, msg.right_ir_delay_ms_cur,
|
||||
msg.right_ir_delay_ms_avg, msg.right_ir_delay_ms_min, msg.right_ir_delay_ms_max);
|
||||
}
|
||||
|
||||
private:
|
||||
void fillStatus(double &frame_rate_cur, double &frame_rate_avg, double &frame_rate_min,
|
||||
double &frame_rate_max, double &delay_ms_cur, double &delay_ms_avg,
|
||||
double &delay_ms_min, double &delay_ms_max) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
msg.depth_frame_rate_cur = last_fps_;
|
||||
msg.depth_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
|
||||
msg.depth_frame_rate_min = fps_min_;
|
||||
msg.depth_frame_rate_max = fps_max_;
|
||||
frame_rate_cur = last_fps_;
|
||||
frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
|
||||
frame_rate_min = frame_count_ > 0 ? fps_min_ : 0;
|
||||
frame_rate_max = frame_count_ > 0 ? fps_max_ : 0;
|
||||
|
||||
msg.depth_delay_ms_cur = last_delay_ms_;
|
||||
msg.depth_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
|
||||
msg.depth_delay_ms_min = delay_min_;
|
||||
msg.depth_delay_ms_max = delay_max_;
|
||||
|
||||
// RCLCPP_ERROR_STREAM(logger_, "Depth status: " << fps_sum_ << "," << frame_count_);
|
||||
delay_ms_cur = last_delay_ms_;
|
||||
delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
|
||||
delay_ms_min = frame_count_ > 0 ? delay_min_ : 0;
|
||||
delay_ms_max = frame_count_ > 0 ? delay_max_ : 0;
|
||||
|
||||
last_delay_ms_ = 0.0;
|
||||
last_fps_ = 0.0;
|
||||
@@ -99,7 +119,6 @@ class FpsDelayStatus {
|
||||
fps_min_ = delay_min_ = 0.0;
|
||||
}
|
||||
|
||||
private:
|
||||
mutable std::mutex mutex_;
|
||||
u_int64_t last_stream_timestamp_{0};
|
||||
double last_delay_ms_{0.0};
|
||||
|
||||
@@ -21,7 +21,7 @@ namespace orbbec_camera {
|
||||
|
||||
class FrameTimestampCsvLogger {
|
||||
public:
|
||||
enum class OutputMode { SYNCED, COLOR, DEPTH };
|
||||
enum class OutputMode { SYNCED, COLOR, LEFT_COLOR, RIGHT_COLOR, DEPTH, LEFT_IR, RIGHT_IR };
|
||||
|
||||
FrameTimestampCsvLogger(bool drop_log_enabled, const std::string &csv_file_path,
|
||||
OutputMode output_mode, rclcpp::Logger logger);
|
||||
|
||||
@@ -237,10 +237,14 @@ class OBCameraNode {
|
||||
}
|
||||
void getColorStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
|
||||
fps_delay_status_color_->fillColorStatus(status_msg);
|
||||
fps_delay_status_left_color_->fillLeftColorStatus(status_msg);
|
||||
fps_delay_status_right_color_->fillRightColorStatus(status_msg);
|
||||
}
|
||||
|
||||
void getDepthStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
|
||||
fps_delay_status_depth_->fillDepthStatus(status_msg);
|
||||
fps_delay_status_left_ir_->fillLeftIrStatus(status_msg);
|
||||
fps_delay_status_right_ir_->fillRightIrStatus(status_msg);
|
||||
}
|
||||
|
||||
bool checkUserCalibrationReady() {
|
||||
@@ -1188,12 +1192,18 @@ class OBCameraNode {
|
||||
bool show_fps_enable_ = false;
|
||||
bool enable_publish_extrinsic_ = false;
|
||||
std::unique_ptr<FpsCounter> fps_counter_color_{nullptr};
|
||||
std::unique_ptr<FpsCounter> fps_counter_left_color_{nullptr};
|
||||
std::unique_ptr<FpsCounter> fps_counter_right_color_{nullptr};
|
||||
std::unique_ptr<FpsCounter> fps_counter_depth_{nullptr};
|
||||
std::unique_ptr<FpsCounter> fps_counter_left_ir_{nullptr};
|
||||
std::unique_ptr<FpsCounter> fps_counter_right_ir_{nullptr};
|
||||
|
||||
std::unique_ptr<FpsDelayStatus> fps_delay_status_color_{nullptr};
|
||||
std::unique_ptr<FpsDelayStatus> fps_delay_status_left_color_{nullptr};
|
||||
std::unique_ptr<FpsDelayStatus> fps_delay_status_right_color_{nullptr};
|
||||
std::unique_ptr<FpsDelayStatus> fps_delay_status_depth_{nullptr};
|
||||
std::unique_ptr<FpsDelayStatus> fps_delay_status_left_ir_{nullptr};
|
||||
std::unique_ptr<FpsDelayStatus> fps_delay_status_right_ir_{nullptr};
|
||||
|
||||
std::string intra_camera_sync_reference_ = "";
|
||||
std::string ae_reference_stream_;
|
||||
|
||||
@@ -22,7 +22,11 @@ class TimestampCsvLogger {
|
||||
std::string csv_file_path;
|
||||
bool frame_sync_enabled = false;
|
||||
bool color_enabled = false;
|
||||
bool left_color_enabled = false;
|
||||
bool right_color_enabled = false;
|
||||
bool depth_enabled = false;
|
||||
bool left_ir_enabled = false;
|
||||
bool right_ir_enabled = false;
|
||||
bool imu_sync_enabled = false;
|
||||
bool accel_enabled = false;
|
||||
bool gyro_enabled = false;
|
||||
@@ -44,6 +48,9 @@ class TimestampCsvLogger {
|
||||
const std::shared_ptr<ob::Frame> &depth_frame, int64_t arrival_system_us,
|
||||
int64_t arrival_steady_us, bool track_color, bool track_depth,
|
||||
bool color_image_publish_expected, bool depth_image_publish_expected);
|
||||
void recordImageFrameArrival(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
|
||||
int64_t arrival_system_us, int64_t arrival_steady_us,
|
||||
bool image_publish_expected);
|
||||
void recordImagePrePublish(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
|
||||
int64_t publish_system_us, int64_t publish_steady_us);
|
||||
void recordImagePublishSkipped(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame);
|
||||
@@ -64,7 +71,11 @@ class TimestampCsvLogger {
|
||||
std::atomic_bool shutdown_requested_{false};
|
||||
std::unique_ptr<FrameTimestampCsvLogger> synced_image_logger_;
|
||||
std::unique_ptr<FrameTimestampCsvLogger> color_logger_;
|
||||
std::unique_ptr<FrameTimestampCsvLogger> left_color_logger_;
|
||||
std::unique_ptr<FrameTimestampCsvLogger> right_color_logger_;
|
||||
std::unique_ptr<FrameTimestampCsvLogger> depth_logger_;
|
||||
std::unique_ptr<FrameTimestampCsvLogger> left_ir_logger_;
|
||||
std::unique_ptr<FrameTimestampCsvLogger> right_ir_logger_;
|
||||
std::unique_ptr<ImuTimestampCsvLogger> synced_imu_logger_;
|
||||
std::unique_ptr<ImuTimestampCsvLogger> accel_logger_;
|
||||
std::unique_ptr<ImuTimestampCsvLogger> gyro_logger_;
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -31,6 +31,47 @@ int64_t getExpectedIntervalUs(const std::shared_ptr<ob::Frame> &frame) {
|
||||
return static_cast<int64_t>(1000000.0 / static_cast<double>(fps));
|
||||
}
|
||||
|
||||
std::optional<FrameTimestampCsvLogger::OutputMode> outputModeForStream(OBStreamType stream_type) {
|
||||
using OutputMode = FrameTimestampCsvLogger::OutputMode;
|
||||
switch (stream_type) {
|
||||
case OB_STREAM_COLOR:
|
||||
return OutputMode::COLOR;
|
||||
case OB_STREAM_COLOR_LEFT:
|
||||
return OutputMode::LEFT_COLOR;
|
||||
case OB_STREAM_COLOR_RIGHT:
|
||||
return OutputMode::RIGHT_COLOR;
|
||||
case OB_STREAM_DEPTH:
|
||||
return OutputMode::DEPTH;
|
||||
case OB_STREAM_IR_LEFT:
|
||||
return OutputMode::LEFT_IR;
|
||||
case OB_STREAM_IR_RIGHT:
|
||||
return OutputMode::RIGHT_IR;
|
||||
default:
|
||||
return std::nullopt;
|
||||
}
|
||||
}
|
||||
|
||||
const char *outputModeName(FrameTimestampCsvLogger::OutputMode output_mode) {
|
||||
using OutputMode = FrameTimestampCsvLogger::OutputMode;
|
||||
switch (output_mode) {
|
||||
case OutputMode::COLOR:
|
||||
return "color";
|
||||
case OutputMode::LEFT_COLOR:
|
||||
return "left_color";
|
||||
case OutputMode::RIGHT_COLOR:
|
||||
return "right_color";
|
||||
case OutputMode::DEPTH:
|
||||
return "depth";
|
||||
case OutputMode::LEFT_IR:
|
||||
return "left_ir";
|
||||
case OutputMode::RIGHT_IR:
|
||||
return "right_ir";
|
||||
case OutputMode::SYNCED:
|
||||
return "synced";
|
||||
}
|
||||
return "unknown";
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
|
||||
@@ -104,9 +145,8 @@ void FrameTimestampCsvLogger::recordStandaloneFrameArrival(OBStreamType stream_t
|
||||
int64_t arrival_system_us,
|
||||
int64_t arrival_steady_us,
|
||||
bool image_publish_expected) {
|
||||
if (!enabled_ || !frame || !isTrackedStream(stream_type) ||
|
||||
(stream_type == OB_STREAM_COLOR && output_mode_ != OutputMode::COLOR) ||
|
||||
(stream_type == OB_STREAM_DEPTH && output_mode_ != OutputMode::DEPTH)) {
|
||||
const auto expected_output_mode = outputModeForStream(stream_type);
|
||||
if (!enabled_ || !frame || !expected_output_mode || output_mode_ != *expected_output_mode) {
|
||||
return;
|
||||
}
|
||||
recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us,
|
||||
@@ -163,14 +203,11 @@ void FrameTimestampCsvLogger::shutdown() {
|
||||
|
||||
FrameTimestampCsvLogger::TrackedStream FrameTimestampCsvLogger::toTrackedStream(
|
||||
OBStreamType stream_type) const {
|
||||
if (stream_type == OB_STREAM_COLOR) {
|
||||
return TrackedStream::COLOR;
|
||||
}
|
||||
return TrackedStream::DEPTH;
|
||||
return stream_type == OB_STREAM_DEPTH ? TrackedStream::DEPTH : TrackedStream::COLOR;
|
||||
}
|
||||
|
||||
bool FrameTimestampCsvLogger::isTrackedStream(OBStreamType stream_type) const {
|
||||
return stream_type == OB_STREAM_COLOR || stream_type == OB_STREAM_DEPTH;
|
||||
return outputModeForStream(stream_type).has_value();
|
||||
}
|
||||
|
||||
void FrameTimestampCsvLogger::recordFrameSetInternal(
|
||||
@@ -294,10 +331,12 @@ void FrameTimestampCsvLogger::completeImagePublishInternal(
|
||||
auto row_id_it = row_map.find(frame_index);
|
||||
if (row_id_it == row_map.end()) {
|
||||
if (publish_system_us.has_value()) {
|
||||
RCLCPP_WARN_STREAM(logger_,
|
||||
"Frame timestamp CSV logger missed row mapping for stream "
|
||||
<< (tracked_stream == TrackedStream::COLOR ? "color" : "depth")
|
||||
<< " frame index " << frame_index);
|
||||
RCLCPP_WARN_STREAM(
|
||||
logger_, "Frame timestamp CSV logger missed row mapping for stream "
|
||||
<< (output_mode_ == OutputMode::SYNCED
|
||||
? (tracked_stream == TrackedStream::COLOR ? "color" : "depth")
|
||||
: outputModeName(output_mode_))
|
||||
<< " frame index " << frame_index);
|
||||
}
|
||||
return;
|
||||
}
|
||||
@@ -357,7 +396,10 @@ void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStr
|
||||
previous.dropped_frames += lost_frames;
|
||||
RCLCPP_WARN_STREAM(logger_,
|
||||
"Frame drop detected: stage=SDK_RECEIVE"
|
||||
<< " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth")
|
||||
<< " stream="
|
||||
<< (output_mode_ == OutputMode::SYNCED
|
||||
? (stream == TrackedStream::COLOR ? "color" : "depth")
|
||||
: outputModeName(output_mode_))
|
||||
<< " frame_index=" << state.frame_index
|
||||
<< " dropped=" << previous.dropped_frames);
|
||||
}
|
||||
@@ -408,7 +450,10 @@ void FrameTimestampCsvLogger::populatePublishData(StreamState &state, TrackedStr
|
||||
previous.publish_dropped_frames += lost_frames;
|
||||
RCLCPP_WARN_STREAM(logger_,
|
||||
"Frame drop detected: stage=ROS_PUBLISH"
|
||||
<< " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth")
|
||||
<< " stream="
|
||||
<< (output_mode_ == OutputMode::SYNCED
|
||||
? (stream == TrackedStream::COLOR ? "color" : "depth")
|
||||
: outputModeName(output_mode_))
|
||||
<< " frame_index=" << state.frame_index
|
||||
<< " dropped=" << previous.publish_dropped_frames);
|
||||
}
|
||||
@@ -485,12 +530,12 @@ void FrameTimestampCsvLogger::eraseFrameIndexMappingLocked(const PendingRow &row
|
||||
}
|
||||
|
||||
std::string FrameTimestampCsvLogger::serializeRow(const PendingRow &row) const {
|
||||
if (output_mode_ == OutputMode::COLOR) {
|
||||
return serializeStreamColumns(row.color);
|
||||
}
|
||||
if (output_mode_ == OutputMode::DEPTH) {
|
||||
return serializeStreamColumns(row.depth);
|
||||
}
|
||||
if (output_mode_ != OutputMode::SYNCED) {
|
||||
return serializeStreamColumns(row.color);
|
||||
}
|
||||
std::ostringstream ss;
|
||||
ss << serializeStreamColumns(row.color) << "," << serializeStreamColumns(row.depth);
|
||||
return ss.str();
|
||||
@@ -560,14 +605,12 @@ std::string FrameTimestampCsvLogger::csvHeader() const {
|
||||
ss << prefix << "_sdk_delay_from_global_us,";
|
||||
ss << prefix << "_sdk_delay_from_system_us";
|
||||
};
|
||||
if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::COLOR) {
|
||||
append_stream_header("color");
|
||||
}
|
||||
if (output_mode_ == OutputMode::SYNCED) {
|
||||
append_stream_header("color");
|
||||
ss << ",";
|
||||
}
|
||||
if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::DEPTH) {
|
||||
append_stream_header("depth");
|
||||
} else {
|
||||
append_stream_header(outputModeName(output_mode_));
|
||||
}
|
||||
return ss.str();
|
||||
}
|
||||
@@ -641,10 +684,8 @@ void FrameTimestampCsvLogger::writerThreadMain() {
|
||||
std::string FrameTimestampCsvLogger::csvFilePathForIndex(uint64_t file_index) const {
|
||||
const std::filesystem::path original_path(csv_file_path_);
|
||||
std::string suffix;
|
||||
if (output_mode_ == OutputMode::COLOR) {
|
||||
suffix = "_color";
|
||||
} else if (output_mode_ == OutputMode::DEPTH) {
|
||||
suffix = "_depth";
|
||||
if (output_mode_ != OutputMode::SYNCED) {
|
||||
suffix = "_" + std::string(outputModeName(output_mode_));
|
||||
}
|
||||
|
||||
auto indexed_filename = original_path.stem().string() + suffix;
|
||||
|
||||
@@ -741,7 +741,11 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
|
||||
timestamp_config.csv_file_path = frame_timestamp_csv_file_;
|
||||
timestamp_config.frame_sync_enabled = enable_frame_sync_;
|
||||
timestamp_config.color_enabled = enable_stream_[COLOR];
|
||||
timestamp_config.left_color_enabled = enable_stream_[COLOR_LEFT];
|
||||
timestamp_config.right_color_enabled = enable_stream_[COLOR_RIGHT];
|
||||
timestamp_config.depth_enabled = enable_stream_[DEPTH];
|
||||
timestamp_config.left_ir_enabled = enable_stream_[INFRA1];
|
||||
timestamp_config.right_ir_enabled = enable_stream_[INFRA2];
|
||||
timestamp_config.imu_sync_enabled = enable_sync_output_accel_gyro_;
|
||||
timestamp_config.accel_enabled = enable_stream_[ACCEL];
|
||||
timestamp_config.gyro_enabled = enable_stream_[GYRO];
|
||||
@@ -761,6 +765,8 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
|
||||
is_camera_node_initialized_ = true;
|
||||
|
||||
fps_counter_color_ = std::make_unique<FpsCounter>("Color", logger_, 1);
|
||||
fps_counter_left_color_ = std::make_unique<FpsCounter>("Left Color", logger_, 1);
|
||||
fps_counter_right_color_ = std::make_unique<FpsCounter>("Right Color", logger_, 1);
|
||||
fps_counter_depth_ = std::make_unique<FpsCounter>("Depth", logger_, 1);
|
||||
fps_counter_left_ir_ = std::make_unique<FpsCounter>("Left Ir", logger_, 1);
|
||||
fps_counter_right_ir_ = std::make_unique<FpsCounter>("Right Ir", logger_, 1);
|
||||
@@ -770,12 +776,18 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
|
||||
log_level = LogLevel::INFO;
|
||||
}
|
||||
fps_counter_color_->setLogLevel(log_level);
|
||||
fps_counter_left_color_->setLogLevel(log_level);
|
||||
fps_counter_right_color_->setLogLevel(log_level);
|
||||
fps_counter_depth_->setLogLevel(log_level);
|
||||
fps_counter_left_ir_->setLogLevel(log_level);
|
||||
fps_counter_right_ir_->setLogLevel(log_level);
|
||||
|
||||
fps_delay_status_color_ = std::make_unique<FpsDelayStatus>(logger_);
|
||||
fps_delay_status_left_color_ = std::make_unique<FpsDelayStatus>(logger_);
|
||||
fps_delay_status_right_color_ = std::make_unique<FpsDelayStatus>(logger_);
|
||||
fps_delay_status_depth_ = std::make_unique<FpsDelayStatus>(logger_);
|
||||
fps_delay_status_left_ir_ = std::make_unique<FpsDelayStatus>(logger_);
|
||||
fps_delay_status_right_ir_ = std::make_unique<FpsDelayStatus>(logger_);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
@@ -6473,6 +6485,20 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
|
||||
final_color_frame, final_depth_frame, frame_set_arrival_system_us,
|
||||
frame_set_arrival_steady_us, track_color, track_depth, color_publish_expected,
|
||||
depth_publish_expected);
|
||||
|
||||
const auto record_side_stream = [&](const stream_index_pair &stream_index,
|
||||
OBFrameType frame_type) {
|
||||
auto frame = frame_set->getFrame(frame_type);
|
||||
if (enable_stream_[stream_index] && frame) {
|
||||
timestamp_csv_logger_->recordImageFrameArrival(stream_index.first, frame,
|
||||
frame_set_arrival_system_us,
|
||||
frame_set_arrival_steady_us, true);
|
||||
}
|
||||
};
|
||||
record_side_stream(COLOR_LEFT, OB_FRAME_COLOR_LEFT);
|
||||
record_side_stream(COLOR_RIGHT, OB_FRAME_COLOR_RIGHT);
|
||||
record_side_stream(INFRA1, OB_FRAME_IR_LEFT);
|
||||
record_side_stream(INFRA2, OB_FRAME_IR_RIGHT);
|
||||
}
|
||||
|
||||
try {
|
||||
@@ -6514,10 +6540,12 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
|
||||
setColorAutoExposureROI();
|
||||
left_color_frame = processColorFrameFilter(left_color_frame);
|
||||
frame_set->pushFrame(left_color_frame);
|
||||
fps_counter_left_color_->tick();
|
||||
}
|
||||
if (right_color_frame) {
|
||||
right_color_frame = processColorFrameFilter(right_color_frame);
|
||||
frame_set->pushFrame(right_color_frame);
|
||||
fps_counter_right_color_->tick();
|
||||
}
|
||||
if (left_ir_frame) {
|
||||
left_ir_frame = processLeftIrFrameFilter(left_ir_frame);
|
||||
@@ -7117,13 +7145,19 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
|
||||
}
|
||||
if ((stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) &&
|
||||
frame->getFormat() == OB_FORMAT_MJPG && has_compressed_image_subscriber) {
|
||||
if (!has_raw_image_subscriber && stream_index == COLOR && log_image_timestamps) {
|
||||
if (!has_raw_image_subscriber && log_image_timestamps) {
|
||||
timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(),
|
||||
getSteadyNowUs());
|
||||
}
|
||||
publishCompressedColorImage(frame, stream_index, timestamp, frame_id);
|
||||
if (!has_raw_image_subscriber && stream_index == COLOR) {
|
||||
fps_delay_status_color_->tick(frame_timestamp);
|
||||
if (!has_raw_image_subscriber) {
|
||||
if (stream_index == COLOR) {
|
||||
fps_delay_status_color_->tick(frame_timestamp);
|
||||
} else if (stream_index == COLOR_LEFT) {
|
||||
fps_delay_status_left_color_->tick(frame_timestamp);
|
||||
} else {
|
||||
fps_delay_status_right_color_->tick(frame_timestamp);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -7148,10 +7182,12 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
|
||||
}
|
||||
if (frame->getType() == OB_FRAME_COLOR_LEFT && !is_left_color_frame_decoded_) {
|
||||
RCLCPP_ERROR(logger_, "left color frame is not decoded");
|
||||
record_image_publish_skipped();
|
||||
return;
|
||||
}
|
||||
if (frame->getType() == OB_FRAME_COLOR_RIGHT && !is_right_color_frame_decoded_) {
|
||||
RCLCPP_ERROR(logger_, "right color frame is not decoded");
|
||||
record_image_publish_skipped();
|
||||
return;
|
||||
}
|
||||
if (frame->getType() == OB_FRAME_COLOR) {
|
||||
@@ -7211,8 +7247,16 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
|
||||
}
|
||||
if (stream_index == COLOR) {
|
||||
fps_delay_status_color_->tick(frame_timestamp);
|
||||
} else if (stream_index == COLOR_LEFT) {
|
||||
fps_delay_status_left_color_->tick(frame_timestamp);
|
||||
} else if (stream_index == COLOR_RIGHT) {
|
||||
fps_delay_status_right_color_->tick(frame_timestamp);
|
||||
} else if (stream_index == DEPTH) {
|
||||
fps_delay_status_depth_->tick(frame_timestamp);
|
||||
} else if (stream_index == INFRA1) {
|
||||
fps_delay_status_left_ir_->tick(frame_timestamp);
|
||||
} else if (stream_index == INFRA2) {
|
||||
fps_delay_status_right_ir_->tick(frame_timestamp);
|
||||
}
|
||||
image_publishers_[stream_index]->publish(std::move(image_msg));
|
||||
}
|
||||
|
||||
@@ -29,6 +29,18 @@ TimestampCsvLogger::TimestampCsvLogger(Config config, rclcpp::Logger logger)
|
||||
depth_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::DEPTH);
|
||||
}
|
||||
}
|
||||
if (config.left_color_enabled) {
|
||||
left_color_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::LEFT_COLOR);
|
||||
}
|
||||
if (config.right_color_enabled) {
|
||||
right_color_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::RIGHT_COLOR);
|
||||
}
|
||||
if (config.left_ir_enabled) {
|
||||
left_ir_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::LEFT_IR);
|
||||
}
|
||||
if (config.right_ir_enabled) {
|
||||
right_ir_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::RIGHT_IR);
|
||||
}
|
||||
|
||||
if (config.csv_file_path.empty()) {
|
||||
return;
|
||||
@@ -64,7 +76,12 @@ bool TimestampCsvLogger::enabled() const {
|
||||
|
||||
bool TimestampCsvLogger::imageEnabled() const {
|
||||
return (synced_image_logger_ && synced_image_logger_->enabled()) ||
|
||||
(color_logger_ && color_logger_->enabled()) || (depth_logger_ && depth_logger_->enabled());
|
||||
(color_logger_ && color_logger_->enabled()) ||
|
||||
(left_color_logger_ && left_color_logger_->enabled()) ||
|
||||
(right_color_logger_ && right_color_logger_->enabled()) ||
|
||||
(depth_logger_ && depth_logger_->enabled()) ||
|
||||
(left_ir_logger_ && left_ir_logger_->enabled()) ||
|
||||
(right_ir_logger_ && right_ir_logger_->enabled());
|
||||
}
|
||||
|
||||
bool TimestampCsvLogger::imageStreamEnabled(OBStreamType stream_type) const {
|
||||
@@ -104,6 +121,18 @@ void TimestampCsvLogger::recordImageFrameSet(const std::shared_ptr<ob::Frame> &c
|
||||
}
|
||||
}
|
||||
|
||||
void TimestampCsvLogger::recordImageFrameArrival(OBStreamType stream_type,
|
||||
const std::shared_ptr<ob::Frame> &frame,
|
||||
int64_t arrival_system_us,
|
||||
int64_t arrival_steady_us,
|
||||
bool image_publish_expected) {
|
||||
auto *timestamp_logger = imageLoggerForStream(stream_type);
|
||||
if (timestamp_logger) {
|
||||
timestamp_logger->recordStandaloneFrameArrival(stream_type, frame, arrival_system_us,
|
||||
arrival_steady_us, image_publish_expected);
|
||||
}
|
||||
}
|
||||
|
||||
void TimestampCsvLogger::recordImagePrePublish(OBStreamType stream_type,
|
||||
const std::shared_ptr<ob::Frame> &frame,
|
||||
int64_t publish_system_us,
|
||||
@@ -164,7 +193,11 @@ void TimestampCsvLogger::shutdown() noexcept {
|
||||
|
||||
shutdown_logger(synced_image_logger_, "synced image timestamp CSV logger");
|
||||
shutdown_logger(color_logger_, "color timestamp CSV logger");
|
||||
shutdown_logger(left_color_logger_, "left color timestamp CSV logger");
|
||||
shutdown_logger(right_color_logger_, "right color timestamp CSV logger");
|
||||
shutdown_logger(depth_logger_, "depth timestamp CSV logger");
|
||||
shutdown_logger(left_ir_logger_, "left IR timestamp CSV logger");
|
||||
shutdown_logger(right_ir_logger_, "right IR timestamp CSV logger");
|
||||
shutdown_logger(synced_imu_logger_, "synced IMU timestamp CSV logger");
|
||||
shutdown_logger(accel_logger_, "accel timestamp CSV logger");
|
||||
shutdown_logger(gyro_logger_, "gyro timestamp CSV logger");
|
||||
@@ -177,6 +210,18 @@ FrameTimestampCsvLogger *TimestampCsvLogger::imageLoggerForStream(OBStreamType s
|
||||
if (stream_type == OB_STREAM_DEPTH) {
|
||||
return synced_image_logger_ ? synced_image_logger_.get() : depth_logger_.get();
|
||||
}
|
||||
if (stream_type == OB_STREAM_COLOR_LEFT) {
|
||||
return left_color_logger_.get();
|
||||
}
|
||||
if (stream_type == OB_STREAM_COLOR_RIGHT) {
|
||||
return right_color_logger_.get();
|
||||
}
|
||||
if (stream_type == OB_STREAM_IR_LEFT) {
|
||||
return left_ir_logger_.get();
|
||||
}
|
||||
if (stream_type == OB_STREAM_IR_RIGHT) {
|
||||
return right_ir_logger_.get();
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
|
||||
@@ -11,6 +11,28 @@ float64 color_delay_ms_avg
|
||||
float64 color_delay_ms_min
|
||||
float64 color_delay_ms_max
|
||||
|
||||
# --- Left color stream ---
|
||||
float64 left_color_frame_rate_cur
|
||||
float64 left_color_frame_rate_avg
|
||||
float64 left_color_frame_rate_min
|
||||
float64 left_color_frame_rate_max
|
||||
|
||||
float64 left_color_delay_ms_cur
|
||||
float64 left_color_delay_ms_avg
|
||||
float64 left_color_delay_ms_min
|
||||
float64 left_color_delay_ms_max
|
||||
|
||||
# --- Right color stream ---
|
||||
float64 right_color_frame_rate_cur
|
||||
float64 right_color_frame_rate_avg
|
||||
float64 right_color_frame_rate_min
|
||||
float64 right_color_frame_rate_max
|
||||
|
||||
float64 right_color_delay_ms_cur
|
||||
float64 right_color_delay_ms_avg
|
||||
float64 right_color_delay_ms_min
|
||||
float64 right_color_delay_ms_max
|
||||
|
||||
# --- Depth stream ---
|
||||
float64 depth_frame_rate_cur
|
||||
float64 depth_frame_rate_avg
|
||||
@@ -22,6 +44,28 @@ float64 depth_delay_ms_avg
|
||||
float64 depth_delay_ms_min
|
||||
float64 depth_delay_ms_max
|
||||
|
||||
# --- Left IR stream ---
|
||||
float64 left_ir_frame_rate_cur
|
||||
float64 left_ir_frame_rate_avg
|
||||
float64 left_ir_frame_rate_min
|
||||
float64 left_ir_frame_rate_max
|
||||
|
||||
float64 left_ir_delay_ms_cur
|
||||
float64 left_ir_delay_ms_avg
|
||||
float64 left_ir_delay_ms_min
|
||||
float64 left_ir_delay_ms_max
|
||||
|
||||
# --- Right IR stream ---
|
||||
float64 right_ir_frame_rate_cur
|
||||
float64 right_ir_frame_rate_avg
|
||||
float64 right_ir_frame_rate_min
|
||||
float64 right_ir_frame_rate_max
|
||||
|
||||
float64 right_ir_delay_ms_cur
|
||||
float64 right_ir_delay_ms_avg
|
||||
float64 right_ir_delay_ms_min
|
||||
float64 right_ir_delay_ms_max
|
||||
|
||||
# --- Device info ---
|
||||
bool device_online
|
||||
string connection_type # e.g. "USB2.0", "USB3.0", "GigE"
|
||||
|
||||
Reference in New Issue
Block a user