diff --git a/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json b/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json index d557155a..dd1d3703 100644 --- a/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json +++ b/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json @@ -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" + ] } -} \ No newline at end of file +} 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 48791ba7..455838ec 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 @@ -33,6 +33,7 @@ #include #include #include +#include #include 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, 7> &supported_stream_suffixes() { + static const std::array, 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(stamp.sec) + static_cast(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 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; diff --git a/orbbec_camera/scripts/common_benchmark_node.py b/orbbec_camera/scripts/common_benchmark_node.py index a012d5a6..54cb5fc1 100644 --- a/orbbec_camera/scripts/common_benchmark_node.py +++ b/orbbec_camera/scripts/common_benchmark_node.py @@ -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" diff --git a/orbbec_camera/tools/list_camera_profile.cpp b/orbbec_camera/tools/list_camera_profile.cpp index 7f3673a6..3c54d8c7 100644 --- a/orbbec_camera/tools/list_camera_profile.cpp +++ b/orbbec_camera/tools/list_camera_profile.cpp @@ -154,8 +154,11 @@ void listSensorProfiles(const std::shared_ptr& 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(); std::cout << magic_enum::enum_name(sensor->getType()) << " profile: " << profile->getWidth() diff --git a/orbbec_camera/tools/multi_save_rgbir.cpp b/orbbec_camera/tools/multi_save_rgbir.cpp index 69bbc290..1719acb8 100644 --- a/orbbec_camera/tools/multi_save_rgbir.cpp +++ b/orbbec_camera/tools/multi_save_rgbir.cpp @@ -1,72 +1,84 @@ - #include #include -#include -#include -#include "orbbec_camera/ob_camera_node.h" -#include "orbbec_camera_msgs/msg/metadata.hpp" -#include + +#include +#include +#include +#include +#include #include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#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> exposure_buffs; - std::vector> gain_buffs; +namespace { + +const std::array 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 images; + std::vector current_timestamps; + std::vector receive_timestamps; + std::vector exposures; + std::vector 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(); - 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(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( "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(); + 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(); - time_domain_ = - (time_domain_ == "device") ? "_d" : (time_domain_ == "global" ? "_g" : "_unknown"); - usb_params_ = json_data["save_rgbir_params"]["usb_ports"].get>(); - camera_name_ = json_data["save_rgbir_params"]["camera_name"].get>(); - 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 ¶ms = json_data["save_rgbir_params"]; + const auto time_domain = params["time_domain"].get(); + time_domain_suffix_ = + time_domain == "device" ? "_d" : (time_domain == "global" ? "_g" : "_unknown"); + usb_ports_ = params["usb_ports"].get>(); + camera_names_ = params["camera_name"].get>(); + + if (params.contains("stream_names")) { + for (const auto &stream_name : params["stream_names"].get>()) { + 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(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 discoverStreamNames(const std::string &camera_name) const { + std::vector 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( - left_ir_topics_[i], custom_qos, - [this, i](std::shared_ptr msg) { - this->irCallback(msg, i); - }, - ir_sub_options); + std::vector 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( - left_ir_metadata_topic_[i], custom_qos, - [this, i](std::shared_ptr 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( - color_topics_[i], custom_qos, - [this, i](std::shared_ptr 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( - color_metadata_topic_[i], custom_qos, - [this, i](std::shared_ptr 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( + image_topic, custom_qos, + [this, camera_index, + stream_name](const std::shared_ptr image) { + 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)); + } } } - 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(saving_images_number_) || - color_images.size() < static_cast(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(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(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(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(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(left_ir_topics_.size(), false); + callback_called_.assign(camera_names_.size(), false); } } void controlCaptureCallback( const std::shared_ptr request, std::shared_ptr response) { - (void)response; - currenttimes_ = getCurrentTimes(); + std::lock_guard 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 image, size_t index) { - std::lock_guard lock(image_mutex_); - if (!callback_called_[index] && static_cast(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(saving_images_number_) && - color_image_buffers_[index].size() >= static_cast(saving_images_number_) && - (!is_gemini330_ || (color_metadata_.exposure_buffs[index].size() >= - static_cast(saving_images_number_) && - left_ir_metadata_.exposure_buffs[index].size() >= - static_cast(saving_images_number_)))) { - saveAlignedImages(index); - } + + void imageCallback(const std::shared_ptr image, + size_t camera_index, const std::string &stream_name) { + std::lock_guard 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 image, size_t index) { - std::lock_guard lock(image_mutex_); - if (!callback_called_[index] && static_cast(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(saving_images_number_) && - color_image_buffers_[index].size() >= static_cast(saving_images_number_) && - (!is_gemini330_ || (color_metadata_.exposure_buffs[index].size() >= - static_cast(saving_images_number_) && - left_ir_metadata_.exposure_buffs[index].size() >= - static_cast(saving_images_number_)))) { - saveAlignedImages(index); + + void metadataCallback(const std::shared_ptr metadata, + size_t camera_index, const std::string &stream_name) { + std::lock_guard 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 msg, - size_t index) { - std::lock_guard lock(meta_mutex_); - if (!callback_called_[index] && static_cast(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 msg, - size_t index) { - std::lock_guard lock(meta_mutex_); - if (!callback_called_[index] && static_cast(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 callback_groups_; + std::vector::SharedPtr> image_subscribers_; std::vector::SharedPtr> - ir_meta_subscribers_; - std::vector::SharedPtr> - color_meta_subscribers_; - std::vector::SharedPtr> ir_subscribers_; - std::vector::SharedPtr> color_subscribers_; + metadata_subscribers_; rclcpp::Service::SharedPtr capture_control_srv_; std::map usb_index_map_; std::map serial_numbers_; - std::array usb_numbers_; - - std::vector usb_params_; - std::vector camera_name_; - std::vector left_ir_metadata_topic_; - std::vector color_metadata_topic_; - std::vector left_ir_topics_; - std::vector color_topics_; - std::string time_domain_; - - std::vector> ir_image_buffers_; - std::vector> color_image_buffers_; - std::vector> ir_current_timestamp_buffers_; - std::vector> color_current_timestamp_buffers_; - std::vector> ir_timestamp_buffers_; - std::vector> color_timestamp_buffers_; - + std::vector usb_ports_; + std::vector camera_names_; + std::vector configured_stream_names_; + std::vector> captures_; std::vector 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) diff --git a/orbbec_camera/tools/start_benchmark.cpp b/orbbec_camera/tools/start_benchmark.cpp index 5987a554..9d1fe622 100644 --- a/orbbec_camera/tools/start_benchmark.cpp +++ b/orbbec_camera/tools/start_benchmark.cpp @@ -19,6 +19,16 @@ class StartBenchmark : public rclcpp::Node { [this, i](std::shared_ptr msg) { this->color_Callback(msg, i); })); + left_color_subs_.push_back(this->create_subscription( + left_color_topics_[i], custom_qos, + [this, i](std::shared_ptr msg) { + this->leftColorCallback(msg, i); + })); + right_color_subs_.push_back(this->create_subscription( + right_color_topics_[i], custom_qos, + [this, i](std::shared_ptr msg) { + this->rightColorCallback(msg, i); + })); depth_subs_.push_back(this->create_subscription( depth_topics_[i], custom_qos, [this, i](std::shared_ptr 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::SharedPtr> color_subs_; + std::vector::SharedPtr> left_color_subs_; + std::vector::SharedPtr> right_color_subs_; std::vector::SharedPtr> depth_subs_; std::vector::SharedPtr> left_ir_subs_; std::vector::SharedPtr> right_ir_subs_; @@ -67,6 +83,8 @@ class StartBenchmark : public rclcpp::Node { std::vector camera_name_; std::vector color_topics_; + std::vector left_color_topics_; + std::vector right_color_topics_; std::vector depth_topics_; std::vector left_ir_topics_; std::vector right_ir_topics_; @@ -88,6 +106,8 @@ class StartBenchmark : public rclcpp::Node { camera_name_ = json_data["start_benchmark_params"]["camera_name"].get>(); 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 msg, size_t index) { + std::lock_guard 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 msg, size_t index) { + std::lock_guard 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 msg, size_t index) { std::lock_guard lock(image_mutex_); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),