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

Merge branch 'feat/subscriber-benchmark' into v2/develop

Browse Source 
# Conflicts:
#	orbbec_camera/src/ob_camera_node.cpp
#	orbbec_camera/tools/multi_save_rgbir.cpp
This commit is contained in:
ob-yalian
2026-09-15 10:37:36 +08:00
parent 37022461c3 91f39d7da9
commit c793fc8130
14 changed files with 1277 additions and 535 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+73 -19
View File
@@ -743,7 +743,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];
@@ -763,6 +767,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);
@@ -772,12 +778,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>
@@ -3237,7 +3249,7 @@ void OBCameraNode::setupLeftIrPostProcessFilter() {
}
auto device_info = device_->getDeviceInfo();
CHECK_NOTNULL(device_info);
if (isGemini330SeriesPID(pid_)) {
if (isGemini330SeriesPID(pid_) || isGemini305SeriesPID(pid_)) {
auto left_ir_sensor = device_->getSensor(OB_SENSOR_IR_LEFT);
left_ir_filter_list_ = left_ir_sensor->createRecommendedFilters();
if (left_ir_filter_list_.empty()) {
@@ -3278,7 +3290,7 @@ void OBCameraNode::setupRightIrPostProcessFilter() {
}
auto device_info = device_->getDeviceInfo();
CHECK_NOTNULL(device_info);
if (isGemini330SeriesPID(pid_)) {
if (isGemini330SeriesPID(pid_) || isGemini305SeriesPID(pid_)) {
auto right_ir_sensor = device_->getSensor(OB_SENSOR_IR_RIGHT);
right_ir_filter_list_ = right_ir_sensor->createRecommendedFilters();
if (right_ir_filter_list_.empty()) {
@@ -3537,35 +3549,44 @@ void OBCameraNode::selectBaseStream() {
void OBCameraNode::printSensorProfiles(const std::shared_ptr<ob::Sensor> &sensor) {
auto profiles = sensor->getStreamProfileList();
const auto sensor_type = sensor->getType();
for (size_t i = 0; i < profiles->getCount(); i++) {
auto origin_profile = profiles->getProfile(i);
if (sensor->getType() == OB_SENSOR_COLOR) {
if (sensor_type == OB_SENSOR_COLOR || sensor_type == OB_SENSOR_COLOR_LEFT ||
sensor_type == OB_SENSOR_COLOR_RIGHT) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(
logger_, "color profile: " << profile->getWidth() << "x" << profile->getHeight() << " "
<< profile->getFps() << "fps " << profile->getFormat());
} else if (sensor->getType() == OB_SENSOR_DEPTH) {
const char *stream_name = sensor_type == OB_SENSOR_COLOR_LEFT ? "left_color"
: sensor_type == OB_SENSOR_COLOR_RIGHT ? "right_color"
: "color";
RCLCPP_INFO_STREAM(logger_, stream_name << " profile: " << profile->getWidth() << "x"
<< profile->getHeight() << " " << profile->getFps()
<< "fps " << profile->getFormat());
} else if (sensor_type == OB_SENSOR_DEPTH) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(
logger_, "depth profile: " << profile->getWidth() << "x" << profile->getHeight() << " "
<< profile->getFps() << "fps " << profile->getFormat());
} else if (sensor->getType() == OB_SENSOR_IR) {
} else if (sensor_type == OB_SENSOR_IR || sensor_type == OB_SENSOR_IR_LEFT ||
sensor_type == OB_SENSOR_IR_RIGHT) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "ir profile: " << profile->getWidth() << "x"
<< profile->getHeight() << " " << profile->getFps()
<< "fps " << profile->getFormat());
} else if (sensor->getType() == OB_SENSOR_ACCEL) {
const char *stream_name = sensor_type == OB_SENSOR_IR_LEFT ? "left_ir"
: sensor_type == OB_SENSOR_IR_RIGHT ? "right_ir"
: "ir";
RCLCPP_INFO_STREAM(logger_, stream_name << " profile: " << profile->getWidth() << "x"
<< profile->getHeight() << " " << profile->getFps()
<< "fps " << profile->getFormat());
} else if (sensor_type == OB_SENSOR_ACCEL) {
auto profile = origin_profile->as<ob::AccelStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "accel profile: sampleRate " << profile->getSampleRate()
<< " full scale_range "
<< profile->getFullScaleRange());
} else if (sensor->getType() == OB_SENSOR_GYRO) {
} else if (sensor_type == OB_SENSOR_GYRO) {
auto profile = origin_profile->as<ob::GyroStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "gyro profile: sampleRate " << profile->getSampleRate()
<< " full scale_range "
<< profile->getFullScaleRange());
} else {
RCLCPP_INFO_STREAM(logger_, "unknown profile: " << magic_enum::enum_name(sensor->getType()));
RCLCPP_INFO_STREAM(logger_, "unknown profile: " << magic_enum::enum_name(sensor_type));
}
}
}
@@ -3667,11 +3688,11 @@ void OBCameraNode::setupProfiles() {
if (selected_profile->format() == OB_FORMAT_BGRA) {
images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0));
encoding_[elem] = sensor_msgs::image_encodings::BGRA8;
unit_step_size_[COLOR] = 4 * sizeof(uint8_t);
unit_step_size_[elem] = 4 * sizeof(uint8_t);
} else if (selected_profile->format() == OB_FORMAT_RGBA) {
images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0));
encoding_[elem] = sensor_msgs::image_encodings::RGBA8;
unit_step_size_[COLOR] = 4 * sizeof(uint8_t);
unit_step_size_[elem] = 4 * sizeof(uint8_t);
} else {
images_[elem] =
cv::Mat(height_[elem], width_[elem], image_format_[elem], cv::Scalar(0, 0, 0));
@@ -6501,6 +6522,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 {
@@ -6542,10 +6577,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);
@@ -7065,6 +7102,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
if (video_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX) ==
interleave_skip_index_) {
RCLCPP_DEBUG(logger_, "interleave filter skip frame type: %d", frame->getType());
record_image_publish_skipped();
return;
}
}
@@ -7147,13 +7185,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);
}
}
}
@@ -7178,10 +7222,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) {
@@ -7241,8 +7287,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));
}