i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-07 21:37: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-07-17 09:48:30 +08:00
parent cb24b007cd 7f5d0e7356
commit ce08bce25f
198 changed files with 11634 additions and 3850 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/dynamic_params.cpp
+14 -14
View File
@@ -1,18 +1,18 @@
/*******************************************************************************
* Copyright (c) 2023 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
* Copyright (c) 2023 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
#include "orbbec_camera/dynamic_params.h"
namespace orbbec_camera {
orbbec_camera/src/frame_timestamp_csv_logger.cpp
+131 -63
View File
@@ -1,5 +1,6 @@
#include "orbbec_camera/frame_timestamp_csv_logger.h"
#include <algorithm>
#include <chrono>
#include <filesystem>
#include <iomanip>
@@ -13,34 +14,64 @@ constexpr size_t kCompletedQueueSoftLimit = 1000;
constexpr size_t kFlushBatchSize = 100;
constexpr auto kFlushInterval = std::chrono::seconds(1);
int64_t getExpectedIntervalUs(const std::shared_ptr<ob::Frame> &frame) {
if (!frame || !frame->is<ob::VideoFrame>()) {
return 0;
}
auto stream_profile = frame->getStreamProfile();
if (!stream_profile || !stream_profile->is<ob::VideoStreamProfile>()) {
return 0;
}
const auto fps = stream_profile->as<ob::VideoStreamProfile>()->getFps();
if (fps == 0) {
return 0;
}
return static_cast<int64_t>(1000000.0 / static_cast<double>(fps));
}
} // namespace
FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool enabled, const std::string &csv_file_path,
FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
const std::string &csv_file_path,
rclcpp::Logger logger)
: logger_(std::move(logger)), enabled_(enabled), csv_file_path_(csv_file_path) {
: logger_(std::move(logger)),
enabled_(drop_log_enabled || !csv_file_path.empty()),
csv_enabled_(!csv_file_path.empty()),
drop_log_enabled_(drop_log_enabled),
csv_file_path_(csv_file_path) {
if (!enabled_) {
return;
}
try {
auto path = std::filesystem::path(csv_file_path_);
if (path.has_parent_path() && !std::filesystem::exists(path.parent_path())) {
std::filesystem::create_directories(path.parent_path());
if (csv_enabled_) {
try {
auto path = std::filesystem::path(csv_file_path_);
if (path.has_parent_path() && !std::filesystem::exists(path.parent_path())) {
std::filesystem::create_directories(path.parent_path());
}
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to prepare frame timestamp CSV path "
<< csv_file_path_ << ": " << e.what());
csv_enabled_ = false;
csv_writer_failed_ = true;
}
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to prepare frame timestamp CSV path " << csv_file_path_
<< ": " << e.what());
enabled_ = false;
writer_failed_ = true;
return;
}
openCsvIfNeeded();
if (!enabled_) {
return;
if (csv_enabled_) {
openCsvIfNeeded();
}
writer_thread_ = std::thread([this]() { writerThreadMain(); });
enabled_ = csv_enabled_ || drop_log_enabled_;
if (csv_enabled_) {
writer_thread_ = std::thread([this]() { writerThreadMain(); });
}
if (enabled_) {
RCLCPP_INFO_STREAM(logger_,
"Frame timestamp logger enabled: csv_file="
<< (csv_enabled_ ? csv_file_path_ : "disabled")
<< " frame_drop_log=" << (drop_log_enabled_ ? "enabled" : "disabled"));
}
}
FrameTimestampCsvLogger::~FrameTimestampCsvLogger() noexcept { shutdown(); }
@@ -51,7 +82,7 @@ void FrameTimestampCsvLogger::recordFrameSet(const std::shared_ptr<ob::Frame> &c
bool track_color, bool track_depth,
bool color_image_publish_expected,
bool depth_image_publish_expected) {
if (!enabled_ || writer_failed_) {
if (!enabled_) {
return;
}
recordFrameSetInternal(color_frame, depth_frame, arrival_system_us, arrival_steady_us,
@@ -64,7 +95,7 @@ void FrameTimestampCsvLogger::recordStandaloneFrameArrival(OBStreamType stream_t
int64_t arrival_system_us,
int64_t arrival_steady_us,
bool image_publish_expected) {
if (!enabled_ || writer_failed_ || !frame || !isTrackedStream(stream_type)) {
if (!enabled_ || !frame || !isTrackedStream(stream_type)) {
return;
}
recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us,
@@ -75,10 +106,18 @@ void FrameTimestampCsvLogger::recordPreImagePublish(OBStreamType stream_type,
const std::shared_ptr<ob::Frame> &frame,
int64_t publish_system_us,
int64_t publish_steady_us) {
if (!enabled_ || writer_failed_ || !frame || !isTrackedStream(stream_type)) {
if (!enabled_ || !frame || !isTrackedStream(stream_type)) {
return;
}
recordPreImagePublishInternal(stream_type, frame, publish_system_us, publish_steady_us);
completeImagePublishInternal(stream_type, frame, publish_system_us, publish_steady_us);
}
void FrameTimestampCsvLogger::recordImagePublishSkipped(OBStreamType stream_type,
const std::shared_ptr<ob::Frame> &frame) {
if (!enabled_ || !frame || !isTrackedStream(stream_type)) {
return;
}
completeImagePublishInternal(stream_type, frame, std::nullopt, std::nullopt);
}
void FrameTimestampCsvLogger::shutdown() {
@@ -226,10 +265,9 @@ void FrameTimestampCsvLogger::recordStandaloneFrameArrivalInternal(
}
}
void FrameTimestampCsvLogger::recordPreImagePublishInternal(OBStreamType stream_type,
const std::shared_ptr<ob::Frame> &frame,
int64_t publish_system_us,
int64_t publish_steady_us) {
void FrameTimestampCsvLogger::completeImagePublishInternal(
OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
std::optional<int64_t> publish_system_us, std::optional<int64_t> publish_steady_us) {
std::optional<PendingRow> ready_row;
const auto frame_index = frame->getIndex();
@@ -244,10 +282,12 @@ void FrameTimestampCsvLogger::recordPreImagePublishInternal(OBStreamType stream_
: depth_frame_index_to_row_id_;
auto row_id_it = row_map.find(frame_index);
if (row_id_it == row_map.end()) {
RCLCPP_WARN_STREAM(logger_,
"Frame timestamp CSV logger missed row mapping for stream "
<< (tracked_stream == TrackedStream::COLOR ? "color" : "depth")
<< " frame index " << frame_index);
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);
}
return;
}
const auto row_id = row_id_it->second;
@@ -259,8 +299,16 @@ void FrameTimestampCsvLogger::recordPreImagePublishInternal(OBStreamType stream_
auto &state = tracked_stream == TrackedStream::COLOR ? pending_it->second.color
: pending_it->second.depth;
populatePublishData(state, tracked_stream, publish_system_us, publish_steady_us);
state.final = true;
if (state.final) {
return;
}
if (publish_system_us.has_value() && publish_steady_us.has_value()) {
populatePublishData(state, tracked_stream, publish_system_us.value(),
publish_steady_us.value());
state.final = true;
} else {
finalizeStreamWithoutPublish(state);
}
if (isRowReady(pending_it->second)) {
ready_row = pending_it->second;
@@ -284,6 +332,26 @@ void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStr
state.has_frame = true;
state.publish_expected = publish_expected;
state.frame_index = frame->getIndex();
state.device_ts_us = static_cast<int64_t>(frame->getTimeStampUs());
if (previous.expected_interval_us <= 0) {
previous.expected_interval_us = getExpectedIntervalUs(frame);
}
state.expected_interval_us = previous.expected_interval_us;
if (previous.device_ts_us.has_value() && state.expected_interval_us > 0) {
const auto device_ts_delta_us = state.device_ts_us - previous.device_ts_us.value();
if (device_ts_delta_us > state.expected_interval_us * 3 / 2) {
const auto lost_frames =
std::max<int64_t>(1, device_ts_delta_us / state.expected_interval_us - 1);
if (drop_log_enabled_) {
previous.dropped_frames += lost_frames;
RCLCPP_WARN_STREAM(logger_,
"Frame drop detected: stage=SDK_RECEIVE"
<< " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth")
<< " frame_index=" << state.frame_index
<< " dropped=" << previous.dropped_frames);
}
}
}
if (frame->hasMetadata(OB_FRAME_METADATA_TYPE_FRAME_NUMBER)) {
state.metadata_frame_number =
static_cast<int64_t>(frame->getMetadataValue(OB_FRAME_METADATA_TYPE_FRAME_NUMBER));
@@ -296,7 +364,6 @@ void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStr
} else {
state.sensor_ts_us.reset();
}
state.device_ts_us = static_cast<int64_t>(frame->getTimeStampUs());
state.global_ts_us = static_cast<int64_t>(frame->getGlobalTimeStampUs());
state.sdk_system_ts_us = static_cast<int64_t>(frame->getSystemTimeStampUs());
state.arrival_system_us = arrival_system_us;
@@ -310,7 +377,7 @@ void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStr
}
state.global_ts_delta_us = updateDelta(previous.global_ts_us, state.global_ts_us);
state.sdk_system_ts_delta_us = updateDelta(previous.sdk_system_ts_us, state.sdk_system_ts_us);
state.arrival_system_delta_us = updateDelta(previous.arrival_system_us, state.arrival_system_us);
previous.arrival_system_us = state.arrival_system_us;
state.arrival_steady_delta_us = updateDelta(previous.arrival_steady_us, state.arrival_steady_us);
state.sdk_delay_from_global_us = state.arrival_system_us - state.global_ts_us;
state.sdk_delay_from_system_us = state.arrival_system_us - state.sdk_system_ts_us;
@@ -321,13 +388,28 @@ void FrameTimestampCsvLogger::populatePublishData(StreamState &state, TrackedStr
int64_t publish_steady_us) {
auto &previous = stream == TrackedStream::COLOR ? color_previous_ : depth_previous_;
if (previous.publish_device_ts_us.has_value()) {
const auto device_ts_delta_us = state.device_ts_us - previous.publish_device_ts_us.value();
if (state.expected_interval_us > 0 && device_ts_delta_us > state.expected_interval_us * 3 / 2) {
const auto lost_frames =
std::max<int64_t>(1, device_ts_delta_us / state.expected_interval_us - 1);
if (drop_log_enabled_) {
previous.publish_dropped_frames += lost_frames;
RCLCPP_WARN_STREAM(logger_,
"Frame drop detected: stage=ROS_PUBLISH"
<< " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth")
<< " frame_index=" << state.frame_index
<< " dropped=" << previous.publish_dropped_frames);
}
}
}
previous.publish_device_ts_us = state.device_ts_us;
state.publish_system_us = publish_system_us;
state.publish_steady_us = publish_steady_us;
state.publish_system_delta_us =
updateDelta(previous.publish_system_us, state.publish_system_us.value());
previous.publish_system_us = state.publish_system_us.value();
state.publish_steady_delta_us =
updateDelta(previous.publish_steady_us, state.publish_steady_us.value());
state.arrival_to_publish_system_us = state.publish_system_us.value() - state.arrival_system_us;
state.arrival_to_publish_steady_us = state.publish_steady_us.value() - state.arrival_steady_us;
}
@@ -350,6 +432,9 @@ bool FrameTimestampCsvLogger::isRowReady(const PendingRow &row) const {
}
void FrameTimestampCsvLogger::enqueueCompletedRow(const PendingRow &row) {
if (!csv_enabled_ || csv_writer_failed_) {
return;
}
std::lock_guard<std::mutex> queue_lock(completed_rows_mutex_);
completed_rows_.push_back(row);
if (completed_rows_.size() > kCompletedQueueSoftLimit) {
@@ -372,6 +457,8 @@ void FrameTimestampCsvLogger::flushPendingRowsLocked(std::vector<PendingRow> &ro
row.depth.final = true;
rows.push_back(std::move(row));
}
std::stable_sort(rows.begin(), rows.end(),
[](const auto &lhs, const auto &rhs) { return lhs.row_id < rhs.row_id; });
pending_rows_.clear();
color_frame_index_to_row_id_.clear();
depth_frame_index_to_row_id_.clear();
@@ -393,7 +480,7 @@ std::string FrameTimestampCsvLogger::serializeRow(const PendingRow &row) const {
}
std::string FrameTimestampCsvLogger::serializeStreamColumns(const StreamState &state) const {
std::vector<std::string> fields(22, "");
std::vector<std::string> fields(15, "");
if (state.has_frame) {
fields[0] = std::to_string(state.frame_index);
fields[1] = formatOptionalIntColumn(state.metadata_frame_number);
@@ -407,22 +494,11 @@ std::string FrameTimestampCsvLogger::serializeStreamColumns(const StreamState &s
fields[7] = formatOptionalIntColumn(state.global_ts_delta_us);
fields[8] = formatSecondsColumn(state.sdk_system_ts_us);
fields[9] = formatOptionalIntColumn(state.sdk_system_ts_delta_us);
fields[10] = formatSecondsColumn(state.arrival_system_us);
fields[11] = formatOptionalIntColumn(state.arrival_system_delta_us);
fields[12] = formatSecondsColumn(state.arrival_steady_us);
fields[13] = formatOptionalIntColumn(state.arrival_steady_delta_us);
if (state.publish_system_us.has_value()) {
fields[14] = formatSecondsColumn(state.publish_system_us.value());
}
fields[15] = formatOptionalIntColumn(state.publish_system_delta_us);
if (state.publish_steady_us.has_value()) {
fields[16] = formatSecondsColumn(state.publish_steady_us.value());
}
fields[17] = formatOptionalIntColumn(state.publish_steady_delta_us);
fields[18] = formatOptionalIntColumn(state.arrival_to_publish_system_us);
fields[19] = formatOptionalIntColumn(state.arrival_to_publish_steady_us);
fields[20] = formatOptionalIntColumn(state.sdk_delay_from_global_us);
fields[21] = formatOptionalIntColumn(state.sdk_delay_from_system_us);
fields[10] = formatOptionalIntColumn(state.arrival_steady_delta_us);
fields[11] = formatOptionalIntColumn(state.publish_steady_delta_us);
fields[12] = formatOptionalIntColumn(state.arrival_to_publish_steady_us);
fields[13] = formatOptionalIntColumn(state.sdk_delay_from_global_us);
fields[14] = formatOptionalIntColumn(state.sdk_delay_from_system_us);
}
std::ostringstream ss;
@@ -461,15 +537,8 @@ std::string FrameTimestampCsvLogger::csvHeader() {
ss << prefix << "_global_ts_delta_us,";
ss << prefix << "_system_ts_sec,";
ss << prefix << "_system_ts_delta_us,";
ss << prefix << "_arrival_system_sec,";
ss << prefix << "_arrival_system_delta_us,";
ss << prefix << "_arrival_steady_sec,";
ss << prefix << "_arrival_steady_delta_us,";
ss << prefix << "_publish_system_sec,";
ss << prefix << "_publish_system_delta_us,";
ss << prefix << "_publish_steady_sec,";
ss << prefix << "_publish_steady_delta_us,";
ss << prefix << "_arrival_to_publish_system_us,";
ss << prefix << "_arrival_to_publish_steady_us,";
ss << prefix << "_sdk_delay_from_global_us,";
ss << prefix << "_sdk_delay_from_system_us";
@@ -481,7 +550,7 @@ std::string FrameTimestampCsvLogger::csvHeader() {
}
void FrameTimestampCsvLogger::writerThreadMain() {
if (!enabled_ || writer_failed_) {
if (!csv_enabled_ || csv_writer_failed_) {
return;
}
@@ -524,13 +593,12 @@ 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_);
enabled_ = false;
writer_failed_ = true;
csv_enabled_ = false;
csv_writer_failed_ = true;
return;
}
csv_stream_ << csvHeader() << "\n";
csv_stream_.flush();
RCLCPP_INFO_STREAM(logger_, "Frame timestamp CSV logger enabled: " << csv_file_path_);
}
} // namespace orbbec_camera
orbbec_camera/src/jetson_nv_decoder.cpp
+14 -14
View File
@@ -1,18 +1,18 @@
/*******************************************************************************
* Copyright (c) 2023 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
* Copyright (c) 2023 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
#include "orbbec_camera/jetson_nv_decoder.h"
#include <NvJpegDecoder.h>
#include <NvV4l2Element.h>
orbbec_camera/src/jpeg_decoder.cpp
+14 -14
View File
@@ -1,18 +1,18 @@
/*******************************************************************************
* Copyright (c) 2023 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
* Copyright (c) 2023 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
#include <orbbec_camera/jpeg_decoder.h>
namespace orbbec_camera {
orbbec_camera/src/ob_camera_node.cpp
+3254 -403
View File
File diff suppressed because it is too large Load Diff
orbbec_camera/src/ob_camera_node_driver.cpp
+251 -17
View File
@@ -32,14 +32,43 @@
#include <fstream>
#include <iomanip> // For std::put_time
#include <malloc.h>
#include <sstream>
std::string g_camera_name = "orbbec_camera"; // Assuming this is declared elsewhere
std::string g_time_domain = "global"; // Assuming this is declared elsewhere
namespace {
constexpr auto kStreamStartDelayAfterReconnect = std::chrono::seconds(5);
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') {
return (std::filesystem::path(log_dir_override) / "Log" / camera_name).string();
}
std::string home_dir = std::getenv("HOME") ? std::getenv("HOME") : "";
return (std::filesystem::path(home_dir) / ".ros" / "Log" / camera_name).string();
}
std::string getDefaultBagRecordFilePath() {
const std::time_t now_time = std::time(nullptr);
std::tm tm{};
localtime_r(&now_time, &tm);
std::ostringstream time_stream;
time_stream << std::put_time(&tm, "%Y_%m_%d_%H_%M_%S");
return (std::filesystem::current_path() / ("orbbec_record_" + time_stream.str() + ".bag"))
.string();
}
std::string makeDefaultSdkLogFileName() {
const std::time_t now_time = std::time(nullptr);
std::tm tm{};
localtime_r(&now_time, &tm);
std::ostringstream file_name;
file_name << "OrbbecSDK_" << std::put_time(&tm, "%Y%m%d_%H%M%S") << ".log";
return file_name.str();
}
} // namespace
void signalHandler(int sig) {
@@ -50,7 +79,7 @@ void signalHandler(int sig) {
_exit(sig);
}
std::cout << "Received signal: " << sig << std::endl;
std::cerr << "Received signal: " << sig << std::endl;
if (sig == SIGINT || sig == SIGTERM) {
static int signal_count = 0;
signal_count++;
@@ -84,7 +113,7 @@ void signalHandler(int sig) {
std::filesystem::create_directories(log_dir);
}
std::cout << "Log crash stack trace to " << log_file_path.string() << std::endl;
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()) {
@@ -147,6 +176,13 @@ OBCameraNodeDriver::OBCameraNodeDriver(const std::string &node_name, const std::
OBCameraNodeDriver::~OBCameraNodeDriver() {
is_alive_.store(false);
// Finalize bag recording before the pipeline is torn down, otherwise the
// bag file can end up truncated/corrupted.
if (record_device_) {
record_device_.reset();
RCLCPP_INFO_STREAM(logger_, "Bag recording stopped");
}
// First stop the camera node cleanly before stopping threads
if (ob_camera_node_) {
try {
@@ -228,7 +264,7 @@ void OBCameraNodeDriver::init() {
signal(SIGTERM, signalHandler);
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", "none");
auto log_level_str = declare_parameter<std::string>("log_level", "info");
auto log_level = obLogSeverityFromString(log_level_str);
auto ros_log_level = rosLogSeverityFromString(log_level_str);
auto log_file_name = declare_parameter<std::string>("log_file_name", "");
@@ -242,16 +278,30 @@ void OBCameraNodeDriver::init() {
RCLCPP_WARN_STREAM(logger_, "Failed to set ROS log level to " << log_level_str);
}
}
// Set custom log file name if specified
if (!log_file_name.empty()) {
try {
ob::Context::setLoggerFileName(log_file_name);
RCLCPP_INFO_STREAM(logger_, "SDK log file name set to: " << log_file_name);
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_, "Failed to set SDK log file name: "
<< orbbec_camera::formatObErrorWithStatus(e));
}
if (log_file_name.empty()) {
log_file_name = makeDefaultSdkLogFileName();
}
try {
ob::Context::setLoggerFileName(log_file_name);
RCLCPP_INFO_STREAM(logger_, "SDK log file path set to: "
<< (std::filesystem::path(log_path) / log_file_name).string());
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(
logger_, "Failed to set SDK log file name: " << orbbec_camera::formatObErrorWithStatus(e));
}
// Bag file playback mode: load a previously recorded .bag file as a virtual
// device instead of enumerating real hardware. Must be checked before the
// ob::Context / device discovery machinery is set up below.
device_type_ = declare_parameter<std::string>("device_type", "camera");
bag_filename_ = declare_parameter<std::string>("bag_filename", "");
bag_loop_ = declare_parameter<bool>("bag_loop", false);
if (!bag_filename_.empty()) {
is_alive_.store(true);
parameters_ = std::make_shared<Parameters>(this);
initializeBagPlayback();
return;
}
// Force IP
force_ip_enable_ = declare_parameter<bool>("force_ip_enable", false);
force_ip_mac_ = declare_parameter<std::string>("force_ip_mac", "");
@@ -263,9 +313,21 @@ void OBCameraNodeDriver::init() {
} else {
ctx_ = std::make_unique<ob::Context>(config_path_.c_str());
}
timestamp_clock_type_str_ = declare_parameter<std::string>("timestamp_clock_type", "");
if (!timestamp_clock_type_str_.empty()) {
auto timestamp_clock_type = timestampClockTypeFromString(timestamp_clock_type_str_);
try {
ctx_->setTimestampClockType(timestamp_clock_type);
auto actual_timestamp_clock_type = ctx_->getTimestampClockType();
RCLCPP_INFO_STREAM(logger_, "Set timestamp clock type to "
<< timestampClockTypeToString(actual_timestamp_clock_type));
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_, "Failed to set SDK timestamp clock type: "
<< orbbec_camera::formatObErrorWithStatus(e));
}
}
applyForceIpConfig();
device_type_ = declare_parameter<std::string>("device_type", "camera");
connection_delay_ = static_cast<int>(declare_parameter<int>("connection_delay", 100));
enable_sync_host_time_ = declare_parameter<bool>("enable_sync_host_time", true);
double time_sync_period = declare_parameter<double>("time_sync_period", 60.0);
@@ -294,6 +356,9 @@ void OBCameraNodeDriver::init() {
reboot_device_srv_ = this->create_service<std_srvs::srv::Empty>(
"reboot_device", std::bind(&OBCameraNodeDriver::rebootDeviceCallback, this,
std::placeholders::_1, std::placeholders::_2));
set_bag_recording_srv_ = this->create_service<orbbec_camera_msgs::srv::SetBagRecording>(
"set_bag_recording", std::bind(&OBCameraNodeDriver::setBagRecordingCallback, this,
std::placeholders::_1, std::placeholders::_2));
pthread_mutexattr_init(&orb_device_lock_attr_);
pthread_mutexattr_setpshared(&orb_device_lock_attr_, PTHREAD_PROCESS_SHARED);
orb_device_lock_ = (pthread_mutex_t *)orb_device_lock_shm_addr_;
@@ -303,6 +368,8 @@ void OBCameraNodeDriver::init() {
last_reset_device_completion_time_ = std::chrono::steady_clock::now() - std::chrono::seconds(10);
parameters_ = std::make_shared<Parameters>(this);
serial_number_ = declare_parameter<std::string>("serial_number", "");
bag_record_filename_ = declare_parameter<std::string>("bag_record_filename", "");
bag_record_compression_ = declare_parameter<bool>("bag_record_compression", true);
device_num_ = static_cast<int>(declare_parameter<int>("device_num", 1));
usb_port_ = declare_parameter<std::string>("usb_port", "");
net_device_ip_ = declare_parameter<std::string>("net_device_ip", "");
@@ -410,6 +477,7 @@ void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceLi
if (uid == device_unique_id_ || serial_number_ == serial_number) {
RCLCPP_INFO_STREAM(logger_,
"device with " << uid << " disconnected, notify reset device thread");
delay_stream_start_after_reconnect_ = true;
reset_device_flag_ = true;
reset_device_cond_.notify_all();
break;
@@ -534,6 +602,12 @@ void OBCameraNodeDriver::resetDevice() {
device_connected_ = false;
device_connecting_ = false; // Clear connecting flag
// Stop recording before tearing down the pipeline so the bag file is finalized
if (record_device_) {
record_device_.reset();
RCLCPP_WARN_STREAM(logger_, "Device disconnected, bag recording stopped");
}
// Reset objects in order, with additional safety checks
if (ob_camera_node_) {
ob_camera_node_.reset();
@@ -750,6 +824,56 @@ void OBCameraNodeDriver::deviceStatusTimer() {
}
// RCLCPP_INFO_STREAM(logger_, "deviceStatusTimer() ");
}
void OBCameraNodeDriver::setBagRecordingCallback(
const std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Request> request,
std::shared_ptr<orbbec_camera_msgs::srv::SetBagRecording::Response> response) {
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
if (!device_) {
response->success = false;
response->message = "No device connected";
return;
}
if (!request->enable) {
if (!record_device_) {
response->success = true;
response->message = "Bag recording is not running";
return;
}
record_device_.reset();
RCLCPP_INFO_STREAM(logger_, "Bag recording stopped");
response->success = true;
response->message = "Bag recording stopped";
return;
}
std::string file_path =
request->file_path.empty() ? getDefaultBagRecordFilePath() : request->file_path;
if (record_device_) {
record_device_.reset();
RCLCPP_INFO_STREAM(logger_, "Bag recording stopped before starting a new recording");
}
try {
exportBagPresetJson(file_path);
record_device_ =
std::make_shared<ob::RecordDevice>(device_, file_path, bag_record_compression_);
} catch (const ob::Error &e) {
response->success = false;
response->message = "Failed to start recording: " + orbbec_camera::formatObErrorWithStatus(e);
RCLCPP_ERROR_STREAM(logger_, response->message);
return;
}
RCLCPP_INFO_STREAM(logger_, "Recording to " << file_path);
response->success = true;
response->message = "Recording started: " + file_path;
}
void OBCameraNodeDriver::rebootDeviceCallback(
const std::shared_ptr<std_srvs::srv::Empty::Request> request,
std::shared_ptr<std_srvs::srv::Empty::Response> response) {
@@ -784,6 +908,7 @@ void OBCameraNodeDriver::rebootDeviceCallback(
} else {
std::string current_device_uid = device_unique_id_;
RCLCPP_INFO_STREAM(logger_, "Rebooting device with UID: " << current_device_uid);
delay_stream_start_after_reconnect_ = true;
if (ob_lidar_node_) {
ob_lidar_node_->rebootDevice();
} else if (ob_camera_node_) {
@@ -951,6 +1076,67 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
return nullptr;
}
void OBCameraNodeDriver::exportBagPresetJson(const std::string &bag_path) {
if (!device_ || bag_path.empty() || playback_device_) {
return;
}
auto json_path = std::filesystem::path(bag_path);
if (json_path.extension() == ".bag") {
json_path.replace_extension(".json");
} else {
json_path += ".json";
}
const auto json_path_str = json_path.string();
try {
const auto parent_path = json_path.parent_path();
if (!parent_path.empty()) {
std::filesystem::create_directories(parent_path);
}
device_->exportSettingsAsPresetJsonFile(json_path_str.c_str());
RCLCPP_INFO_STREAM(logger_, "Exported bag preset JSON: " << json_path_str);
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_, "Failed to export bag preset JSON "
<< json_path_str << ": "
<< orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_WARN_STREAM(logger_,
"Failed to export bag preset JSON " << json_path_str << ": " << e.what());
}
}
void OBCameraNodeDriver::initializeBagPlayback() {
RCLCPP_INFO_STREAM(logger_, "Starting bag file playback: " << bag_filename_);
try {
playback_device_ = std::make_shared<ob::PlaybackDevice>(bag_filename_);
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_,
"Failed to open bag file: " << orbbec_camera::formatObErrorWithStatus(e));
return;
}
if (bag_loop_) {
playback_device_->setPlaybackStatusChangeCallback([this](OBPlaybackStatus status) {
if (status == OB_PLAYBACK_STOPPED && is_alive_) {
RCLCPP_INFO_STREAM(logger_, "Bag playback completed, restarting from beginning...");
try {
playback_device_->seek(0);
if (ob_camera_node_) {
ob_camera_node_->restartPlaybackStreams();
}
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_, "Failed to restart bag playback: "
<< orbbec_camera::formatObErrorWithStatus(e));
}
}
});
}
std::shared_ptr<ob::Device> device = playback_device_;
initializeDevice(device);
}
void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &device) {
device_ = device;
updatePresetFirmware(preset_firmware_path_);
@@ -971,7 +1157,8 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
try {
if (device_type_ == "camera") {
ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_,
node_options_.use_intra_process_comms());
node_options_.use_intra_process_comms(),
playback_device_ != nullptr);
} else if (device_type_ == "lidar") {
ob_lidar_node_ = std::make_unique<orbbec_lidar::OBLidarNode>(
this, device_, parameters_, node_options_.use_intra_process_comms());
@@ -1006,7 +1193,8 @@ 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") {
if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid()) && device_type_ == "camera" &&
!playback_device_) {
TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
if (g_time_domain != "global") {
device_->enableGlobalTimestamp(false);
@@ -1082,8 +1270,8 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
}
} catch (ob::Error &e) {
// Some devices don't support ISP firmware version query
RCLCPP_DEBUG_STREAM(
logger_, "Current device not support ISP firmware version query: " << e.getMessage());
RCLCPP_DEBUG_STREAM(logger_, "Current device not support ISP firmware version query: "
<< orbbec_camera::formatObErrorWithStatus(e));
}
RCLCPP_INFO_STREAM(logger_, "usb connect type: " << device_info_->getConnectionType());
});
@@ -1145,6 +1333,12 @@ 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());
if (should_delay_stream_start) {
std::this_thread::sleep_for(kStreamStartDelayAfterReconnect);
}
if (ob_camera_node_) {
ob_camera_node_->startIMU();
ob_camera_node_->startStreams();
@@ -1155,6 +1349,18 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
RCLCPP_WARN_STREAM(logger_, "Camera or LiDAR node is null after device initialization");
}
if (!bag_record_filename_.empty() && !record_device_) {
try {
exportBagPresetJson(bag_record_filename_);
record_device_ = std::make_shared<ob::RecordDevice>(device_, bag_record_filename_,
bag_record_compression_);
RCLCPP_INFO_STREAM(logger_, "Recording to bag file: " << bag_record_filename_);
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(
logger_, "Failed to start recording: " << orbbec_camera::formatObErrorWithStatus(e));
}
}
} // namespace orbbec_camera
bool OBCameraNodeDriver::applyForceIpConfig() {
@@ -1543,6 +1749,7 @@ void OBCameraNodeDriver::firmwareUpdateCallback(OBFwUpdateState state, const cha
RCLCPP_WARN_STREAM(logger_, "Exception during sync timer cleanup in firmware update");
}
}
delay_stream_start_after_reconnect_ = true;
device_->reboot();
} else if (ob_lidar_node_) {
ob_lidar_node_.reset();
@@ -1577,6 +1784,33 @@ OBDeviceAccessMode OBCameraNodeDriver::stringToAccessMode(const std::string &mod
}
}
OBClockType OBCameraNodeDriver::timestampClockTypeFromString(const std::string &clock_type_str) {
std::string lower_type;
std::transform(clock_type_str.begin(), clock_type_str.end(), std::back_inserter(lower_type),
[](auto ch) { return tolower(ch); });
if (lower_type == "realtime") {
return OB_CLOCK_TYPE_REALTIME;
}
if (lower_type == "monotonic") {
return OB_CLOCK_TYPE_MONOTONIC;
}
RCLCPP_WARN_STREAM(logger_,
"Unknown timestamp_clock_type: " << clock_type_str << ", using realtime");
return OB_CLOCK_TYPE_REALTIME;
}
std::string OBCameraNodeDriver::timestampClockTypeToString(OBClockType clock_type) {
switch (clock_type) {
case OB_CLOCK_TYPE_MONOTONIC:
return "monotonic";
case OB_CLOCK_TYPE_REALTIME:
default:
return "realtime";
}
}
std::string OBCameraNodeDriver::accessModeToString(OBDeviceAccessMode mode) {
switch (mode) {
case OB_DEVICE_EXCLUSIVE_ACCESS:
orbbec_camera/src/ob_lidar_node.cpp
+6 -5
View File
@@ -296,8 +296,9 @@ void OBLidarNode::setupProfiles() {
}
} catch (const ob::Error &ex) {
RCLCPP_ERROR_STREAM(
logger_, "Failed to get " << stream_name_[elem] << " profile: " << ex.getMessage());
RCLCPP_ERROR_STREAM(logger_, "Failed to get "
<< stream_name_[elem] << " profile: "
<< orbbec_camera::formatObErrorWithStatus(ex));
RCLCPP_ERROR_STREAM(logger_, "Stream: " << magic_enum::enum_name(elem.first)
<< ", Stream Index: " << elem.second
<< ", Scan Rate: " << rate_[elem]
@@ -1261,9 +1262,9 @@ void OBLidarNode::calcAndPublishStaticTransform() {
ex = base_stream_profile->getExtrinsicTo(stream_profile_[GYRO]);
RCLCPP_INFO_STREAM(logger_, "Using GYRO extrinsic for IMU");
} catch (const ob::Error &e2) {
RCLCPP_ERROR_STREAM(
logger_, "Failed to get "
<< frame_id << " extrinsic from both ACCEL and GYRO: " << e2.getMessage());
RCLCPP_ERROR_STREAM(logger_, "Failed to get "
<< frame_id << " extrinsic from both ACCEL and GYRO: "
<< orbbec_camera::formatObErrorWithStatus(e2));
ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
}
}
orbbec_camera/src/rk_mpp_decoder.cpp
+14 -14
View File
@@ -1,18 +1,18 @@
/*******************************************************************************
* Copyright (c) 2023 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
* Copyright (c) 2023 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
#include "orbbec_camera/rk_mpp_decoder.h"
#include <rclcpp/rclcpp.hpp>
orbbec_camera/src/ros_service.cpp
+677 -88
View File
File diff suppressed because it is too large Load Diff
orbbec_camera/src/synced_imu_publisher.cpp
+14 -14
View File
@@ -1,18 +1,18 @@
/*******************************************************************************
* Copyright (c) 2023 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
* Copyright (c) 2023 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
#include "orbbec_camera/utils.h"
#include "orbbec_camera/synced_imu_publisher.h"
#include <rclcpp/rclcpp.hpp>
orbbec_camera/src/utils.cpp
+12 -9
View File
@@ -46,7 +46,11 @@ OBLogSeverity obLogSeverityFromString(const std::string_view &log_level) {
return OBLogSeverity::OB_LOG_SEVERITY_OFF;
}
std::string getRosLogDirectory() {
std::string getObSdkLogDirectory() {
const char *log_dir_override = std::getenv("ORBBEC_LOG_DIR");
if (log_dir_override && log_dir_override[0] != '\0') {
return (std::filesystem::path(log_dir_override) / "Log").string();
}
const char *home = std::getenv("HOME");
const std::filesystem::path home_dir = home != nullptr ? home : "";
return (home_dir / ".ros" / "Log").string();
@@ -63,7 +67,7 @@ std::string configureObSdkLoggerForTool(const std::string &tool_name,
return "";
}
const std::filesystem::path log_dir(getRosLogDirectory());
const std::filesystem::path log_dir(getObSdkLogDirectory());
std::filesystem::create_directories(log_dir);
const auto now = std::chrono::system_clock::now();
@@ -304,7 +308,7 @@ orbbec_camera_msgs::msg::Extrinsics obExtrinsicsToMsg(const OBD2CTransform &extr
}
rclcpp::Time fromMsToROSTime(uint64_t ms) {
auto total = static_cast<uint64_t>(ms * 1e6);
auto total = ms * 1000000ULL;
uint64_t sec = total / 1000000000;
uint64_t nano_sec = total % 1000000000;
rclcpp::Time stamp(sec, nano_sec);
@@ -312,7 +316,7 @@ rclcpp::Time fromMsToROSTime(uint64_t ms) {
}
rclcpp::Time fromUsToROSTime(uint64_t us) {
auto total = static_cast<uint64_t>(us * 1e3);
auto total = us * 1000ULL;
uint64_t sec = total / 1000000000;
uint64_t nano_sec = total % 1000000000;
rclcpp::Time stamp(sec, nano_sec);
@@ -1098,8 +1102,8 @@ UndistortedImageResult undistortImage(const cv::Mat &image, const OBCameraIntrin
static std::vector<UndistortMapCacheEntry> map_cache;
std::lock_guard<std::mutex> lock(cache_mutex);
auto cache_it = std::find_if(
map_cache.begin(), map_cache.end(), [&](const UndistortMapCacheEntry &entry) {
auto cache_it =
std::find_if(map_cache.begin(), map_cache.end(), [&](const UndistortMapCacheEntry &entry) {
return entry.width == image.cols && entry.height == image.rows &&
entry.image_type == image.type() && isSameIntrinsic(entry.intrinsic, intrinsic) &&
isSameDistortion(entry.distortion, distortion);
@@ -1113,9 +1117,8 @@ UndistortedImageResult undistortImage(const cv::Mat &image, const OBCameraIntrin
entry.intrinsic = intrinsic;
entry.distortion = distortion;
cv::Mat camera_matrix =
(cv::Mat_<double>(3, 3) << intrinsic.fx, 0.0, intrinsic.cx, 0.0, intrinsic.fy,
intrinsic.cy, 0.0, 0.0, 1.0);
cv::Mat camera_matrix = (cv::Mat_<double>(3, 3) << intrinsic.fx, 0.0, intrinsic.cx, 0.0,
intrinsic.fy, intrinsic.cy, 0.0, 0.0, 1.0);
cv::Mat dist_coeffs =
(cv::Mat_<double>(8, 1) << distortion.k1, distortion.k2, distortion.p1, distortion.p2,
distortion.k3, distortion.k4, distortion.k5, distortion.k6);