From 7c65ea9983f241b2ab1aeffda979623c6e6dea9e Mon Sep 17 00:00:00 2001 From: slz Date: Thu, 10 Sep 2026 14:25:33 +0800 Subject: [PATCH] fix: harden multi-camera verification capture --- .../image_sync_example_node.cpp | 21 +- orbbec_camera/src/ob_camera_node.cpp | 1 + orbbec_camera/tools/multi_save_rgbir.cpp | 285 ++++++++++++++---- 3 files changed, 238 insertions(+), 69 deletions(-) diff --git a/orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp b/orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp index 455838ec..eb446ad7 100755 --- a/orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp +++ b/orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp @@ -1,4 +1,5 @@ #include +#include #include #if __has_include() @@ -298,7 +299,17 @@ class ImageSyncNode : public rclcpp::Node { try { for (const auto &msg : msgs) { auto cv_image = cv_bridge::toCvShare(msg); - images.push_back(cv_image->image.clone()); + cv::Mat image; + if (msg->encoding == sensor_msgs::image_encodings::RGB8) { + cv::cvtColor(cv_image->image, image, cv::COLOR_RGB2BGR); + } else if (msg->encoding == sensor_msgs::image_encodings::RGBA8) { + cv::cvtColor(cv_image->image, image, cv::COLOR_RGBA2BGR); + } else if (msg->encoding == sensor_msgs::image_encodings::BGRA8) { + cv::cvtColor(cv_image->image, image, cv::COLOR_BGRA2BGR); + } else { + image = cv_image->image.clone(); + } + images.push_back(std::move(image)); timestamps.push_back(stamp_to_seconds(msg->header.stamp)); } } catch (cv_bridge::Exception &e) { @@ -477,8 +488,14 @@ class ImageSyncNode : public rclcpp::Node { cv::applyColorMap(tmp, image, cv::COLORMAP_JET); } else if (images[i].channels() == 3) { image = images[i].clone(); + } else if (images[i].channels() == 4) { + cv::cvtColor(images[i], image, cv::COLOR_BGRA2BGR); } else { - cv::cvtColor(images[i], image, cv::COLOR_GRAY2BGR); + RCLCPP_WARN(this->get_logger(), "Display first channel of unsupported %d-channel image %s", + images[i].channels(), topic_infos[i].topic.c_str()); + cv::Mat first_channel; + cv::extractChannel(images[i], first_channel, 0); + cv::cvtColor(first_channel, image, cv::COLOR_GRAY2BGR); } const std::string text = topic_infos[i].camera_name + " " + topic_infos[i].image_type + diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 77c3fef8..98e492ae 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -7072,6 +7072,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &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; } } diff --git a/orbbec_camera/tools/multi_save_rgbir.cpp b/orbbec_camera/tools/multi_save_rgbir.cpp index 1719acb8..43901740 100644 --- a/orbbec_camera/tools/multi_save_rgbir.cpp +++ b/orbbec_camera/tools/multi_save_rgbir.cpp @@ -16,10 +16,13 @@ #include #include #include +#include +#include #include #include #include +#include #include "orbbec_camera/ob_camera_node.h" #include "orbbec_camera/utils.h" @@ -32,6 +35,19 @@ namespace { const std::array kSupportedStreamNames = { "color", "left_color", "right_color", "ir", "left_ir", "right_ir", }; +constexpr auto kStreamDiscoveryPollInterval = std::chrono::milliseconds(100); +constexpr auto kStreamDiscoveryStablePeriod = std::chrono::seconds(1); +constexpr auto kStreamDiscoveryTimeout = std::chrono::seconds(5); +constexpr size_t kMinimumPendingFrameLimit = 30; + +struct StreamTopicInfo { + std::string name; + bool metadata_available = false; + + bool operator==(const StreamTopicInfo &other) const { + return name == other.name && metadata_available == other.metadata_available; + } +}; bool isSupportedStreamName(const std::string &stream_name) { return std::find(kSupportedStreamNames.begin(), kSupportedStreamNames.end(), stream_name) != @@ -52,18 +68,32 @@ std::string cameraNamespace(const std::string &camera_name) { } // namespace struct StreamCapture { + struct FrameMetadata { + std::string exposure; + std::string gain; + }; + + struct PendingImage { + cv::Mat image; + std::string current_timestamp; + std::string receive_timestamp; + }; + std::vector images; std::vector current_timestamps; std::vector receive_timestamps; - std::vector exposures; - std::vector gains; + std::vector frame_metadata; + std::map pending_images; + std::map pending_metadata; + bool metadata_required = false; void clear() { images.clear(); current_timestamps.clear(); receive_timestamps.clear(); - exposures.clear(); - gains.clear(); + frame_metadata.clear(); + pending_images.clear(); + pending_metadata.clear(); } }; @@ -95,7 +125,6 @@ class MultiCameraSubscriber : public rclcpp::Node { auto device_info = list->getDevice(i)->getDeviceInfo(); const auto usb_port = parseUsbPort(device_info->uid()); serial_numbers_[usb_port] = device_info->serialNumber(); - has_gemini330_device_ = has_gemini330_device_ || isGemini330Series(device_info->getPid()); } } catch (const ob::Error &e) { RCLCPP_ERROR_STREAM(get_logger(), orbbec_camera::formatObErrorWithStatus(e)); @@ -106,16 +135,6 @@ class MultiCameraSubscriber : public rclcpp::Node { } } - bool isGemini330Series(uint32_t pid) const { - return pid == GEMINI_335_PID || pid == GEMINI_330_PID || pid == GEMINI_336_PID || - pid == GEMINI_335L_PID || pid == GEMINI_330L_PID || pid == GEMINI_336L_PID || - pid == GEMINI_335LG_PID || pid == GEMINI_336LG_PID || pid == GEMINI_335LE_PID || - pid == GEMINI_336LE_PID || pid == CUSTOM_ADVANTECH_GEMINI_336_PID || - pid == CUSTOM_ADVANTECH_GEMINI_336L_PID || pid == GEMINI_338_PID || - pid == GEMINI_338LG_PID || pid == GEMINI_338LE_PID || pid == GEMINI_338L_PID || - pid == GEMINI_331L_PID; - } - void loadParameters() { std::ifstream file( "install/orbbec_camera/share/orbbec_camera/config/tools/multisavergbir/" @@ -148,57 +167,113 @@ class MultiCameraSubscriber : public rclcpp::Node { } } - std::vector discoverStreamNames(const std::string &camera_name) const { - std::vector stream_names; + std::vector discoverStreams(const std::string &camera_name) const { + std::vector streams; const auto names_and_types = this->get_topic_names_and_types(); const std::string prefix = cameraNamespace(camera_name) + "/"; for (const auto &stream_name : kSupportedStreamNames) { - const std::string topic = prefix + stream_name + "/image_raw"; - const auto topic_it = names_and_types.find(topic); - if (topic_it == names_and_types.end()) { + const auto image_topic_it = names_and_types.find(prefix + stream_name + "/image_raw"); + if (image_topic_it == names_and_types.end()) { continue; } - const auto &types = topic_it->second; - if (std::find(types.begin(), types.end(), "sensor_msgs/msg/Image") != types.end()) { - stream_names.push_back(stream_name); + const auto &image_types = image_topic_it->second; + if (std::find(image_types.begin(), image_types.end(), "sensor_msgs/msg/Image") == + image_types.end()) { + continue; } + + const auto metadata_topic_it = names_and_types.find(prefix + stream_name + "/metadata"); + const bool metadata_available = + metadata_topic_it != names_and_types.end() && + std::find(metadata_topic_it->second.begin(), metadata_topic_it->second.end(), + "orbbec_camera_msgs/msg/Metadata") != metadata_topic_it->second.end(); + streams.push_back(StreamTopicInfo{stream_name, metadata_available}); } - return stream_names; + return streams; } - std::vector selectStreamNames(const std::string &camera_name) const { + std::vector> waitForStableStreams() const { if (!configured_stream_names_.empty()) { - return configured_stream_names_; - } - auto stream_names = discoverStreamNames(camera_name); - if (!stream_names.empty()) { - return stream_names; + std::vector> configured_streams; + configured_streams.reserve(camera_names_.size()); + for (const auto &camera_name : camera_names_) { + const auto discovered_streams = discoverStreams(camera_name); + std::vector camera_streams; + camera_streams.reserve(configured_stream_names_.size()); + for (const auto &stream_name : configured_stream_names_) { + const auto discovered_it = std::find_if( + discovered_streams.begin(), discovered_streams.end(), + [&stream_name](const auto &stream) { return stream.name == stream_name; }); + camera_streams.push_back(StreamTopicInfo{ + stream_name, + discovered_it != discovered_streams.end() && discovered_it->metadata_available}); + } + configured_streams.push_back(std::move(camera_streams)); + } + return configured_streams; } - RCLCPP_WARN(get_logger(), - "No supported image topics discovered for %s; using legacy RGB/IR topics", - camera_name.c_str()); - return {has_gemini330_device_ ? "left_ir" : "ir", "color"}; + std::vector> candidate; + auto candidate_since = std::chrono::steady_clock::time_point{}; + const auto deadline = std::chrono::steady_clock::now() + kStreamDiscoveryTimeout; + while (rclcpp::ok() && std::chrono::steady_clock::now() < deadline) { + std::vector> current; + current.reserve(camera_names_.size()); + bool all_cameras_discovered = !camera_names_.empty(); + for (const auto &camera_name : camera_names_) { + current.push_back(discoverStreams(camera_name)); + all_cameras_discovered = all_cameras_discovered && !current.back().empty(); + } + + const auto now = std::chrono::steady_clock::now(); + if (!all_cameras_discovered) { + candidate.clear(); + } else if (current != candidate) { + candidate = std::move(current); + candidate_since = now; + } else if (now - candidate_since >= kStreamDiscoveryStablePeriod) { + return candidate; + } + + std::this_thread::sleep_for(kStreamDiscoveryPollInterval); + } + + RCLCPP_WARN_STREAM(get_logger(), + "Supported image topics did not become stable within " + << kStreamDiscoveryTimeout.count() + << " seconds; start all cameras and streams first, retry the request, " + "or configure stream_names explicitly"); + return {}; } - void initializeTopics() { + bool initializeTopics() { + const auto streams_by_camera = waitForStableStreams(); + if (streams_by_camera.size() != camera_names_.size()) { + return false; + } + + callback_groups_.clear(); + image_subscribers_.clear(); + metadata_subscribers_.clear(); captures_.resize(camera_names_.size()); callback_called_.assign(camera_names_.size(), false); const auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); for (size_t camera_index = 0; camera_index < camera_names_.size(); ++camera_index) { - const auto stream_names = selectStreamNames(camera_names_[camera_index]); + const auto &streams = streams_by_camera[camera_index]; const std::string prefix = cameraNamespace(camera_names_[camera_index]) + "/"; auto callback_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); callback_groups_.push_back(callback_group); rclcpp::SubscriptionOptions options; options.callback_group = callback_group; - for (const auto &stream_name : stream_names) { - captures_[camera_index].emplace(stream_name, StreamCapture{}); + for (const auto &stream : streams) { + const auto &stream_name = stream.name; + StreamCapture capture; + capture.metadata_required = stream.metadata_available; + captures_[camera_index].emplace(stream_name, std::move(capture)); const std::string image_topic = prefix + stream_name + "/image_raw"; - const std::string metadata_topic = prefix + stream_name + "/metadata"; RCLCPP_INFO(get_logger(), "Subscribing to %s", image_topic.c_str()); image_subscribers_.push_back(this->create_subscription( @@ -208,16 +283,21 @@ class MultiCameraSubscriber : public rclcpp::Node { imageCallback(image, camera_index, stream_name); }, options)); - metadata_subscribers_.push_back( - this->create_subscription( - metadata_topic, custom_qos, - [this, camera_index, stream_name]( - const std::shared_ptr metadata) { - metadataCallback(metadata, camera_index, stream_name); - }, - options)); + if (stream.metadata_available) { + const std::string metadata_topic = prefix + stream_name + "/metadata"; + metadata_subscribers_.push_back( + this->create_subscription( + metadata_topic, custom_qos, + [this, camera_index, stream_name]( + const std::shared_ptr metadata) { + metadataCallback(metadata, camera_index, stream_name); + }, + options)); + } } } + return !captures_.empty() && std::all_of(captures_.begin(), captures_.end(), + [](const auto &streams) { return !streams.empty(); }); } std::string currentDateTime() const { @@ -261,13 +341,42 @@ class MultiCameraSubscriber : public rclcpp::Node { }); } - std::string metadataSuffix(const StreamCapture &capture, size_t frame_index) const { - std::string suffix; - if (frame_index < capture.exposures.size()) { - suffix += "_e" + capture.exposures[frame_index]; + int64_t messageStampNs(const std_msgs::msg::Header &header) const { + return static_cast(header.stamp.sec) * 1000000000LL + header.stamp.nanosec; + } + + size_t pendingFrameLimit() const { + return std::max(kMinimumPendingFrameLimit, static_cast(saving_images_number_) * 2); + } + + template + void trimPendingFrames(std::map &pending) const { + while (pending.size() > pendingFrameLimit()) { + pending.erase(pending.begin()); } - if (frame_index < capture.gains.size()) { - suffix += "_d" + capture.gains[frame_index]; + } + + void appendCompletedFrame(StreamCapture &capture, StreamCapture::PendingImage pending_image, + StreamCapture::FrameMetadata metadata = {}) { + if (capture.images.size() >= static_cast(saving_images_number_)) { + return; + } + capture.images.push_back(std::move(pending_image.image)); + capture.current_timestamps.push_back(std::move(pending_image.current_timestamp)); + capture.receive_timestamps.push_back(std::move(pending_image.receive_timestamp)); + capture.frame_metadata.push_back(std::move(metadata)); + } + + std::string metadataSuffix(const StreamCapture &capture, size_t frame_index) const { + if (frame_index >= capture.frame_metadata.size()) { + return ""; + } + std::string suffix; + if (!capture.frame_metadata[frame_index].exposure.empty()) { + suffix += "_e" + capture.frame_metadata[frame_index].exposure; + } + if (!capture.frame_metadata[frame_index].gain.empty()) { + suffix += "_d" + capture.frame_metadata[frame_index].gain; } return suffix; } @@ -333,15 +442,20 @@ class MultiCameraSubscriber : public rclcpp::Node { return; } if (!topics_initialized_) { - initializeTopics(); + if (!initializeTopics()) { + callback_groups_.clear(); + image_subscribers_.clear(); + metadata_subscribers_.clear(); + captures_.clear(); + callback_called_.clear(); + response->success = false; + response->message = + "supported image streams did not become stable; retry after all cameras and streams " + "start or configure stream_names"; + return; + } topics_initialized_ = true; } - if (captures_.empty() || std::any_of(captures_.begin(), captures_.end(), - [](const auto &streams) { return streams.empty(); })) { - response->success = false; - response->message = "no supported image streams are configured"; - return; - } for (auto &camera_captures : captures_) { for (auto &entry : camera_captures) { @@ -364,16 +478,37 @@ class MultiCameraSubscriber : public rclcpp::Node { return; } auto &capture = captures_[camera_index].at(stream_name); + if (capture.images.size() >= static_cast(saving_images_number_)) { + saveImages(camera_index); + return; + } cv::Mat output = cv_bridge::toCvCopy(image, image->encoding)->image; if (isColorCaptureStreamName(stream_name) && image->encoding == sensor_msgs::image_encodings::RGB8) { cv::Mat converted; cv::cvtColor(output, converted, cv::COLOR_RGB2BGR); output = converted; + } else if (isColorCaptureStreamName(stream_name) && + image->encoding == sensor_msgs::image_encodings::RGBA8) { + cv::Mat converted; + cv::cvtColor(output, converted, cv::COLOR_RGBA2BGRA); + output = converted; + } + StreamCapture::PendingImage pending_image{std::move(output), imageTimestamp(image), + receiveTimestamp()}; + if (capture.metadata_required) { + const auto stamp_ns = messageStampNs(image->header); + auto metadata_it = capture.pending_metadata.find(stamp_ns); + if (metadata_it == capture.pending_metadata.end()) { + capture.pending_images.insert_or_assign(stamp_ns, std::move(pending_image)); + trimPendingFrames(capture.pending_images); + return; + } + appendCompletedFrame(capture, std::move(pending_image), std::move(metadata_it->second)); + capture.pending_metadata.erase(metadata_it); + } else { + appendCompletedFrame(capture, std::move(pending_image)); } - capture.images.push_back(output); - capture.current_timestamps.push_back(imageTimestamp(image)); - capture.receive_timestamps.push_back(receiveTimestamp()); RCLCPP_INFO(get_logger(), "%s[%zu]: %zu/%d", stream_name.c_str(), camera_index, capture.images.size(), saving_images_number_); saveImages(camera_index); @@ -385,18 +520,35 @@ class MultiCameraSubscriber : public rclcpp::Node { if (saving_images_number_ <= 0 || callback_called_[camera_index]) { return; } + auto &capture = captures_[camera_index].at(stream_name); + if (!capture.metadata_required || + capture.images.size() >= static_cast(saving_images_number_)) { + return; + } + StreamCapture::FrameMetadata frame_metadata; try { const auto json_data = nlohmann::json::parse(metadata->json_data); - auto &capture = captures_[camera_index].at(stream_name); if (json_data.contains("exposure")) { - capture.exposures.push_back(json_data["exposure"].dump()); + frame_metadata.exposure = json_data["exposure"].dump(); } if (json_data.contains("gain")) { - capture.gains.push_back(json_data["gain"].dump()); + frame_metadata.gain = json_data["gain"].dump(); } } catch (const std::exception &e) { RCLCPP_WARN(get_logger(), "Failed to parse %s metadata: %s", stream_name.c_str(), e.what()); } + const auto stamp_ns = messageStampNs(metadata->header); + auto image_it = capture.pending_images.find(stamp_ns); + if (image_it == capture.pending_images.end()) { + capture.pending_metadata.insert_or_assign(stamp_ns, std::move(frame_metadata)); + trimPendingFrames(capture.pending_metadata); + return; + } + appendCompletedFrame(capture, std::move(image_it->second), std::move(frame_metadata)); + capture.pending_images.erase(image_it); + RCLCPP_INFO(get_logger(), "%s[%zu]: %zu/%d", stream_name.c_str(), camera_index, + capture.images.size(), saving_images_number_); + saveImages(camera_index); } std::mutex capture_mutex_; @@ -417,7 +569,6 @@ class MultiCameraSubscriber : public rclcpp::Node { std::string current_date_time_; int saving_images_number_ = 0; bool topics_initialized_ = false; - bool has_gemini330_device_ = false; }; } // namespace tools