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

fix: support side streams in camera tools

Browse Source
This commit is contained in:
slz
2026-09-10 10:45:25 +08:00
parent ae44449e12
commit b76e062fe5
6 changed files with 534 additions and 406 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json
+11 -10
View File
@@ -1,13 +1,14 @@
{
"save_rgbir_params": {
"time_domain": "global",
"usb_ports": [
"2-1",
"2-3"
],
"camera_name": [
"camera_01",
"camera_02"
]
"time_domain": "global",
"stream_names": [],
"usb_ports": [
"2-1",
"2-3"
],
"camera_name": [
"camera_01",
"camera_02"
]
}
}
}
orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp
+33 -17
View File
@@ -33,6 +33,7 @@
#include <opencv2/opencv.hpp>
#include <string>
#include <thread>
#include <utility>
#include <vector>
using Image = sensor_msgs::msg::Image;
@@ -69,7 +70,7 @@ class ImageSyncNode : public rclcpp::Node {
if (sync_topics_.empty()) {
sync_topics_ = discover_image_topics();
RCLCPP_INFO(this->get_logger(),
"Parameter sync_topics is empty. Auto-discovered %zu color/depth image topics.",
"Parameter sync_topics is empty. Auto-discovered %zu supported image topics.",
sync_topics_.size());
} else {
RCLCPP_INFO(this->get_logger(), "Using %zu image topics from parameter sync_topics.",
@@ -166,6 +167,24 @@ class ImageSyncNode : public rclcpp::Node {
str.compare(str.size() - suffix.size(), suffix.size(), suffix) == 0;
}
static const std::array<std::pair<const char *, const char *>, 7> &supported_stream_suffixes() {
static const std::array<std::pair<const char *, const char *>, 7> suffixes = {{
{"left_color", "/left_color/image_raw"},
{"right_color", "/right_color/image_raw"},
{"left_ir", "/left_ir/image_raw"},
{"right_ir", "/right_ir/image_raw"},
{"color", "/color/image_raw"},
{"depth", "/depth/image_raw"},
{"ir", "/ir/image_raw"},
}};
return suffixes;
}
static bool is_supported_image_topic(const std::string &topic) {
return std::any_of(supported_stream_suffixes().begin(), supported_stream_suffixes().end(),
[&topic](const auto &entry) { return has_suffix(topic, entry.second); });
}
static double stamp_to_seconds(const builtin_interfaces::msg::Time &stamp) {
return static_cast<double>(stamp.sec) + static_cast<double>(stamp.nanosec) * 1e-9;
}
@@ -180,7 +199,7 @@ class ImageSyncNode : public rclcpp::Node {
const auto names_and_types = this->get_topic_names_and_types();
for (const auto &entry : names_and_types) {
const auto &topic = entry.first;
if (!has_suffix(topic, "/color/image_raw") && !has_suffix(topic, "/depth/image_raw")) {
if (!is_supported_image_topic(topic)) {
continue;
}
@@ -207,8 +226,8 @@ class ImageSyncNode : public rclcpp::Node {
void validate_topics() {
if (sync_topics_.empty()) {
throw std::runtime_error(
"No image topics to synchronize. Set parameter sync_topics or start color/depth cameras "
"before this node.");
"No image topics to synchronize. Set parameter sync_topics or start supported camera "
"streams before this node.");
}
std::vector<std::string> deduplicated_topics;
@@ -233,7 +252,7 @@ class ImageSyncNode : public rclcpp::Node {
"Official ROS message_filters::Synchronizer supports at most 9 inputs, and this example "
"supports 1-8 image topics. Found " +
std::to_string(sync_topics_.size()) +
" color/depth image topics. Please pass <= 8 topics with sync_topics or split the sync "
" image topics. Please pass <= 8 topics with sync_topics or split the sync "
"into multiple stages.");
}
}
@@ -247,14 +266,13 @@ class ImageSyncNode : public rclcpp::Node {
info.image_type = "image";
info.camera_name = topic;
const auto color_pos = topic.rfind("/color/image_raw");
const auto depth_pos = topic.rfind("/depth/image_raw");
if (color_pos != std::string::npos) {
info.image_type = "color";
info.camera_name = topic.substr(0, color_pos);
} else if (depth_pos != std::string::npos) {
info.image_type = "depth";
info.camera_name = topic.substr(0, depth_pos);
for (const auto &entry : supported_stream_suffixes()) {
const std::string suffix = entry.second;
if (has_suffix(topic, suffix)) {
info.image_type = entry.first;
info.camera_name = topic.substr(0, topic.size() - suffix.size());
break;
}
}
const auto slash_pos = info.camera_name.find_last_of('/');
@@ -550,10 +568,8 @@ class ImageSyncNode : public rclcpp::Node {
const double avg_diff = diff_sum_ / count_;
std::cout << "\nImage Timestamp Difference Statistics" << std::endl;
std::cout << "cur: " << cur << " ms"
<< " avg: " << avg_diff << " ms"
<< " max: " << max_diff_ << " ms"
<< " min: " << min_diff_ << " ms" << std::endl;
std::cout << "cur: " << cur << " ms" << " avg: " << avg_diff << " ms" << " max: " << max_diff_
<< " ms" << " min: " << min_diff_ << " ms" << std::endl;
if (last_time_ == 0.0) {
last_time_ = base_t;
orbbec_camera/scripts/common_benchmark_node.py
+117 -58
View File
@@ -26,6 +26,14 @@ from sensor_msgs.msg import Image
from tabulate import tabulate
CAMERA_NODE_NAMES = ["component_container", "orbbec_camera_node", "nodelet"]
MONITORED_STREAMS = (
"color",
"depth",
"left_ir",
"right_ir",
"left_color",
"right_color",
)
DOCUMENTATION_URL = (
"https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/"
"6_benchmark/benchmark_tools.html"
@@ -110,13 +118,18 @@ class TopicTracker:
def on_msg(self, header, avg_fps):
stamp = header.stamp.sec + header.stamp.nanosec * 1e-9
self.received += 1
observed_fps = None
if self.last_time is not None and avg_fps > 0:
if self.last_time is not None:
dt = stamp - self.last_time
expected_interval = 1.0 / avg_fps
self.drop_frames += estimate_dropped_frames(dt, expected_interval)
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)
self.last_time = stamp
return stamp, observed_fps
def frames_loss_rate(self):
total = self.received + self.drop_frames
@@ -154,8 +167,8 @@ class CameraMonitorNode(Node):
"cpu_stats": make_stat(),
"ram_stats": make_stat(),
"trackers": {
"color": TopicTracker(logger=self.get_logger()),
"depth": TopicTracker(logger=self.get_logger())
stream: TopicTracker(logger=self.get_logger())
for stream in MONITORED_STREAMS
}
}
@@ -177,18 +190,15 @@ class CameraMonitorNode(Node):
lambda msg, name=camera_name: self.status_callback(msg, name),
5
)
self.create_subscription(
Image,
f"{ns}/color/image_raw",
lambda msg, name=camera_name: self.image_callback(msg, name, "color"),
5
)
self.create_subscription(
Image,
f"{ns}/depth/image_raw",
lambda msg, name=camera_name: self.image_callback(msg, name, "depth"),
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)
@@ -318,14 +328,27 @@ class CameraMonitorNode(Node):
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)
def image_callback(self, msg: Image, camera_name: str, stream: str):
if stream not in ("color", "depth"):
if stream not in MONITORED_STREAMS:
return
header = msg.header
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"]
tracker.on_msg(header, fps_to_use)
fps_to_use = (
self.ideal_fps
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)
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
@@ -338,6 +361,17 @@ 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:
@@ -365,17 +399,28 @@ class CameraMonitorNode(Node):
def build_csv_header(self):
header = ["time(s)"]
camera_fields = [
"connection_type", "status_online", "disconnects",
"color_fps_cur", "color_fps_avg", "color_fps_min", "color_fps_max",
"color_delay_cur", "color_delay_avg", "color_delay_min", "color_delay_max",
"depth_fps_cur", "depth_fps_avg", "depth_fps_min", "depth_fps_max",
"depth_delay_cur", "depth_delay_avg", "depth_delay_min", "depth_delay_max",
"cpu_cur", "cpu_avg", "cpu_min", "cpu_max",
"ram_cur", "ram_avg", "ram_min", "ram_max",
"color_frames_loss", "color_frames_loss_rate(%)",
"depth_frames_loss", "depth_frames_loss_rate(%)"
]
camera_fields = ["connection_type", "status_online", "disconnects"]
for stream in MONITORED_STREAMS:
camera_fields.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(%)",
]
)
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])
@@ -386,9 +431,6 @@ class CameraMonitorNode(Node):
return header
def build_camera_csv_values(self, camera):
color_tracker = camera["trackers"]["color"]
depth_tracker = camera["trackers"]["depth"]
def safe(k):
v = camera["stats"].get(k, {})
return (
@@ -398,29 +440,46 @@ class CameraMonitorNode(Node):
self.format_csv_number(v.get("max", 0.0)),
)
if not camera["prev_online"]:
return [
camera["connection_type"], camera["prev_online"], camera["disconnect_count"],
*["N/A"] * 16,
round(camera["cpu_stats"]["cur"], 2), "N/A", "N/A", "N/A",
round(camera["ram_stats"]["cur"], 2), "N/A", "N/A", "N/A",
color_tracker.drop_frames, round(color_tracker.frames_loss_rate() * 100.0, 3),
depth_tracker.drop_frames, round(depth_tracker.frames_loss_rate() * 100.0, 3)
]
return [
camera["connection_type"], camera["prev_online"], camera["disconnect_count"],
*safe("color_fps"),
*safe("color_delay"),
*safe("depth_fps"),
*safe("depth_delay"),
round(camera["cpu_stats"]["cur"], 2), round(camera["cpu_stats"]["avg"], 2),
self.format_csv_number(camera["cpu_stats"]["min"]), self.format_csv_number(camera["cpu_stats"]["max"]),
round(camera["ram_stats"]["cur"], 2), round(camera["ram_stats"]["avg"], 2),
self.format_csv_number(camera["ram_stats"]["min"]), self.format_csv_number(camera["ram_stats"]["max"]),
color_tracker.drop_frames, round(color_tracker.frames_loss_rate() * 100.0, 3),
depth_tracker.drop_frames, round(depth_tracker.frames_loss_rate() * 100.0, 3)
values = [
camera["connection_type"],
camera["prev_online"],
camera["disconnect_count"],
]
for stream in MONITORED_STREAMS:
tracker = camera["trackers"][stream]
if camera["prev_online"]:
values.extend(safe(f"{stream}_fps"))
values.extend(safe(f"{stream}_delay"))
else:
values.extend(["N/A"] * 8)
values.extend(
[
tracker.drop_frames,
round(tracker.frames_loss_rate() * 100.0, 3),
]
)
if camera["prev_online"]:
values.extend(
[
round(camera["cpu_stats"]["cur"], 2),
round(camera["cpu_stats"]["avg"], 2),
self.format_csv_number(camera["cpu_stats"]["min"]),
self.format_csv_number(camera["cpu_stats"]["max"]),
round(camera["ram_stats"]["cur"], 2),
round(camera["ram_stats"]["avg"], 2),
self.format_csv_number(camera["ram_stats"]["min"]),
self.format_csv_number(camera["ram_stats"]["max"]),
]
)
else:
values.extend(
[
round(camera["cpu_stats"]["cur"], 2), "N/A", "N/A", "N/A",
round(camera["ram_stats"]["cur"], 2), "N/A", "N/A", "N/A",
]
)
return values
def format_csv_number(self, value):
if value == float("inf") or value == float("-inf"):
@@ -436,7 +495,7 @@ class CameraMonitorNode(Node):
rows = []
for camera_name in self.camera_names:
camera = self.cameras[camera_name]
for stream in ["color", "depth"]:
for stream in MONITORED_STREAMS:
fps_key = f"{stream}_fps"
delay_key = f"{stream}_delay"
topic_name = f"/{camera_name}/{stream}/image_raw"
orbbec_camera/tools/list_camera_profile.cpp
+5 -2
View File
@@ -154,8 +154,11 @@ void listSensorProfiles(const std::shared_ptr<ob::Device>& device) {
<< " | width: " << profile->getDecimationConfig().originWidth
<< " height: " << profile->getDecimationConfig().originHeight
<< " downscale:" << profile->getDecimationConfig().factor << std::endl;
} else if (sensor->getType() == OB_SENSOR_COLOR || sensor->getType() == OB_SENSOR_DEPTH ||
sensor->getType() == OB_SENSOR_IR || sensor->getType() == OB_SENSOR_IR_LEFT ||
} else if (sensor->getType() == OB_SENSOR_COLOR ||
sensor->getType() == OB_SENSOR_COLOR_LEFT ||
sensor->getType() == OB_SENSOR_COLOR_RIGHT ||
sensor->getType() == OB_SENSOR_DEPTH || sensor->getType() == OB_SENSOR_IR ||
sensor->getType() == OB_SENSOR_IR_LEFT ||
sensor->getType() == OB_SENSOR_IR_RIGHT) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
std::cout << magic_enum::enum_name(sensor->getType()) << " profile: " << profile->getWidth()
orbbec_camera/tools/multi_save_rgbir.cpp
+335 -319
View File
@@ -1,72 +1,84 @@
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <orbbec_camera/ob_camera_node_driver.h>
#include <orbbec_camera/utils.h>
#include "orbbec_camera/ob_camera_node.h"
#include "orbbec_camera_msgs/msg/metadata.hpp"
#include <std_msgs/msg/int32.hpp>
#include <algorithm>
#include <array>
#include <chrono>
#include <cstdint>
#include <ctime>
#include <filesystem>
#include <regex>
#include <fstream>
#include <functional>
#include <iomanip>
#include <map>
#include <memory>
#include <mutex>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <nlohmann/json.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include "orbbec_camera/ob_camera_node.h"
#include "orbbec_camera/utils.h"
#include "orbbec_camera_msgs/msg/metadata.hpp"
namespace orbbec_camera {
namespace tools {
struct ImageMetadata {
std::vector<std::vector<std::string>> exposure_buffs;
std::vector<std::vector<std::string>> gain_buffs;
namespace {
const std::array<std::string, 6> kSupportedStreamNames = {
"color", "left_color", "right_color", "ir", "left_ir", "right_ir",
};
bool isSupportedStreamName(const std::string &stream_name) {
return std::find(kSupportedStreamNames.begin(), kSupportedStreamNames.end(), stream_name) !=
kSupportedStreamNames.end();
}
bool isColorCaptureStreamName(const std::string &stream_name) {
return stream_name == "color" || stream_name == "left_color" || stream_name == "right_color";
}
std::string cameraNamespace(const std::string &camera_name) {
if (!camera_name.empty() && camera_name.front() == '/') {
return camera_name;
}
return "/" + camera_name;
}
} // namespace
struct StreamCapture {
std::vector<cv::Mat> images;
std::vector<std::string> current_timestamps;
std::vector<std::string> receive_timestamps;
std::vector<std::string> exposures;
std::vector<std::string> gains;
void clear() {
images.clear();
current_timestamps.clear();
receive_timestamps.clear();
exposures.clear();
gains.clear();
}
};
class MultiCameraSubscriber : public rclcpp::Node {
public:
explicit MultiCameraSubscriber(const rclcpp::NodeOptions &options)
: Node("MultiCameraSubscriber", options) {
device_init();
}
~MultiCameraSubscriber() {
ir_image_buffers_.clear();
ir_current_timestamp_buffers_.clear();
ir_timestamp_buffers_.clear();
color_image_buffers_.clear();
color_current_timestamp_buffers_.clear();
color_timestamp_buffers_.clear();
left_ir_metadata_.exposure_buffs.clear();
left_ir_metadata_.gain_buffs.clear();
color_metadata_.exposure_buffs.clear();
color_metadata_.gain_buffs.clear();
}
void device_init() {
try {
auto context = std::make_unique<ob::Context>();
context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE);
auto list = context->queryDeviceList();
for (size_t i = 0; i < list->deviceCount(); i++) {
auto device = list->getDevice(i);
auto device_info = device->getDeviceInfo();
auto pid = device_info->getPid();
std::string serial = device_info->serialNumber();
std::string uid = device_info->uid();
auto usb_port = parseUsbPort(uid);
serial_numbers_[usb_port] = serial;
is_gemini330_ = isGemini335PID(pid);
}
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM(get_logger(), orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(get_logger(), e.what());
} catch (...) {
RCLCPP_ERROR_STREAM(get_logger(), "unknown error");
initializeDeviceInfo();
loadParameters();
for (size_t i = 0; i < usb_ports_.size(); ++i) {
usb_index_map_[usb_ports_[i]] = static_cast<int>(i);
}
params_init();
for (size_t i = 0; i < usb_params_.size(); i++) {
usb_numbers_[i] = usb_params_[i];
usb_index_map_[usb_params_[i]] = i;
}
for (const auto &pair : serial_numbers_) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"usb_port: " << pair.first << ", serial: " << pair.second);
}
for (const auto &pair : usb_index_map_) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"usb_port: " << pair.first << ", index: " << pair.second);
for (const auto &entry : serial_numbers_) {
RCLCPP_INFO(get_logger(), "usb_port: %s, serial: %s", entry.first.c_str(),
entry.second.c_str());
}
capture_control_srv_ = this->create_service<orbbec_camera_msgs::srv::SetInt32>(
"start_capture", std::bind(&MultiCameraSubscriber::controlCaptureCallback, this,
@@ -74,9 +86,27 @@ class MultiCameraSubscriber : public rclcpp::Node {
}
private:
std::mutex image_mutex_;
std::mutex meta_mutex_;
bool isGemini335PID(uint32_t pid) {
void initializeDeviceInfo() {
try {
auto context = std::make_unique<ob::Context>();
context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE);
auto list = context->queryDeviceList();
for (size_t i = 0; i < list->deviceCount(); ++i) {
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));
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(get_logger(), e.what());
} catch (...) {
RCLCPP_ERROR(get_logger(), "unknown error while querying devices");
}
}
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 ||
@@ -85,326 +115,312 @@ class MultiCameraSubscriber : public rclcpp::Node {
pid == GEMINI_338LG_PID || pid == GEMINI_338LE_PID || pid == GEMINI_338L_PID ||
pid == GEMINI_331L_PID;
}
void params_init() {
void loadParameters() {
std::ifstream file(
"install/orbbec_camera/share/orbbec_camera/config/tools/multisavergbir/"
"multi_save_rgbir_params.json");
if (!file.is_open()) {
RCLCPP_ERROR(this->get_logger(), "Failed to open JSON file.");
RCLCPP_ERROR(get_logger(), "Failed to open JSON file.");
return;
}
nlohmann::json json_data;
file >> json_data;
time_domain_ = json_data["save_rgbir_params"]["time_domain"].get<std::string>();
time_domain_ =
(time_domain_ == "device") ? "_d" : (time_domain_ == "global" ? "_g" : "_unknown");
usb_params_ = json_data["save_rgbir_params"]["usb_ports"].get<std::vector<std::string>>();
camera_name_ = json_data["save_rgbir_params"]["camera_name"].get<std::vector<std::string>>();
left_ir_topics_.resize(camera_name_.size());
left_ir_metadata_topic_.resize(camera_name_.size());
color_topics_.resize(camera_name_.size());
color_metadata_topic_.resize(camera_name_.size());
for (size_t i = 0; i < camera_name_.size(); ++i) {
left_ir_topics_[i] =
"/" + camera_name_[i] + "/" + (is_gemini330_ ? "left_ir" : "ir") + "/image_raw";
left_ir_metadata_topic_[i] =
"/" + camera_name_[i] + "/" + (is_gemini330_ ? "left_ir" : "ir") + "/metadata";
color_topics_[i] = "/" + camera_name_[i] + "/color/image_raw";
color_metadata_topic_[i] = "/" + camera_name_[i] + "/color/metadata";
const auto &params = json_data["save_rgbir_params"];
const auto time_domain = params["time_domain"].get<std::string>();
time_domain_suffix_ =
time_domain == "device" ? "_d" : (time_domain == "global" ? "_g" : "_unknown");
usb_ports_ = params["usb_ports"].get<std::vector<std::string>>();
camera_names_ = params["camera_name"].get<std::vector<std::string>>();
if (params.contains("stream_names")) {
for (const auto &stream_name : params["stream_names"].get<std::vector<std::string>>()) {
if (!isSupportedStreamName(stream_name)) {
throw std::invalid_argument("Unsupported stream name in multi_save_rgbir config: " +
stream_name);
}
if (std::find(configured_stream_names_.begin(), configured_stream_names_.end(),
stream_name) == configured_stream_names_.end()) {
configured_stream_names_.push_back(stream_name);
}
}
}
}
void topic_init() {
ir_image_buffers_.resize(left_ir_topics_.size());
color_image_buffers_.resize(left_ir_topics_.size());
ir_current_timestamp_buffers_.resize(left_ir_topics_.size());
color_current_timestamp_buffers_.resize(left_ir_topics_.size());
ir_timestamp_buffers_.resize(left_ir_topics_.size());
color_timestamp_buffers_.resize(left_ir_topics_.size());
left_ir_metadata_.exposure_buffs.resize(left_ir_topics_.size());
left_ir_metadata_.gain_buffs.resize(left_ir_topics_.size());
color_metadata_.exposure_buffs.resize(left_ir_topics_.size());
color_metadata_.gain_buffs.resize(left_ir_topics_.size());
callback_called_ = std::vector<bool>(left_ir_topics_.size(), false);
auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
rclcpp::CallbackGroup::SharedPtr reentrant_callback_group_;
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"camera_name_.size(): " << camera_name_.size());
for (size_t i = 0; i < camera_name_.size(); ++i) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"left_ir_topic: " << left_ir_topics_[i]);
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"left_ir_metadata_topic_: " << left_ir_metadata_topic_[i]);
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"color_topic: " << color_topics_[i]);
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"color_metadata_topic_: " << color_metadata_topic_[i]);
reentrant_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
rclcpp::SubscriptionOptions ir_sub_options;
ir_sub_options.callback_group = reentrant_callback_group_;
rclcpp::SubscriptionOptions color_sub_options;
color_sub_options.callback_group = reentrant_callback_group_;
std::vector<std::string> discoverStreamNames(const std::string &camera_name) const {
std::vector<std::string> stream_names;
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()) {
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);
}
}
return stream_names;
}
auto ir_sub = this->create_subscription<sensor_msgs::msg::Image>(
left_ir_topics_[i], custom_qos,
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
this->irCallback(msg, i);
},
ir_sub_options);
std::vector<std::string> selectStreamNames(const std::string &camera_name) const {
if (!configured_stream_names_.empty()) {
return configured_stream_names_;
}
auto stream_names = discoverStreamNames(camera_name);
if (!stream_names.empty()) {
return stream_names;
}
auto ir_metadata_sub = this->create_subscription<orbbec_camera_msgs::msg::Metadata>(
left_ir_metadata_topic_[i], custom_qos,
[this, i](std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg) {
this->ir_meta_Callback(msg, i);
});
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"};
}
auto color_sub = this->create_subscription<sensor_msgs::msg::Image>(
color_topics_[i], custom_qos,
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
this->colorCallback(msg, i);
},
color_sub_options);
void initializeTopics() {
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));
auto color_metadata_sub = this->create_subscription<orbbec_camera_msgs::msg::Metadata>(
color_metadata_topic_[i], custom_qos,
[this, i](std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg) {
this->color_meta_Callback(msg, i);
});
for (size_t camera_index = 0; camera_index < camera_names_.size(); ++camera_index) {
const auto stream_names = selectStreamNames(camera_names_[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;
ir_subscribers_.push_back(ir_sub);
ir_meta_subscribers_.push_back(ir_metadata_sub);
color_subscribers_.push_back(color_sub);
color_meta_subscribers_.push_back(color_metadata_sub);
for (const auto &stream_name : stream_names) {
captures_[camera_index].emplace(stream_name, StreamCapture{});
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<sensor_msgs::msg::Image>(
image_topic, custom_qos,
[this, camera_index,
stream_name](const std::shared_ptr<const sensor_msgs::msg::Image> image) {
imageCallback(image, camera_index, stream_name);
},
options));
metadata_subscribers_.push_back(
this->create_subscription<orbbec_camera_msgs::msg::Metadata>(
metadata_topic, custom_qos,
[this, camera_index, stream_name](
const std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> metadata) {
metadataCallback(metadata, camera_index, stream_name);
},
options));
}
}
}
std::string getCurrentTimes() {
auto now = std::chrono::system_clock::now();
auto now_time_t = std::chrono::system_clock::to_time_t(now);
std::tm tm = *std::localtime(&now_time_t);
std::ostringstream date_stream;
date_stream << std::put_time(&tm, "%Y%m%d%H%M%S");
std::string date_str = date_stream.str();
return date_str;
std::string currentDateTime() const {
const auto now = std::chrono::system_clock::now();
const auto now_time = std::chrono::system_clock::to_time_t(now);
const std::tm time_info = *std::localtime(&now_time);
std::ostringstream output;
output << std::put_time(&time_info, "%Y%m%d%H%M%S");
return output.str();
}
std::string generateFolderName(const std::string &serial_number, size_t serial_index) {
std::string path = std::string("multicamera_sync/output/") + currenttimes_ + "/" +
"TotalModeFrames/" + "/" + "SN" + serial_number + "_Index" +
std::to_string(serial_index);
std::string generateFolderName(const std::string &serial_number, size_t serial_index) const {
const std::string path = "multicamera_sync/output/" + current_date_time_ +
"/TotalModeFrames/SN" + serial_number + "_Index" +
std::to_string(serial_index);
std::filesystem::create_directories(path);
return path;
}
std::string getTimestamp() {
auto now = this->get_clock()->now();
int64_t seconds = now.seconds();
int64_t nanoseconds = now.nanoseconds() % 1000000000;
int64_t milliseconds = nanoseconds / 1000000;
std::string receiveTimestamp() const {
const auto now = this->get_clock()->now();
const int64_t seconds = now.seconds();
const int64_t milliseconds = now.nanoseconds() % 1000000000 / 1000000;
return std::to_string(seconds) + std::to_string(milliseconds);
}
std::string getCurrentTimestamp(const sensor_msgs::msg::Image::ConstSharedPtr &image_msg) {
int64_t seconds = image_msg->header.stamp.sec;
int64_t nanoseconds = image_msg->header.stamp.nanosec;
int64_t milliseconds = nanoseconds / 1000000;
std::string imageTimestamp(const sensor_msgs::msg::Image::ConstSharedPtr &image) const {
const int64_t milliseconds = image->header.stamp.nanosec / 1000000;
std::ostringstream timestamp;
timestamp << seconds << std::setw(3) << std::setfill('0') << milliseconds;
timestamp << image->header.stamp.sec << std::setw(3) << std::setfill('0') << milliseconds;
return timestamp.str();
}
void saveAlignedImages(size_t index) {
auto &ir_images = ir_image_buffers_[index];
auto &ir_current_timestamps = ir_current_timestamp_buffers_[index];
auto &ir_timestamps = ir_timestamp_buffers_[index];
auto &color_images = color_image_buffers_[index];
auto &color_current_timestamps = color_current_timestamp_buffers_[index];
auto &color_timestamps = color_timestamp_buffers_[index];
auto &left_ir_meta_exposure = left_ir_metadata_.exposure_buffs[index];
auto &left_ir_meta_gain = left_ir_metadata_.gain_buffs[index];
auto &color_meta_exposure = color_metadata_.exposure_buffs[index];
auto &color_meta_gain = color_metadata_.gain_buffs[index];
callback_called_[index] = true;
if (ir_images.size() < static_cast<size_t>(saving_images_number_) ||
color_images.size() < static_cast<size_t>(saving_images_number_)) {
bool captureReady(size_t camera_index) const {
if (camera_index >= captures_.size() || captures_[camera_index].empty()) {
return false;
}
return std::all_of(
captures_[camera_index].begin(), captures_[camera_index].end(), [this](const auto &entry) {
return entry.second.images.size() >= static_cast<size_t>(saving_images_number_);
});
}
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];
}
if (frame_index < capture.gains.size()) {
suffix += "_d" + capture.gains[frame_index];
}
return suffix;
}
void saveImages(size_t camera_index) {
if (!captureReady(camera_index)) {
return;
}
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "index:" << index);
auto usb_iter = usb_index_map_.find(usb_numbers_[index]);
auto serial_iter = serial_numbers_.find(usb_numbers_[index]);
int usb_index = usb_iter->second;
if (serial_iter == serial_numbers_.end()) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "serial_iter is empty");
if (camera_index >= usb_ports_.size()) {
RCLCPP_ERROR(get_logger(), "Missing USB port configuration for camera index %zu",
camera_index);
return;
}
std::string serial_index = serial_iter->second;
const auto serial_it = serial_numbers_.find(usb_ports_[camera_index]);
if (serial_it == serial_numbers_.end()) {
RCLCPP_ERROR(get_logger(), "No serial number found for USB port %s",
usb_ports_[camera_index].c_str());
return;
}
const auto usb_index_it = usb_index_map_.find(usb_ports_[camera_index]);
const size_t usb_index = usb_index_it == usb_index_map_.end()
? camera_index
: static_cast<size_t>(usb_index_it->second);
const std::string &serial_number = serial_it->second;
const std::string folder = generateFolderName(serial_number, usb_index);
callback_called_[camera_index] = true;
for (size_t i = 0; i < static_cast<size_t>(saving_images_number_); i++) {
std::string folder = generateFolderName(serial_index, usb_index);
std::string ir_filename =
folder + "/ir#left_SN" + serial_index + "_Index" + std::to_string(usb_index) +
time_domain_ + ir_current_timestamps[i] + "_f" + std::to_string(i) + "_s" +
ir_timestamps[i] +
(is_gemini330_ ? ("_e" + left_ir_meta_exposure[i] + "_d" + left_ir_meta_gain[i]) : "") +
"_.jpg";
if (ir_images[i].empty()) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over ");
continue;
for (const auto &entry : captures_[camera_index]) {
const std::string &stream_name = entry.first;
const auto &capture = entry.second;
for (size_t i = 0; i < static_cast<size_t>(saving_images_number_); ++i) {
if (capture.images[i].empty()) {
continue;
}
const std::string filename = folder + "/" + stream_name + "_SN" + serial_number + "_Index" +
std::to_string(usb_index) + time_domain_suffix_ +
capture.current_timestamps[i] + "_f" + std::to_string(i) +
"_s" + capture.receive_timestamps[i] +
metadataSuffix(capture, i) + "_.jpg";
cv::imwrite(filename, capture.images[i]);
}
cv::imwrite(ir_filename, ir_images[i]);
std::string color_filename =
folder + "/color_SN" + serial_index + "_Index" + std::to_string(usb_index) +
time_domain_ + color_current_timestamps[i] + "_f" + std::to_string(i) + "_s" +
color_timestamps[i] +
(is_gemini330_ ? ("_e" + color_meta_exposure[i] + "_d" + color_meta_gain[i]) : "") +
"_.jpg";
if (color_images[i].empty()) {
continue;
}
cv::imwrite(color_filename, color_images[i]);
// RCLCPP_INFO(this->get_logger(), "Saved Color image to: %s", color_filename.c_str());
}
ir_image_buffers_[index].clear();
ir_current_timestamp_buffers_[index].clear();
ir_timestamp_buffers_[index].clear();
color_image_buffers_[index].clear();
color_current_timestamp_buffers_[index].clear();
color_timestamp_buffers_[index].clear();
left_ir_metadata_.exposure_buffs[index].clear();
left_ir_metadata_.gain_buffs[index].clear();
color_metadata_.exposure_buffs[index].clear();
color_metadata_.gain_buffs[index].clear();
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"callback_called_ " << index << ":" << callback_called_[index]);
bool all_true =
std::all_of(callback_called_.begin(), callback_called_.end(), [](bool v) { return v; });
if (all_true) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over ");
for (auto &entry : captures_[camera_index]) {
entry.second.clear();
}
const bool all_cameras_complete = std::all_of(callback_called_.begin(), callback_called_.end(),
[](bool value) { return value; });
if (all_cameras_complete) {
RCLCPP_INFO(get_logger(), "Capture completed for all cameras");
saving_images_number_ = 0;
callback_called_.clear();
callback_called_ = std::vector<bool>(left_ir_topics_.size(), false);
callback_called_.assign(camera_names_.size(), false);
}
}
void controlCaptureCallback(
const std::shared_ptr<orbbec_camera_msgs::srv::SetInt32::Request> request,
std::shared_ptr<orbbec_camera_msgs::srv::SetInt32::Response> response) {
(void)response;
currenttimes_ = getCurrentTimes();
std::lock_guard<std::mutex> lock(capture_mutex_);
if (request->data <= 0) {
response->success = false;
response->message = "capture image count must be greater than zero";
return;
}
if (!topics_initialized_) {
initializeTopics();
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) {
entry.second.clear();
}
}
callback_called_.assign(camera_names_.size(), false);
current_date_time_ = currentDateTime();
saving_images_number_ = request->data;
if (!topic_init_) {
topic_init();
topic_init_ = true;
}
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"saving_images_number_: " << saving_images_number_);
response->success = true;
response->message = "capture started";
RCLCPP_INFO(get_logger(), "Capturing %d image(s) from each configured stream",
saving_images_number_);
}
void irCallback(std::shared_ptr<const sensor_msgs::msg::Image> image, size_t index) {
std::lock_guard<std::mutex> lock(image_mutex_);
if (!callback_called_[index] && static_cast<size_t>(saving_images_number_)) {
cv::Mat ir_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
std::string current_timestamp_ir = getCurrentTimestamp(image);
std::string timestamp_ir = getTimestamp();
ir_image_buffers_[index].push_back(ir_mat);
ir_current_timestamp_buffers_[index].push_back(current_timestamp_ir);
ir_timestamp_buffers_[index].push_back(timestamp_ir);
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
":ir: " << index << ":" << ir_image_buffers_[index].size());
if (ir_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
color_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
(!is_gemini330_ || (color_metadata_.exposure_buffs[index].size() >=
static_cast<size_t>(saving_images_number_) &&
left_ir_metadata_.exposure_buffs[index].size() >=
static_cast<size_t>(saving_images_number_)))) {
saveAlignedImages(index);
}
void imageCallback(const std::shared_ptr<const sensor_msgs::msg::Image> image,
size_t camera_index, const std::string &stream_name) {
std::lock_guard<std::mutex> lock(capture_mutex_);
if (saving_images_number_ <= 0 || callback_called_[camera_index]) {
return;
}
auto &capture = captures_[camera_index].at(stream_name);
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;
}
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);
}
void colorCallback(std::shared_ptr<const sensor_msgs::msg::Image> image, size_t index) {
std::lock_guard<std::mutex> lock(image_mutex_);
if (!callback_called_[index] && static_cast<size_t>(saving_images_number_)) {
cv::Mat color_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
cv::Mat corrected_image;
cv::cvtColor(color_mat, corrected_image, cv::COLOR_RGB2BGR);
std::string current_timestamp_color = getCurrentTimestamp(image);
std::string timestamp_color = getTimestamp();
color_image_buffers_[index].push_back(corrected_image);
color_current_timestamp_buffers_[index].push_back(current_timestamp_color);
color_timestamp_buffers_[index].push_back(timestamp_color);
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
":color: " << index << ":" << color_image_buffers_[index].size());
if (ir_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
color_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
(!is_gemini330_ || (color_metadata_.exposure_buffs[index].size() >=
static_cast<size_t>(saving_images_number_) &&
left_ir_metadata_.exposure_buffs[index].size() >=
static_cast<size_t>(saving_images_number_)))) {
saveAlignedImages(index);
void metadataCallback(const std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> metadata,
size_t camera_index, const std::string &stream_name) {
std::lock_guard<std::mutex> lock(capture_mutex_);
if (saving_images_number_ <= 0 || callback_called_[camera_index]) {
return;
}
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());
}
if (json_data.contains("gain")) {
capture.gains.push_back(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());
}
}
void ir_meta_Callback(std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg,
size_t index) {
std::lock_guard<std::mutex> lock(meta_mutex_);
if (!callback_called_[index] && static_cast<size_t>(saving_images_number_)) {
nlohmann::json json_data = nlohmann::json::parse(msg->json_data);
left_ir_metadata_.exposure_buffs[index].push_back(json_data["exposure"].dump());
left_ir_metadata_.gain_buffs[index].push_back(json_data["gain"].dump());
}
}
void color_meta_Callback(std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg,
size_t index) {
std::lock_guard<std::mutex> lock(meta_mutex_);
if (!callback_called_[index] && static_cast<size_t>(saving_images_number_)) {
nlohmann::json json_data = nlohmann::json::parse(msg->json_data);
color_metadata_.exposure_buffs[index].push_back(json_data["exposure"].dump());
color_metadata_.gain_buffs[index].push_back(json_data["gain"].dump());
}
}
std::mutex capture_mutex_;
std::vector<rclcpp::CallbackGroup::SharedPtr> callback_groups_;
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> image_subscribers_;
std::vector<rclcpp::Subscription<orbbec_camera_msgs::msg::Metadata>::SharedPtr>
ir_meta_subscribers_;
std::vector<rclcpp::Subscription<orbbec_camera_msgs::msg::Metadata>::SharedPtr>
color_meta_subscribers_;
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> ir_subscribers_;
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> color_subscribers_;
metadata_subscribers_;
rclcpp::Service<orbbec_camera_msgs::srv::SetInt32>::SharedPtr capture_control_srv_;
std::map<std::string, int> usb_index_map_;
std::map<std::string, std::string> serial_numbers_;
std::array<std::string, 10> usb_numbers_;
std::vector<std::string> usb_params_;
std::vector<std::string> camera_name_;
std::vector<std::string> left_ir_metadata_topic_;
std::vector<std::string> color_metadata_topic_;
std::vector<std::string> left_ir_topics_;
std::vector<std::string> color_topics_;
std::string time_domain_;
std::vector<std::vector<cv::Mat>> ir_image_buffers_;
std::vector<std::vector<cv::Mat>> color_image_buffers_;
std::vector<std::vector<std::string>> ir_current_timestamp_buffers_;
std::vector<std::vector<std::string>> color_current_timestamp_buffers_;
std::vector<std::vector<std::string>> ir_timestamp_buffers_;
std::vector<std::vector<std::string>> color_timestamp_buffers_;
std::vector<std::string> usb_ports_;
std::vector<std::string> camera_names_;
std::vector<std::string> configured_stream_names_;
std::vector<std::map<std::string, StreamCapture>> captures_;
std::vector<bool> callback_called_;
std::string currenttimes_;
int saving_images_number_ = 100;
bool topic_init_ = false;
bool is_gemini330_ = true;
ImageMetadata left_ir_metadata_ = ImageMetadata();
ImageMetadata color_metadata_ = ImageMetadata();
std::string time_domain_suffix_;
std::string current_date_time_;
int saving_images_number_ = 0;
bool topics_initialized_ = false;
bool has_gemini330_device_ = false;
};
} // namespace tools
} // namespace orbbec_camera
RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::tools::MultiCameraSubscriber)
orbbec_camera/tools/start_benchmark.cpp
+33
View File
@@ -19,6 +19,16 @@ class StartBenchmark : public rclcpp::Node {
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
this->color_Callback(msg, i);
}));
left_color_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
left_color_topics_[i], custom_qos,
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
this->leftColorCallback(msg, i);
}));
right_color_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
right_color_topics_[i], custom_qos,
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
this->rightColorCallback(msg, i);
}));
depth_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
depth_topics_[i], custom_qos,
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
@@ -45,6 +55,10 @@ class StartBenchmark : public rclcpp::Node {
this->color_point_cloud_Callback(msg, i);
}));
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), color_topics_[i] << " is subed ");
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"),
left_color_topics_[i] << " is subed ");
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"),
right_color_topics_[i] << " is subed ");
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), depth_topics_[i] << " is subed ");
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), left_ir_topics_[i] << " is subed ");
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), right_ir_topics_[i] << " is subed ");
@@ -57,6 +71,8 @@ class StartBenchmark : public rclcpp::Node {
private:
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> color_subs_;
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> left_color_subs_;
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> right_color_subs_;
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> depth_subs_;
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> left_ir_subs_;
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> right_ir_subs_;
@@ -67,6 +83,8 @@ class StartBenchmark : public rclcpp::Node {
std::vector<std::string> camera_name_;
std::vector<std::string> color_topics_;
std::vector<std::string> left_color_topics_;
std::vector<std::string> right_color_topics_;
std::vector<std::string> depth_topics_;
std::vector<std::string> left_ir_topics_;
std::vector<std::string> right_ir_topics_;
@@ -88,6 +106,8 @@ class StartBenchmark : public rclcpp::Node {
camera_name_ =
json_data["start_benchmark_params"]["camera_name"].get<std::vector<std::string>>();
color_topics_.resize(camera_name_.size());
left_color_topics_.resize(camera_name_.size());
right_color_topics_.resize(camera_name_.size());
depth_topics_.resize(camera_name_.size());
left_ir_topics_.resize(camera_name_.size());
right_ir_topics_.resize(camera_name_.size());
@@ -95,6 +115,8 @@ class StartBenchmark : public rclcpp::Node {
color_point_cloud_topics_.resize(camera_name_.size());
for (size_t i = 0; i < camera_name_.size(); ++i) {
color_topics_[i] = "/" + camera_name_[i] + "/color/image_raw";
left_color_topics_[i] = "/" + camera_name_[i] + "/left_color/image_raw";
right_color_topics_[i] = "/" + camera_name_[i] + "/right_color/image_raw";
depth_topics_[i] = "/" + camera_name_[i] + "/depth/image_raw";
left_ir_topics_[i] = "/" + camera_name_[i] + "/left_ir/image_raw";
right_ir_topics_[i] = "/" + camera_name_[i] + "/right_ir/image_raw";
@@ -108,6 +130,17 @@ class StartBenchmark : public rclcpp::Node {
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),
"time is : " << msg->step << "color is subed " << index << "is subed");
}
void leftColorCallback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
std::lock_guard<std::mutex> lock(image_mutex_);
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),
"time is : " << msg->step << "left_color is subed " << index << "is subed");
}
void rightColorCallback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
std::lock_guard<std::mutex> lock(image_mutex_);
RCLCPP_DEBUG_STREAM(
rclcpp::get_logger("StartBenchmark"),
"time is : " << msg->step << "right_color is subed " << index << "is subed");
}
void depth_Callback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
std::lock_guard<std::mutex> lock(image_mutex_);
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),