i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 20:17:47 +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-08-21 15:21:08 +08:00
parent 8e7cad2bfa 50fa1d20fe
commit cee38ebf5e
179 changed files with 23270 additions and 4686 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/d2c_viewer.cpp
+14 -2
View File
@@ -30,9 +30,21 @@ D2CViewer::D2CViewer(rclcpp::Node* const node, rmw_qos_profile_t rgb_qos,
rmw_qos_profile_t depth_qos, bool use_intra_process)
: node_(node), logger_(rclcpp::get_logger("d2c_viewer")), is_active_(true) {
rgb_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
node_, "color/image_raw", rgb_qos);
node_, "color/image_raw",
#ifdef ORBBEC_MESSAGE_FILTERS_USES_RCLCPP_QOS
rclcpp::QoS{rclcpp::QoSInitialization::from_rmw(rgb_qos), rgb_qos}
#else
rgb_qos
#endif
);
depth_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
node_, "depth/image_raw", depth_qos);
node_, "depth/image_raw",
#ifdef ORBBEC_MESSAGE_FILTERS_USES_RCLCPP_QOS
rclcpp::QoS{rclcpp::QoSInitialization::from_rmw(depth_qos), depth_qos}
#else
depth_qos
#endif
);
sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(MySyncPolicy(10), *rgb_sub_,
*depth_sub_);
sync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(1.0)); // 1s
orbbec_camera/src/frame_timestamp_csv_logger.cpp
+80 -11
View File
@@ -12,6 +12,8 @@ namespace {
constexpr size_t kCompletedQueueSoftLimit = 1000;
constexpr size_t kFlushBatchSize = 100;
// The header occupies the first row, leaving 1,024,575 rows for frame data.
constexpr uint64_t kMaxCsvRowsPerFileIncludingHeader = 1'024'576;
constexpr auto kFlushInterval = std::chrono::seconds(1);
int64_t getExpectedIntervalUs(const std::shared_ptr<ob::Frame> &frame) {
@@ -58,7 +60,10 @@ FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
}
if (csv_enabled_) {
openCsvIfNeeded();
if (!openCsvFile(0)) {
csv_enabled_ = false;
csv_writer_failed_ = true;
}
}
enabled_ = csv_enabled_ || drop_log_enabled_;
@@ -567,11 +572,37 @@ void FrameTimestampCsvLogger::writerThreadMain() {
rows_to_write.swap(completed_rows_);
}
std::stable_sort(rows_to_write.begin(), rows_to_write.end(),
[](const auto &lhs, const auto &rhs) { return lhs.row_id < rhs.row_id; });
for (const auto &row : rows_to_write) {
if (csv_stream_.is_open()) {
csv_stream_ << serializeRow(row) << "\n";
rows_since_flush++;
if (csv_rows_written_ >= kMaxCsvRowsPerFileIncludingHeader) {
if (!rotateCsvFile()) {
csv_writer_failed_ = true;
break;
}
rows_since_flush = 0;
last_flush = std::chrono::steady_clock::now();
}
if (!csv_stream_.is_open()) {
csv_writer_failed_ = true;
break;
}
csv_stream_ << serializeRow(row) << "\n";
if (!csv_stream_) {
RCLCPP_ERROR_STREAM(logger_, "Failed to write frame timestamp CSV file: "
<< csvFilePathForIndex(csv_file_index_));
csv_writer_failed_ = true;
break;
}
++csv_rows_written_;
++rows_since_flush;
}
if (csv_writer_failed_) {
break;
}
const auto now = std::chrono::steady_clock::now();
@@ -589,16 +620,54 @@ void FrameTimestampCsvLogger::writerThreadMain() {
}
}
void FrameTimestampCsvLogger::openCsvIfNeeded() {
csv_stream_.open(csv_file_path_, std::ios::out | std::ios::trunc);
if (!csv_stream_.is_open()) {
RCLCPP_ERROR_STREAM(logger_, "Failed to open frame timestamp CSV file: " << csv_file_path_);
csv_enabled_ = false;
csv_writer_failed_ = true;
return;
std::string FrameTimestampCsvLogger::csvFilePathForIndex(uint64_t file_index) const {
if (file_index == 0) {
return csv_file_path_;
}
const std::filesystem::path original_path(csv_file_path_);
const auto indexed_filename = original_path.stem().string() + "_" + std::to_string(file_index) +
original_path.extension().string();
return (original_path.parent_path() / indexed_filename).string();
}
bool FrameTimestampCsvLogger::openCsvFile(uint64_t file_index) {
const auto file_path = csvFilePathForIndex(file_index);
csv_stream_.clear();
csv_stream_.open(file_path, std::ios::out | std::ios::trunc);
if (!csv_stream_.is_open()) {
RCLCPP_ERROR_STREAM(logger_, "Failed to open frame timestamp CSV file: " << file_path);
return false;
}
csv_stream_ << csvHeader() << "\n";
csv_stream_.flush();
if (!csv_stream_) {
RCLCPP_ERROR_STREAM(logger_, "Failed to write frame timestamp CSV header: " << file_path);
csv_stream_.close();
return false;
}
csv_file_index_ = file_index;
csv_rows_written_ = 1;
return true;
}
bool FrameTimestampCsvLogger::rotateCsvFile() {
if (csv_stream_.is_open()) {
csv_stream_.flush();
csv_stream_.close();
}
const auto next_file_index = csv_file_index_ + 1;
if (!openCsvFile(next_file_index)) {
return false;
}
RCLCPP_INFO_STREAM(logger_, "Frame timestamp CSV reached "
<< kMaxCsvRowsPerFileIncludingHeader << " rows; continuing in "
<< csvFilePathForIndex(csv_file_index_));
return true;
}
} // namespace orbbec_camera
orbbec_camera/src/image_publisher.cpp
+89 -3
View File
@@ -14,8 +14,47 @@
#include "orbbec_camera/image_publisher.h"
#include <map>
#include <mutex>
#include <utility>
namespace orbbec_camera {
namespace {
using ImageTransportPublisherCacheKey = std::pair<std::string, std::string>;
struct CachedImageTransportPublisher {
rmw_qos_profile_t qos;
std::shared_ptr<image_publisher> publisher;
};
using ImageTransportPublisherCache =
std::map<ImageTransportPublisherCacheKey, CachedImageTransportPublisher>;
std::mutex& imageTransportPublisherCacheMutex() {
static std::mutex mutex;
return mutex;
}
ImageTransportPublisherCache& imageTransportPublisherCache() {
static ImageTransportPublisherCache cache;
return cache;
}
bool rmwTimeEqual(const rmw_time_t& lhs, const rmw_time_t& rhs) {
return lhs.sec == rhs.sec && lhs.nsec == rhs.nsec;
}
bool qosProfilesEqual(const rmw_qos_profile_t& lhs, const rmw_qos_profile_t& rhs) {
return lhs.history == rhs.history && lhs.depth == rhs.depth &&
lhs.reliability == rhs.reliability && lhs.durability == rhs.durability &&
rmwTimeEqual(lhs.deadline, rhs.deadline) && rmwTimeEqual(lhs.lifespan, rhs.lifespan) &&
lhs.liveliness == rhs.liveliness &&
rmwTimeEqual(lhs.liveliness_lease_duration, rhs.liveliness_lease_duration) &&
lhs.avoid_ros_namespace_conventions == rhs.avoid_ros_namespace_conventions;
}
} // namespace
// --- image_rcl_publisher implementation ---
image_rcl_publisher::image_rcl_publisher(rclcpp::Node& node, const std::string& topic_name,
const rmw_qos_profile_t& qos) {
@@ -35,8 +74,20 @@ size_t image_rcl_publisher::get_subscription_count() const {
image_transport_publisher::image_transport_publisher(rclcpp::Node& node,
const std::string& topic_name,
const rmw_qos_profile_t& qos) {
image_publisher_impl = std::make_shared<image_transport::Publisher>(
image_transport::create_publisher(&node, topic_name, qos));
image_publisher_impl =
std::make_shared<image_transport::Publisher>(image_transport::create_publisher(
#ifdef ORBBEC_IMAGE_TRANSPORT_USES_REQUIRED_INTERFACES
image_transport::RequiredInterfaces{node},
#else
&node,
#endif
topic_name,
#ifdef ORBBEC_IMAGE_TRANSPORT_USES_REQUIRED_INTERFACES
rclcpp::QoS{rclcpp::QoSInitialization::from_rmw(qos), qos}
#else
qos
#endif
));
}
void image_transport_publisher::publish(sensor_msgs::msg::Image::UniquePtr image_ptr) {
image_publisher_impl->publish(*image_ptr);
@@ -45,4 +96,39 @@ void image_transport_publisher::publish(sensor_msgs::msg::Image::UniquePtr image
size_t image_transport_publisher::get_subscription_count() const {
return image_publisher_impl->getNumSubscribers();
}
} // namespace orbbec_camera
std::shared_ptr<image_publisher> getGlobalImageTransportPublisher(rclcpp::Node& node,
const std::string& topic_name,
const rmw_qos_profile_t& qos) {
const ImageTransportPublisherCacheKey key{node.get_fully_qualified_name(), topic_name};
std::lock_guard<std::mutex> lock(imageTransportPublisherCacheMutex());
auto& cache = imageTransportPublisherCache();
auto cached = cache.find(key);
if (cached != cache.end() && qosProfilesEqual(cached->second.qos, qos)) {
return cached->second.publisher;
}
auto publisher = std::make_shared<image_transport_publisher>(node, topic_name, qos);
cache[key] = CachedImageTransportPublisher{qos, publisher};
return publisher;
}
void releaseGlobalImageTransportPublisher(rclcpp::Node& node, const std::string& topic_name) {
const ImageTransportPublisherCacheKey key{node.get_fully_qualified_name(), topic_name};
std::lock_guard<std::mutex> lock(imageTransportPublisherCacheMutex());
imageTransportPublisherCache().erase(key);
}
void clearGlobalImageTransportPublishers(rclcpp::Node& node) {
const std::string node_name = node.get_fully_qualified_name();
std::lock_guard<std::mutex> lock(imageTransportPublisherCacheMutex());
auto& cache = imageTransportPublisherCache();
for (auto publisher = cache.begin(); publisher != cache.end();) {
if (publisher->first.first == node_name) {
publisher = cache.erase(publisher);
} else {
++publisher;
}
}
}
} // namespace orbbec_camera
orbbec_camera/src/ob_camera_node.cpp
+470 -129
View File
@@ -19,6 +19,7 @@
#include <thread>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <sstream>
#include <array>
#include <algorithm>
#include <cctype>
#include <cmath>
@@ -27,6 +28,7 @@
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include <magic_enum/magic_enum.hpp>
#include "orbbec_camera/utils.h"
#include <filesystem>
@@ -65,6 +67,10 @@ namespace {
constexpr char kEnhancedDepthSupportedTargetResolutions[] = "640x480/1280x720/1280x800";
constexpr char kEnhancedDepthSupportedDepthFormats[] = "Y10/Y11/Y12/Y14/Y16/Z16";
constexpr double kViewerColorizerGamma = 0.65;
constexpr uint16_t kViewerColorizerMaxDistanceMm = 10000;
constexpr uint16_t kViewerColorizerDefaultMinDistanceMm = 100;
constexpr uint16_t kViewerColorizerG305MinDistanceMm = 40;
std::string getDepthFilterStatusName(const std::string &filter_name) {
if (filter_name == "SpatialAdvancedFilter") {
@@ -451,8 +457,8 @@ void OBCameraNode::publishDepthFiltersStatus() {
depth_filters_snapshot = depth_filter_list_;
}
auto find_depth_filter = [&depth_filters_snapshot,
this](const std::string &filter_name) -> std::shared_ptr<ob::Filter> {
auto find_depth_filter =
[&depth_filters_snapshot](const std::string &filter_name) -> std::shared_ptr<ob::Filter> {
const auto normalized_name = normalizeDepthFilterName(filter_name);
auto it = std::find_if(depth_filters_snapshot.begin(), depth_filters_snapshot.end(),
[&normalized_name](const auto &filter) {
@@ -1104,24 +1110,31 @@ void OBCameraNode::setupDevices() {
<< disparity_to_depth_mode_ << "', keeping default settings");
}
}
if (should_apply_launch_config("enable_ldp") &&
device_->isPropertySupported(OB_PROP_LDP_BOOL, OB_PERMISSION_READ_WRITE)) {
if (device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
auto laser_enable = device_->getIntProperty(OB_PROP_LASER_CONTROL_INT);
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_LDP_BOOL, enable_ldp_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_CONTROL_INT, laser_enable);
} else if (device_->isPropertySupported(OB_PROP_LASER_BOOL, OB_PERMISSION_READ_WRITE)) {
if (!enable_ldp_) {
auto laser_enable = device_->getIntProperty(OB_PROP_LASER_BOOL);
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_LDP_BOOL, enable_ldp_);
std::this_thread::sleep_for(std::chrono::milliseconds(3));
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_BOOL, laser_enable);
} else {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_LDP_BOOL, enable_ldp_);
try {
if (should_apply_launch_config("enable_ldp") &&
device_->isPropertySupported(OB_PROP_LDP_BOOL, OB_PERMISSION_READ_WRITE)) {
if (device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
auto laser_enable = device_->getIntProperty(OB_PROP_LASER_CONTROL_INT);
device_->setBoolProperty(OB_PROP_LDP_BOOL, enable_ldp_);
device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, laser_enable);
} else if (device_->isPropertySupported(OB_PROP_LASER_BOOL, OB_PERMISSION_READ_WRITE)) {
if (!enable_ldp_) {
auto laser_enable = device_->getIntProperty(OB_PROP_LASER_BOOL);
device_->setBoolProperty(OB_PROP_LDP_BOOL, enable_ldp_);
std::this_thread::sleep_for(std::chrono::milliseconds(3));
device_->setIntProperty(OB_PROP_LASER_BOOL, laser_enable);
} else {
device_->setBoolProperty(OB_PROP_LDP_BOOL, enable_ldp_);
}
}
RCLCPP_INFO_STREAM(
logger_, "Current LDP: " << (device_->getBoolProperty(OB_PROP_LDP_BOOL) ? "ON" : "OFF"));
}
RCLCPP_INFO_STREAM(
logger_, "Current LDP: " << (device_->getBoolProperty(OB_PROP_LDP_BOOL) ? "ON" : "OFF"));
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_,
"Skipping LDP configuration: " << orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_WARN_STREAM(logger_, "Skipping LDP configuration: " << e.what());
}
if (ldp_power_level_ != -1 &&
device_->isPropertySupported(OB_PROP_LASER_POWER_LEVEL_CONTROL_INT, OB_PERMISSION_WRITE)) {
@@ -1810,8 +1823,6 @@ void OBCameraNode::setupDevices() {
<< (device_->getBoolProperty(OB_PROP_SDK_GYRO_FRAME_TRANSFORMED_BOOL) ? "ON" : "OFF"));
}
if (isGemini335PID(pid_) && !intra_camera_sync_reference_.empty() &&
(sync_mode_ == OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING ||
sync_mode_ == OB_MULTI_DEVICE_SYNC_MODE_HARDWARE_TRIGGERING) &&
device_->isPropertySupported(OB_PROP_INTRA_CAMERA_SYNC_REFERENCE_INT, OB_PERMISSION_WRITE)) {
if (intra_camera_sync_reference_ == "Start") {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_INTRA_CAMERA_SYNC_REFERENCE_INT, 0);
@@ -3238,6 +3249,10 @@ void OBCameraNode::setupDepthPostProcessFilter() {
auto threshold_filter = filter->as<ob::ThresholdFilter>();
if (threshold_filter_min_ != -1 && threshold_filter_max_ != -1) {
threshold_filter->setValueRange(threshold_filter_min_, threshold_filter_max_);
} else if (threshold_filter_min_ != -1) {
threshold_filter->setConfigValue("min", threshold_filter_min_);
} else if (threshold_filter_max_ != -1) {
threshold_filter->setConfigValue("max", threshold_filter_max_);
}
RCLCPP_INFO_STREAM(logger_, "Current threshold filter value range: "
<< static_cast<int>(threshold_filter->getConfigValue("min"))
@@ -3737,6 +3752,8 @@ bool OBCameraNode::applyStreamProfiles(const std::vector<PendingStreamProfile> &
const bool interleave_frame_enable = interleave_frame_enable_;
if (restart_pipeline) {
stopStreams();
RCLCPP_DEBUG_STREAM(logger_, "Wait 1 second for streams to stop before applying profiles");
std::this_thread::sleep_for(std::chrono::seconds(1));
interleave_frame_enable_ = interleave_frame_enable;
}
stopColorFrameThreads();
@@ -3805,17 +3822,17 @@ bool OBCameraNode::applyStreamProfiles(const std::vector<PendingStreamProfile> &
void OBCameraNode::clearColorFrameQueues() {
{
std::lock_guard<std::mutex> lock(color_frame_queue_lock_);
std::queue<std::shared_ptr<ob::FrameSet>> empty;
ColorFrameQueue empty;
std::swap(color_frame_queue_, empty);
}
{
std::lock_guard<std::mutex> lock(left_color_frame_queue_lock_);
std::queue<std::shared_ptr<ob::FrameSet>> empty;
ColorFrameQueue empty;
std::swap(left_color_frame_queue_, empty);
}
{
std::lock_guard<std::mutex> lock(right_color_frame_queue_lock_);
std::queue<std::shared_ptr<ob::FrameSet>> empty;
ColorFrameQueue empty;
std::swap(right_color_frame_queue_, empty);
}
is_color_frame_decoded_ = false;
@@ -3823,6 +3840,56 @@ void OBCameraNode::clearColorFrameQueues() {
is_right_color_frame_decoded_ = false;
}
void OBCameraNode::enqueueColorFrame(ColorFrameQueue &queue, std::mutex &mutex,
std::condition_variable &condition_variable,
ColorQueueStats &stats, int capacity_frames,
const std::shared_ptr<ob::FrameSet> &frame_set,
const char *queue_name) {
uint64_t overflow_count = 0;
{
std::lock_guard<std::mutex> lock(mutex);
const auto now = std::chrono::steady_clock::now();
if (queue.size() >= static_cast<size_t>(capacity_frames)) {
const auto oldest_age =
std::chrono::duration<double, std::milli>(now - queue.front().enqueue_time).count();
stats.max_queue_wait_ms = std::max(stats.max_queue_wait_ms, oldest_age);
queue.pop();
overflow_count = ++stats.overflow_count;
}
queue.push(QueuedColorFrame{frame_set, now});
stats.max_queue_size = std::max(stats.max_queue_size, queue.size());
}
condition_variable.notify_one();
if (overflow_count == 1 || (overflow_count > 0 && overflow_count % 100 == 0)) {
RCLCPP_WARN_STREAM(
logger_, "Color frame queue overflow: queue=" << queue_name << " count=" << overflow_count
<< " capacity_frames=" << capacity_frames);
}
}
OBCameraNode::ColorQueueStatsSnapshot OBCameraNode::getColorQueueStats(ColorFrameQueue &queue,
std::mutex &mutex,
ColorQueueStats &stats,
int capacity_frames,
bool reset) {
std::lock_guard<std::mutex> lock(mutex);
const auto oldest_queue_wait_ms =
queue.empty() ? 0.0
: std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() -
queue.front().enqueue_time)
.count();
stats.max_queue_wait_ms = std::max(stats.max_queue_wait_ms, oldest_queue_wait_ms);
const ColorQueueStatsSnapshot snapshot{capacity_frames, queue.size(),
stats.max_queue_size, stats.overflow_count,
oldest_queue_wait_ms, stats.max_queue_wait_ms};
if (reset) {
stats.max_queue_size = queue.size();
stats.overflow_count = 0;
stats.max_queue_wait_ms = oldest_queue_wait_ms;
}
return snapshot;
}
void OBCameraNode::stopColorFrameThreads() {
if (!colorFrameThread_ && !leftColorFrameThread_ && !rightColorFrameThread_) {
return;
@@ -3928,6 +3995,12 @@ void OBCameraNode::updateImageConfig(const stream_index_pair &stream_index) {
encoding_[stream_index] = is_depth_stream ? sensor_msgs::image_encodings::TYPE_16UC1
: sensor_msgs::image_encodings::MONO16;
unit_step_size_[stream_index] = sizeof(uint16_t);
} else if (is_color_stream &&
(format == OB_FORMAT_YUYV || format == OB_FORMAT_UYVY || format == OB_FORMAT_I420 ||
format == OB_FORMAT_NV12 || format == OB_FORMAT_NV21)) {
image_format_[stream_index] = CV_8UC3;
encoding_[stream_index] = sensor_msgs::image_encodings::RGB8;
unit_step_size_[stream_index] = 3 * sizeof(uint8_t);
} else if (format == OB_FORMAT_MJPG || format == OB_FORMAT_MJPEG) {
if (is_ir_stream) {
image_format_[stream_index] = CV_8UC1;
@@ -4371,6 +4444,20 @@ void OBCameraNode::setupDefaultImageFormat() {
void OBCameraNode::getParameters() {
setAndGetNodeParameter<std::string>(camera_name_, "camera_name", "camera");
setAndGetNodeParameter<int>(color_frame_queue_max_frames_, "color_frame_queue_max_frames", 10);
setAndGetNodeParameter<int>(left_color_frame_queue_max_frames_,
"left_color_frame_queue_max_frames", 10);
setAndGetNodeParameter<int>(right_color_frame_queue_max_frames_,
"right_color_frame_queue_max_frames", 10);
const auto validate_queue_capacity = [](const char *name, int capacity) {
if (capacity < 1) {
throw std::invalid_argument(std::string(name) + " must be greater than zero");
}
};
validate_queue_capacity("color_frame_queue_max_frames", color_frame_queue_max_frames_);
validate_queue_capacity("left_color_frame_queue_max_frames", left_color_frame_queue_max_frames_);
validate_queue_capacity("right_color_frame_queue_max_frames",
right_color_frame_queue_max_frames_);
camera_link_frame_id_ = camera_name_ + "_link";
for (auto stream_index : IMAGE_STREAMS) {
std::string param_name = stream_name_[stream_index] + "_width";
@@ -4404,6 +4491,20 @@ void OBCameraNode::getParameters() {
updateImageConfig(stream_index);
param_name = stream_name_[stream_index] + "_qos";
setAndGetNodeParameter<std::string>(image_qos_[stream_index], param_name, "default");
param_name = stream_name_[stream_index] + "_qos_history";
setAndGetNodeParameter<std::string>(image_qos_history_[stream_index], param_name, "default");
std::transform(image_qos_history_[stream_index].begin(), image_qos_history_[stream_index].end(),
image_qos_history_[stream_index].begin(), ::toupper);
if (image_qos_history_[stream_index] != "DEFAULT" &&
image_qos_history_[stream_index] != "KEEP_LAST" &&
image_qos_history_[stream_index] != "KEEP_ALL") {
throw std::invalid_argument(param_name + " must be DEFAULT, KEEP_LAST, or KEEP_ALL");
}
param_name = stream_name_[stream_index] + "_qos_depth";
setAndGetNodeParameter<int>(image_qos_depth_[stream_index], param_name, -1);
if (image_qos_depth_[stream_index] == 0 || image_qos_depth_[stream_index] < -1) {
throw std::invalid_argument(param_name + " must be -1 or greater than zero");
}
param_name = stream_name_[stream_index] + "_camera_info_qos";
setAndGetNodeParameter<std::string>(camera_info_qos_[stream_index], param_name, "default");
param_name = "enable_" + stream_name_[stream_index] + "_undistortion";
@@ -4457,6 +4558,10 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<int>(point_cloud_decimation_filter_factor_,
"point_cloud_decimation_filter_factor", 1);
setAndGetNodeParameter<std::string>(point_cloud_qos_, "point_cloud_qos", "default");
setAndGetNodeParameter<std::string>(colorizer_mode_, "depth_colorizer_mode", "none");
colorizer_mode_ = lowerParameterValue(colorizer_mode_);
colorizer_mode_ = normalizeClosedSetParameterValue(
logger_, "depth_colorizer_mode", colorizer_mode_, {"none", "jet", "jet_inv", "gray"}, "none");
setAndGetNodeParameter<bool>(enable_d2c_viewer_, "enable_d2c_viewer", false);
setAndGetNodeParameter<std::string>(disparity_to_depth_mode_, "disparity_to_depth_mode", "");
disparity_to_depth_mode_ =
@@ -5256,10 +5361,7 @@ void OBCameraNode::setupConfidencePublishers() {
if (confidence_image_publisher_) {
return;
}
auto image_qos_profile = getRMWQosProfileFromString(image_qos_[DEPTH]);
if (use_intra_process_) {
image_qos_profile = rmw_qos_profile_default;
}
const auto image_qos_profile = getImageQosProfile(DEPTH);
confidence_image_publisher_ = node_->create_publisher<sensor_msgs::msg::Image>(
"confidence/image_raw",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_qos_profile), image_qos_profile));
@@ -5311,41 +5413,57 @@ void OBCameraNode::publishConfidenceFrame(const std::shared_ptr<ob::Frame> &conf
}
void OBCameraNode::setupCameraInfo() {
std::string color_camera_name = camera_name_ + "_color";
const auto create_camera_info_manager = [this](const std::string &camera_name,
const std::string &camera_info_url) {
#ifdef ORBBEC_CAMERA_INFO_MANAGER_USES_NODE_INTERFACES
#ifdef ORBBEC_CAMERA_INFO_MANAGER_USES_RCLCPP_QOS
return std::make_unique<camera_info_manager::CameraInfoManager>(
node_->get_node_base_interface(), node_->get_node_services_interface(),
node_->get_node_logging_interface(), camera_name, camera_info_url,
rclcpp::SystemDefaultsQoS());
#else
return std::make_unique<camera_info_manager::CameraInfoManager>(
node_->get_node_base_interface(), node_->get_node_services_interface(),
node_->get_node_logging_interface(), camera_name, camera_info_url, rmw_qos_profile_default);
#endif
#else
return std::make_unique<camera_info_manager::CameraInfoManager>(node_, camera_name,
camera_info_url);
#endif
};
const std::string color_camera_name = camera_name_ + "_color";
if (!color_info_url_.empty()) {
color_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(
node_, color_camera_name, color_info_url_);
color_info_manager_ = create_camera_info_manager(color_camera_name, color_info_url_);
}
std::string ir_camera_name = camera_name_ + "_ir";
const std::string ir_camera_name = camera_name_ + "_ir";
if (!ir_info_url_.empty()) {
ir_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(
node_, ir_camera_name, ir_info_url_);
ir_info_manager_ = create_camera_info_manager(ir_camera_name, ir_info_url_);
}
}
void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) {
const std::string topic = stream_name_[stream_index] + "/image_raw";
if (!enable_stream_[stream_index]) {
releaseGlobalImageTransportPublisher(*node_, topic);
image_publishers_.erase(stream_index);
compressed_image_publishers_.erase(stream_index);
return;
}
const std::string topic = stream_name_[stream_index] + "/image_raw";
auto image_qos_profile = getRMWQosProfileFromString(image_qos_[stream_index]);
if (use_intra_process_) {
image_qos_profile = rmw_qos_profile_default;
}
const auto image_qos_profile = getImageQosProfile(stream_index);
const bool is_mjpg_color_stream =
(stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) &&
format_[stream_index] == OB_FORMAT_MJPG;
if (use_intra_process_ || is_mjpg_color_stream) {
releaseGlobalImageTransportPublisher(*node_, topic);
image_publishers_[stream_index] =
std::make_shared<image_rcl_publisher>(*node_, topic, image_qos_profile);
} else {
image_publishers_[stream_index] =
std::make_shared<image_transport_publisher>(*node_, topic, image_qos_profile);
getGlobalImageTransportPublisher(*node_, topic, image_qos_profile);
}
RCLCPP_INFO_STREAM(logger_, topic << " QoS: " << getRMWQosProfileDescription(image_qos_profile));
if (is_mjpg_color_stream) {
compressed_image_publishers_[stream_index] =
@@ -5357,6 +5475,24 @@ void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) {
}
}
rmw_qos_profile_t OBCameraNode::getImageQosProfile(const stream_index_pair &stream_index) const {
auto image_qos_profile = getRMWQosProfileFromString(image_qos_.at(stream_index));
if (use_intra_process_) {
image_qos_profile = rmw_qos_profile_default;
}
const auto &history = image_qos_history_.at(stream_index);
if (history == "KEEP_LAST") {
image_qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
} else if (history == "KEEP_ALL") {
image_qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL;
}
const auto depth = image_qos_depth_.at(stream_index);
if (depth > 0) {
image_qos_profile.depth = static_cast<size_t>(depth);
}
return image_qos_profile;
}
void OBCameraNode::setupPublishers() {
using PointCloud2 = sensor_msgs::msg::PointCloud2;
using CameraInfo = sensor_msgs::msg::CameraInfo;
@@ -5402,6 +5538,12 @@ void OBCameraNode::setupPublishers() {
}
}
if (colorizer_mode_ != "none" && enable_stream_[DEPTH]) {
RCLCPP_INFO_STREAM(logger_, "Depth colorizer mode '"
<< colorizer_mode_
<< "' enabled, publishing colorized images on depth/image_raw");
}
syncSoftwareAlignment();
if (enable_sync_output_accel_gyro_) {
@@ -5501,15 +5643,12 @@ void OBCameraNode::syncSoftwareAlignment() {
RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
}
if (!depth_unaligned_publisher_) {
auto depth_image_qos_profile = getRMWQosProfileFromString(image_qos_[DEPTH]);
if (use_intra_process_) {
depth_image_qos_profile = rmw_qos_profile_default;
}
const auto depth_image_qos_profile = getImageQosProfile(DEPTH);
if (use_intra_process_) {
depth_unaligned_publisher_ = std::make_shared<image_rcl_publisher>(
*node_, "depth/image_unaligned", depth_image_qos_profile);
} else {
depth_unaligned_publisher_ = std::make_shared<image_transport_publisher>(
depth_unaligned_publisher_ = getGlobalImageTransportPublisher(
*node_, "depth/image_unaligned", depth_image_qos_profile);
}
}
@@ -5517,6 +5656,7 @@ void OBCameraNode::syncSoftwareAlignment() {
}
align_filter_.reset();
releaseGlobalImageTransportPublisher(*node_, "depth/image_unaligned");
depth_unaligned_publisher_.reset();
}
@@ -5575,6 +5715,91 @@ void OBCameraNode::publishRawDepthImage(const std::shared_ptr<ob::Frame> &depth_
depth_unaligned_publisher_->publish(std::move(image_msg));
}
cv::Mat OBCameraNode::colorizeDepthImage(const cv::Mat &depth_image,
const std::string &colorizer_mode) {
if (depth_image.empty() || depth_image.channels() != 1) {
return {};
}
if (colorizer_mode == "none") {
return {};
}
if (depth_image.type() != CV_16UC1 && depth_image.type() != CV_32FC1 &&
depth_image.type() != CV_8UC1) {
RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 5000,
"Unsupported depth image type for colorizer: %d", depth_image.type());
return {};
}
cv::Mat depth_16u;
depth_image.convertTo(depth_16u, CV_16UC1);
const uint16_t min_depth = isGemini305SeriesPID(pid_) ? kViewerColorizerG305MinDistanceMm
: kViewerColorizerDefaultMinDistanceMm;
const uint16_t max_depth = kViewerColorizerMaxDistanceMm;
const uint32_t value_range = static_cast<uint32_t>(max_depth) - min_depth + 1;
std::array<uint32_t, static_cast<size_t>(kViewerColorizerMaxDistanceMm) + 1> histogram{};
uint32_t valid_pixel_count = 0;
for (int row = 0; row < depth_16u.rows; ++row) {
const auto *depth_row = depth_16u.ptr<uint16_t>(row);
for (int col = 0; col < depth_16u.cols; ++col) {
const uint16_t depth_value = depth_row[col];
if (depth_value >= min_depth && depth_value <= max_depth) {
++histogram[depth_value];
++valid_pixel_count;
}
}
}
for (uint32_t depth_value = 1; depth_value <= max_depth; ++depth_value) {
histogram[depth_value] += histogram[depth_value - 1];
}
cv::Mat depth_8u(depth_16u.size(), CV_8UC1);
for (int row = 0; row < depth_16u.rows; ++row) {
const auto *depth_row = depth_16u.ptr<uint16_t>(row);
auto *mapped_row = depth_8u.ptr<uint8_t>(row);
for (int col = 0; col < depth_16u.cols; ++col) {
uint16_t depth_value = depth_row[col];
if (depth_value > max_depth) {
depth_value = max_depth;
}
if (valid_pixel_count != 0 && depth_value >= min_depth) {
depth_value = static_cast<uint16_t>(static_cast<float>(value_range) *
histogram[depth_value] / valid_pixel_count +
min_depth);
}
const double normalized_depth = std::max(
0.0,
std::min(1.0, (static_cast<double>(depth_value) - min_depth) / (max_depth - min_depth)));
const double scale_value = 255.0 * std::pow(normalized_depth, kViewerColorizerGamma);
mapped_row[col] = static_cast<uint8_t>(std::max(0.0, std::min(255.0, scale_value)));
}
}
cv::Mat invalid_depth_mask;
cv::compare(depth_image, cv::Scalar(0), invalid_depth_mask, cv::CMP_EQ);
depth_8u.setTo(cv::Scalar::all(0), invalid_depth_mask);
if (colorizer_mode == "gray") {
return depth_8u;
}
if (colorizer_mode == "jet_inv") {
depth_8u = 255 - depth_8u;
depth_8u.setTo(cv::Scalar::all(0), invalid_depth_mask);
}
cv::Mat colorized_bgr;
cv::applyColorMap(depth_8u, colorized_bgr, cv::COLORMAP_JET);
colorized_bgr.setTo(cv::Scalar::all(0), invalid_depth_mask);
cv::Mat colorized_rgb;
cv::cvtColor(colorized_bgr, colorized_rgb, cv::COLOR_BGR2RGB);
return colorized_rgb;
}
void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
if (!depth_cloud_pub_ || depth_cloud_pub_->get_subscription_count() == 0 ||
!enable_point_cloud_) {
@@ -6243,22 +6468,22 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
}
if (enable_stream_[COLOR] && color_frame) {
std::unique_lock<std::mutex> lock(color_frame_queue_lock_);
color_frame_queue_.push(frame_set);
color_frame_queue_cv_.notify_all();
enqueueColorFrame(color_frame_queue_, color_frame_queue_lock_, color_frame_queue_cv_,
color_frame_queue_stats_, color_frame_queue_max_frames_, frame_set,
"color");
} else {
publishPointCloud(frame_set);
}
if (enable_stream_[COLOR_LEFT] && left_color_frame) {
std::unique_lock<std::mutex> lock(left_color_frame_queue_lock_);
left_color_frame_queue_.push(frame_set);
left_color_frame_queue_cv_.notify_all();
enqueueColorFrame(left_color_frame_queue_, left_color_frame_queue_lock_,
left_color_frame_queue_cv_, left_color_frame_queue_stats_,
left_color_frame_queue_max_frames_, frame_set, "left_color");
}
if (enable_stream_[COLOR_RIGHT] && right_color_frame) {
std::unique_lock<std::mutex> lock(right_color_frame_queue_lock_);
right_color_frame_queue_.push(frame_set);
right_color_frame_queue_cv_.notify_all();
enqueueColorFrame(right_color_frame_queue_, right_color_frame_queue_lock_,
right_color_frame_queue_cv_, right_color_frame_queue_stats_,
right_color_frame_queue_max_frames_, frame_set, "right_color");
}
for (const auto &stream_index : IMAGE_STREAMS) {
@@ -6310,20 +6535,30 @@ void OBCameraNode::logFrameInfoOnce(const stream_index_pair &stream_index,
void OBCameraNode::onNewColorFrameCallback() {
while (enable_stream_[COLOR] && rclcpp::ok() && is_running_.load() &&
!stop_color_frame_threads_.load()) {
std::unique_lock<std::mutex> lock(color_frame_queue_lock_);
color_frame_queue_cv_.wait(lock, [this]() {
return !color_frame_queue_.empty() || !(is_running_.load()) ||
stop_color_frame_threads_.load();
});
std::shared_ptr<ob::FrameSet> frameSet;
{
std::unique_lock<std::mutex> lock(color_frame_queue_lock_);
color_frame_queue_cv_.wait(lock, [this]() {
return !color_frame_queue_.empty() || !(is_running_.load()) ||
stop_color_frame_threads_.load();
});
if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) {
break;
if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) {
break;
}
const auto queued = color_frame_queue_.front();
color_frame_queue_.pop();
frameSet = queued.frame_set;
color_frame_queue_stats_.max_queue_wait_ms =
std::max(color_frame_queue_stats_.max_queue_wait_ms,
std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() -
queued.enqueue_time)
.count());
}
std::shared_ptr<ob::FrameSet> frameSet = color_frame_queue_.front();
is_color_frame_decoded_ = decodeColorFrameToBuffer(frameSet->colorFrame(), rgb_buffer_);
onNewFrameCallback(frameSet->colorFrame(), COLOR);
publishPointCloud(frameSet);
color_frame_queue_.pop();
}
RCLCPP_DEBUG_STREAM(logger_, "Color frame thread exited");
@@ -6332,20 +6567,30 @@ void OBCameraNode::onNewColorFrameCallback() {
void OBCameraNode::onNewLeftColorFrameCallback() {
while (enable_stream_[COLOR_LEFT] && rclcpp::ok() && is_running_.load() &&
!stop_color_frame_threads_.load()) {
std::unique_lock<std::mutex> lock(left_color_frame_queue_lock_);
left_color_frame_queue_cv_.wait(lock, [this]() {
return !left_color_frame_queue_.empty() || !(is_running_.load()) ||
stop_color_frame_threads_.load();
});
std::shared_ptr<ob::FrameSet> frameSet;
{
std::unique_lock<std::mutex> lock(left_color_frame_queue_lock_);
left_color_frame_queue_cv_.wait(lock, [this]() {
return !left_color_frame_queue_.empty() || !(is_running_.load()) ||
stop_color_frame_threads_.load();
});
if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) {
break;
if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) {
break;
}
const auto queued = left_color_frame_queue_.front();
left_color_frame_queue_.pop();
frameSet = queued.frame_set;
left_color_frame_queue_stats_.max_queue_wait_ms =
std::max(left_color_frame_queue_stats_.max_queue_wait_ms,
std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() -
queued.enqueue_time)
.count());
}
std::shared_ptr<ob::FrameSet> frameSet = left_color_frame_queue_.front();
is_left_color_frame_decoded_ =
decodeColorFrameToBuffer(frameSet->getFrame(OB_FRAME_COLOR_LEFT), rgb_buffer_left_);
onNewFrameCallback(frameSet->getFrame(OB_FRAME_COLOR_LEFT), COLOR_LEFT);
left_color_frame_queue_.pop();
}
RCLCPP_DEBUG_STREAM(logger_, "Left color frame thread exited");
}
@@ -6353,20 +6598,30 @@ void OBCameraNode::onNewLeftColorFrameCallback() {
void OBCameraNode::onNewRightColorFrameCallback() {
while (enable_stream_[COLOR_RIGHT] && rclcpp::ok() && is_running_.load() &&
!stop_color_frame_threads_.load()) {
std::unique_lock<std::mutex> lock(right_color_frame_queue_lock_);
right_color_frame_queue_cv_.wait(lock, [this]() {
return !right_color_frame_queue_.empty() || !(is_running_.load()) ||
stop_color_frame_threads_.load();
});
std::shared_ptr<ob::FrameSet> frameSet;
{
std::unique_lock<std::mutex> lock(right_color_frame_queue_lock_);
right_color_frame_queue_cv_.wait(lock, [this]() {
return !right_color_frame_queue_.empty() || !(is_running_.load()) ||
stop_color_frame_threads_.load();
});
if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) {
break;
if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_threads_.load()) {
break;
}
const auto queued = right_color_frame_queue_.front();
right_color_frame_queue_.pop();
frameSet = queued.frame_set;
right_color_frame_queue_stats_.max_queue_wait_ms =
std::max(right_color_frame_queue_stats_.max_queue_wait_ms,
std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() -
queued.enqueue_time)
.count());
}
std::shared_ptr<ob::FrameSet> frameSet = right_color_frame_queue_.front();
is_right_color_frame_decoded_ =
decodeColorFrameToBuffer(frameSet->getFrame(OB_FRAME_COLOR_RIGHT), rgb_buffer_right_);
onNewFrameCallback(frameSet->getFrame(OB_FRAME_COLOR_RIGHT), COLOR_RIGHT);
right_color_frame_queue_.pop();
}
RCLCPP_DEBUG_STREAM(logger_, "Right color frame thread exited");
}
@@ -6759,24 +7014,38 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
camera_info_publishers_[stream_index]->publish(camera_info);
publishMetadata(frame, stream_index, camera_info.header);
if (!has_raw_image_subscriber && !save_images_[stream_index]) {
return;
}
cv::Mat raw_image = image;
cv::Mat image_to_publish = image;
std::string image_encoding = encoding_[stream_index];
uint32_t image_step = width * unit_step_size_[stream_index];
if (stream_index == DEPTH) {
auto depth_scale = video_frame->as<ob::DepthFrame>()->getValueScale();
image = image * depth_scale;
image_to_publish = image;
if (colorizer_mode_ != "none" && (has_raw_image_subscriber || save_images_[stream_index])) {
auto colorized_image = colorizeDepthImage(image, colorizer_mode_);
if (!colorized_image.empty()) {
image_to_publish = std::move(colorized_image);
image_encoding = colorizer_mode_ == "gray" ? sensor_msgs::image_encodings::MONO8
: sensor_msgs::image_encodings::RGB8;
image_step = static_cast<uint32_t>(image_to_publish.cols * image_to_publish.elemSize());
}
}
}
if (!has_raw_image_subscriber && !save_images_[stream_index]) {
return;
}
CHECK(image_publishers_.count(stream_index) > 0);
if (has_raw_image_subscriber || save_images_[stream_index]) {
sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image)
cv_bridge::CvImage(std_msgs::msg::Header(), image_encoding, image_to_publish)
.toImageMsg(*image_msg);
CHECK_NOTNULL(image_msg.get());
image_msg->header.stamp = timestamp;
image_msg->is_bigendian = false;
image_msg->step = width * unit_step_size_[stream_index];
image_msg->step = image_step;
image_msg->header.frame_id = frame_id;
saveImageToFile(stream_index, image, *image_msg);
saveImageToFile(stream_index, raw_image, image_to_publish, *image_msg, frame);
if (!has_raw_image_subscriber) {
record_image_publish_skipped();
return;
@@ -6830,8 +7099,15 @@ void OBCameraNode::publishMetadata(const std::shared_ptr<ob::Frame> &frame,
}
orbbec_camera_msgs::msg::Metadata metadata_msg;
metadata_msg.header = header;
nlohmann::json json_data;
metadata_msg.json_data = createFrameMetadataJson(frame);
metadata_publisher->publish(metadata_msg);
}
std::string OBCameraNode::createFrameMetadataJson(const std::shared_ptr<ob::Frame> &frame) const {
nlohmann::json json_data;
if (frame == nullptr) {
return json_data.dump(2);
}
for (int i = 0; i < OB_FRAME_METADATA_TYPE_COUNT; i++) {
auto meta_data_type = static_cast<OBFrameMetadataType>(i);
std::string field_name = metaDataTypeToString(meta_data_type);
@@ -6841,59 +7117,124 @@ void OBCameraNode::publishMetadata(const std::shared_ptr<ob::Frame> &frame,
int64_t value = frame->getMetadataValue(meta_data_type);
json_data[field_name] = value;
}
metadata_msg.json_data = json_data.dump(2);
metadata_publisher->publish(metadata_msg);
return json_data.dump(2);
}
void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const cv::Mat &image,
const sensor_msgs::msg::Image &image_msg) {
if (save_images_[stream_index]) {
void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const cv::Mat &raw_image,
const cv::Mat &image_to_save,
const sensor_msgs::msg::Image &image_msg,
const std::shared_ptr<ob::Frame> &frame) {
if (save_images_[stream_index].load(std::memory_order_acquire)) {
int index = 0;
{
std::lock_guard<std::mutex> lock(save_images_mutex_);
if (!save_images_[stream_index].load(std::memory_order_relaxed)) {
return;
}
index = save_images_count_[stream_index]++;
if (save_images_count_[stream_index] >= max_save_images_count_) {
save_images_[stream_index].store(false, std::memory_order_release);
}
}
auto now = std::chrono::system_clock::now();
auto in_time_t = std::chrono::system_clock::to_time_t(now);
auto us =
std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()) % 1000000;
std::stringstream ss;
ss << std::put_time(std::localtime(&in_time_t), "%Y%m%d_%H%M%S");
ss << "_" << std::setw(6) << std::setfill('0') << us.count();
auto current_path = std::filesystem::current_path().string();
auto fps = fps_[stream_index];
int index = save_images_count_[stream_index];
std::string file_suffix = stream_index == COLOR ? ".png" : ".raw";
std::string filename = current_path + "/image/" + stream_name_[stream_index] + "_" +
std::to_string(image_msg.width) + "x" +
std::to_string(image_msg.height) + "_" + std::to_string(fps) + "hz_" +
ss.str() + "_" + std::to_string(index) + file_suffix;
if (!std::filesystem::exists(current_path + "/image")) {
std::filesystem::create_directory(current_path + "/image");
std::tm local_time{};
if (localtime_r(&in_time_t, &local_time) == nullptr) {
RCLCPP_ERROR_STREAM(logger_, "Failed to convert image save timestamp to local time");
return;
}
RCLCPP_INFO_STREAM(logger_, "Saving image to " << filename);
if (stream_index.first == OB_STREAM_COLOR) {
auto image_to_save =
cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8)->image;
cv::imwrite(filename, image_to_save);
} else if (stream_index.first == OB_STREAM_IR || stream_index.first == OB_STREAM_IR_LEFT ||
stream_index.first == OB_STREAM_IR_RIGHT || stream_index.first == OB_STREAM_DEPTH) {
std::ofstream ofs(filename, std::ios::out | std::ios::binary);
if (!ofs.is_open()) {
RCLCPP_ERROR_STREAM(logger_, "Failed to open file: " << filename);
return;
std::stringstream ss;
ss << std::put_time(&local_time, "%Y%m%d_%H%M%S");
ss << "_" << std::setw(6) << std::setfill('0') << us.count();
const auto output_directory = std::filesystem::current_path() / "image";
auto fps = fps_[stream_index];
const std::string file_name = stream_name_[stream_index] + "_" +
std::to_string(image_msg.width) + "x" +
std::to_string(image_msg.height) + "_" + std::to_string(fps) +
"hz_" + ss.str() + "_" + std::to_string(index);
if (!std::filesystem::exists(output_directory)) {
std::filesystem::create_directories(output_directory);
}
const auto file_stem = (output_directory / file_name).string();
const auto raw_filename = file_stem + ".raw";
const auto png_filename = file_stem + ".png";
const auto metadata_filename = file_stem + ".json";
RCLCPP_INFO_STREAM(logger_, "Saving frame files to " << file_stem << " (.raw, .png, .json)");
const auto *frame_data = frame ? frame->getData() : nullptr;
const auto frame_data_size = frame ? frame->getDataSize() : 0;
std::ofstream ofs(raw_filename, std::ios::out | std::ios::binary);
if (!ofs.is_open()) {
RCLCPP_ERROR_STREAM(logger_, "Failed to open raw file: " << raw_filename);
} else if (frame_data != nullptr && frame_data_size > 0) {
ofs.write(reinterpret_cast<const char *>(frame_data),
static_cast<std::streamsize>(frame_data_size));
if (!ofs.good()) {
RCLCPP_ERROR_STREAM(logger_, "Failed to write raw file: " << raw_filename);
}
if (image.isContinuous()) {
ofs.write(reinterpret_cast<const char *>(image.data), image.total() * image.elemSize());
} else if (!raw_image.empty()) {
if (raw_image.isContinuous()) {
ofs.write(reinterpret_cast<const char *>(raw_image.data),
static_cast<std::streamsize>(raw_image.total() * raw_image.elemSize()));
} else {
int rows = image.rows;
int cols = image.cols * image.channels();
for (int r = 0; r < rows; ++r) {
ofs.write(reinterpret_cast<const char *>(image.ptr<uchar>(r)), cols);
const auto row_size = static_cast<std::streamsize>(raw_image.cols * raw_image.elemSize());
for (int row = 0; row < raw_image.rows; ++row) {
ofs.write(reinterpret_cast<const char *>(raw_image.ptr<uchar>(row)), row_size);
}
}
ofs.close();
if (!ofs.good()) {
RCLCPP_ERROR_STREAM(logger_, "Failed to write raw file: " << raw_filename);
}
} else {
RCLCPP_ERROR_STREAM(logger_, "Unsupported stream type: " << stream_index.first);
RCLCPP_ERROR_STREAM(logger_, "Failed to save raw image: frame data and image are empty");
}
if (++save_images_count_[stream_index] >= max_save_images_count_) {
save_images_[stream_index] = false;
if (ofs.is_open()) {
ofs.close();
}
cv::Mat png_image = image_to_save.empty() ? raw_image : image_to_save;
if (stream_index == DEPTH && colorizer_mode_ == "none") {
// Keep the ROS topic raw in none mode, but save a viewable depth preview.
auto depth_preview = colorizeDepthImage(png_image, "gray");
if (!depth_preview.empty()) {
png_image = std::move(depth_preview);
}
}
if (png_image.empty()) {
RCLCPP_ERROR_STREAM(logger_, "Failed to save PNG image: image is empty");
} else {
cv::Mat converted_png_image;
if (image_msg.encoding == sensor_msgs::image_encodings::RGB8 && png_image.channels() == 3) {
cv::cvtColor(png_image, converted_png_image, cv::COLOR_RGB2BGR);
png_image = converted_png_image;
} else if (image_msg.encoding == sensor_msgs::image_encodings::RGBA8 &&
png_image.channels() == 4) {
cv::cvtColor(png_image, converted_png_image, cv::COLOR_RGBA2BGRA);
png_image = converted_png_image;
}
try {
if (!cv::imwrite(png_filename, png_image)) {
RCLCPP_ERROR_STREAM(logger_, "Failed to write PNG file: " << png_filename);
}
} catch (const cv::Exception &exception) {
RCLCPP_ERROR_STREAM(
logger_, "Failed to write PNG file " << png_filename << ": " << exception.what());
}
}
std::ofstream metadata_ofs(metadata_filename);
if (!metadata_ofs.is_open()) {
RCLCPP_ERROR_STREAM(logger_, "Failed to open metadata file: " << metadata_filename);
} else {
metadata_ofs << createFrameMetadataJson(frame) << '\n';
if (!metadata_ofs.good()) {
RCLCPP_ERROR_STREAM(logger_, "Failed to write metadata file: " << metadata_filename);
}
metadata_ofs.close();
}
}
}
@@ -7287,8 +7628,8 @@ void OBCameraNode::publishStaticTransforms() {
if (!publish_tf_) {
return;
}
static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_);
dynamic_tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*node_);
dynamic_tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*node_);
calcAndPublishStaticTransform();
if (tf_publish_rate_ > 0) {
tf_thread_ = std::make_shared<std::thread>([this]() { publishDynamicTransforms(); });
orbbec_camera/src/ob_camera_node_driver.cpp
+88 -61
View File
@@ -19,9 +19,21 @@
#include <fcntl.h>
#include <semaphore.h>
#include <sys/shm.h>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <ament_index_cpp/get_package_prefix.hpp>
#if __has_include(<ament_index_cpp/get_package_share_path.hpp>)
#include <ament_index_cpp/get_package_share_path.hpp>
#define ORBBEC_AMENT_INDEX_USES_FILESYSTEM_PATHS
#else
#include <ament_index_cpp/get_package_share_directory.hpp>
#endif
#include <rclcpp_components/register_node_macro.hpp>
#if __has_include(<rclcpp/version.h>)
#include <rclcpp/version.h>
#define ORBBEC_RCLCPP_HANDLES_SIGTERM \
((RCLCPP_VERSION_MAJOR > 13) || (RCLCPP_VERSION_MAJOR == 13 && RCLCPP_VERSION_MINOR >= 1))
#else
#define ORBBEC_RCLCPP_HANDLES_SIGTERM 0
#endif
#include <rcutils/logging.h>
#include <csignal>
#include <sys/mman.h>
@@ -39,6 +51,24 @@ std::string g_time_domain = "global"; // Assuming this is declared elsew
namespace {
constexpr auto kStreamStartDelayAfterReconnect = std::chrono::seconds(5);
std::filesystem::path getPackageSharePath(const std::string &package_name) {
#ifdef ORBBEC_AMENT_INDEX_USES_FILESYSTEM_PATHS
return ament_index_cpp::get_package_share_path(package_name);
#else
return ament_index_cpp::get_package_share_directory(package_name);
#endif
}
std::filesystem::path getPackagePrefixPath(const std::string &package_name) {
#ifdef ORBBEC_AMENT_INDEX_USES_FILESYSTEM_PATHS
std::filesystem::path package_prefix;
ament_index_cpp::get_package_prefix(package_name, package_prefix);
return package_prefix;
#else
return ament_index_cpp::get_package_prefix(package_name);
#endif
}
std::string getLogDirectoryForCamera(const std::string &camera_name) {
const char *log_dir_override = std::getenv("ORBBEC_LOG_DIR");
if (log_dir_override && log_dir_override[0] != '\0') {
@@ -71,65 +101,56 @@ std::string makeDefaultSdkLogFileName() {
}
} // namespace
void signalHandler(int sig) {
// Prevent recursive signal handling
void crashSignalHandler(int sig) {
// Prevent recursive crash signal handling.
static std::atomic<bool> in_signal_handler{false};
if (in_signal_handler.exchange(true)) {
// Already in signal handler, force exit immediately
_exit(sig);
}
std::cerr << "Received signal: " << sig << std::endl;
if (sig == SIGINT || sig == SIGTERM) {
static int signal_count = 0;
signal_count++;
std::filesystem::path log_dir = getLogDirectoryForCamera(g_camera_name);
if (signal_count <= 3) {
rclcpp::shutdown();
// Give some time for graceful shutdown
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
} else if (signal_count >= 5) {
// Force exit after second signal
std::cout << "Force exit due to multiple signals" << std::endl;
_exit(sig);
}
in_signal_handler.store(false);
} else {
std::filesystem::path log_dir = getLogDirectoryForCamera(g_camera_name);
// get current time
std::time_t now = std::time(nullptr);
std::tm *local_time = std::localtime(&now);
// get current time
std::time_t now = std::time(nullptr);
std::tm *local_time = std::localtime(&now);
// format date and time, format "2024_05_20_12_34_56"
std::ostringstream time_stream;
time_stream << std::put_time(local_time, "%Y_%m_%d_%H_%M_%S");
// format date and time to string, format as "2024_05_20_12_34_56"
std::ostringstream time_stream;
time_stream << std::put_time(local_time, "%Y_%m_%d_%H_%M_%S");
// generate log file name
std::string log_file_name = g_camera_name + "_crash_stack_trace_" + time_stream.str() + ".log";
std::filesystem::path log_file_path = log_dir / log_file_name;
// generate log file name
std::string log_file_name = g_camera_name + "_crash_stack_trace_" + time_stream.str() + ".log";
std::filesystem::path log_file_path = log_dir / log_file_name;
if (!std::filesystem::exists(log_dir)) {
std::filesystem::create_directories(log_dir);
}
std::cerr << "Log crash stack trace to " << log_file_path.string() << std::endl;
std::ofstream log_file(log_file_path, std::ios::app);
if (log_file.is_open()) {
log_file << "Received signal: " << sig << std::endl;
backward::StackTrace st;
st.load_here(32); // Capture stack
backward::Printer p;
p.print(st, log_file); // Print stack to log file
}
log_file.close();
_exit(sig); // Use _exit instead of exit to avoid cleanup that may crash
if (!std::filesystem::exists(log_dir)) {
std::filesystem::create_directories(log_dir);
}
std::cerr << "Log crash stack trace to " << log_file_path.string() << std::endl;
std::ofstream log_file(log_file_path, std::ios::app);
if (log_file.is_open()) {
log_file << "Received signal: " << sig << std::endl;
backward::StackTrace st;
st.load_here(32); // Capture stack
backward::Printer p;
p.print(st, log_file); // Print stack to log file
}
log_file.close();
_exit(sig); // Use _exit instead of exit to avoid cleanup that may crash
}
#if !ORBBEC_RCLCPP_HANDLES_SIGTERM
void forwardSigtermToRclcpp(int) {
// Older rclcpp versions such as Foxy's only handle SIGINT. Forward SIGTERM to that signal-safe
// shutdown path instead of calling rclcpp::shutdown() directly from this signal handler.
kill(getpid(), SIGINT);
}
#endif
namespace orbbec_camera {
backward::SignalHandling OBCameraNodeDriver::sh;
@@ -153,10 +174,10 @@ int rosLogSeverityFromString(const std::string_view &log_level) {
OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
: Node("orbbec_camera_node", "/", node_options),
node_options_(node_options),
config_path_(ament_index_cpp::get_package_share_directory("orbbec_camera") +
"/config/OrbbecSDKConfig_v2.0.xml"),
config_path_(
(getPackageSharePath("orbbec_camera") / "config" / "OrbbecSDKConfig_v2.0.xml").string()),
logger_(this->get_logger()),
extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") + "/lib/extensions") {
extension_path_((getPackagePrefixPath("orbbec_camera") / "lib" / "extensions").string()) {
node_name_ = "orbbec_camera_node";
init();
}
@@ -165,10 +186,10 @@ OBCameraNodeDriver::OBCameraNodeDriver(const std::string &node_name, const std::
const rclcpp::NodeOptions &node_options)
: Node(node_name, ns, node_options),
node_options_(node_options),
config_path_(ament_index_cpp::get_package_share_directory("orbbec_camera") +
"/config/OrbbecSDKConfig_v2.0.xml"),
config_path_(
(getPackageSharePath("orbbec_camera") / "config" / "OrbbecSDKConfig_v2.0.xml").string()),
logger_(this->get_logger()),
extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") + "/lib/extensions") {
extension_path_((getPackagePrefixPath("orbbec_camera") / "lib" / "extensions").string()) {
node_name_ = node_name;
init();
}
@@ -252,16 +273,22 @@ OBCameraNodeDriver::~OBCameraNodeDriver() {
orb_device_lock_shm_fd_ = -1;
}
shm_unlink(ORB_DEFAULT_LOCK_NAME.c_str());
clearGlobalImageTransportPublishers(*this);
}
void OBCameraNodeDriver::init() {
// Set signal handlers for crash reporting
signal(SIGSEGV, signalHandler); // segment fault
signal(SIGABRT, signalHandler); // abort
signal(SIGFPE, signalHandler); // float point exception
signal(SIGILL, signalHandler); // illegal instruction
signal(SIGINT, signalHandler);
signal(SIGTERM, signalHandler);
// Keep shutdown signals managed by rclcpp. Overriding them from a composable node bypasses its
// deferred signal handling and can leave SDK streaming threads running after the ROS context has
// already been shut down.
#if !ORBBEC_RCLCPP_HANDLES_SIGTERM
// Older rclcpp versions such as Foxy's predate native SIGTERM handling, so translate it to the
// SIGINT path that rclcpp does manage. Newer distributions handle both signals themselves.
signal(SIGTERM, forwardSigtermToRclcpp);
#endif
signal(SIGSEGV, crashSignalHandler); // segment fault
signal(SIGABRT, crashSignalHandler); // abort
signal(SIGFPE, crashSignalHandler); // float point exception
signal(SIGILL, crashSignalHandler); // illegal instruction
ob::Context::setExtensionsDirectory(extension_path_.c_str());
g_camera_name = declare_parameter<std::string>("camera_name", g_camera_name);
auto log_level_str = declare_parameter<std::string>("log_level", "info");
@@ -1575,7 +1602,7 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
}
auto pid = device->getDeviceInfo()->getPid();
if (GEMINI_335LG_PID == pid || GEMINI_338LG_PID == pid) {
if (isGmslCameraPID(pid)) {
ob_camera_node_->startGmslTrigger();
}
// if (isGemini305SeriesPID(pid)) {
orbbec_camera/src/ob_lidar_node.cpp
+3 -2
View File
@@ -18,6 +18,7 @@
#include <rclcpp/rclcpp.hpp>
#include <thread>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <magic_enum/magic_enum.hpp>
#include "orbbec_camera/utils.h"
#include <filesystem>
@@ -1164,8 +1165,8 @@ void OBLidarNode::publishStaticTransforms() {
if (!publish_tf_) {
return;
}
static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_);
dynamic_tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*node_);
dynamic_tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*node_);
calcAndPublishStaticTransform();
if (tf_publish_rate_ > 0) {
tf_thread_ = std::make_shared<std::thread>([this]() { publishDynamicTransforms(); });
orbbec_camera/src/rk_mpp_decoder.cpp
-1
View File
@@ -16,7 +16,6 @@
#include "orbbec_camera/rk_mpp_decoder.h"
#include <rclcpp/rclcpp.hpp>
#include <magic_enum/magic_enum.hpp>
namespace orbbec_camera {
orbbec_camera/src/ros_service.cpp
+72 -1
View File
@@ -121,6 +121,11 @@ std::string OBSyncModeToString(const OBMultiDeviceSyncMode& mode) {
void OBCameraNode::setupCameraCtrlServices() {
using std_srvs::srv::SetBool;
get_color_queue_stats_srv_ = node_->create_service<SetBool>(
"get_color_queue_stats", [this](const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
getColorQueueStatsCallback(request, response);
});
for (auto stream_index : IMAGE_STREAMS) {
if (!enable_stream_[stream_index]) {
continue;
@@ -458,6 +463,69 @@ void OBCameraNode::setupCameraCtrlServices() {
}
}
void OBCameraNode::getColorQueueStatsCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
try {
const auto to_json = [](const ColorQueueStatsSnapshot& stats) {
return nlohmann::json{
{"capacity_frames", stats.capacity_frames},
{"queue_size", stats.queue_size},
{"max_queue_size", stats.max_queue_size},
{"overflow_count", stats.overflow_count},
{"oldest_queue_wait_ms", stats.oldest_queue_wait_ms},
{"max_queue_wait_ms", stats.max_queue_wait_ms},
};
};
nlohmann::json queues = nlohmann::json::object();
uint64_t overflow_count = 0;
const bool reset = request->data;
if (enable_stream_[COLOR] || reset) {
const auto stats =
getColorQueueStats(color_frame_queue_, color_frame_queue_lock_, color_frame_queue_stats_,
color_frame_queue_max_frames_, reset);
if (enable_stream_[COLOR]) {
queues["color"] = to_json(stats);
overflow_count += stats.overflow_count;
}
}
if (enable_stream_[COLOR_LEFT] || reset) {
const auto stats = getColorQueueStats(left_color_frame_queue_, left_color_frame_queue_lock_,
left_color_frame_queue_stats_,
left_color_frame_queue_max_frames_, reset);
if (enable_stream_[COLOR_LEFT]) {
queues["left_color"] = to_json(stats);
overflow_count += stats.overflow_count;
}
}
if (enable_stream_[COLOR_RIGHT] || reset) {
const auto stats = getColorQueueStats(right_color_frame_queue_, right_color_frame_queue_lock_,
right_color_frame_queue_stats_,
right_color_frame_queue_max_frames_, reset);
if (enable_stream_[COLOR_RIGHT]) {
queues["right_color"] = to_json(stats);
overflow_count += stats.overflow_count;
}
}
response->success = true;
response->message =
nlohmann::json{
{"namespace", node_->get_namespace()},
{"overflow_count", overflow_count},
{"statistics_reset", reset},
{"queues", queues},
}
.dump();
if (queues.empty()) {
RCLCPP_WARN(logger_, "No enabled color streams; color queue statistics are empty");
}
} catch (const std::exception& error) {
response->success = false;
response->message = error.what();
}
}
void OBCameraNode::getPointCloudDecimationCallback(
const std::shared_ptr<GetInt32::Request>& request,
std::shared_ptr<GetInt32::Response>& response) {
@@ -1942,6 +2010,8 @@ bool OBCameraNode::toggleSensor(const stream_index_pair& stream_index, bool enab
try {
const bool interleave_frame_enable = interleave_frame_enable_;
stopStreams();
RCLCPP_DEBUG_STREAM(logger_, "Wait 1 second for streams to stop before toggling sensor");
std::this_thread::sleep_for(std::chrono::seconds(1));
interleave_frame_enable_ = interleave_frame_enable;
stopColorFrameThreads();
clearColorFrameQueues();
@@ -1965,10 +2035,11 @@ void OBCameraNode::saveImageCallback(const std::shared_ptr<std_srvs::srv::Empty:
std::shared_ptr<std_srvs::srv::Empty::Response>& response) {
(void)request;
(void)response;
std::lock_guard<std::mutex> lock(save_images_mutex_);
for (const auto& stream_index : IMAGE_STREAMS) {
if (enable_stream_[stream_index]) {
save_images_[stream_index] = true;
save_images_count_[stream_index] = 0;
save_images_[stream_index].store(true, std::memory_order_release);
}
}
}
orbbec_camera/src/utils.cpp
+30 -6
View File
@@ -23,6 +23,7 @@
#include <regex>
#include <sstream>
#include <vector>
#include <magic_enum/magic_enum.hpp>
#include "orbbec_camera/utils.h"
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include "orbbec_camera/constants.h"
@@ -566,6 +567,29 @@ rmw_qos_profile_t getRMWQosProfileFromString(const std::string &str_qos) {
}
}
std::string getRMWQosProfileDescription(const rmw_qos_profile_t &qos_profile) {
const auto short_qos_name = [](auto policy) {
auto name = magic_enum::enum_name(policy);
constexpr size_t prefix_size = sizeof("RMW_QOS_POLICY_") - 1;
if (name.size() <= prefix_size) {
return name;
}
name.remove_prefix(prefix_size);
const auto separator = name.find('_');
if (separator < name.size()) {
name.remove_prefix(separator + 1);
}
return name;
};
std::string history(short_qos_name(qos_profile.history));
if (qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_LAST) {
history += "(" + std::to_string(qos_profile.depth) + ")";
}
return std::string(short_qos_name(qos_profile.reliability)) + "/" +
std::string(short_qos_name(qos_profile.durability)) + "/" + history;
}
bool isOpenNIDevice(int pid) {
static const std::vector<int> OPENNI_DEVICE_PIDS = {
0x0300, 0x0301, 0x0400, 0x0401, 0x0402, 0x0403, 0x0404, 0x0407, 0x0601, 0x060b, 0x060e,
@@ -718,17 +742,17 @@ OB_SAMPLE_RATE sampleRateFromString(std::string &sample_rate) {
return OB_SAMPLE_RATE_200_HZ;
} else if (sample_rate == "500hz") {
return OB_SAMPLE_RATE_500_HZ;
} else if (sample_rate == "1khz") {
} else if (sample_rate == "1khz" || sample_rate == "1000hz") {
return OB_SAMPLE_RATE_1_KHZ;
} else if (sample_rate == "2khz") {
} else if (sample_rate == "2khz" || sample_rate == "2000hz") {
return OB_SAMPLE_RATE_2_KHZ;
} else if (sample_rate == "4khz") {
} else if (sample_rate == "4khz" || sample_rate == "4000hz") {
return OB_SAMPLE_RATE_4_KHZ;
} else if (sample_rate == "8khz") {
} else if (sample_rate == "8khz" || sample_rate == "8000hz") {
return OB_SAMPLE_RATE_8_KHZ;
} else if (sample_rate == "16khz") {
} else if (sample_rate == "16khz" || sample_rate == "16000hz") {
return OB_SAMPLE_RATE_16_KHZ;
} else if (sample_rate == "32khz") {
} else if (sample_rate == "32khz" || sample_rate == "32000hz") {
return OB_SAMPLE_RATE_32_KHZ;
} else {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("utils"), "Unknown OB_SAMPLE_RATE: " << sample_rate);