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

Merge branch 'v2/develop' into v2-main

Browse Source
This commit is contained in:
ob-yalian
2026-09-16 13:59:52 +08:00
parent 7c4954791c 7a5f11ae39
commit 4b621d6eb2
28 changed files with 1840 additions and 678 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/CMakeLists.txt
+34 -2
View File
@@ -142,8 +142,40 @@ if(USE_NV_HW_DECODER)
set(TEGRA_ARMABI /usr/lib/aarch64-linux-gnu/)
add_definitions(-DUSE_NV_HW_DECODER)
add_compile_options(-Wno-missing-field-initializers -Wno-unused-parameter)
set(NV_LIBRARIES -lnvjpeg -lnvbufsurface -lnvbufsurftransform -lyuv -lv4l2)
list(APPEND NV_LIBRARIES -L${TEGRA_ARMABI} -L${TEGRA_ARMABI}/tegra)
# Search Jetson Multimedia API libraries, excluding CUDA library directories.
find_library(JETSON_JPEG_LIBRARY
NAMES nvmm_jpeg nvjpeg
PATHS
"${TEGRA_ARMABI}/nvidia"
"${TEGRA_ARMABI}/tegra"
NO_DEFAULT_PATH
)
if(NOT JETSON_JPEG_LIBRARY)
message(FATAL_ERROR
"Jetson Multimedia API JPEG library not found. "
"Install the Multimedia API matching the system L4T version."
)
endif()
message(STATUS "Jetson JPEG library: ${JETSON_JPEG_LIBRARY}")
set(NV_LIBRARIES
"${JETSON_JPEG_LIBRARY}"
-lnvbufsurface -lnvbufsurftransform -lyuv -lv4l2
)
list(APPEND NV_LIBRARIES
"-L${TEGRA_ARMABI}"
"-L${TEGRA_ARMABI}/nvidia"
"-L${TEGRA_ARMABI}/tegra"
)
# The nvmm_jpeg Multimedia API classes use the CUDA driver API.
if(JETSON_JPEG_LIBRARY MATCHES "/libnvmm_jpeg\\.so")
if(CMAKE_VERSION VERSION_LESS "3.17")
message(FATAL_ERROR "nvmm_jpeg support requires CMake 3.17 or newer to find CUDAToolkit.")
endif()
find_package(CUDAToolkit REQUIRED)
list(APPEND NV_LIBRARIES CUDA::cuda_driver)
endif()
endif()
set(COMMON_INCLUDE_DIRS
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
+52 -19
View File
@@ -1,4 +1,5 @@
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <sensor_msgs/msg/image.hpp>
#if __has_include(<message_filters/subscriber.hpp>)
@@ -33,6 +34,7 @@
#include <opencv2/opencv.hpp>
#include <string>
#include <thread>
#include <utility>
#include <vector>
using Image = sensor_msgs::msg::Image;
@@ -69,7 +71,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 +168,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 +200,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 +227,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 +253,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 +267,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('/');
@@ -280,7 +299,17 @@ class ImageSyncNode : public rclcpp::Node {
try {
for (const auto &msg : msgs) {
auto cv_image = cv_bridge::toCvShare(msg);
images.push_back(cv_image->image.clone());
cv::Mat image;
if (msg->encoding == sensor_msgs::image_encodings::RGB8) {
cv::cvtColor(cv_image->image, image, cv::COLOR_RGB2BGR);
} else if (msg->encoding == sensor_msgs::image_encodings::RGBA8) {
cv::cvtColor(cv_image->image, image, cv::COLOR_RGBA2BGR);
} else if (msg->encoding == sensor_msgs::image_encodings::BGRA8) {
cv::cvtColor(cv_image->image, image, cv::COLOR_BGRA2BGR);
} else {
image = cv_image->image.clone();
}
images.push_back(std::move(image));
timestamps.push_back(stamp_to_seconds(msg->header.stamp));
}
} catch (cv_bridge::Exception &e) {
@@ -459,8 +488,14 @@ class ImageSyncNode : public rclcpp::Node {
cv::applyColorMap(tmp, image, cv::COLORMAP_JET);
} else if (images[i].channels() == 3) {
image = images[i].clone();
} else if (images[i].channels() == 4) {
cv::cvtColor(images[i], image, cv::COLOR_BGRA2BGR);
} else {
cv::cvtColor(images[i], image, cv::COLOR_GRAY2BGR);
RCLCPP_WARN(this->get_logger(), "Display first channel of unsupported %d-channel image %s",
images[i].channels(), topic_infos[i].topic.c_str());
cv::Mat first_channel;
cv::extractChannel(images[i], first_channel, 0);
cv::cvtColor(first_channel, image, cv::COLOR_GRAY2BGR);
}
const std::string text = topic_infos[i].camera_name + " " + topic_infos[i].image_type +
@@ -550,10 +585,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/include/orbbec_camera/constants.h
+13 -2
View File
@@ -143,15 +143,26 @@ const int32_t GEMINI_435Le_PID = 0x815; // Gemini 435Le
const int32_t GEMINI_305_PID = 0x0840; // Gemini 305
const int32_t GEMINI_305_PID2 = 0x0841; // Gemini 305
const int32_t GEMINI_305G_PID = 0x0842; // Gemini 305g
const int32_t GEMINI_301G_PID = 0x0843; // Gemini 301g
const int32_t GEMINI_309G_PID = 0x0845; // Gemini 309g
const int32_t GEMINI_338LG_PID = 0x081A; // Gemini 338Lg
const int32_t GEMINI_338LE_PID = 0x081B; // Gemini 338Le
const int32_t GEMINI_338L_PID = 0x081C; // Gemini 338L
const int32_t GEMINI_331L_PID = 0x081D; // Gemini 331L
inline bool isGemini305SeriesPID(uint32_t pid) {
inline bool isGemini330SeriesPID(uint32_t pid) {
return pid == GEMINI_335_PID || pid == GEMINI_330_PID || pid == GEMINI_336_PID ||
pid == GEMINI_335L_PID || pid == GEMINI_330L_PID || pid == GEMINI_336L_PID ||
pid == GEMINI_335LG_PID || pid == GEMINI_336LG_PID || pid == GEMINI_335LE_PID ||
pid == GEMINI_336LE_PID || pid == CUSTOM_ADVANTECH_GEMINI_336_PID ||
pid == CUSTOM_ADVANTECH_GEMINI_336L_PID || pid == GEMINI_338_PID ||
pid == GEMINI_338LG_PID || pid == GEMINI_338LE_PID || pid == GEMINI_338L_PID ||
pid == GEMINI_331L_PID;
}
inline bool isGemini301SeriesPID(uint32_t pid) {
return pid == GEMINI_305_PID || pid == GEMINI_305_PID2 || pid == GEMINI_305G_PID ||
pid == GEMINI_309G_PID;
pid == GEMINI_301G_PID || pid == GEMINI_309G_PID;
}
inline bool isGmslCameraPID(uint32_t pid) {
orbbec_camera/include/orbbec_camera/fps_delay_status.hpp
+45 -26
View File
@@ -20,6 +20,7 @@
#include "orbbec_camera_msgs/msg/device_status.hpp"
#include <chrono>
#include <functional>
#include <limits>
#include <mutex>
namespace orbbec_camera {
class FpsDelayStatus {
@@ -58,38 +59,57 @@ class FpsDelayStatus {
}
void fillColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
std::lock_guard<std::mutex> lock(mutex_);
msg.color_frame_rate_cur = last_fps_;
msg.color_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
msg.color_frame_rate_min = fps_min_;
msg.color_frame_rate_max = fps_max_;
fillStatus(msg.color_frame_rate_cur, msg.color_frame_rate_avg, msg.color_frame_rate_min,
msg.color_frame_rate_max, msg.color_delay_ms_cur, msg.color_delay_ms_avg,
msg.color_delay_ms_min, msg.color_delay_ms_max);
}
msg.color_delay_ms_cur = last_delay_ms_;
msg.color_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
msg.color_delay_ms_min = delay_min_;
msg.color_delay_ms_max = delay_max_;
void fillLeftColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.left_color_frame_rate_cur, msg.left_color_frame_rate_avg,
msg.left_color_frame_rate_min, msg.left_color_frame_rate_max,
msg.left_color_delay_ms_cur, msg.left_color_delay_ms_avg,
msg.left_color_delay_ms_min, msg.left_color_delay_ms_max);
}
last_delay_ms_ = 0.0;
last_fps_ = 0.0;
frame_count_ = 0;
fps_sum_ = delay_sum_ = 0.0;
fps_max_ = delay_max_ = 0.0;
fps_min_ = delay_min_ = 0.0;
void fillRightColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.right_color_frame_rate_cur, msg.right_color_frame_rate_avg,
msg.right_color_frame_rate_min, msg.right_color_frame_rate_max,
msg.right_color_delay_ms_cur, msg.right_color_delay_ms_avg,
msg.right_color_delay_ms_min, msg.right_color_delay_ms_max);
}
void fillDepthStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.depth_frame_rate_cur, msg.depth_frame_rate_avg, msg.depth_frame_rate_min,
msg.depth_frame_rate_max, msg.depth_delay_ms_cur, msg.depth_delay_ms_avg,
msg.depth_delay_ms_min, msg.depth_delay_ms_max);
}
void fillLeftIrStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.left_ir_frame_rate_cur, msg.left_ir_frame_rate_avg, msg.left_ir_frame_rate_min,
msg.left_ir_frame_rate_max, msg.left_ir_delay_ms_cur, msg.left_ir_delay_ms_avg,
msg.left_ir_delay_ms_min, msg.left_ir_delay_ms_max);
}
void fillRightIrStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
fillStatus(msg.right_ir_frame_rate_cur, msg.right_ir_frame_rate_avg,
msg.right_ir_frame_rate_min, msg.right_ir_frame_rate_max, msg.right_ir_delay_ms_cur,
msg.right_ir_delay_ms_avg, msg.right_ir_delay_ms_min, msg.right_ir_delay_ms_max);
}
private:
void fillStatus(double &frame_rate_cur, double &frame_rate_avg, double &frame_rate_min,
double &frame_rate_max, double &delay_ms_cur, double &delay_ms_avg,
double &delay_ms_min, double &delay_ms_max) {
std::lock_guard<std::mutex> lock(mutex_);
msg.depth_frame_rate_cur = last_fps_;
msg.depth_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
msg.depth_frame_rate_min = fps_min_;
msg.depth_frame_rate_max = fps_max_;
frame_rate_cur = last_fps_;
frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
frame_rate_min = frame_count_ > 0 ? fps_min_ : 0;
frame_rate_max = frame_count_ > 0 ? fps_max_ : 0;
msg.depth_delay_ms_cur = last_delay_ms_;
msg.depth_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
msg.depth_delay_ms_min = delay_min_;
msg.depth_delay_ms_max = delay_max_;
// RCLCPP_ERROR_STREAM(logger_, "Depth status: " << fps_sum_ << "," << frame_count_);
delay_ms_cur = last_delay_ms_;
delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
delay_ms_min = frame_count_ > 0 ? delay_min_ : 0;
delay_ms_max = frame_count_ > 0 ? delay_max_ : 0;
last_delay_ms_ = 0.0;
last_fps_ = 0.0;
@@ -99,7 +119,6 @@ class FpsDelayStatus {
fps_min_ = delay_min_ = 0.0;
}
private:
mutable std::mutex mutex_;
u_int64_t last_stream_timestamp_{0};
double last_delay_ms_{0.0};
orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h
+1 -1
View File
@@ -21,7 +21,7 @@ namespace orbbec_camera {
class FrameTimestampCsvLogger {
public:
enum class OutputMode { SYNCED, COLOR, DEPTH };
enum class OutputMode { SYNCED, COLOR, LEFT_COLOR, RIGHT_COLOR, DEPTH, LEFT_IR, RIGHT_IR };
FrameTimestampCsvLogger(bool drop_log_enabled, const std::string &csv_file_path,
OutputMode output_mode, rclcpp::Logger logger);
orbbec_camera/include/orbbec_camera/jetson_nv_decoder.h
+5 -9
View File
@@ -14,15 +14,10 @@
* limitations under the License.
*******************************************************************************/
#pragma once
#include "utils.h"
#include <opencv2/opencv.hpp>
#include <memory>
#include "jpeg_decoder.h"
#include <NvJpegDecoder.h>
#include <NvUtils.h>
#include <NvV4l2Element.h>
#include <NvJpegDecoder.h>
#include <NvV4l2Element.h>
namespace orbbec_camera {
class JetsonNvJPEGDecoder : public JPEGDecoder {
@@ -33,6 +28,7 @@ class JetsonNvJPEGDecoder : public JPEGDecoder {
bool decode(const std::shared_ptr<ob::ColorFrame>& frame, uint8_t* dest) override;
private:
NvJPEGDecoder* decoder_;
class Impl;
std::unique_ptr<Impl> decoder_;
};
} // namespace orbbec_camera
} // namespace orbbec_camera
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+15 -4
View File
@@ -237,10 +237,14 @@ class OBCameraNode {
}
void getColorStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
fps_delay_status_color_->fillColorStatus(status_msg);
fps_delay_status_left_color_->fillLeftColorStatus(status_msg);
fps_delay_status_right_color_->fillRightColorStatus(status_msg);
}
void getDepthStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
fps_delay_status_depth_->fillDepthStatus(status_msg);
fps_delay_status_left_ir_->fillLeftIrStatus(status_msg);
fps_delay_status_right_ir_->fillRightIrStatus(status_msg);
}
bool checkUserCalibrationReady() {
@@ -305,6 +309,9 @@ class OBCameraNode {
void setupProfiles();
bool validate301SeriesStreamFrameRates(const std::map<stream_index_pair, int>& fps,
std::string& message) const;
std::shared_ptr<ob::VideoStreamProfile> selectVideoStreamProfile(
const stream_index_pair& stream_index, int width, int height, int fps, OBFormat format);
@@ -470,7 +477,7 @@ class OBCameraNode {
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
void setFloorEnableCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
void setFloodEnableCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
@@ -666,8 +673,6 @@ class OBCameraNode {
orbbec_camera_msgs::msg::IMUInfo createIMUInfo(const stream_index_pair& stream_index);
static bool isGemini335PID(uint32_t pid);
static bool isGemini435LePID(uint32_t pid);
static bool isPublishMetaData(uint32_t pid);
static bool isDabaiASeriesForHwD2C(uint32_t pid);
@@ -804,7 +809,7 @@ class OBCameraNode {
rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_laser_status_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ptp_config_srv_;
rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_ptp_config_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_floor_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_flood_enable_srv_;
rclcpp::Service<SetInt32>::SharedPtr set_fan_work_mode_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_sensors_srv_;
rclcpp::Service<GetInt32>::SharedPtr get_lrm_measure_distance_srv_;
@@ -1188,12 +1193,18 @@ class OBCameraNode {
bool show_fps_enable_ = false;
bool enable_publish_extrinsic_ = false;
std::unique_ptr<FpsCounter> fps_counter_color_{nullptr};
std::unique_ptr<FpsCounter> fps_counter_left_color_{nullptr};
std::unique_ptr<FpsCounter> fps_counter_right_color_{nullptr};
std::unique_ptr<FpsCounter> fps_counter_depth_{nullptr};
std::unique_ptr<FpsCounter> fps_counter_left_ir_{nullptr};
std::unique_ptr<FpsCounter> fps_counter_right_ir_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_color_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_left_color_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_right_color_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_depth_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_left_ir_{nullptr};
std::unique_ptr<FpsDelayStatus> fps_delay_status_right_ir_{nullptr};
std::string intra_camera_sync_reference_ = "";
std::string ae_reference_stream_;
orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h
+11
View File
@@ -22,7 +22,11 @@ class TimestampCsvLogger {
std::string csv_file_path;
bool frame_sync_enabled = false;
bool color_enabled = false;
bool left_color_enabled = false;
bool right_color_enabled = false;
bool depth_enabled = false;
bool left_ir_enabled = false;
bool right_ir_enabled = false;
bool imu_sync_enabled = false;
bool accel_enabled = false;
bool gyro_enabled = false;
@@ -44,6 +48,9 @@ class TimestampCsvLogger {
const std::shared_ptr<ob::Frame> &depth_frame, int64_t arrival_system_us,
int64_t arrival_steady_us, bool track_color, bool track_depth,
bool color_image_publish_expected, bool depth_image_publish_expected);
void recordImageFrameArrival(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
int64_t arrival_system_us, int64_t arrival_steady_us,
bool image_publish_expected);
void recordImagePrePublish(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
int64_t publish_system_us, int64_t publish_steady_us);
void recordImagePublishSkipped(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame);
@@ -64,7 +71,11 @@ class TimestampCsvLogger {
std::atomic_bool shutdown_requested_{false};
std::unique_ptr<FrameTimestampCsvLogger> synced_image_logger_;
std::unique_ptr<FrameTimestampCsvLogger> color_logger_;
std::unique_ptr<FrameTimestampCsvLogger> left_color_logger_;
std::unique_ptr<FrameTimestampCsvLogger> right_color_logger_;
std::unique_ptr<FrameTimestampCsvLogger> depth_logger_;
std::unique_ptr<FrameTimestampCsvLogger> left_ir_logger_;
std::unique_ptr<FrameTimestampCsvLogger> right_ir_logger_;
std::unique_ptr<ImuTimestampCsvLogger> synced_imu_logger_;
std::unique_ptr<ImuTimestampCsvLogger> accel_logger_;
std::unique_ptr<ImuTimestampCsvLogger> gyro_logger_;
orbbec_camera/launch/gemini_301_series.launch.py
+20 -20
View File
@@ -116,11 +116,11 @@ def generate_launch_description():
DeclareLaunchArgument('left_color_frame_queue_max_frames', default_value='10'),
DeclareLaunchArgument('right_color_frame_queue_max_frames', default_value='10'),
DeclareLaunchArgument('ae_reference_stream', default_value='depth'), # depth or color
DeclareLaunchArgument('ae_strategy', default_value='motion'), # default or motion
DeclareLaunchArgument('color_width', default_value='848'),
DeclareLaunchArgument('color_height', default_value='530'),
DeclareLaunchArgument('color_fps', default_value='30'),
DeclareLaunchArgument('color_format', default_value='YUYV'),
DeclareLaunchArgument('ae_strategy', default_value='default'), # default or motion
DeclareLaunchArgument('color_width', default_value='0'),
DeclareLaunchArgument('color_height', default_value='0'),
DeclareLaunchArgument('color_fps', default_value='0'),
DeclareLaunchArgument('color_format', default_value='ANY'),
DeclareLaunchArgument('enable_color', default_value='true'),
DeclareLaunchArgument('color_qos', default_value='default'),
DeclareLaunchArgument('color_qos_history', default_value='default'),
@@ -142,7 +142,6 @@ def generate_launch_description():
DeclareLaunchArgument('color_gain', default_value='-1'),
DeclareLaunchArgument('enable_color_auto_white_balance', default_value='true'),
DeclareLaunchArgument('color_white_balance', default_value='-1'),
DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'),
DeclareLaunchArgument('color_ae_max_exposure', default_value='-1'),
DeclareLaunchArgument('color_brightness', default_value='-1'),
DeclareLaunchArgument('color_sharpness', default_value='-1'),
@@ -158,11 +157,11 @@ def generate_launch_description():
DeclareLaunchArgument('color_denoising_level', default_value='-1'),#0: Auto; 1-8: higher values indicate stronger denoising.
#Note: The color_denoising_level configuration is supported only when AE is enabled, and requires new firmware support.
DeclareLaunchArgument('depth_width', default_value='848'),
DeclareLaunchArgument('depth_height', default_value='530'),
DeclareLaunchArgument('depth_width', default_value='0'),
DeclareLaunchArgument('depth_height', default_value='0'),
DeclareLaunchArgument('depth_decimation_factor', default_value='1'),
DeclareLaunchArgument('depth_fps', default_value='30'),
DeclareLaunchArgument('depth_format', default_value='Y16'),
DeclareLaunchArgument('depth_fps', default_value='0'),
DeclareLaunchArgument('depth_format', default_value='ANY'),
DeclareLaunchArgument('enable_depth', default_value='true'),
DeclareLaunchArgument('depth_qos', default_value='default'),
DeclareLaunchArgument('depth_qos_history', default_value='default'),
@@ -178,11 +177,11 @@ def generate_launch_description():
DeclareLaunchArgument('depth_ae_roi_top', default_value='-1'),
DeclareLaunchArgument('depth_ae_roi_bottom', default_value='-1'),
DeclareLaunchArgument('mean_intensity_set_point', default_value='-1'),
DeclareLaunchArgument('left_ir_width', default_value='848'),
DeclareLaunchArgument('left_ir_height', default_value='530'),
DeclareLaunchArgument('left_ir_width', default_value='0'),
DeclareLaunchArgument('left_ir_height', default_value='0'),
DeclareLaunchArgument('left_ir_decimation_factor', default_value='1'),
DeclareLaunchArgument('left_ir_fps', default_value='30'),
DeclareLaunchArgument('left_ir_format', default_value='Y8'),
DeclareLaunchArgument('left_ir_fps', default_value='0'),
DeclareLaunchArgument('left_ir_format', default_value='ANY'),
DeclareLaunchArgument('enable_left_ir', default_value='false'),
DeclareLaunchArgument('left_ir_qos', default_value='default'),
DeclareLaunchArgument('left_ir_qos_history', default_value='default'),
@@ -193,11 +192,11 @@ def generate_launch_description():
DeclareLaunchArgument('left_ir_mirror', default_value='false'),
DeclareLaunchArgument('enable_left_ir_sequence_id_filter', default_value='false'),
DeclareLaunchArgument('left_ir_sequence_id_filter_id', default_value='-1'),
DeclareLaunchArgument('right_ir_width', default_value='848'),
DeclareLaunchArgument('right_ir_height', default_value='530'),
DeclareLaunchArgument('right_ir_width', default_value='0'),
DeclareLaunchArgument('right_ir_height', default_value='0'),
DeclareLaunchArgument('right_ir_decimation_factor', default_value='1'),
DeclareLaunchArgument('right_ir_fps', default_value='30'),
DeclareLaunchArgument('right_ir_format', default_value='Y8'),
DeclareLaunchArgument('right_ir_fps', default_value='0'),
DeclareLaunchArgument('right_ir_format', default_value='ANY'),
DeclareLaunchArgument('enable_right_ir', default_value='false'),
DeclareLaunchArgument('right_ir_qos', default_value='default'),
DeclareLaunchArgument('right_ir_qos_history', default_value='default'),
@@ -208,7 +207,8 @@ def generate_launch_description():
DeclareLaunchArgument('right_ir_mirror', default_value='false'),
DeclareLaunchArgument('enable_right_ir_sequence_id_filter', default_value='false'),
DeclareLaunchArgument('right_ir_sequence_id_filter_id', default_value='-1'),
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
# Gemini 301 color, depth, and IR streams share one auto-exposure switch.
DeclareLaunchArgument('enable_auto_exposure', default_value='true'),
DeclareLaunchArgument('ir_exposure', default_value='-1'),
DeclareLaunchArgument('ir_gain', default_value='-1'),
DeclareLaunchArgument('ir_ae_max_exposure', default_value='-1'),
@@ -291,7 +291,7 @@ def generate_launch_description():
DeclareLaunchArgument('config_file_path', default_value=''),
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
DeclareLaunchArgument('enable_firmware_log', default_value='false'),
DeclareLaunchArgument('enable_fps_boost', default_value='false'),
DeclareLaunchArgument('enable_fps_boost', default_value='true'),
DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
orbbec_camera/scripts/common_benchmark_node.py
+380 -108
View File
@@ -4,11 +4,14 @@
Support: ROS2
name: common_benchmark_node.py
function: A ROS2 node to monitor and log the performance of an Orbbec camera node:
frame rates, delays, CPU and RAM usage, packet/frame loss statistics.
subscriber-side frame rates, end-to-end delays, CPU and RAM usage,
and estimated frame loss statistics.
usage:
ros2 run orbbec_camera common_benchmark_node.py --run_time 20 --csv_file /tmp/cam_log.csv
You can also pass an ideal frame rate for drop detection: --ideal_fps 30
Monitor multiple cameras: --camera_names camera,camera01
Select topics explicitly: --topics color,depth
Compressed images and point clouds require full topic names.
"""
import argparse
@@ -21,11 +24,22 @@ import csv
import os
from collections import defaultdict
from orbbec_camera_msgs.msg import DeviceStatus
from sensor_msgs.msg import Image
from sensor_msgs.msg import CompressedImage, Image, PointCloud2
from tabulate import tabulate
CAMERA_NODE_NAMES = ["component_container", "orbbec_camera_node", "nodelet"]
MONITORED_STREAMS = (
"color",
"depth",
"ir",
"left_ir",
"right_ir",
"left_color",
"right_color",
)
DISCOVERY_INTERVAL_SECONDS = 0.1
DISCOVERY_DURATION_SECONDS = 1.0
DOCUMENTATION_URL = (
"https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/"
"6_benchmark/benchmark_tools.html"
@@ -99,24 +113,62 @@ def estimate_dropped_frames(dt, expected_interval):
# ----------------------------------------------
class TopicTracker:
def __init__(self, logger=None):
def __init__(self, logger=None, sample_start_time=None):
self.received = 0
self.last_time = None
self.sample_start_time = sample_start_time or time.monotonic()
self.last_sample_time = self.sample_start_time
self.last_sample_received = 0
self.last_header_stamp = None
self.estimated_interval = None
self.drop_frames = 0
self.logger = logger
def on_msg(self, header, avg_fps):
stamp = header.stamp.sec + header.stamp.nanosec * 1e-9
def on_msg(self, header, ros_receive_time, ideal_fps):
"""Record one received message and return its age in milliseconds."""
header_stamp = header.stamp.sec + header.stamp.nanosec * 1e-9
self.received += 1
delay_ms = None
if self.last_time is not None and avg_fps > 0:
dt = stamp - self.last_time
expected_interval = 1.0 / avg_fps
self.drop_frames += estimate_dropped_frames(dt, expected_interval)
if header_stamp > 0:
delay_ms = (ros_receive_time - header_stamp) * 1000.0
if self.last_header_stamp is not None:
header_dt = header_stamp - self.last_header_stamp
if header_dt > 0:
expected_interval = (
1.0 / ideal_fps
if ideal_fps and ideal_fps > 0.0
else self.estimated_interval
)
if expected_interval is not None:
self.drop_frames += estimate_dropped_frames(
header_dt, expected_interval
)
self.update_estimated_interval(header_dt)
self.last_header_stamp = header_stamp
self.last_time = stamp
return delay_ms
def sample_fps(self, sample_time):
"""Calculate current and average subscriber throughput."""
window_elapsed = sample_time - self.last_sample_time
total_elapsed = sample_time - self.sample_start_time
if window_elapsed <= 0.0 or total_elapsed <= 0.0:
return None
window_received = self.received - self.last_sample_received
current_fps = window_received / window_elapsed
average_fps = self.received / total_elapsed
self.last_sample_time = sample_time
self.last_sample_received = self.received
return current_fps, average_fps
def update_estimated_interval(self, interval):
"""Learn the nominal source interval while excluding likely frame gaps."""
if self.estimated_interval is None or interval < 0.75 * self.estimated_interval:
self.estimated_interval = interval
elif interval <= 1.5 * self.estimated_interval:
self.estimated_interval = 0.9 * self.estimated_interval + 0.1 * interval
def frames_loss_rate(self):
total = self.received + self.drop_frames
@@ -129,17 +181,27 @@ class TopicTracker:
class CameraMonitorNode(Node):
def __init__(self, run_time, csv_file="camera_monitor_log.csv", ideal_fps: float = 0.0, camera_names=None):
def __init__(
self,
run_time,
csv_file="camera_monitor_log.csv",
ideal_fps: float = 0.0,
camera_names=None,
topics=None,
):
super().__init__("camera_monitor_node")
self.run_time = run_time
self.start_time = time.time()
self.discovery_start_time = time.time()
self.discovery_start_monotonic = None
self.start_time = None
self.process = psutil.Process(os.getpid())
self.first_data_collected = False
self.camera_names = parse_camera_names(camera_names)
self.node_names = {camera_name: "Not Found" for camera_name in self.camera_names}
self.total_node_name = "Not Found"
# If > 0, use this ideal fps value for drop-frame detection instead of the reported average
# If > 0, use this ideal FPS for drop detection instead of learning the
# nominal interval from received image timestamps.
self.ideal_fps = float(ideal_fps) if ideal_fps is not None else 0.0
self.finished = False
@@ -153,22 +215,26 @@ class CameraMonitorNode(Node):
"stats": defaultdict(make_stat),
"cpu_stats": make_stat(),
"ram_stats": make_stat(),
"trackers": {
"color": TopicTracker(logger=self.get_logger()),
"depth": TopicTracker(logger=self.get_logger())
}
"trackers": {},
}
self.total_cpu_stats = make_stat()
self.total_ram_stats = make_stat()
self.topic_subscriptions = {}
self.topic_configs = {}
self.discovered_streams = {}
self.discovery_complete = False
self.discovery_timer = None
self.timer = None
self.requested_streams = self.parse_requested_topics(topics)
# CSV
self.csv_file = csv_file
self.csv_fh = open(self.csv_file, "w", newline="")
self.csv_writer = csv.writer(self.csv_fh)
self.csv_writer.writerow(self.build_csv_header())
# subscriptions
# Device status subscriptions are always present. Data subscriptions
# are created once after automatic discovery or explicit selection.
for camera_name in self.camera_names:
ns = self.camera_namespace(camera_name)
self.create_subscription(
@@ -177,29 +243,26 @@ 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
)
# timer runs every 1s to update system stats, log csv and print status
self.timer = self.create_timer(1.0, self.timer_callback)
if self.requested_streams:
self.finish_topic_discovery(self.requested_streams)
else:
self.discovery_start_monotonic = time.monotonic()
self.discovery_timer = self.create_timer(
DISCOVERY_INTERVAL_SECONDS, self.update_topic_discovery
)
def timer_callback(self):
if not self.discovery_complete:
return
elapsed = time.time() - self.start_time
if elapsed > self.run_time:
self.finish()
rclpy.shutdown()
return
self.update_topic_fps_stats()
camera_sys_stats, total_cpu, total_ram, self.total_node_name = self.get_camera_stats()
for camera_name in self.camera_names:
camera = self.cameras[camera_name]
@@ -220,7 +283,8 @@ class CameraMonitorNode(Node):
return
self.finished = True
elapsed = time.time() - self.start_time
timer_start = self.start_time or self.discovery_start_time
elapsed = time.time() - timer_start
try:
self.csv_fh.close()
except Exception:
@@ -231,6 +295,150 @@ class CameraMonitorNode(Node):
def camera_namespace(self, camera_name):
return "/" + camera_name.strip("/")
def image_topic_name(self, camera_name, stream):
return f"{self.camera_namespace(camera_name)}/{stream}/image_raw"
def make_raw_image_config(self, camera_name, stream):
return {
"camera_name": camera_name,
"topic_id": stream,
"topic_name": self.image_topic_name(camera_name, stream),
"msg_type": Image,
}
def parse_full_topic(self, topic_name):
normalized_topic = "/" + topic_name.strip("/")
for camera_name in self.camera_names:
namespace_prefix = self.camera_namespace(camera_name) + "/"
if not normalized_topic.startswith(namespace_prefix):
continue
relative_name = normalized_topic[len(namespace_prefix):]
for stream in MONITORED_STREAMS:
raw_name = f"{stream}/image_raw"
if relative_name == raw_name:
return self.make_raw_image_config(camera_name, stream)
if relative_name == f"{raw_name}/compressed":
return {
"camera_name": camera_name,
"topic_id": f"{stream}_compressed",
"topic_name": normalized_topic,
"msg_type": CompressedImage,
}
if relative_name == f"{raw_name}/compressedDepth":
return {
"camera_name": camera_name,
"topic_id": f"{stream}_compressed_depth",
"topic_name": normalized_topic,
"msg_type": CompressedImage,
}
point_cloud_ids = {
"depth/points": "depth_points",
"depth_registered/points": "depth_registered_points",
}
if relative_name in point_cloud_ids:
return {
"camera_name": camera_name,
"topic_id": point_cloud_ids[relative_name],
"topic_name": normalized_topic,
"msg_type": PointCloud2,
}
return None
def parse_requested_topics(self, topics):
if not topics:
return {}
requested = str(topics).replace(";", ",").split(",")
selected = {}
for value in requested:
value = value.strip()
if not value:
continue
if value in MONITORED_STREAMS:
for camera_name in self.camera_names:
config = self.make_raw_image_config(camera_name, value)
selected[(camera_name, config["topic_id"])] = config
continue
config = self.parse_full_topic(value)
if config is None:
raise ValueError(
f"Unsupported topic '{value}'. Specify a raw or compressed image topic, "
"or a depth/points or depth_registered/points topic under a configured "
"camera namespace."
)
selected[(config["camera_name"], config["topic_id"])] = config
return selected
def find_published_image_streams(self):
published_streams = {}
for camera_name in self.camera_names:
for stream in MONITORED_STREAMS:
config = self.make_raw_image_config(camera_name, stream)
key = (camera_name, config["topic_id"])
if self.count_publishers(config["topic_name"]) > 0:
published_streams[key] = config
return published_streams
def update_topic_discovery(self):
if self.discovery_complete:
return
self.discovered_streams.update(self.find_published_image_streams())
elapsed = time.monotonic() - self.discovery_start_monotonic
if elapsed >= DISCOVERY_DURATION_SECONDS:
self.finish_topic_discovery(self.discovered_streams)
def finish_topic_discovery(self, selected_streams):
self.topic_configs = dict(selected_streams)
sample_start_time = time.monotonic()
for key, config in self.topic_configs.items():
camera_name = config["camera_name"]
topic_id = config["topic_id"]
self.cameras[camera_name]["trackers"][topic_id] = TopicTracker(
logger=self.get_logger(), sample_start_time=sample_start_time
)
self.topic_subscriptions[key] = self.create_subscription(
config["msg_type"],
config["topic_name"],
lambda msg, name=camera_name, selected_topic_id=topic_id: self.topic_callback(
msg, name, selected_topic_id
),
5,
)
self.csv_writer.writerow(self.build_csv_header())
self.csv_fh.flush()
self.start_time = time.time()
self.discovery_complete = True
self.timer = self.create_timer(1.0, self.timer_callback)
if self.discovery_timer is not None:
self.discovery_timer.cancel()
def topics_for_camera(self, camera_name):
return [
config
for config in self.topic_configs.values()
if config["camera_name"] == camera_name
]
def update_topic_fps_stats(self):
sample_time = time.monotonic()
for config in self.topic_configs.values():
camera = self.cameras[config["camera_name"]]
topic_id = config["topic_id"]
fps_sample = camera["trackers"][topic_id].sample_fps(sample_time)
if fps_sample is None:
continue
current_fps, average_fps = fps_sample
self.update_sample_stat(
camera["stats"][f"{topic_id}_fps"],
current_fps,
average=average_fps,
)
def cmdline_has_camera_namespace(self, cmdline_args, camera_name):
ns = self.camera_namespace(camera_name)
candidates = [
@@ -311,32 +519,30 @@ class CameraMonitorNode(Node):
camera["prev_online"] = msg.device_online
# update stats from DeviceStatus message fields
self.update_stats(camera["stats"], "color_fps", msg.color_frame_rate_cur, msg.color_frame_rate_min, msg.color_frame_rate_max, msg.color_frame_rate_avg)
self.update_stats(camera["stats"], "color_delay", msg.color_delay_ms_cur, msg.color_delay_ms_min, msg.color_delay_ms_max, msg.color_delay_ms_avg)
self.update_stats(camera["stats"], "depth_fps", msg.depth_frame_rate_cur, msg.depth_frame_rate_min, msg.depth_frame_rate_max, msg.depth_frame_rate_avg)
self.update_stats(camera["stats"], "depth_delay", msg.depth_delay_ms_cur, msg.depth_delay_ms_min, msg.depth_delay_ms_max, msg.depth_delay_ms_avg)
def image_callback(self, msg: Image, camera_name: str, stream: str):
if stream not in ("color", "depth"):
return
header = msg.header
def topic_callback(self, msg, camera_name: str, topic_id: str):
self.first_data_collected = True
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)
camera["data_collected"] = True
tracker = camera["trackers"][topic_id]
ros_receive_time = self.get_clock().now().nanoseconds * 1e-9
delay_ms = tracker.on_msg(
msg.header,
ros_receive_time,
self.ideal_fps,
)
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
return
s = stats[key]
s["cur"] = (cur)
s["count"] += 1
s["sum"] += avg_val
s["avg"] = s["sum"] / s["count"] if s["count"] > 0 else 0.0
s["min"] = min(s["min"], min_val)
s["max"] = max(s["max"], max_val)
if delay_ms is not None:
self.update_sample_stat(
camera["stats"][f"{topic_id}_delay"], delay_ms
)
def update_sample_stat(self, stat, value, average=None):
stat["cur"] = value
stat["count"] += 1
stat["sum"] += value
stat["avg"] = average if average is not None else stat["sum"] / stat["count"]
stat["min"] = min(stat["min"], value)
stat["max"] = max(stat["max"], value)
def update_sys_stat(self, stat_dict, value, online=True):
stat_dict["cur"] = value
@@ -353,7 +559,7 @@ class CameraMonitorNode(Node):
row = [round(elapsed, 2)]
for camera_name in self.camera_names:
camera = self.cameras[camera_name]
row.extend(self.build_camera_csv_values(camera))
row.extend(self.build_camera_csv_values(camera_name, camera))
row.extend([
round(self.total_cpu_stats["cur"], 2), round(self.total_cpu_stats["avg"], 2),
@@ -365,19 +571,42 @@ 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(%)"
]
for camera_name in self.camera_names:
header.extend([f"{camera_name}_{field}" for field in camera_fields])
header.extend(
[
f"{camera_name}_connection_type",
f"{camera_name}_status_online",
f"{camera_name}_disconnects",
]
)
for config in self.topics_for_camera(camera_name):
topic_id = config["topic_id"]
header.extend(
[
f"{camera_name}_{topic_id}_fps_cur",
f"{camera_name}_{topic_id}_fps_avg",
f"{camera_name}_{topic_id}_fps_min",
f"{camera_name}_{topic_id}_fps_max",
f"{camera_name}_{topic_id}_delay_cur",
f"{camera_name}_{topic_id}_delay_avg",
f"{camera_name}_{topic_id}_delay_min",
f"{camera_name}_{topic_id}_delay_max",
f"{camera_name}_{topic_id}_sub_lost_count",
f"{camera_name}_{topic_id}_sub_lost_rate(%)",
]
)
header.extend(
[
f"{camera_name}_cpu_cur",
f"{camera_name}_cpu_avg",
f"{camera_name}_cpu_min",
f"{camera_name}_cpu_max",
f"{camera_name}_ram_cur",
f"{camera_name}_ram_avg",
f"{camera_name}_ram_min",
f"{camera_name}_ram_max",
]
)
header.extend([
"total_cpu_cur", "total_cpu_avg", "total_cpu_min", "total_cpu_max",
@@ -385,10 +614,7 @@ class CameraMonitorNode(Node):
])
return header
def build_camera_csv_values(self, camera):
color_tracker = camera["trackers"]["color"]
depth_tracker = camera["trackers"]["depth"]
def build_camera_csv_values(self, camera_name, camera):
def safe(k):
v = camera["stats"].get(k, {})
return (
@@ -398,29 +624,47 @@ 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 config in self.topics_for_camera(camera_name):
topic_id = config["topic_id"]
tracker = camera["trackers"][topic_id]
if camera["prev_online"]:
values.extend(safe(f"{topic_id}_fps"))
values.extend(safe(f"{topic_id}_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,25 +680,30 @@ class CameraMonitorNode(Node):
rows = []
for camera_name in self.camera_names:
camera = self.cameras[camera_name]
for stream in ["color", "depth"]:
fps_key = f"{stream}_fps"
delay_key = f"{stream}_delay"
topic_name = f"/{camera_name}/{stream}/image_raw"
for config in self.topics_for_camera(camera_name):
topic_id = config["topic_id"]
fps_key = f"{topic_id}_fps"
delay_key = f"{topic_id}_delay"
topic_name = config["topic_name"]
if not camera["prev_online"]:
rows.append([camera_name, topic_name, *["N/A"] * 10])
else:
fps_vals = format_stats(camera["stats"][fps_key])
delay_vals = format_stats(camera["stats"][delay_key])
tracker = camera["trackers"][stream]
tracker = camera["trackers"][topic_id]
frames_loss = tracker.drop_frames
frames_loss_rate = round(tracker.frames_loss_rate() * 100.0, 3)
rows.append([camera_name, topic_name, *fps_vals, *delay_vals, frames_loss, frames_loss_rate])
header_bottom = ["Camera", "Topic", "fps_cur", "fps_avg", "fps_min", "fps_max", "delay_cur(ms)", "delay_avg(ms)", "delay_min(ms)", "delay_max(ms)", "Pub_lost_count", "Pub_lost_rate(%)"]
header_bottom = [
"Camera", "Topic", "fps_cur", "fps_avg", "fps_min", "fps_max",
"delay_cur(ms)", "delay_avg(ms)", "delay_min(ms)", "delay_max(ms)",
"Sub_lost_count", "Sub_lost_rate(%)",
]
os.system("clear")
print("Orbbec Camera Benchmark\n")
print("Orbbec Camera Subscriber Benchmark\n")
print(tabulate([header_bottom] + rows, tablefmt="fancy_grid"))
sys_rows = []
@@ -491,13 +740,36 @@ def main(argv=None):
)
parser.add_argument("--run_time", type=str, default="10s", help="Total run time for monitoring, e.g., 10s, 5m, 1h.")
parser.add_argument("--csv_file", type=str, default="camera_monitor_log.csv")
parser.add_argument("--ideal_fps", type=float, default=0.0, help="Optional ideal frame rate to use for drop detection (overrides reported avg).")
parser.add_argument(
"--ideal_fps",
type=float,
default=0.0,
help=(
"Optional ideal frame rate for subscriber-side drop detection; "
"otherwise it is learned from image timestamps."
),
)
parser.add_argument("--camera_names", type=str, default="camera", help="Comma-separated camera namespaces, e.g., camera,camera01,camera02.")
parser.add_argument(
"--topics",
type=str,
default="",
help=(
"Comma-separated raw stream names or full raw/compressed image and point cloud "
"topics. Automatic discovery only selects raw image topics."
),
)
cli_args, _ = parser.parse_known_args(argv)
rclpy.init(args=argv)
run_time = parse_duration(cli_args.run_time)
node = CameraMonitorNode(run_time, cli_args.csv_file, ideal_fps=cli_args.ideal_fps, camera_names=cli_args.camera_names)
node = CameraMonitorNode(
run_time,
cli_args.csv_file,
ideal_fps=cli_args.ideal_fps,
camera_names=cli_args.camera_names,
topics=cli_args.topics,
)
try:
rclpy.spin(node)
orbbec_camera/scripts/default_service.yaml
+1 -1
View File
@@ -192,7 +192,7 @@ services:
- name: /camera/read_customer_data
type: orbbec_camera_msgs/srv/GetString
- name: /camera/set_floor_enable
- name: /camera/set_flood_enable
type: std_srvs/srv/SetBool
request: {data: false}
- name: /camera/set_fan_work_mode
orbbec_camera/src/dynamic_params.cpp
+13 -11
View File
@@ -21,21 +21,23 @@ Parameters::Parameters(rclcpp::Node *node)
: node_(node), logger_(node_->get_logger()), params_backend_(node) {
params_backend_.addOnSetParametersCallback(
[this](const std::vector<rclcpp::Parameter> &parameters) {
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
for (const auto &parameter : parameters) {
if (param_functions_.find(parameter.get_name()) != param_functions_.end()) {
auto functions = param_functions_[parameter.get_name()];
if (functions.empty()) {
RCLCPP_WARN_STREAM(logger_, "Parameter " << parameter.get_name()
<< " can not be changed in runtime.");
} else {
for (const auto &func : param_functions_[parameter.get_name()]) {
func(parameter);
}
const auto function_it = param_functions_.find(parameter.get_name());
if (function_it == param_functions_.end()) {
continue;
}
if (function_it->second.empty()) {
result.successful = false;
result.reason = "Parameter " + parameter.get_name() + " can not be changed in runtime.";
RCLCPP_WARN_STREAM(logger_, result.reason);
} else {
for (const auto &func : function_it->second) {
func(parameter);
}
}
}
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
return result;
});
}
orbbec_camera/src/frame_timestamp_csv_logger.cpp
+67 -26
View File
@@ -31,6 +31,47 @@ int64_t getExpectedIntervalUs(const std::shared_ptr<ob::Frame> &frame) {
return static_cast<int64_t>(1000000.0 / static_cast<double>(fps));
}
std::optional<FrameTimestampCsvLogger::OutputMode> outputModeForStream(OBStreamType stream_type) {
using OutputMode = FrameTimestampCsvLogger::OutputMode;
switch (stream_type) {
case OB_STREAM_COLOR:
return OutputMode::COLOR;
case OB_STREAM_COLOR_LEFT:
return OutputMode::LEFT_COLOR;
case OB_STREAM_COLOR_RIGHT:
return OutputMode::RIGHT_COLOR;
case OB_STREAM_DEPTH:
return OutputMode::DEPTH;
case OB_STREAM_IR_LEFT:
return OutputMode::LEFT_IR;
case OB_STREAM_IR_RIGHT:
return OutputMode::RIGHT_IR;
default:
return std::nullopt;
}
}
const char *outputModeName(FrameTimestampCsvLogger::OutputMode output_mode) {
using OutputMode = FrameTimestampCsvLogger::OutputMode;
switch (output_mode) {
case OutputMode::COLOR:
return "color";
case OutputMode::LEFT_COLOR:
return "left_color";
case OutputMode::RIGHT_COLOR:
return "right_color";
case OutputMode::DEPTH:
return "depth";
case OutputMode::LEFT_IR:
return "left_ir";
case OutputMode::RIGHT_IR:
return "right_ir";
case OutputMode::SYNCED:
return "synced";
}
return "unknown";
}
} // namespace
FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
@@ -104,9 +145,8 @@ void FrameTimestampCsvLogger::recordStandaloneFrameArrival(OBStreamType stream_t
int64_t arrival_system_us,
int64_t arrival_steady_us,
bool image_publish_expected) {
if (!enabled_ || !frame || !isTrackedStream(stream_type) ||
(stream_type == OB_STREAM_COLOR && output_mode_ != OutputMode::COLOR) ||
(stream_type == OB_STREAM_DEPTH && output_mode_ != OutputMode::DEPTH)) {
const auto expected_output_mode = outputModeForStream(stream_type);
if (!enabled_ || !frame || !expected_output_mode || output_mode_ != *expected_output_mode) {
return;
}
recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us,
@@ -163,14 +203,11 @@ void FrameTimestampCsvLogger::shutdown() {
FrameTimestampCsvLogger::TrackedStream FrameTimestampCsvLogger::toTrackedStream(
OBStreamType stream_type) const {
if (stream_type == OB_STREAM_COLOR) {
return TrackedStream::COLOR;
}
return TrackedStream::DEPTH;
return stream_type == OB_STREAM_DEPTH ? TrackedStream::DEPTH : TrackedStream::COLOR;
}
bool FrameTimestampCsvLogger::isTrackedStream(OBStreamType stream_type) const {
return stream_type == OB_STREAM_COLOR || stream_type == OB_STREAM_DEPTH;
return outputModeForStream(stream_type).has_value();
}
void FrameTimestampCsvLogger::recordFrameSetInternal(
@@ -294,10 +331,12 @@ void FrameTimestampCsvLogger::completeImagePublishInternal(
auto row_id_it = row_map.find(frame_index);
if (row_id_it == row_map.end()) {
if (publish_system_us.has_value()) {
RCLCPP_WARN_STREAM(logger_,
"Frame timestamp CSV logger missed row mapping for stream "
<< (tracked_stream == TrackedStream::COLOR ? "color" : "depth")
<< " frame index " << frame_index);
RCLCPP_WARN_STREAM(
logger_, "Frame timestamp CSV logger missed row mapping for stream "
<< (output_mode_ == OutputMode::SYNCED
? (tracked_stream == TrackedStream::COLOR ? "color" : "depth")
: outputModeName(output_mode_))
<< " frame index " << frame_index);
}
return;
}
@@ -357,7 +396,10 @@ void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStr
previous.dropped_frames += lost_frames;
RCLCPP_WARN_STREAM(logger_,
"Frame drop detected: stage=SDK_RECEIVE"
<< " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth")
<< " stream="
<< (output_mode_ == OutputMode::SYNCED
? (stream == TrackedStream::COLOR ? "color" : "depth")
: outputModeName(output_mode_))
<< " frame_index=" << state.frame_index
<< " dropped=" << previous.dropped_frames);
}
@@ -408,7 +450,10 @@ void FrameTimestampCsvLogger::populatePublishData(StreamState &state, TrackedStr
previous.publish_dropped_frames += lost_frames;
RCLCPP_WARN_STREAM(logger_,
"Frame drop detected: stage=ROS_PUBLISH"
<< " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth")
<< " stream="
<< (output_mode_ == OutputMode::SYNCED
? (stream == TrackedStream::COLOR ? "color" : "depth")
: outputModeName(output_mode_))
<< " frame_index=" << state.frame_index
<< " dropped=" << previous.publish_dropped_frames);
}
@@ -485,12 +530,12 @@ void FrameTimestampCsvLogger::eraseFrameIndexMappingLocked(const PendingRow &row
}
std::string FrameTimestampCsvLogger::serializeRow(const PendingRow &row) const {
if (output_mode_ == OutputMode::COLOR) {
return serializeStreamColumns(row.color);
}
if (output_mode_ == OutputMode::DEPTH) {
return serializeStreamColumns(row.depth);
}
if (output_mode_ != OutputMode::SYNCED) {
return serializeStreamColumns(row.color);
}
std::ostringstream ss;
ss << serializeStreamColumns(row.color) << "," << serializeStreamColumns(row.depth);
return ss.str();
@@ -560,14 +605,12 @@ std::string FrameTimestampCsvLogger::csvHeader() const {
ss << prefix << "_sdk_delay_from_global_us,";
ss << prefix << "_sdk_delay_from_system_us";
};
if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::COLOR) {
append_stream_header("color");
}
if (output_mode_ == OutputMode::SYNCED) {
append_stream_header("color");
ss << ",";
}
if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::DEPTH) {
append_stream_header("depth");
} else {
append_stream_header(outputModeName(output_mode_));
}
return ss.str();
}
@@ -641,10 +684,8 @@ void FrameTimestampCsvLogger::writerThreadMain() {
std::string FrameTimestampCsvLogger::csvFilePathForIndex(uint64_t file_index) const {
const std::filesystem::path original_path(csv_file_path_);
std::string suffix;
if (output_mode_ == OutputMode::COLOR) {
suffix = "_color";
} else if (output_mode_ == OutputMode::DEPTH) {
suffix = "_depth";
if (output_mode_ != OutputMode::SYNCED) {
suffix = "_" + std::string(outputModeName(output_mode_));
}
auto indexed_filename = original_path.stem().string() + suffix;
orbbec_camera/src/jetson_nv_decoder.cpp
+127 -10
View File
@@ -14,22 +14,125 @@
* limitations under the License.
*******************************************************************************/
#include "orbbec_camera/jetson_nv_decoder.h"
#include <NvJpegDecoder.h>
#include <NvV4l2Element.h>
#include <algorithm>
#include <linux/videodev2.h>
#include <setjmp.h>
#include <unistd.h>
#include <NvBufSurface.h>
#include <cstring>
#include <jpeglib.h>
#include <nvbufsurface.h>
#include <nvbufsurftransform.h>
#include <NvBufSurface.h>
#include <fstream>
#include <libyuv.h>
#include <rclcpp/rclcpp.hpp>
#include <v4l2_nv_extensions.h>
#include "jpegint.h"
#include "orbbec_camera/utils.h"
namespace orbbec_camera {
namespace {
JetsonNvJPEGDecoder::JetsonNvJPEGDecoder(int width, int height) : JPEGDecoder(width, height) {}
struct JpegErrorManager {
jpeg_error_mgr base;
jmp_buf jump_buffer;
char message[JMSG_LENGTH_MAX];
};
JetsonNvJPEGDecoder::~JetsonNvJPEGDecoder() { delete decoder_; }
void jpegErrorExit(j_common_ptr cinfo) {
auto *error = reinterpret_cast<JpegErrorManager *>(cinfo->err);
(*cinfo->err->format_message)(cinfo, error->message);
longjmp(error->jump_buffer, 1);
}
} // namespace
class JetsonNvJPEGDecoder::Impl {
public:
Impl() {
std::memset(&cinfo_, 0, sizeof(cinfo_));
std::memset(&error_, 0, sizeof(error_));
cinfo_.err = jpeg_std_error(&error_.base);
error_.base.error_exit = jpegErrorExit;
if (setjmp(error_.jump_buffer) != 0) {
return;
}
jpeg_create_decompress(&cinfo_);
initialized_ = true;
cinfo_.mjpeg_decode = TRUE;
}
~Impl() {
if (initialized_) {
jpeg_destroy_decompress(&cinfo_);
}
}
Impl(const Impl &) = delete;
Impl &operator=(const Impl &) = delete;
bool isInitialized() const { return initialized_; }
const char *lastError() const { return error_.message; }
int decodeToFd(int &fd, unsigned char *input, unsigned long input_size, uint32_t &pixfmt,
uint32_t &width, uint32_t &height) {
if (!initialized_ || input == nullptr || input_size == 0) {
return -1;
}
error_.message[0] = '\0';
if (setjmp(error_.jump_buffer) != 0) {
return -1;
}
NvBufSurface surface;
cinfo_.out_color_space = JCS_YCbCr;
jpeg_mem_src(&cinfo_, input, input_size);
(void)jpeg_read_header(&cinfo_, TRUE);
cinfo_.out_color_space = JCS_YCbCr;
cinfo_.IsVendorbuf = TRUE;
cinfo_.pVendor_buf = reinterpret_cast<unsigned char *>(&surface);
uint32_t pixel_format = 0;
if (cinfo_.comp_info[0].h_samp_factor == 2) {
pixel_format =
cinfo_.comp_info[0].v_samp_factor == 2 ? V4L2_PIX_FMT_YUV420M : V4L2_PIX_FMT_YUV422M;
} else {
pixel_format =
cinfo_.comp_info[0].v_samp_factor == 1 ? V4L2_PIX_FMT_YUV444M : V4L2_PIX_FMT_YUV422RM;
}
jpeg_start_decompress(&cinfo_);
if (cinfo_.global_state != DSTATE_READY) {
return -1;
}
jpeg_read_raw_data(&cinfo_, nullptr, cinfo_.comp_info[0].v_samp_factor * DCTSIZE);
jpeg_finish_decompress(&cinfo_);
width = cinfo_.image_width % 2 == 1 ? cinfo_.image_width + 1 : cinfo_.image_width;
height = cinfo_.image_height % 2 == 1 ? cinfo_.image_height + 1 : cinfo_.image_height;
pixfmt = pixel_format;
fd = cinfo_.fd;
return 0;
}
private:
jpeg_decompress_struct cinfo_{};
JpegErrorManager error_{};
bool initialized_ = false;
};
JetsonNvJPEGDecoder::JetsonNvJPEGDecoder(int width, int height)
: JPEGDecoder(width, height), decoder_(std::make_unique<Impl>()) {}
JetsonNvJPEGDecoder::~JetsonNvJPEGDecoder() = default;
bool JetsonNvJPEGDecoder::decode(const std::shared_ptr<ob::ColorFrame> &frame, uint8_t *dest) {
if (!isValidJPEG(frame)) {
@@ -44,10 +147,24 @@ bool JetsonNvJPEGDecoder::decode(const std::shared_ptr<ob::ColorFrame> &frame, u
while (data_size > 4 && data[data_size - 1] == 0x00) {
data_size--;
}
if (!decoder_ || !decoder_->isInitialized()) {
decoder_ = std::make_unique<Impl>();
}
if (!decoder_->isInitialized()) {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"),
"Failed to initialize NVIDIA JPEG decoder");
decoder_.reset();
return false;
}
int fd = -1;
decoder_ = NvJPEGDecoder::createJPEGDecoder("jpegdec");
std::shared_ptr<int> decoder_deleter(nullptr, [&](int *) { delete decoder_; });
decoder_->decodeToFd(fd, data, data_size, pixfmt, width, height);
if (decoder_->decodeToFd(fd, data, data_size, pixfmt, width, height) != 0) {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Failed to decode JPEG frame");
decoder_.reset();
return false;
}
if (pixfmt != V4L2_PIX_FMT_YUV422M) {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Unexpected pixfmt: " << pixfmt);
if (fd != -1) {
orbbec_camera/src/ob_camera_node.cpp
+245 -77
View File
@@ -650,7 +650,9 @@ void OBCameraNode::publishDepthFiltersStatus() {
if (disp_outliers_filter_supported) {
append_unique_filter_name("DispOutliersFilter");
}
append_unique_filter_name("EnhancedDepthFilter");
if (isGemini330SeriesPID(pid_)) {
append_unique_filter_name("EnhancedDepthFilter");
}
msg.filters.reserve(ordered_filter_names.size());
for (const auto &filter_name : ordered_filter_names) {
@@ -741,7 +743,11 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
timestamp_config.csv_file_path = frame_timestamp_csv_file_;
timestamp_config.frame_sync_enabled = enable_frame_sync_;
timestamp_config.color_enabled = enable_stream_[COLOR];
timestamp_config.left_color_enabled = enable_stream_[COLOR_LEFT];
timestamp_config.right_color_enabled = enable_stream_[COLOR_RIGHT];
timestamp_config.depth_enabled = enable_stream_[DEPTH];
timestamp_config.left_ir_enabled = enable_stream_[INFRA1];
timestamp_config.right_ir_enabled = enable_stream_[INFRA2];
timestamp_config.imu_sync_enabled = enable_sync_output_accel_gyro_;
timestamp_config.accel_enabled = enable_stream_[ACCEL];
timestamp_config.gyro_enabled = enable_stream_[GYRO];
@@ -761,6 +767,8 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
is_camera_node_initialized_ = true;
fps_counter_color_ = std::make_unique<FpsCounter>("Color", logger_, 1);
fps_counter_left_color_ = std::make_unique<FpsCounter>("Left Color", logger_, 1);
fps_counter_right_color_ = std::make_unique<FpsCounter>("Right Color", logger_, 1);
fps_counter_depth_ = std::make_unique<FpsCounter>("Depth", logger_, 1);
fps_counter_left_ir_ = std::make_unique<FpsCounter>("Left Ir", logger_, 1);
fps_counter_right_ir_ = std::make_unique<FpsCounter>("Right Ir", logger_, 1);
@@ -770,12 +778,18 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
log_level = LogLevel::INFO;
}
fps_counter_color_->setLogLevel(log_level);
fps_counter_left_color_->setLogLevel(log_level);
fps_counter_right_color_->setLogLevel(log_level);
fps_counter_depth_->setLogLevel(log_level);
fps_counter_left_ir_->setLogLevel(log_level);
fps_counter_right_ir_->setLogLevel(log_level);
fps_delay_status_color_ = std::make_unique<FpsDelayStatus>(logger_);
fps_delay_status_left_color_ = std::make_unique<FpsDelayStatus>(logger_);
fps_delay_status_right_color_ = std::make_unique<FpsDelayStatus>(logger_);
fps_delay_status_depth_ = std::make_unique<FpsDelayStatus>(logger_);
fps_delay_status_left_ir_ = std::make_unique<FpsDelayStatus>(logger_);
fps_delay_status_right_ir_ = std::make_unique<FpsDelayStatus>(logger_);
}
template <class T>
@@ -1652,24 +1666,38 @@ void OBCameraNode::setupDevices() {
"Current color anti-flicker to "
<< (device_->getBoolProperty(OB_PROP_COLOR_ANTI_FLICKER_BOOL) ? "ON" : "OFF")));
}
if (!color_powerline_freq_.empty() &&
device_->isPropertySupported(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, OB_PERMISSION_WRITE)) {
if (color_powerline_freq_ == "disable") {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, 0);
} else if (color_powerline_freq_ == "50hz") {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, 1);
} else if (color_powerline_freq_ == "60hz") {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, 2);
} else if (color_powerline_freq_ == "auto") {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, 3);
if (!color_powerline_freq_.empty()) {
const auto normalized_color_powerline_freq = lowerParameterValue(color_powerline_freq_);
int color_powerline_freq_value = -1;
if (normalized_color_powerline_freq == "disable") {
color_powerline_freq_value = 0;
} else if (normalized_color_powerline_freq == "50hz") {
color_powerline_freq_value = 1;
} else if (normalized_color_powerline_freq == "60hz") {
color_powerline_freq_value = 2;
} else if (normalized_color_powerline_freq == "auto") {
color_powerline_freq_value = 3;
} else {
RCLCPP_WARN_STREAM(logger_,
"Invalid parameter color_powerline_freq "
<< formatParameterValue(color_powerline_freq_) << ". Valid values: "
<< formatValidParameterValues({"disable", "50hz", "60hz", "auto"})
<< ". Skip setting.");
color_powerline_freq_.clear();
}
if (color_powerline_freq_value >= 0 &&
device_->isPropertySupported(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, OB_PERMISSION_WRITE)) {
color_powerline_freq_ = normalized_color_powerline_freq;
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT,
color_powerline_freq_value);
TRY_EXECUTE_BLOCK({
const auto current_color_powerline_freq =
device_->getIntProperty(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT);
RCLCPP_INFO_STREAM(logger_,
"Current color powerline freq: "
<< colorPowerLineFrequencyToString(current_color_powerline_freq));
});
}
TRY_EXECUTE_BLOCK({
const auto current_color_powerline_freq =
device_->getIntProperty(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT);
RCLCPP_INFO_STREAM(logger_,
"Current color powerline freq: "
<< colorPowerLineFrequencyToString(current_color_powerline_freq));
});
}
if (depth_exposure_ != -1 &&
device_->isPropertySupported(OB_PROP_DEPTH_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
@@ -1707,7 +1735,8 @@ void OBCameraNode::setupDevices() {
"Current depth auto exposure priority: "
<< (device_->getIntProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_PRIORITY_INT) ? "ON" : "OFF")));
}
if (should_apply_launch_config("enable_ir_auto_exposure") &&
if ((should_apply_launch_config("enable_auto_exposure") ||
should_apply_launch_config("enable_ir_auto_exposure")) &&
device_->isPropertySupported(OB_PROP_IR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
@@ -3220,7 +3249,7 @@ void OBCameraNode::setupLeftIrPostProcessFilter() {
}
auto device_info = device_->getDeviceInfo();
CHECK_NOTNULL(device_info);
if (isGemini335PID(pid_)) {
if (isGemini330SeriesPID(pid_) || isGemini301SeriesPID(pid_)) {
auto left_ir_sensor = device_->getSensor(OB_SENSOR_IR_LEFT);
left_ir_filter_list_ = left_ir_sensor->createRecommendedFilters();
if (left_ir_filter_list_.empty()) {
@@ -3261,7 +3290,7 @@ void OBCameraNode::setupRightIrPostProcessFilter() {
}
auto device_info = device_->getDeviceInfo();
CHECK_NOTNULL(device_info);
if (isGemini335PID(pid_)) {
if (isGemini330SeriesPID(pid_) || isGemini301SeriesPID(pid_)) {
auto right_ir_sensor = device_->getSensor(OB_SENSOR_IR_RIGHT);
right_ir_filter_list_ = right_ir_sensor->createRecommendedFilters();
if (right_ir_filter_list_.empty()) {
@@ -3520,35 +3549,44 @@ void OBCameraNode::selectBaseStream() {
void OBCameraNode::printSensorProfiles(const std::shared_ptr<ob::Sensor> &sensor) {
auto profiles = sensor->getStreamProfileList();
const auto sensor_type = sensor->getType();
for (size_t i = 0; i < profiles->getCount(); i++) {
auto origin_profile = profiles->getProfile(i);
if (sensor->getType() == OB_SENSOR_COLOR) {
if (sensor_type == OB_SENSOR_COLOR || sensor_type == OB_SENSOR_COLOR_LEFT ||
sensor_type == OB_SENSOR_COLOR_RIGHT) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(
logger_, "color profile: " << profile->getWidth() << "x" << profile->getHeight() << " "
<< profile->getFps() << "fps " << profile->getFormat());
} else if (sensor->getType() == OB_SENSOR_DEPTH) {
const char *stream_name = sensor_type == OB_SENSOR_COLOR_LEFT ? "left_color"
: sensor_type == OB_SENSOR_COLOR_RIGHT ? "right_color"
: "color";
RCLCPP_INFO_STREAM(logger_, stream_name << " profile: " << profile->getWidth() << "x"
<< profile->getHeight() << " " << profile->getFps()
<< "fps " << profile->getFormat());
} else if (sensor_type == OB_SENSOR_DEPTH) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(
logger_, "depth profile: " << profile->getWidth() << "x" << profile->getHeight() << " "
<< profile->getFps() << "fps " << profile->getFormat());
} else if (sensor->getType() == OB_SENSOR_IR) {
} else if (sensor_type == OB_SENSOR_IR || sensor_type == OB_SENSOR_IR_LEFT ||
sensor_type == OB_SENSOR_IR_RIGHT) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "ir profile: " << profile->getWidth() << "x"
<< profile->getHeight() << " " << profile->getFps()
<< "fps " << profile->getFormat());
} else if (sensor->getType() == OB_SENSOR_ACCEL) {
const char *stream_name = sensor_type == OB_SENSOR_IR_LEFT ? "left_ir"
: sensor_type == OB_SENSOR_IR_RIGHT ? "right_ir"
: "ir";
RCLCPP_INFO_STREAM(logger_, stream_name << " profile: " << profile->getWidth() << "x"
<< profile->getHeight() << " " << profile->getFps()
<< "fps " << profile->getFormat());
} else if (sensor_type == OB_SENSOR_ACCEL) {
auto profile = origin_profile->as<ob::AccelStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "accel profile: sampleRate " << profile->getSampleRate()
<< " full scale_range "
<< profile->getFullScaleRange());
} else if (sensor->getType() == OB_SENSOR_GYRO) {
} else if (sensor_type == OB_SENSOR_GYRO) {
auto profile = origin_profile->as<ob::GyroStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "gyro profile: sampleRate " << profile->getSampleRate()
<< " full scale_range "
<< profile->getFullScaleRange());
} else {
RCLCPP_INFO_STREAM(logger_, "unknown profile: " << magic_enum::enum_name(sensor->getType()));
RCLCPP_INFO_STREAM(logger_, "unknown profile: " << magic_enum::enum_name(sensor_type));
}
}
}
@@ -3571,33 +3609,30 @@ void OBCameraNode::setupProfiles() {
if (profile == nullptr) {
throw std::runtime_error("Failed cast profile to VideoStreamProfile");
}
RCLCPP_DEBUG_STREAM(
logger_, "Sensor profile: "
<< "stream_type: " << magic_enum::enum_name(profile->getType())
<< "Format: " << profile->getFormat() << ", Width: " << profile->getWidth()
<< ", Height: " << profile->getHeight() << ", FPS: " << profile->getFps());
supported_profiles_[elem].emplace_back(profile);
}
std::shared_ptr<ob::VideoStreamProfile> selected_profile;
std::shared_ptr<ob::VideoStreamProfile> default_profile;
try {
if (width_[elem] == 0 && height_[elem] == 0 && fps_[elem] == 0 &&
format_[elem] == OB_FORMAT_UNKNOWN) {
if (is_playback_device_) {
selected_profile = profiles->getProfile(0)->as<ob::VideoStreamProfile>();
} else if (width_[elem] == 0 && height_[elem] == 0 && fps_[elem] == 0 &&
format_[elem] == OB_FORMAT_UNKNOWN) {
selected_profile = profiles->getProfile(0)->as<ob::VideoStreamProfile>();
} else {
if (isGemini305SeriesPID(pid_) && elem == DEPTH) {
if (isGemini301SeriesPID(pid_) && elem == DEPTH) {
OBHardwareDecimationConfig conf;
conf.originWidth = width_[elem];
conf.originHeight = height_[elem];
conf.factor = depth_decimation_factor_;
selected_profile = profiles->getVideoStreamProfile(conf, format_[elem], fps_[elem]);
} else if (isGemini305SeriesPID(pid_) && elem == INFRA1) {
} else if (isGemini301SeriesPID(pid_) && elem == INFRA1) {
OBHardwareDecimationConfig conf;
conf.originWidth = width_[elem];
conf.originHeight = height_[elem];
conf.factor = left_ir_decimation_factor_;
selected_profile = profiles->getVideoStreamProfile(conf, format_[elem], fps_[elem]);
} else if (isGemini305SeriesPID(pid_) && elem == INFRA2) {
} else if (isGemini301SeriesPID(pid_) && elem == INFRA2) {
OBHardwareDecimationConfig conf;
conf.originWidth = width_[elem];
conf.originHeight = height_[elem];
@@ -3651,21 +3686,35 @@ void OBCameraNode::setupProfiles() {
width_[elem] = static_cast<int>(selected_profile->getWidth());
fps_[elem] = static_cast<int>(selected_profile->getFps());
format_[elem] = selected_profile->getFormat();
if (is_playback_device_) {
format_str_[elem] = OBFormatToString(format_[elem]);
RCLCPP_INFO_STREAM(logger_, "Bag playback: using recorded "
<< stream_name_[elem] << " profile " << width_[elem] << "x"
<< height_[elem] << " " << fps_[elem] << "fps "
<< format_str_[elem]);
}
updateImageConfig(elem);
if (selected_profile->format() == OB_FORMAT_BGRA) {
images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0));
encoding_[elem] = sensor_msgs::image_encodings::BGRA8;
unit_step_size_[COLOR] = 4 * sizeof(uint8_t);
unit_step_size_[elem] = 4 * sizeof(uint8_t);
} else if (selected_profile->format() == OB_FORMAT_RGBA) {
images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0));
encoding_[elem] = sensor_msgs::image_encodings::RGBA8;
unit_step_size_[COLOR] = 4 * sizeof(uint8_t);
unit_step_size_[elem] = 4 * sizeof(uint8_t);
} else {
images_[elem] =
cv::Mat(height_[elem], width_[elem], image_format_[elem], cv::Scalar(0, 0, 0));
}
}
}
std::string stream_fps_message;
if (!validate301SeriesStreamFrameRates(fps_, stream_fps_message)) {
RCLCPP_ERROR_STREAM(logger_, stream_fps_message);
throw std::runtime_error(stream_fps_message);
}
// IMU
for (const auto &stream_index : HID_STREAMS) {
if (!enable_stream_[stream_index]) {
@@ -3673,7 +3722,20 @@ void OBCameraNode::setupProfiles() {
}
try {
auto profile_list = sensors_[stream_index]->getStreamProfileList();
if (stream_index == ACCEL) {
if (is_playback_device_) {
stream_profile_[stream_index] = profile_list->getProfile(0);
if (stream_index == ACCEL) {
auto profile = stream_profile_[stream_index]->as<ob::AccelStreamProfile>();
CHECK_NOTNULL(profile.get());
imu_range_[stream_index] = fullAccelScaleRangeToString(profile->getFullScaleRange());
imu_rate_[stream_index] = sampleRateToString(profile->getSampleRate());
} else if (stream_index == GYRO) {
auto profile = stream_profile_[stream_index]->as<ob::GyroStreamProfile>();
CHECK_NOTNULL(profile.get());
imu_range_[stream_index] = fullGyroScaleRangeToString(profile->getFullScaleRange());
imu_rate_[stream_index] = sampleRateToString(profile->getSampleRate());
}
} else if (stream_index == ACCEL) {
auto full_scale_range = fullAccelScaleRangeFromString(imu_range_[stream_index]);
auto sample_rate = sampleRateFromString(imu_rate_[stream_index]);
auto profile = profile_list->getAccelStreamProfile(full_scale_range, sample_rate);
@@ -3697,6 +3759,55 @@ void OBCameraNode::setupProfiles() {
}
}
bool OBCameraNode::validate301SeriesStreamFrameRates(const std::map<stream_index_pair, int> &fps,
std::string &message) const {
if (!isGemini301SeriesPID(pid_)) {
return true;
}
int active_fps = 0;
bool fps_mismatch = false;
std::string active_streams;
for (const auto &stream_index : IMAGE_STREAMS) {
if (stream_index.first == OB_STREAM_LIDAR) {
continue;
}
const auto enable_it = enable_stream_.find(stream_index);
const auto fps_it = fps.find(stream_index);
if (enable_it == enable_stream_.end() || !enable_it->second || fps_it == fps.end() ||
fps_it->second <= 0) {
continue;
}
if (!active_streams.empty()) {
active_streams += ", ";
}
const auto name_it = stream_name_.find(stream_index);
if (name_it != stream_name_.end()) {
active_streams += name_it->second;
} else {
active_streams += std::string(magic_enum::enum_name(stream_index.first));
}
active_streams += "=" + std::to_string(fps_it->second);
if (active_fps == 0) {
active_fps = fps_it->second;
} else if (active_fps != fps_it->second) {
fps_mismatch = true;
}
}
if (!fps_mismatch) {
return true;
}
message =
"Gemini 301 series requires the same FPS for all enabled image streams. "
"Active stream FPS: " +
active_streams + ". Set all enabled image streams to the same FPS or disable unused streams.";
return false;
}
std::shared_ptr<ob::VideoStreamProfile> OBCameraNode::selectVideoStreamProfile(
const stream_index_pair &stream_index, int width, int height, int fps, OBFormat format) {
auto sensor_it = sensors_.find(stream_index);
@@ -3712,19 +3823,19 @@ std::shared_ptr<ob::VideoStreamProfile> OBCameraNode::selectVideoStreamProfile(
std::shared_ptr<ob::VideoStreamProfile> selected_profile;
if (width == 0 && height == 0 && fps == 0) {
selected_profile = profiles->getProfile(0)->as<ob::VideoStreamProfile>();
} else if (isGemini305SeriesPID(pid_) && stream_index == DEPTH) {
} else if (!is_playback_device_ && isGemini301SeriesPID(pid_) && stream_index == DEPTH) {
OBHardwareDecimationConfig conf;
conf.originWidth = width;
conf.originHeight = height;
conf.factor = depth_decimation_factor_;
selected_profile = profiles->getVideoStreamProfile(conf, format, fps);
} else if (isGemini305SeriesPID(pid_) && stream_index == INFRA1) {
} else if (!is_playback_device_ && isGemini301SeriesPID(pid_) && stream_index == INFRA1) {
OBHardwareDecimationConfig conf;
conf.originWidth = width;
conf.originHeight = height;
conf.factor = left_ir_decimation_factor_;
selected_profile = profiles->getVideoStreamProfile(conf, format, fps);
} else if (isGemini305SeriesPID(pid_) && stream_index == INFRA2) {
} else if (!is_playback_device_ && isGemini301SeriesPID(pid_) && stream_index == INFRA2) {
OBHardwareDecimationConfig conf;
conf.originWidth = width;
conf.originHeight = height;
@@ -3851,6 +3962,16 @@ bool OBCameraNode::validateStreamProfileRequest(
return false;
}
}
auto requested_fps = fps_;
for (const auto &pending_profile : pending_profiles) {
requested_fps[pending_profile.stream_index] =
static_cast<int>(pending_profile.profile->getFps());
}
if (!validate301SeriesStreamFrameRates(requested_fps, message)) {
return false;
}
if (!has_changes) {
message = "requested stream profiles are already active";
return false;
@@ -4758,7 +4879,11 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<int>(mean_intensity_set_point_, "mean_intensity_set_point",
depth_brightness_);
setAndGetNodeParameter<std::string>(depth_precision_str_, "depth_precision", "");
setAndGetNodeParameter<bool>(enable_ir_auto_exposure_, "enable_ir_auto_exposure", true);
setAndGetNodeParameter<bool>(enable_ir_auto_exposure_,
isLaunchParamProvided("enable_auto_exposure")
? "enable_auto_exposure"
: "enable_ir_auto_exposure",
true);
setAndGetNodeParameter<int>(ir_exposure_, "ir_exposure", -1);
setAndGetNodeParameter<int>(ir_gain_, "ir_gain", -1);
setAndGetNodeParameter<int>(ir_ae_max_exposure_, "ir_ae_max_exposure", -1);
@@ -5274,6 +5399,11 @@ bool OBCameraNode::validateEnhancedDepthFilterConfig(std::string &message) const
constexpr char kEnhancedDepthSupportedTargetResolutions[] = "640x480/1280x720/1280x800";
constexpr char kEnhancedDepthSupportedDepthFormats[] = "Y10/Y11/Y12/Y14/Y16/Z16";
if (!isGemini330SeriesPID(pid_)) {
message = "Enhanced depth filter is only supported by Gemini 330 series devices";
return false;
}
if (!enable_stream_.count(COLOR) || !enable_stream_.at(COLOR) || !enable_stream_.count(DEPTH) ||
!enable_stream_.at(DEPTH)) {
message = "Enhanced depth filter requires color and depth streams";
@@ -5792,6 +5922,11 @@ void OBCameraNode::syncSoftwareAlignment() {
align_filter_ = std::make_unique<ob::Align>(align_target_stream_);
RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
}
if (align_target_stream_ != OB_STREAM_COLOR) {
releaseGlobalImageTransportPublisher(*node_, "depth/image_unaligned");
depth_unaligned_publisher_.reset();
return;
}
if (!depth_unaligned_publisher_) {
const auto depth_image_qos_profile = getImageQosProfile(DEPTH);
if (use_intra_process_) {
@@ -5890,7 +6025,7 @@ cv::Mat OBCameraNode::colorizeDepthImage(const cv::Mat &depth_image,
cv::Mat depth_16u;
depth_image.convertTo(depth_16u, CV_16UC1);
const uint16_t min_depth = isGemini305SeriesPID(pid_) ? kViewerColorizerG305MinDistanceMm
const uint16_t min_depth = isGemini301SeriesPID(pid_) ? kViewerColorizerG305MinDistanceMm
: kViewerColorizerDefaultMinDistanceMm;
const uint16_t max_depth = kViewerColorizerMaxDistanceMm;
const uint32_t value_range = static_cast<uint32_t>(max_depth) - min_depth + 1;
@@ -6036,7 +6171,9 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
}
auto frame_timestamp = getFrameTimestampUs(depth_frame);
auto timestamp = fromUsToROSTime(frame_timestamp);
std::string frame_id = depth_registration_ ? optical_frame_id_[COLOR] : optical_frame_id_[DEPTH];
std::string frame_id = depth_registration_ && align_target_stream_ == OB_STREAM_COLOR
? depth_aligned_frame_id_[DEPTH]
: optical_frame_id_[DEPTH];
if (!cloud_frame_id_.empty()) {
frame_id = cloud_frame_id_;
}
@@ -6351,7 +6488,7 @@ void OBCameraNode::setDepthAutoExposureROI() {
if (depth_roi_has_run) {
return;
}
if (isGemini305SeriesPID(pid_) && ae_reference_stream_ == "color") {
if (isGemini301SeriesPID(pid_) && ae_reference_stream_ == "color") {
RCLCPP_WARN_STREAM(logger_, "Skip setting depth AE ROI because AE Reference Stream is color");
depth_roi_has_run = true;
return;
@@ -6396,7 +6533,7 @@ void OBCameraNode::setColorAutoExposureROI() {
if (color_roi_has_run) {
return;
}
if (isGemini305SeriesPID(pid_) && ae_reference_stream_ == "depth") {
if (isGemini301SeriesPID(pid_) && ae_reference_stream_ == "depth") {
RCLCPP_WARN_STREAM(logger_, "Skip setting color AE ROI because AE Reference Stream is depth");
color_roi_has_run = true;
return;
@@ -6473,6 +6610,20 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
final_color_frame, final_depth_frame, frame_set_arrival_system_us,
frame_set_arrival_steady_us, track_color, track_depth, color_publish_expected,
depth_publish_expected);
const auto record_side_stream = [&](const stream_index_pair &stream_index,
OBFrameType frame_type) {
auto frame = frame_set->getFrame(frame_type);
if (enable_stream_[stream_index] && frame) {
timestamp_csv_logger_->recordImageFrameArrival(stream_index.first, frame,
frame_set_arrival_system_us,
frame_set_arrival_steady_us, true);
}
};
record_side_stream(COLOR_LEFT, OB_FRAME_COLOR_LEFT);
record_side_stream(COLOR_RIGHT, OB_FRAME_COLOR_RIGHT);
record_side_stream(INFRA1, OB_FRAME_IR_LEFT);
record_side_stream(INFRA2, OB_FRAME_IR_RIGHT);
}
try {
@@ -6514,10 +6665,12 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
setColorAutoExposureROI();
left_color_frame = processColorFrameFilter(left_color_frame);
frame_set->pushFrame(left_color_frame);
fps_counter_left_color_->tick();
}
if (right_color_frame) {
right_color_frame = processColorFrameFilter(right_color_frame);
frame_set->pushFrame(right_color_frame);
fps_counter_right_color_->tick();
}
if (left_ir_frame) {
left_ir_frame = processLeftIrFrameFilter(left_ir_frame);
@@ -6536,11 +6689,12 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
}
}
if (depth_registration_ && align_filter_ && depth_frame) {
publishRawDepthImage(depth_frame);
auto target_frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(align_target_stream_);
if (!frame_set->getFrame(target_frame_type) || !color_frame) {
RCLCPP_DEBUG_STREAM(
logger_, "Depth registration requires depth and color frames, skip software alignment");
if (align_target_stream_ == OB_STREAM_COLOR) {
publishRawDepthImage(depth_frame);
}
if (!color_frame) {
RCLCPP_DEBUG_STREAM(logger_, "Software alignment requires a color frame, skip frame set");
return;
} else {
auto align_color_frame = color_frame;
if (align_target_stream_ == OB_STREAM_DEPTH) {
@@ -6563,12 +6717,13 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
}
if (!align_color_frame) {
RCLCPP_ERROR_STREAM(logger_, "Failed to convert color frame for C2D alignment");
return;
} else if (align_color_frame != color_frame) {
color_frame = align_color_frame;
frame_set->pushFrame(color_frame);
}
}
if (align_color_frame) {
if (align_target_stream_ != OB_STREAM_DEPTH || align_color_frame) {
if (auto new_frame = align_filter_->process(frame_set)) {
auto new_frame_set = new_frame->as<ob::FrameSet>();
CHECK_NOTNULL(new_frame_set.get());
@@ -6583,8 +6738,8 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
}
} else {
RCLCPP_DEBUG_ONCE(logger_,
"Depth registration is disabled or align filter is null or depth frame is "
"null or color frame is null");
"Depth registration is disabled, align filter is null, or depth frame is "
"null");
}
if (enable_enhanced_depth_.load()) {
@@ -7035,6 +7190,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
if (video_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX) ==
interleave_skip_index_) {
RCLCPP_DEBUG(logger_, "interleave filter skip frame type: %d", frame->getType());
record_image_publish_skipped();
return;
}
}
@@ -7077,7 +7233,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
distortion = camera_params.rgbDistortion;
}
std::string frame_id = optical_frame_id_[stream_index];
if (depth_registration_ && stream_index == DEPTH) {
if (depth_registration_ && align_target_stream_ == OB_STREAM_COLOR && stream_index == DEPTH) {
frame_id = depth_aligned_frame_id_[stream_index];
}
sensor_msgs::msg::CameraInfo camera_info{};
@@ -7117,13 +7273,19 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
}
if ((stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) &&
frame->getFormat() == OB_FORMAT_MJPG && has_compressed_image_subscriber) {
if (!has_raw_image_subscriber && stream_index == COLOR && log_image_timestamps) {
if (!has_raw_image_subscriber && log_image_timestamps) {
timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(),
getSteadyNowUs());
}
publishCompressedColorImage(frame, stream_index, timestamp, frame_id);
if (!has_raw_image_subscriber && stream_index == COLOR) {
fps_delay_status_color_->tick(frame_timestamp);
if (!has_raw_image_subscriber) {
if (stream_index == COLOR) {
fps_delay_status_color_->tick(frame_timestamp);
} else if (stream_index == COLOR_LEFT) {
fps_delay_status_left_color_->tick(frame_timestamp);
} else {
fps_delay_status_right_color_->tick(frame_timestamp);
}
}
}
@@ -7148,10 +7310,12 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
}
if (frame->getType() == OB_FRAME_COLOR_LEFT && !is_left_color_frame_decoded_) {
RCLCPP_ERROR(logger_, "left color frame is not decoded");
record_image_publish_skipped();
return;
}
if (frame->getType() == OB_FRAME_COLOR_RIGHT && !is_right_color_frame_decoded_) {
RCLCPP_ERROR(logger_, "right color frame is not decoded");
record_image_publish_skipped();
return;
}
if (frame->getType() == OB_FRAME_COLOR) {
@@ -7211,8 +7375,16 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
}
if (stream_index == COLOR) {
fps_delay_status_color_->tick(frame_timestamp);
} else if (stream_index == COLOR_LEFT) {
fps_delay_status_left_color_->tick(frame_timestamp);
} else if (stream_index == COLOR_RIGHT) {
fps_delay_status_right_color_->tick(frame_timestamp);
} else if (stream_index == DEPTH) {
fps_delay_status_depth_->tick(frame_timestamp);
} else if (stream_index == INFRA1) {
fps_delay_status_left_ir_->tick(frame_timestamp);
} else if (stream_index == INFRA2) {
fps_delay_status_right_ir_->tick(frame_timestamp);
}
image_publishers_[stream_index]->publish(std::move(image_msg));
}
@@ -7917,18 +8089,9 @@ bool OBCameraNode::setupFormatConvertType(OBFormat format, ob::FormatConvertFilt
return true;
}
bool OBCameraNode::isGemini335PID(uint32_t pid) {
return pid == GEMINI_335_PID || pid == GEMINI_330_PID || pid == GEMINI_336_PID ||
pid == GEMINI_335L_PID || pid == GEMINI_330L_PID || pid == GEMINI_336L_PID ||
pid == GEMINI_335LG_PID || pid == GEMINI_336LG_PID || pid == GEMINI_335LE_PID ||
pid == GEMINI_336LE_PID || pid == CUSTOM_ADVANTECH_GEMINI_336_PID ||
pid == CUSTOM_ADVANTECH_GEMINI_336L_PID || pid == GEMINI_338_PID ||
pid == GEMINI_338L_PID || pid == GEMINI_338LE_PID || pid == GEMINI_338LG_PID;
}
bool OBCameraNode::isGemini435LePID(uint32_t pid) { return pid == GEMINI_435Le_PID; }
bool OBCameraNode::isPublishMetaData(uint32_t pid) {
return isGemini335PID(pid) || isGemini435LePID(pid) || isGemini305SeriesPID(pid);
return isGemini330SeriesPID(pid) || isGemini435LePID(pid) || isGemini301SeriesPID(pid);
}
bool OBCameraNode::isDabaiASeriesForHwD2C(uint32_t pid) {
@@ -7938,7 +8101,7 @@ bool OBCameraNode::isDabaiASeriesForHwD2C(uint32_t pid) {
bool OBCameraNode::isDepthWorkModeDevices(uint32_t pid) { return pid == GEMINI_435Le_PID; }
bool OBCameraNode::isnotLaserDevices(uint32_t pid) { return isGemini305SeriesPID(pid); }
bool OBCameraNode::isnotLaserDevices(uint32_t pid) { return isGemini301SeriesPID(pid); }
orbbec_camera_msgs::msg::IMUInfo OBCameraNode::createIMUInfo(
const stream_index_pair &stream_index) {
@@ -8265,6 +8428,11 @@ bool OBCameraNode::applyEnhancedDepthFilterConfig(
bool enabled, const std::vector<float> &positional_params,
const std::vector<orbbec_camera_msgs::msg::DepthFilterParam> &named_params,
std::string &message) {
if (!isGemini330SeriesPID(pid_)) {
message = "Enhanced depth filter is only supported by Gemini 330 series devices";
return false;
}
if (positional_params.size() > 1) {
message = "EnhancedDepthFilter only supports one positional parameter";
return false;
orbbec_camera/src/ob_camera_node_driver.cpp
+5 -6
View File
@@ -1232,10 +1232,9 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
CHECK_NOTNULL(device_info_.get());
device_unique_id_ = device_info_->getUid();
if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid()) && device_type_ == "camera" &&
!playback_device_) {
if (!isOpenNIDevice(device_info_->pid()) && device_type_ == "camera" && !playback_device_) {
TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
if (g_time_domain != "global") {
if (enable_sync_host_time_ && g_time_domain != "global") {
device_->enableGlobalTimestamp(false);
sync_host_time_timer_ = this->create_wall_timer(time_sync_period_, [this]() {
// Multiple safety checks before attempting time sync
@@ -1373,7 +1372,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
}
const bool should_delay_stream_start = delay_stream_start_after_reconnect_.exchange(false) &&
isGemini305SeriesPID(device_info_->getPid());
isGemini301SeriesPID(device_info_->getPid());
if (should_delay_stream_start) {
std::this_thread::sleep_for(kStreamStartDelayAfterReconnect);
}
@@ -1605,8 +1604,8 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
if (isGmslCameraPID(pid)) {
ob_camera_node_->startGmslTrigger();
}
// if (isGemini305SeriesPID(pid)) {
// // Fixing 305 series hot-swap not outputting power
// if (isGemini301SeriesPID(pid)) {
// // Fixing 301 series hot-swap not outputting power
// ob_camera_node_->startStreams();
// }
} catch (ob::Error &e) {
orbbec_camera/src/ros_service.cpp
+12 -8
View File
@@ -217,11 +217,11 @@ void OBCameraNode::setupCameraCtrlServices() {
});
}
if (isPropertyWritable(device_, OB_PROP_FLOOD_BOOL)) {
set_floor_enable_srv_ = node_->create_service<SetBool>(
"set_floor_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
set_flood_enable_srv_ = node_->create_service<SetBool>(
"set_flood_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setFloorEnableCallback(request_header, request, response);
setFloodEnableCallback(request_header, request, response);
});
}
if (isPropertyWritable(device_, OB_PROP_LASER_CONTROL_INT) ||
@@ -903,6 +903,8 @@ void OBCameraNode::setImageRegistrationModeCallback(
auto rollback_after_error = [&](const std::string& error_message) {
try {
stopColorFrameThreads();
clearColorFrameQueues();
restore_old_mode();
if (was_running && !pipeline_started_.load()) {
startStreams();
@@ -924,6 +926,8 @@ void OBCameraNode::setImageRegistrationModeCallback(
if (was_running) {
stopStreams();
}
stopColorFrameThreads();
clearColorFrameQueues();
apply_image_registration_mode(mode);
@@ -1124,13 +1128,13 @@ void OBCameraNode::setAeRoiCallback(const std::shared_ptr<SetArrays ::Request>&
std::shared_ptr<SetArrays::Response>& response,
const stream_index_pair& stream_index) {
auto stream = stream_index.first;
if (isGemini305SeriesPID(device_->getDeviceInfo()->getPid()) &&
if (isGemini301SeriesPID(device_->getDeviceInfo()->getPid()) &&
(stream != OB_STREAM_COLOR && ae_reference_stream_ == "color")) {
response->success = false;
response->message = "AE Reference Stream is color, other sensors setting is not supported";
return;
}
if (isGemini305SeriesPID(device_->getDeviceInfo()->getPid()) &&
if (isGemini301SeriesPID(device_->getDeviceInfo()->getPid()) &&
(stream != OB_STREAM_DEPTH && ae_reference_stream_ == "depth")) {
response->success = false;
response->message =
@@ -1530,15 +1534,15 @@ void OBCameraNode::setFanWorkModeCallback(const std::shared_ptr<SetInt32::Reques
}
}
void OBCameraNode::setFloorEnableCallback(
void OBCameraNode::setFloodEnableCallback(
const std::shared_ptr<rmw_request_id_t>& request_header,
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
(void)request_header;
(void)response;
bool floor_enable = request->data;
bool flood_enable = request->data;
try {
device_->setBoolProperty(OB_PROP_FLOOD_BOOL, floor_enable);
device_->setBoolProperty(OB_PROP_FLOOD_BOOL, flood_enable);
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
orbbec_camera/src/timestamp_csv_logger.cpp
+46 -1
View File
@@ -29,6 +29,18 @@ TimestampCsvLogger::TimestampCsvLogger(Config config, rclcpp::Logger logger)
depth_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::DEPTH);
}
}
if (config.left_color_enabled) {
left_color_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::LEFT_COLOR);
}
if (config.right_color_enabled) {
right_color_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::RIGHT_COLOR);
}
if (config.left_ir_enabled) {
left_ir_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::LEFT_IR);
}
if (config.right_ir_enabled) {
right_ir_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::RIGHT_IR);
}
if (config.csv_file_path.empty()) {
return;
@@ -64,7 +76,12 @@ bool TimestampCsvLogger::enabled() const {
bool TimestampCsvLogger::imageEnabled() const {
return (synced_image_logger_ && synced_image_logger_->enabled()) ||
(color_logger_ && color_logger_->enabled()) || (depth_logger_ && depth_logger_->enabled());
(color_logger_ && color_logger_->enabled()) ||
(left_color_logger_ && left_color_logger_->enabled()) ||
(right_color_logger_ && right_color_logger_->enabled()) ||
(depth_logger_ && depth_logger_->enabled()) ||
(left_ir_logger_ && left_ir_logger_->enabled()) ||
(right_ir_logger_ && right_ir_logger_->enabled());
}
bool TimestampCsvLogger::imageStreamEnabled(OBStreamType stream_type) const {
@@ -104,6 +121,18 @@ void TimestampCsvLogger::recordImageFrameSet(const std::shared_ptr<ob::Frame> &c
}
}
void TimestampCsvLogger::recordImageFrameArrival(OBStreamType stream_type,
const std::shared_ptr<ob::Frame> &frame,
int64_t arrival_system_us,
int64_t arrival_steady_us,
bool image_publish_expected) {
auto *timestamp_logger = imageLoggerForStream(stream_type);
if (timestamp_logger) {
timestamp_logger->recordStandaloneFrameArrival(stream_type, frame, arrival_system_us,
arrival_steady_us, image_publish_expected);
}
}
void TimestampCsvLogger::recordImagePrePublish(OBStreamType stream_type,
const std::shared_ptr<ob::Frame> &frame,
int64_t publish_system_us,
@@ -164,7 +193,11 @@ void TimestampCsvLogger::shutdown() noexcept {
shutdown_logger(synced_image_logger_, "synced image timestamp CSV logger");
shutdown_logger(color_logger_, "color timestamp CSV logger");
shutdown_logger(left_color_logger_, "left color timestamp CSV logger");
shutdown_logger(right_color_logger_, "right color timestamp CSV logger");
shutdown_logger(depth_logger_, "depth timestamp CSV logger");
shutdown_logger(left_ir_logger_, "left IR timestamp CSV logger");
shutdown_logger(right_ir_logger_, "right IR timestamp CSV logger");
shutdown_logger(synced_imu_logger_, "synced IMU timestamp CSV logger");
shutdown_logger(accel_logger_, "accel timestamp CSV logger");
shutdown_logger(gyro_logger_, "gyro timestamp CSV logger");
@@ -177,6 +210,18 @@ FrameTimestampCsvLogger *TimestampCsvLogger::imageLoggerForStream(OBStreamType s
if (stream_type == OB_STREAM_DEPTH) {
return synced_image_logger_ ? synced_image_logger_.get() : depth_logger_.get();
}
if (stream_type == OB_STREAM_COLOR_LEFT) {
return left_color_logger_.get();
}
if (stream_type == OB_STREAM_COLOR_RIGHT) {
return right_color_logger_.get();
}
if (stream_type == OB_STREAM_IR_LEFT) {
return left_ir_logger_.get();
}
if (stream_type == OB_STREAM_IR_RIGHT) {
return right_ir_logger_.get();
}
return nullptr;
}
orbbec_camera/src/utils.cpp
+13 -2
View File
@@ -946,17 +946,28 @@ std::string parseUsbPort(const std::string &line) {
}
bool isValidJPEG(const std::shared_ptr<ob::ColorFrame> &frame) {
if (frame->getDataSize() < 2) { // Checking both start and end markers, so minimal size is 4
if (!frame) {
return false;
}
const auto data_size = frame->getDataSize();
const auto *data = static_cast<const uint8_t *>(frame->getData());
if (data == nullptr || data_size < 4) {
return false;
}
// Check for JPEG start marker
if (data[0] != 0xFF || data[1] != 0xD8) {
return false;
}
return true;
auto jpeg_size = data_size;
while (jpeg_size > 2 && data[jpeg_size - 1] == 0x00) {
--jpeg_size;
}
// Check for JPEG end marker after trimming zero padding.
return jpeg_size >= 4 && data[jpeg_size - 2] == 0xFF && data[jpeg_size - 1] == 0xD9;
}
std::string metaDataTypeToString(const OBFrameMetadataType &meta_data_type) {
orbbec_camera/test/camera_info_distortion_test.cpp
+21
View File
@@ -1,5 +1,6 @@
#include <gtest/gtest.h>
#include <cstring>
#include <vector>
#include "orbbec_camera/utils.h"
@@ -32,6 +33,14 @@ OBCameraDistortion makeDistortion(OBCameraDistortionModel model) {
return distortion;
}
std::shared_ptr<ob::ColorFrame> makeMjpegFrame(const std::vector<uint8_t>& data) {
auto frame = ob::FrameFactory::createFrame(OB_FRAME_COLOR, OB_FORMAT_MJPG,
static_cast<uint32_t>(data.size()));
auto color_frame = frame->as<ob::ColorFrame>();
std::memcpy(color_frame->getData(), data.data(), data.size());
return color_frame;
}
TEST(CameraInfoDistortionTest, ConvertsBrownConradyToPlumbBob) {
const auto intrinsic = makeIntrinsic();
const auto distortion = makeDistortion(OB_DISTORTION_BROWN_CONRADY);
@@ -69,5 +78,17 @@ TEST(CameraInfoDistortionTest, ConvertsKannalaBrandtToEquidistant) {
std::vector<double>({distortion.k1, distortion.k2, distortion.k3, distortion.k4}));
}
TEST(JpegValidationTest, AcceptsEoiBeforeZeroPadding) {
const auto frame = makeMjpegFrame({0xFF, 0xD8, 0x01, 0x02, 0xFF, 0xD9, 0x00, 0x00});
EXPECT_TRUE(isValidJPEG(frame));
}
TEST(JpegValidationTest, RejectsMjpegWithoutEoi) {
const auto frame = makeMjpegFrame({0xFF, 0xD8, 0x01, 0x02, 0x00, 0x00});
EXPECT_FALSE(isValidJPEG(frame));
}
} // namespace
} // namespace orbbec_camera
orbbec_camera/tools/list_camera_profile.cpp
+7 -4
View File
@@ -145,8 +145,8 @@ void listSensorProfiles(const std::shared_ptr<ob::Device>& device) {
auto origin_profile = profile_list->getProfile(j);
if ((sensor->getType() == OB_SENSOR_DEPTH || sensor->getType() == OB_SENSOR_IR_LEFT ||
sensor->getType() == OB_SENSOR_IR_RIGHT) &&
isGemini305SeriesPID(pid)) {
// Gemini 305 series
isGemini301SeriesPID(pid)) {
// Gemini 301 series
auto profile = origin_profile->as<ob::VideoStreamProfile>();
std::cout << magic_enum::enum_name(sensor->getType()) << " profile: " << profile->getWidth()
<< "x" << profile->getHeight() << " " << profile->getFps() << "fps "
@@ -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
+498 -331
View File
@@ -1,72 +1,114 @@
#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 <thread>
#include <utility>
#include <vector>
#include <nlohmann/json.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <std_msgs/msg/header.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",
};
constexpr auto kStreamDiscoveryPollInterval = std::chrono::milliseconds(100);
constexpr auto kStreamDiscoveryStablePeriod = std::chrono::seconds(1);
constexpr auto kStreamDiscoveryTimeout = std::chrono::seconds(5);
constexpr size_t kMinimumPendingFrameLimit = 30;
struct StreamTopicInfo {
std::string name;
bool metadata_available = false;
bool operator==(const StreamTopicInfo &other) const {
return name == other.name && metadata_available == other.metadata_available;
}
};
bool isSupportedStreamName(const std::string &stream_name) {
return std::find(kSupportedStreamNames.begin(), kSupportedStreamNames.end(), stream_name) !=
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 {
struct FrameMetadata {
std::string exposure;
std::string gain;
};
struct PendingImage {
cv::Mat image;
std::string current_timestamp;
std::string receive_timestamp;
};
std::vector<cv::Mat> images;
std::vector<std::string> current_timestamps;
std::vector<std::string> receive_timestamps;
std::vector<FrameMetadata> frame_metadata;
std::map<int64_t, PendingImage> pending_images;
std::map<int64_t, FrameMetadata> pending_metadata;
bool metadata_required = false;
void clear() {
images.clear();
current_timestamps.clear();
receive_timestamps.clear();
frame_metadata.clear();
pending_images.clear();
pending_metadata.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,337 +116,462 @@ class MultiCameraSubscriber : public rclcpp::Node {
}
private:
std::mutex image_mutex_;
std::mutex meta_mutex_;
bool isGemini335PID(uint32_t pid) {
return pid == GEMINI_335_PID || pid == GEMINI_330_PID || pid == GEMINI_336_PID ||
pid == GEMINI_335L_PID || pid == GEMINI_330L_PID || pid == GEMINI_336L_PID ||
pid == GEMINI_335LG_PID || pid == GEMINI_336LG_PID || pid == GEMINI_335LE_PID ||
pid == GEMINI_336LE_PID || pid == CUSTOM_ADVANTECH_GEMINI_336_PID ||
pid == CUSTOM_ADVANTECH_GEMINI_336L_PID || pid == GEMINI_338_PID ||
pid == GEMINI_338LG_PID || pid == GEMINI_338LE_PID || pid == GEMINI_338L_PID ||
pid == GEMINI_331L_PID;
void 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();
}
} 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");
}
}
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<StreamTopicInfo> discoverStreams(const std::string &camera_name) const {
std::vector<StreamTopicInfo> streams;
const auto names_and_types = this->get_topic_names_and_types();
const std::string prefix = cameraNamespace(camera_name) + "/";
for (const auto &stream_name : kSupportedStreamNames) {
const auto image_topic_it = names_and_types.find(prefix + stream_name + "/image_raw");
if (image_topic_it == names_and_types.end()) {
continue;
}
const auto &image_types = image_topic_it->second;
if (std::find(image_types.begin(), image_types.end(), "sensor_msgs/msg/Image") ==
image_types.end()) {
continue;
}
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);
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);
});
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);
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);
});
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);
const auto metadata_topic_it = names_and_types.find(prefix + stream_name + "/metadata");
const bool metadata_available =
metadata_topic_it != names_and_types.end() &&
std::find(metadata_topic_it->second.begin(), metadata_topic_it->second.end(),
"orbbec_camera_msgs/msg/Metadata") != metadata_topic_it->second.end();
streams.push_back(StreamTopicInfo{stream_name, metadata_available});
}
return streams;
}
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::vector<std::vector<StreamTopicInfo>> waitForStableStreams() const {
if (!configured_stream_names_.empty()) {
std::vector<std::vector<StreamTopicInfo>> configured_streams;
configured_streams.reserve(camera_names_.size());
for (const auto &camera_name : camera_names_) {
const auto discovered_streams = discoverStreams(camera_name);
std::vector<StreamTopicInfo> camera_streams;
camera_streams.reserve(configured_stream_names_.size());
for (const auto &stream_name : configured_stream_names_) {
const auto discovered_it = std::find_if(
discovered_streams.begin(), discovered_streams.end(),
[&stream_name](const auto &stream) { return stream.name == stream_name; });
camera_streams.push_back(StreamTopicInfo{
stream_name,
discovered_it != discovered_streams.end() && discovered_it->metadata_available});
}
configured_streams.push_back(std::move(camera_streams));
}
return configured_streams;
}
std::vector<std::vector<StreamTopicInfo>> candidate;
auto candidate_since = std::chrono::steady_clock::time_point{};
const auto deadline = std::chrono::steady_clock::now() + kStreamDiscoveryTimeout;
while (rclcpp::ok() && std::chrono::steady_clock::now() < deadline) {
std::vector<std::vector<StreamTopicInfo>> current;
current.reserve(camera_names_.size());
bool all_cameras_discovered = !camera_names_.empty();
for (const auto &camera_name : camera_names_) {
current.push_back(discoverStreams(camera_name));
all_cameras_discovered = all_cameras_discovered && !current.back().empty();
}
const auto now = std::chrono::steady_clock::now();
if (!all_cameras_discovered) {
candidate.clear();
} else if (current != candidate) {
candidate = std::move(current);
candidate_since = now;
} else if (now - candidate_since >= kStreamDiscoveryStablePeriod) {
return candidate;
}
std::this_thread::sleep_for(kStreamDiscoveryPollInterval);
}
RCLCPP_WARN_STREAM(get_logger(),
"Supported image topics did not become stable within "
<< kStreamDiscoveryTimeout.count()
<< " seconds; start all cameras and streams first, retry the request, "
"or configure stream_names explicitly");
return {};
}
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);
bool initializeTopics() {
const auto streams_by_camera = waitForStableStreams();
if (streams_by_camera.size() != camera_names_.size()) {
return false;
}
callback_groups_.clear();
image_subscribers_.clear();
metadata_subscribers_.clear();
captures_.resize(camera_names_.size());
callback_called_.assign(camera_names_.size(), false);
const auto custom_qos =
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
for (size_t camera_index = 0; camera_index < camera_names_.size(); ++camera_index) {
const auto &streams = streams_by_camera[camera_index];
const std::string prefix = cameraNamespace(camera_names_[camera_index]) + "/";
auto callback_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
callback_groups_.push_back(callback_group);
rclcpp::SubscriptionOptions options;
options.callback_group = callback_group;
for (const auto &stream : streams) {
const auto &stream_name = stream.name;
StreamCapture capture;
capture.metadata_required = stream.metadata_available;
captures_[camera_index].emplace(stream_name, std::move(capture));
const std::string image_topic = prefix + stream_name + "/image_raw";
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));
if (stream.metadata_available) {
const std::string metadata_topic = prefix + stream_name + "/metadata";
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));
}
}
}
return !captures_.empty() && std::all_of(captures_.begin(), captures_.end(),
[](const auto &streams) { return !streams.empty(); });
}
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) 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 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_);
});
}
int64_t messageStampNs(const std_msgs::msg::Header &header) const {
return static_cast<int64_t>(header.stamp.sec) * 1000000000LL + header.stamp.nanosec;
}
size_t pendingFrameLimit() const {
return std::max(kMinimumPendingFrameLimit, static_cast<size_t>(saving_images_number_) * 2);
}
template <typename Value>
void trimPendingFrames(std::map<int64_t, Value> &pending) const {
while (pending.size() > pendingFrameLimit()) {
pending.erase(pending.begin());
}
}
void appendCompletedFrame(StreamCapture &capture, StreamCapture::PendingImage pending_image,
StreamCapture::FrameMetadata metadata = {}) {
if (capture.images.size() >= static_cast<size_t>(saving_images_number_)) {
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");
capture.images.push_back(std::move(pending_image.image));
capture.current_timestamps.push_back(std::move(pending_image.current_timestamp));
capture.receive_timestamps.push_back(std::move(pending_image.receive_timestamp));
capture.frame_metadata.push_back(std::move(metadata));
}
std::string metadataSuffix(const StreamCapture &capture, size_t frame_index) const {
if (frame_index >= capture.frame_metadata.size()) {
return "";
}
std::string suffix;
if (!capture.frame_metadata[frame_index].exposure.empty()) {
suffix += "_e" + capture.frame_metadata[frame_index].exposure;
}
if (!capture.frame_metadata[frame_index].gain.empty()) {
suffix += "_d" + capture.frame_metadata[frame_index].gain;
}
return suffix;
}
void saveImages(size_t camera_index) {
if (!captureReady(camera_index)) {
return;
}
std::string serial_index = serial_iter->second;
if (camera_index >= usb_ports_.size()) {
RCLCPP_ERROR(get_logger(), "Missing USB port configuration for camera index %zu",
camera_index);
return;
}
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_) {
if (!initializeTopics()) {
callback_groups_.clear();
image_subscribers_.clear();
metadata_subscribers_.clear();
captures_.clear();
callback_called_.clear();
response->success = false;
response->message =
"supported image streams did not become stable; retry after all cameras and streams "
"start or configure stream_names";
return;
}
topics_initialized_ = true;
}
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_);
}
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 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);
}
}
response->success = true;
response->message = "capture started";
RCLCPP_INFO(get_logger(), "Capturing %d image(s) from each configured stream",
saving_images_number_);
}
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 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;
}
}
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());
auto &capture = captures_[camera_index].at(stream_name);
if (capture.images.size() >= static_cast<size_t>(saving_images_number_)) {
saveImages(camera_index);
return;
}
cv::Mat output = cv_bridge::toCvCopy(image, image->encoding)->image;
if (isColorCaptureStreamName(stream_name) &&
image->encoding == sensor_msgs::image_encodings::RGB8) {
cv::Mat converted;
cv::cvtColor(output, converted, cv::COLOR_RGB2BGR);
output = converted;
} else if (isColorCaptureStreamName(stream_name) &&
image->encoding == sensor_msgs::image_encodings::RGBA8) {
cv::Mat converted;
cv::cvtColor(output, converted, cv::COLOR_RGBA2BGRA);
output = converted;
}
StreamCapture::PendingImage pending_image{std::move(output), imageTimestamp(image),
receiveTimestamp()};
if (capture.metadata_required) {
const auto stamp_ns = messageStampNs(image->header);
auto metadata_it = capture.pending_metadata.find(stamp_ns);
if (metadata_it == capture.pending_metadata.end()) {
capture.pending_images.insert_or_assign(stamp_ns, std::move(pending_image));
trimPendingFrames(capture.pending_images);
return;
}
appendCompletedFrame(capture, std::move(pending_image), std::move(metadata_it->second));
capture.pending_metadata.erase(metadata_it);
} else {
appendCompletedFrame(capture, std::move(pending_image));
}
RCLCPP_INFO(get_logger(), "%s[%zu]: %zu/%d", stream_name.c_str(), camera_index,
capture.images.size(), saving_images_number_);
saveImages(camera_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;
}
auto &capture = captures_[camera_index].at(stream_name);
if (!capture.metadata_required ||
capture.images.size() >= static_cast<size_t>(saving_images_number_)) {
return;
}
StreamCapture::FrameMetadata frame_metadata;
try {
const auto json_data = nlohmann::json::parse(metadata->json_data);
if (json_data.contains("exposure")) {
frame_metadata.exposure = json_data["exposure"].dump();
}
if (json_data.contains("gain")) {
frame_metadata.gain = json_data["gain"].dump();
}
} catch (const std::exception &e) {
RCLCPP_WARN(get_logger(), "Failed to parse %s metadata: %s", stream_name.c_str(), e.what());
}
const auto stamp_ns = messageStampNs(metadata->header);
auto image_it = capture.pending_images.find(stamp_ns);
if (image_it == capture.pending_images.end()) {
capture.pending_metadata.insert_or_assign(stamp_ns, std::move(frame_metadata));
trimPendingFrames(capture.pending_metadata);
return;
}
appendCompletedFrame(capture, std::move(image_it->second), std::move(frame_metadata));
capture.pending_images.erase(image_it);
RCLCPP_INFO(get_logger(), "%s[%zu]: %zu/%d", stream_name.c_str(), camera_index,
capture.images.size(), saving_images_number_);
saveImages(camera_index);
}
std::mutex capture_mutex_;
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;
};
} // 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"),
orbbec_camera_msgs/msg/DeviceStatus.msg
+44
View File
@@ -11,6 +11,28 @@ float64 color_delay_ms_avg
float64 color_delay_ms_min
float64 color_delay_ms_max
# --- Left color stream ---
float64 left_color_frame_rate_cur
float64 left_color_frame_rate_avg
float64 left_color_frame_rate_min
float64 left_color_frame_rate_max
float64 left_color_delay_ms_cur
float64 left_color_delay_ms_avg
float64 left_color_delay_ms_min
float64 left_color_delay_ms_max
# --- Right color stream ---
float64 right_color_frame_rate_cur
float64 right_color_frame_rate_avg
float64 right_color_frame_rate_min
float64 right_color_frame_rate_max
float64 right_color_delay_ms_cur
float64 right_color_delay_ms_avg
float64 right_color_delay_ms_min
float64 right_color_delay_ms_max
# --- Depth stream ---
float64 depth_frame_rate_cur
float64 depth_frame_rate_avg
@@ -22,6 +44,28 @@ float64 depth_delay_ms_avg
float64 depth_delay_ms_min
float64 depth_delay_ms_max
# --- Left IR stream ---
float64 left_ir_frame_rate_cur
float64 left_ir_frame_rate_avg
float64 left_ir_frame_rate_min
float64 left_ir_frame_rate_max
float64 left_ir_delay_ms_cur
float64 left_ir_delay_ms_avg
float64 left_ir_delay_ms_min
float64 left_ir_delay_ms_max
# --- Right IR stream ---
float64 right_ir_frame_rate_cur
float64 right_ir_frame_rate_avg
float64 right_ir_frame_rate_min
float64 right_ir_frame_rate_max
float64 right_ir_delay_ms_cur
float64 right_ir_delay_ms_avg
float64 right_ir_delay_ms_min
float64 right_ir_delay_ms_max
# --- Device info ---
bool device_online
string connection_type # e.g. "USB2.0", "USB3.0", "GigE"
orbbec_description/meshes/gemini435Le/camera_screw_frame.STL
BIN
View File
Binary file not shown.
orbbec_description/urdf/gemini_435_Le.urdf.xacro
+109
View File
@@ -0,0 +1,109 @@
<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="gemini_435_Le">
<xacro:property name="mesh_path" value="package://orbbec_description/meshes/gemini435Le/" />
<!--
The root link owns the complete physical model. Sensor links below are
coordinate frames only, so their geometry, collision, mass, and inertia
are not counted repeatedly.
-->
<link name="camera_screw_frame">
<inertial>
<origin xyz="0.00362974814795868 0.0327239520253598 0.0201570762447024" rpy="0 0 0" />
<mass value="0.52" />
<inertia
ixx="0.000996547989654272"
ixy="-1.27347531187977E-05"
ixz="-6.37274993760709E-08"
iyy="0.000114258647753823"
iyz="2.49203178977503E-06"
izz="0.000945634655484896" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="${mesh_path}camera_screw_frame.STL" />
</geometry>
<material name="camera_body">
<color rgba="0.501960784313725 0.250980392156863 0.250980392156863 1" />
</material>
</visual>
<!-- Primitive collision avoids using a high-triangle-count dynamic mesh. -->
<collision>
<origin xyz="0.0024025 0.03375 0.0202" rpy="0 0 0" />
<geometry>
<box size="0.075505 0.1384 0.0404" />
</geometry>
</collision>
</link>
<link name="camera_link" />
<joint name="camera_link_joint" type="fixed">
<origin xyz="0.0354680400000002 0.0811997 0.0201999999999996" rpy="0 0 0" />
<parent link="camera_screw_frame" />
<child link="camera_link" />
</joint>
<link name="camera_depth_frame" />
<joint name="camera_depth_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="camera_link" />
<child link="camera_depth_frame" />
</joint>
<link name="camera_IMU_frame" />
<joint name="camera_IMU_joint" type="fixed">
<origin xyz="-0.0550580400000001 -0.0152696999999998 9.99999999999141E-05" rpy="0 0 0" />
<parent link="camera_depth_frame" />
<child link="camera_IMU_frame" />
</joint>
<link name="camera_color_frame" />
<joint name="camera_color_joint" type="fixed">
<origin xyz="0 -0.0237497 0" rpy="0 0 0" />
<parent link="camera_depth_frame" />
<child link="camera_color_frame" />
</joint>
<link name="camera_color_optical_frame" />
<joint name="camera_color_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="-1.57079632679489 0 -1.5707963267949" />
<parent link="camera_color_frame" />
<child link="camera_color_optical_frame" />
</joint>
<link name="camera_left_ir_frame" />
<joint name="camera_left_ir_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="camera_depth_frame" />
<child link="camera_left_ir_frame" />
</joint>
<link name="camera_left_ir_optical_frame" />
<joint name="camera_left_ir_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="-1.57079632679489 0 -1.5707963267949" />
<parent link="camera_left_ir_frame" />
<child link="camera_left_ir_optical_frame" />
</joint>
<link name="camera_right_ir_frame" />
<joint name="camera_right_ir_joint" type="fixed">
<origin xyz="0 -0.0949976 0" rpy="0 0 0" />
<parent link="camera_depth_frame" />
<child link="camera_right_ir_frame" />
</joint>
<link name="camera_right_ir_optical_frame" />
<joint name="camera_right_ir_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="-1.57079632679489 0 -1.5707963267949" />
<parent link="camera_right_ir_frame" />
<child link="camera_right_ir_optical_frame" />
</joint>
<link name="camera_depth_optical_frame" />
<joint name="camera_depth_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="-1.57079632679489 0 -1.5707963267949" />
<parent link="camera_depth_frame" />
<child link="camera_depth_optical_frame" />
</joint>
</robot>
orbbec_description/urdf/test_gemini_435_Le.urdf.xacro
+12
View File
@@ -0,0 +1,12 @@
<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="gemini_435_Le">
<xacro:include filename="gemini_435_Le.urdf.xacro" />
<!-- Define a virtual root link for model tests and robot mounting. -->
<link name="base_link" />
<joint name="base_link_to_camera_screw_frame" type="fixed">
<parent link="base_link" />
<child link="camera_screw_frame" />
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
</robot>