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 'merge/sdk_2.8.6' into v2-main

Browse Source
This commit is contained in:
ob-yalian
2026-04-30 15:57:40 +08:00
parent 94fc83f2ac f7ad98ed51
commit b948d96640
90 changed files with 6436 additions and 1249 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/frame_timestamp_csv_logger.cpp
+536
View File
@@ -0,0 +1,536 @@
#include "orbbec_camera/frame_timestamp_csv_logger.h"
#include <chrono>
#include <filesystem>
#include <iomanip>
#include <sstream>
#include <utility>
namespace orbbec_camera {
namespace {
constexpr size_t kCompletedQueueSoftLimit = 1000;
constexpr size_t kFlushBatchSize = 100;
constexpr auto kFlushInterval = std::chrono::seconds(1);
} // namespace
FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool enabled, const std::string &csv_file_path,
rclcpp::Logger logger)
: logger_(std::move(logger)), enabled_(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());
}
} 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;
}
writer_thread_ = std::thread([this]() { writerThreadMain(); });
}
FrameTimestampCsvLogger::~FrameTimestampCsvLogger() noexcept { shutdown(); }
void FrameTimestampCsvLogger::recordFrameSet(const std::shared_ptr<ob::Frame> &color_frame,
const std::shared_ptr<ob::Frame> &depth_frame,
int64_t arrival_system_us, int64_t arrival_steady_us,
bool track_color, bool track_depth,
bool color_image_publish_expected,
bool depth_image_publish_expected) {
if (!enabled_ || writer_failed_) {
return;
}
recordFrameSetInternal(color_frame, depth_frame, arrival_system_us, arrival_steady_us,
track_color, track_depth, color_image_publish_expected,
depth_image_publish_expected);
}
void FrameTimestampCsvLogger::recordStandaloneFrameArrival(OBStreamType stream_type,
const std::shared_ptr<ob::Frame> &frame,
int64_t arrival_system_us,
int64_t arrival_steady_us,
bool image_publish_expected) {
if (!enabled_ || writer_failed_ || !frame || !isTrackedStream(stream_type)) {
return;
}
recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us,
image_publish_expected);
}
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)) {
return;
}
recordPreImagePublishInternal(stream_type, frame, publish_system_us, publish_steady_us);
}
void FrameTimestampCsvLogger::shutdown() {
if (!enabled_) {
return;
}
{
std::lock_guard<std::mutex> state_lock(state_mutex_);
if (shutdown_requested_) {
return;
}
shutdown_requested_ = true;
std::vector<PendingRow> rows_to_flush;
flushPendingRowsLocked(rows_to_flush);
for (const auto &row : rows_to_flush) {
enqueueCompletedRow(row);
}
}
completed_rows_cv_.notify_all();
if (writer_thread_.joinable()) {
writer_thread_.join();
}
if (csv_stream_.is_open()) {
csv_stream_.flush();
csv_stream_.close();
}
}
FrameTimestampCsvLogger::TrackedStream FrameTimestampCsvLogger::toTrackedStream(
OBStreamType stream_type) const {
if (stream_type == OB_STREAM_COLOR) {
return TrackedStream::COLOR;
}
return TrackedStream::DEPTH;
}
bool FrameTimestampCsvLogger::isTrackedStream(OBStreamType stream_type) const {
return stream_type == OB_STREAM_COLOR || stream_type == OB_STREAM_DEPTH;
}
void FrameTimestampCsvLogger::recordFrameSetInternal(
const std::shared_ptr<ob::Frame> &color_frame, const std::shared_ptr<ob::Frame> &depth_frame,
int64_t arrival_system_us, int64_t arrival_steady_us, bool track_color, bool track_depth,
bool color_image_publish_expected, bool depth_image_publish_expected) {
if (!track_color && !track_depth) {
return;
}
std::vector<PendingRow> ready_rows;
{
std::lock_guard<std::mutex> lock(state_mutex_);
if (shutdown_requested_) {
return;
}
PendingRow row;
row.row_id = next_row_id_++;
if (track_color && color_frame) {
populateArrivalData(row.color, TrackedStream::COLOR, color_frame, arrival_system_us,
arrival_steady_us, color_image_publish_expected);
color_frame_index_to_row_id_[row.color.frame_index] = row.row_id;
if (!color_image_publish_expected) {
finalizeStreamWithoutPublish(row.color);
}
} else {
row.color.final = true;
}
if (track_depth && depth_frame) {
populateArrivalData(row.depth, TrackedStream::DEPTH, depth_frame, arrival_system_us,
arrival_steady_us, depth_image_publish_expected);
depth_frame_index_to_row_id_[row.depth.frame_index] = row.row_id;
if (!depth_image_publish_expected) {
finalizeStreamWithoutPublish(row.depth);
}
} else {
row.depth.final = true;
}
pending_rows_.emplace(row.row_id, row);
if (isRowReady(row)) {
auto it = pending_rows_.find(row.row_id);
if (it != pending_rows_.end()) {
ready_rows.push_back(it->second);
eraseFrameIndexMappingLocked(it->second);
pending_rows_.erase(it);
}
}
}
for (const auto &ready_row : ready_rows) {
enqueueCompletedRow(ready_row);
}
}
void FrameTimestampCsvLogger::recordStandaloneFrameArrivalInternal(
OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame, int64_t arrival_system_us,
int64_t arrival_steady_us, bool image_publish_expected) {
std::optional<PendingRow> ready_row;
{
std::lock_guard<std::mutex> lock(state_mutex_);
if (shutdown_requested_) {
return;
}
PendingRow row;
row.row_id = next_row_id_++;
const auto tracked_stream = toTrackedStream(stream_type);
auto &state = tracked_stream == TrackedStream::COLOR ? row.color : row.depth;
auto &other_state = tracked_stream == TrackedStream::COLOR ? row.depth : row.color;
populateArrivalData(state, tracked_stream, frame, arrival_system_us, arrival_steady_us,
image_publish_expected);
other_state.final = true;
if (tracked_stream == TrackedStream::COLOR) {
color_frame_index_to_row_id_[state.frame_index] = row.row_id;
} else {
depth_frame_index_to_row_id_[state.frame_index] = row.row_id;
}
if (!image_publish_expected) {
finalizeStreamWithoutPublish(state);
}
pending_rows_.emplace(row.row_id, row);
if (isRowReady(row)) {
auto it = pending_rows_.find(row.row_id);
if (it != pending_rows_.end()) {
ready_row = it->second;
eraseFrameIndexMappingLocked(*ready_row);
pending_rows_.erase(it);
}
}
}
if (ready_row.has_value()) {
enqueueCompletedRow(*ready_row);
}
}
void FrameTimestampCsvLogger::recordPreImagePublishInternal(OBStreamType stream_type,
const std::shared_ptr<ob::Frame> &frame,
int64_t publish_system_us,
int64_t publish_steady_us) {
std::optional<PendingRow> ready_row;
const auto frame_index = frame->getIndex();
{
std::lock_guard<std::mutex> lock(state_mutex_);
if (shutdown_requested_) {
return;
}
const auto tracked_stream = toTrackedStream(stream_type);
auto &row_map = tracked_stream == TrackedStream::COLOR ? color_frame_index_to_row_id_
: 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);
return;
}
const auto row_id = row_id_it->second;
auto pending_it = pending_rows_.find(row_id);
if (pending_it == pending_rows_.end()) {
return;
}
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 (isRowReady(pending_it->second)) {
ready_row = pending_it->second;
eraseFrameIndexMappingLocked(*ready_row);
pending_rows_.erase(pending_it);
}
}
if (ready_row.has_value()) {
enqueueCompletedRow(*ready_row);
}
}
void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStream stream,
const std::shared_ptr<ob::Frame> &frame,
int64_t arrival_system_us,
int64_t arrival_steady_us,
bool publish_expected) {
auto &previous = stream == TrackedStream::COLOR ? color_previous_ : depth_previous_;
state.has_frame = true;
state.publish_expected = publish_expected;
state.frame_index = frame->getIndex();
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));
} else {
state.metadata_frame_number.reset();
}
if (frame->hasMetadata(OB_FRAME_METADATA_TYPE_SENSOR_TIMESTAMP)) {
state.sensor_ts_us =
static_cast<int64_t>(frame->getMetadataValue(OB_FRAME_METADATA_TYPE_SENSOR_TIMESTAMP));
} 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;
state.arrival_steady_us = arrival_steady_us;
state.device_ts_delta_us = updateDelta(previous.device_ts_us, state.device_ts_us);
if (state.sensor_ts_us.has_value()) {
state.sensor_ts_delta_us = updateDelta(previous.sensor_ts_us, state.sensor_ts_us.value());
} else {
state.sensor_ts_delta_us.reset();
previous.sensor_ts_us.reset();
}
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);
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;
}
void FrameTimestampCsvLogger::populatePublishData(StreamState &state, TrackedStream stream,
int64_t publish_system_us,
int64_t publish_steady_us) {
auto &previous = stream == TrackedStream::COLOR ? color_previous_ : depth_previous_;
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());
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;
}
std::optional<int64_t> FrameTimestampCsvLogger::updateDelta(std::optional<int64_t> &previous,
int64_t current) {
std::optional<int64_t> delta;
if (previous.has_value()) {
delta = current - previous.value();
}
previous = current;
return delta;
}
void FrameTimestampCsvLogger::finalizeStreamWithoutPublish(StreamState &state) {
state.final = true;
}
bool FrameTimestampCsvLogger::isRowReady(const PendingRow &row) const {
return row.color.final && row.depth.final;
}
void FrameTimestampCsvLogger::enqueueCompletedRow(const PendingRow &row) {
std::lock_guard<std::mutex> queue_lock(completed_rows_mutex_);
completed_rows_.push_back(row);
if (completed_rows_.size() > kCompletedQueueSoftLimit) {
if (!queue_warning_active_) {
RCLCPP_WARN_STREAM(logger_, "Frame timestamp CSV queue size exceeded "
<< kCompletedQueueSoftLimit << " rows");
queue_warning_active_ = true;
}
} else {
queue_warning_active_ = false;
}
completed_rows_cv_.notify_one();
}
void FrameTimestampCsvLogger::flushPendingRowsLocked(std::vector<PendingRow> &rows) {
rows.reserve(rows.size() + pending_rows_.size());
for (auto &item : pending_rows_) {
auto row = item.second;
row.color.final = true;
row.depth.final = true;
rows.push_back(std::move(row));
}
pending_rows_.clear();
color_frame_index_to_row_id_.clear();
depth_frame_index_to_row_id_.clear();
}
void FrameTimestampCsvLogger::eraseFrameIndexMappingLocked(const PendingRow &row) {
if (row.color.has_frame) {
color_frame_index_to_row_id_.erase(row.color.frame_index);
}
if (row.depth.has_frame) {
depth_frame_index_to_row_id_.erase(row.depth.frame_index);
}
}
std::string FrameTimestampCsvLogger::serializeRow(const PendingRow &row) const {
std::ostringstream ss;
ss << serializeStreamColumns(row.color) << "," << serializeStreamColumns(row.depth);
return ss.str();
}
std::string FrameTimestampCsvLogger::serializeStreamColumns(const StreamState &state) const {
std::vector<std::string> fields(22, "");
if (state.has_frame) {
fields[0] = std::to_string(state.frame_index);
fields[1] = formatOptionalIntColumn(state.metadata_frame_number);
if (state.sensor_ts_us.has_value()) {
fields[2] = formatSecondsColumn(state.sensor_ts_us.value());
}
fields[3] = formatOptionalIntColumn(state.sensor_ts_delta_us);
fields[4] = formatSecondsColumn(state.device_ts_us);
fields[5] = formatOptionalIntColumn(state.device_ts_delta_us);
fields[6] = formatSecondsColumn(state.global_ts_us);
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);
}
std::ostringstream ss;
for (size_t i = 0; i < fields.size(); ++i) {
if (i != 0) {
ss << ",";
}
ss << fields[i];
}
return ss.str();
}
std::string FrameTimestampCsvLogger::formatSecondsColumn(int64_t time_us) {
std::ostringstream ss;
ss << std::fixed << std::setprecision(6) << (static_cast<long double>(time_us) / 1000000.0L);
return ss.str();
}
std::string FrameTimestampCsvLogger::formatOptionalIntColumn(const std::optional<int64_t> &value) {
if (!value.has_value()) {
return "";
}
return std::to_string(*value);
}
std::string FrameTimestampCsvLogger::csvHeader() {
std::ostringstream ss;
for (const auto *prefix : {"color", "depth"}) {
ss << prefix << "_sdk_frame_index,";
ss << prefix << "_hardware_frame_number,";
ss << prefix << "_sensor_ts_sec,";
ss << prefix << "_sensor_ts_delta_us,";
ss << prefix << "_device_ts_sec,";
ss << prefix << "_device_ts_delta_us,";
ss << prefix << "_global_ts_sec,";
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";
if (std::string(prefix) == "color") {
ss << ",";
}
}
return ss.str();
}
void FrameTimestampCsvLogger::writerThreadMain() {
if (!enabled_ || writer_failed_) {
return;
}
size_t rows_since_flush = 0;
auto last_flush = std::chrono::steady_clock::now();
while (true) {
std::deque<PendingRow> rows_to_write;
{
std::unique_lock<std::mutex> lock(completed_rows_mutex_);
completed_rows_cv_.wait_for(lock, kFlushInterval, [this]() {
return shutdown_requested_ || !completed_rows_.empty();
});
rows_to_write.swap(completed_rows_);
}
for (const auto &row : rows_to_write) {
if (csv_stream_.is_open()) {
csv_stream_ << serializeRow(row) << "\n";
rows_since_flush++;
}
}
const auto now = std::chrono::steady_clock::now();
if (csv_stream_.is_open() && (rows_since_flush >= kFlushBatchSize ||
now - last_flush >= kFlushInterval || shutdown_requested_)) {
csv_stream_.flush();
rows_since_flush = 0;
last_flush = now;
}
std::lock_guard<std::mutex> lock(completed_rows_mutex_);
if (shutdown_requested_ && completed_rows_.empty()) {
break;
}
}
}
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;
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/ob_camera_node.cpp
+1264 -625
View File
File diff suppressed because it is too large Load Diff
orbbec_camera/src/ob_camera_node_driver.cpp
+117 -70
View File
@@ -22,6 +22,7 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <ament_index_cpp/get_package_prefix.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <rcutils/logging.h>
#include <csignal>
#include <sys/mman.h>
#include <unistd.h>
@@ -34,6 +35,12 @@
std::string g_camera_name = "orbbec_camera"; // Assuming this is declared elsewhere
std::string g_time_domain = "global"; // Assuming this is declared elsewhere
namespace {
std::string getLogDirectoryForCamera(const std::string &camera_name) {
std::string home_dir = std::getenv("HOME") ? std::getenv("HOME") : "";
return (std::filesystem::path(home_dir) / ".ros" / "Log" / camera_name).string();
}
} // namespace
void signalHandler(int sig) {
// Prevent recursive signal handling
@@ -59,7 +66,7 @@ void signalHandler(int sig) {
}
in_signal_handler.store(false);
} else {
std::string log_dir = "Log/";
std::filesystem::path log_dir = getLogDirectoryForCamera(g_camera_name);
// get current time
std::time_t now = std::time(nullptr);
@@ -71,13 +78,13 @@ void signalHandler(int sig) {
// generate log file name
std::string log_file_name = g_camera_name + "_crash_stack_trace_" + time_stream.str() + ".log";
std::string log_file_path = log_dir + log_file_name;
std::filesystem::path log_file_path = log_dir / log_file_name;
if (!std::filesystem::exists(log_dir)) {
std::filesystem::create_directories(log_dir);
}
std::cout << "Log crash stack trace to " << log_file_path << std::endl;
std::cout << "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()) {
@@ -96,6 +103,24 @@ void signalHandler(int sig) {
namespace orbbec_camera {
backward::SignalHandling OBCameraNodeDriver::sh;
namespace {
int rosLogSeverityFromString(const std::string_view &log_level) {
if (log_level == "debug") {
return RCUTILS_LOG_SEVERITY_DEBUG;
} else if (log_level == "info") {
return RCUTILS_LOG_SEVERITY_INFO;
} else if (log_level == "warn") {
return RCUTILS_LOG_SEVERITY_WARN;
} else if (log_level == "error") {
return RCUTILS_LOG_SEVERITY_ERROR;
} else if (log_level == "fatal") {
return RCUTILS_LOG_SEVERITY_FATAL;
}
return RCUTILS_LOG_SEVERITY_UNSET;
}
} // namespace
OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
: Node("orbbec_camera_node", "/", node_options),
node_options_(node_options),
@@ -205,19 +230,26 @@ void OBCameraNodeDriver::init() {
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 = obLogSeverityFromString(log_level_str);
auto ros_log_level = rosLogSeverityFromString(log_level_str);
auto log_file_name = declare_parameter<std::string>("log_file_name", "");
std::string pwd_dir = std::getenv("PWD") ? std::getenv("PWD") : std::getenv("HOME");
std::string log_path = pwd_dir + "/Log/" + g_camera_name;
std::string log_path = getLogDirectoryForCamera(g_camera_name);
// Set logger to console
ob::Context::setLoggerToConsole(log_level);
ob::Context::setLoggerToFile(log_level, log_path.c_str());
if (ros_log_level != RCUTILS_LOG_SEVERITY_UNSET) {
auto ret = rcutils_logging_set_logger_level(this->get_logger().get_name(), ros_log_level);
if (ret != RCUTILS_RET_OK) {
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: " << e.getMessage());
RCLCPP_WARN_STREAM(logger_, "Failed to set SDK log file name: "
<< orbbec_camera::formatObErrorWithStatus(e));
}
}
// Force IP
@@ -283,13 +315,14 @@ void OBCameraNodeDriver::init() {
<< device_access_mode_ << ")");
if (uvc_backend_ == "libuvc") {
ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC);
RCLCPP_INFO_STREAM(logger_, "setUvcBackendType:" << uvc_backend_);
RCLCPP_INFO_STREAM(logger_, "Set UVC backend to " << uvc_backend_);
} else if (uvc_backend_ == "v4l2") {
ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_V4L2);
RCLCPP_INFO_STREAM(logger_, "setUvcBackendType:" << uvc_backend_);
RCLCPP_INFO_STREAM(logger_, "Set UVC backend to " << uvc_backend_);
} else {
ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC);
RCLCPP_INFO_STREAM(logger_, "setUvcBackendType:" << uvc_backend_);
RCLCPP_WARN_STREAM(logger_,
"Unsupported uvc_backend '" << uvc_backend_ << "', using default libuvc");
}
ctx_->enableNetDeviceEnumeration(enumerate_net_device_);
device_changed_callback_id_ = ctx_->registerDeviceChangedCallback(
@@ -319,17 +352,16 @@ void OBCameraNodeDriver::init() {
void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {
CHECK_NOTNULL(device_list);
{
RCLCPP_INFO_STREAM(logger_, "onDeviceConnected called");
RCLCPP_INFO_STREAM(logger_, "Device connected callback triggered");
std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_);
if (reset_device_flag_) {
RCLCPP_INFO_STREAM(logger_, "onDeviceConnected : device reset in progress, waiting...");
RCLCPP_INFO_STREAM(logger_, "Device reset in progress, waiting before connecting");
reset_device_cond_.wait(
reset_lock, [this]() { return !reset_device_flag_ || !is_alive_ || !rclcpp::ok(); });
if (!is_alive_) {
return;
}
RCLCPP_INFO_STREAM(logger_,
"onDeviceConnected : device reset completed, continuing connection");
RCLCPP_INFO_STREAM(logger_, "Device reset completed, continuing connection");
}
}
if (device_list->getCount() == 0) {
@@ -377,11 +409,9 @@ void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceLi
RCLCPP_INFO_STREAM(logger_, "device with " << uid << " disconnected");
if (uid == device_unique_id_ || serial_number_ == serial_number) {
RCLCPP_INFO_STREAM(logger_,
"device with " << uid << " disconnected, notify reset device thread 1.");
"device with " << uid << " disconnected, notify reset device thread");
reset_device_flag_ = true;
reset_device_cond_.notify_all();
RCLCPP_INFO_STREAM(logger_,
"device with " << uid << " disconnected, notify reset device thread 2.");
break;
}
}
@@ -497,7 +527,7 @@ void OBCameraNodeDriver::resetDevice() {
}
}
RCLCPP_INFO_STREAM(logger_, "resetDevice : Reset device uid: " << device_unique_id_);
RCLCPP_INFO_STREAM(logger_, "Resetting device UID: " << device_unique_id_);
std::lock_guard<decltype(device_lock_)> device_lock(device_lock_);
{
// Mark device as disconnected immediately to prevent other threads from accessing it
@@ -516,7 +546,7 @@ void OBCameraNodeDriver::resetDevice() {
if (device_) {
try {
RCLCPP_INFO_STREAM(logger_, "Resetting device_");
RCLCPP_INFO_STREAM(logger_, "Resetting device handle");
// Force free any idle memory before device reset
if (ctx_) {
try {
@@ -526,9 +556,10 @@ void OBCameraNodeDriver::resetDevice() {
}
}
device_.reset();
RCLCPP_INFO_STREAM(logger_, "device_ reset completed");
RCLCPP_INFO_STREAM(logger_, "Device handle reset complete");
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_, "OB Exception during device reset: " << e.getMessage());
RCLCPP_WARN_STREAM(logger_, "OB Exception during device reset: "
<< orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_WARN_STREAM(logger_, "Standard exception during device reset: " << e.what());
} catch (...) {
@@ -538,9 +569,9 @@ void OBCameraNodeDriver::resetDevice() {
if (device_info_) {
try {
RCLCPP_INFO_STREAM(logger_, "Resetting device_info_");
RCLCPP_INFO_STREAM(logger_, "Resetting device info");
device_info_.reset();
RCLCPP_INFO_STREAM(logger_, "device_info_ reset completed");
RCLCPP_INFO_STREAM(logger_, "Device info reset complete");
} catch (...) {
RCLCPP_WARN_STREAM(logger_, "Exception during device_info reset");
}
@@ -553,7 +584,7 @@ void OBCameraNodeDriver::resetDevice() {
}
reset_device_cond_.notify_all();
malloc_trim(0);
RCLCPP_INFO_STREAM(logger_, "Reset device uid: " << device_unique_id_ << " done");
RCLCPP_INFO_STREAM(logger_, "Device reset complete");
}
}
@@ -586,7 +617,7 @@ void OBCameraNodeDriver::deviceStatusTimer() {
ob_camera_node_->getColorStatus(status_msg);
ob_camera_node_->getDepthStatus(status_msg);
} catch (const ob::Error &e) {
std::string error_msg = e.getMessage() ? e.getMessage() : "Unknown OB error";
std::string error_msg = orbbec_camera::formatObErrorWithStatus(e);
if (error_msg.find("Device is deactivated") != std::string::npos ||
error_msg.find("disconnected") != std::string::npos ||
error_msg.find("Send control transfer failed") != std::string::npos) {
@@ -615,7 +646,7 @@ void OBCameraNodeDriver::deviceStatusTimer() {
status_msg.connection_type = device_info_->getConnectionType();
}
} catch (const ob::Error &e) {
std::string error_msg = e.getMessage() ? e.getMessage() : "Unknown OB error";
std::string error_msg = orbbec_camera::formatObErrorWithStatus(e);
if (error_msg.find("Device is deactivated") != std::string::npos ||
error_msg.find("disconnected") != std::string::npos ||
error_msg.find("Send control transfer failed") != std::string::npos) {
@@ -642,7 +673,7 @@ void OBCameraNodeDriver::deviceStatusTimer() {
status_msg.calibration_from_factory = calibration_from_factory;
}
} catch (const ob::Error &e) {
std::string error_msg = e.getMessage() ? e.getMessage() : "Unknown OB error";
std::string error_msg = orbbec_camera::formatObErrorWithStatus(e);
if (error_msg.find("Device is deactivated") != std::string::npos ||
error_msg.find("disconnected") != std::string::npos ||
error_msg.find("Send control transfer failed") != std::string::npos) {
@@ -681,7 +712,7 @@ void OBCameraNodeDriver::deviceStatusTimer() {
status_msg.customer_calibration_ready = false;
}
} catch (const ob::Error &e) {
std::string error_msg = e.getMessage() ? e.getMessage() : "Unknown OB error";
std::string error_msg = orbbec_camera::formatObErrorWithStatus(e);
if (error_msg.find("Device is deactivated") != std::string::npos ||
error_msg.find("disconnected") != std::string::npos ||
error_msg.find("Send control transfer failed") != std::string::npos) {
@@ -811,11 +842,9 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
std::transform(serial_number.begin(), serial_number.end(), std::back_inserter(lower_sn),
[](auto ch) { return isalpha(ch) ? tolower(ch) : static_cast<int>(ch); });
for (size_t i = 0; i < list->getCount(); i++) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Before lock: Select device serial number: " << serial_number);
RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Selecting device by serial number: " << serial_number);
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"After lock: Select device serial number: " << serial_number);
try {
auto pid = list->getPid(i);
if (isOpenNIDevice(pid)) {
@@ -825,21 +854,23 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
if (device_info->getSerialNumber() == serial_number) {
RCLCPP_INFO_STREAM_THROTTLE(
logger_, *get_clock(), 5000,
"Device serial number " << device_info->getSerialNumber() << " matched");
"Matched device serial number: " << device_info->getSerialNumber());
return device;
}
} else {
std::string sn = list->getSerialNumber(i);
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000, "Device serial number: " << sn);
RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Checking device serial number: " << sn);
if (sn == serial_number) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Device serial number " << sn << " matched");
"Matched device serial number: " << sn);
return list->getDevice(i, device_access_mode_);
}
}
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Failed to get device info " << e.getMessage());
RCLCPP_ERROR_STREAM_THROTTLE(
logger_, *get_clock(), 5000,
"Failed to get device info " << orbbec_camera::formatObErrorWithStatus(e));
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Failed to get device info " << e.what());
@@ -849,15 +880,12 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
}
return nullptr;
}
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
const std::shared_ptr<ob::DeviceList> &list, const std::string &usb_port) {
try {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Before lock: Select device usb port: " << usb_port);
"Selecting device by USB port: " << usb_port);
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"After lock: Select device usb port: " << usb_port);
auto device = list->getDeviceByUid(usb_port.c_str(), device_access_mode_);
if (device) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
@@ -871,8 +899,9 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
}
return device;
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Failed to get device info " << e.getMessage());
RCLCPP_ERROR_STREAM_THROTTLE(
logger_, *get_clock(), 5000,
"Failed to get device info " << orbbec_camera::formatObErrorWithStatus(e));
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Failed to get device info " << e.what());
@@ -885,11 +914,9 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
const std::shared_ptr<ob::DeviceList> &list, const std::string &net_ip) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Before lock: Select device net ip: " << net_ip);
RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Selecting device by network IP: " << net_ip);
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"After lock: Select device net ip: " << net_ip);
std::shared_ptr<ob::Device> device = nullptr;
for (size_t i = 0; i < list->getCount(); i++) {
try {
@@ -899,16 +926,17 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
if (list->getIpAddress(i) == nullptr) {
continue;
}
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"FindDeviceByNetIP device net ip " << list->getIpAddress(i));
RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"FindDeviceByNetIP device net ip " << list->getIpAddress(i));
if (std::string(list->getIpAddress(i)) == net_ip) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"getDeviceByNetIP device net ip " << net_ip << " done");
return list->getDevice(i, device_access_mode_);
}
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Failed to get device info " << e.getMessage());
RCLCPP_ERROR_STREAM_THROTTLE(
logger_, *get_clock(), 5000,
"Failed to get device info " << orbbec_camera::formatObErrorWithStatus(e));
continue;
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
@@ -937,7 +965,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
constexpr int max_retries = 3;
bool initialized = false;
device_info_ = device_->getDeviceInfo();
RCLCPP_INFO_STREAM(logger_, "Try to connect device via " << device_info_->connectionType());
RCLCPP_DEBUG_STREAM(logger_, "Try to connect device via " << device_info_->connectionType());
while (retry_count < max_retries && !initialized) {
try {
@@ -953,7 +981,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt "
<< retry_count + 1 << " of " << max_retries
<< "): " << e.getMessage());
<< "): " << orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt " << retry_count + 1
<< " of " << max_retries
@@ -981,6 +1009,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid()) && device_type_ == "camera") {
TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
if (g_time_domain != "global") {
device_->enableGlobalTimestamp(false);
sync_host_time_timer_ = this->create_wall_timer(time_sync_period_, [this]() {
// Multiple safety checks before attempting time sync
if (!device_) {
@@ -1027,9 +1056,10 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
return;
}
RCLCPP_INFO_STREAM(logger_, "Sync device time with host");
TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
});
RCLCPP_INFO_STREAM(logger_, "Enabled timer sync with host with period "
<< time_sync_period_.count() << " ms");
}
}
@@ -1041,20 +1071,34 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
RCLCPP_INFO_STREAM(logger_, "ROS Wrapper version: " << OB_ROS_VERSION_STR);
RCLCPP_INFO_STREAM(logger_, "SDK version: " << getObSDKVersion());
RCLCPP_INFO_STREAM(logger_, "Hardware version: " << device_info_->getHardwareVersion());
try {
std::string isp_fw_version = device_->getExtensionInfo("IspFwVer");
if (!isp_fw_version.empty()) {
RCLCPP_INFO_STREAM(logger_, "ISP firmware version: " << isp_fw_version);
}
std::string isp_need_version = device_->getExtensionInfo("IspNeedVer");
if (!isp_need_version.empty()) {
RCLCPP_INFO_STREAM(logger_, "ISP needed version: " << isp_need_version);
}
} 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_INFO_STREAM(logger_, "usb connect type: " << device_info_->getConnectionType());
});
RCLCPP_INFO_STREAM(logger_, "device unique id: " << device_unique_id_);
RCLCPP_INFO_STREAM(logger_, "Current node pid: " << getpid());
auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start_time_);
RCLCPP_INFO_STREAM(logger_, "Start device cost " << time_cost.count() << " ms");
RCLCPP_DEBUG_STREAM(logger_, "Start device cost: " << time_cost.count() << " ms");
if (!upgrade_firmware_.empty()) {
// Check if this is a second update (reupdate scenario)
bool is_second_update = is_reupdating_.load();
if (is_second_update) {
RCLCPP_INFO(logger_, "Device reconnected, starting the second firmware update...");
RCLCPP_INFO(logger_, "Device reconnected, starting the second firmware update");
} else {
RCLCPP_INFO(logger_, "Starting firmware update from file: %s", upgrade_firmware_.c_str());
}
@@ -1082,7 +1126,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
if (need_reupdate_) {
// Some devices require a second update after reboot
RCLCPP_INFO(logger_, "The device will reboot and perform a second update automatically.");
RCLCPP_INFO(logger_, "The device will reboot and perform a second update automatically");
// Set flag to indicate we're waiting for device to reboot for second update
is_reupdating_ = true;
// Keep upgrade_firmware_ path and wait for device to reconnect
@@ -1092,10 +1136,10 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
if (firmware_update_success_) {
if (is_second_update) {
RCLCPP_INFO(logger_, "Second firmware update completed successfully!");
RCLCPP_INFO(logger_, "Second firmware update completed successfully");
is_reupdating_ = false;
} else {
RCLCPP_INFO(logger_, "Firmware update completed successfully!");
RCLCPP_INFO(logger_, "Firmware update completed successfully");
}
return;
}
@@ -1108,7 +1152,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
ob_lidar_node_->startStreams();
ob_lidar_node_->startIMU();
} else {
RCLCPP_INFO_STREAM(logger_, "ob_camera_node_ is nullptr");
RCLCPP_WARN_STREAM(logger_, "Camera or LiDAR node is null after device initialization");
}
} // namespace orbbec_camera
@@ -1180,7 +1224,8 @@ bool OBCameraNodeDriver::applyForceIpConfig() {
RCLCPP_ERROR(logger_, "[ForceIP] Failed to apply config (SDK returned false)");
}
} catch (const ob::Error &e) {
RCLCPP_ERROR(logger_, "[ForceIP] ob::Error: %s", e.getMessage());
RCLCPP_ERROR(logger_, "[ForceIP] ob::Error: %s",
orbbec_camera::formatObErrorWithStatus(e).c_str());
} catch (const std::exception &e) {
RCLCPP_ERROR(logger_, "[ForceIP] std::exception: %s", e.what());
} catch (...) {
@@ -1267,7 +1312,7 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
// success get lock,break
break;
} else if (try_lock_result == EBUSY) {
RCLCPP_INFO_STREAM(logger_, "Device lock is held by another process, waiting 100ms");
RCLCPP_WARN_STREAM(logger_, "Device lock is held by another process, waiting 100ms");
std::this_thread::sleep_for(std::chrono::milliseconds(100));
} else {
RCLCPP_ERROR_STREAM(logger_, "Failed to lock orb_device_lock_");
@@ -1293,12 +1338,12 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
}
auto end_time = std::chrono::high_resolution_clock::now();
auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
RCLCPP_INFO_STREAM(logger_, "Select device cost " << time_cost.count() << " ms");
RCLCPP_DEBUG_STREAM(logger_, "Select device cost " << time_cost.count() << " ms");
start_time = std::chrono::high_resolution_clock::now();
initializeDevice(device);
end_time = std::chrono::high_resolution_clock::now();
time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
RCLCPP_INFO_STREAM(logger_, "Initialize device cost " << time_cost.count() << " ms");
RCLCPP_INFO_STREAM(logger_, "Initialize device cost: " << time_cost.count() << " ms");
if (firmware_update_success_) {
firmware_update_success_ = false;
@@ -1312,15 +1357,16 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
}
auto pid = device->getDeviceInfo()->getPid();
if (GEMINI_335LG_PID == pid) {
if (GEMINI_335LG_PID == pid || GEMINI_338LG_PID == pid) {
ob_camera_node_->startGmslTrigger();
}
if (pid == GEMINI_305_PID) {
// Fixing 305 hot-swap not outputting power
ob_camera_node_->startStreams();
}
// if (isGemini305SeriesPID(pid)) {
// // Fixing 305 series hot-swap not outputting power
// ob_camera_node_->startStreams();
// }
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device " << e.getMessage());
RCLCPP_ERROR_STREAM(
logger_, "Failed to initialize device " << orbbec_camera::formatObErrorWithStatus(e));
start_device_failed = true;
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device " << e.what());
@@ -1393,7 +1439,8 @@ void OBCameraNodeDriver::updatePresetFirmware(std::string path) {
}
}
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware " << e.getMessage());
RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware "
<< orbbec_camera::formatObErrorWithStatus(e));
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware " << e.what());
} catch (...) {
orbbec_camera/src/ob_lidar_node.cpp
+68 -60
View File
@@ -64,27 +64,26 @@ void OBLidarNode::setAndGetNodeParameter(
OBLidarNode::~OBLidarNode() noexcept { clean(); }
void OBLidarNode::rebootDevice() {
RCLCPP_WARN_STREAM(logger_, "Reboot device");
RCLCPP_INFO_STREAM(logger_, "Rebooting device");
clean();
if (device_) {
device_->reboot();
RCLCPP_WARN_STREAM(logger_, "Reboot device DONE");
RCLCPP_DEBUG_STREAM(logger_, "Reboot device complete");
}
}
void OBLidarNode::clean() noexcept {
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBLidarNode");
RCLCPP_DEBUG_STREAM(logger_, "Destroying OBLidarNode");
is_running_.store(false);
RCLCPP_WARN_STREAM(logger_, "Stop tf thread");
RCLCPP_DEBUG_STREAM(logger_, "Stop tf thread");
if (tf_thread_ && tf_thread_->joinable()) {
tf_thread_->join();
}
RCLCPP_WARN_STREAM(logger_, "Stop color frame thread");
RCLCPP_WARN_STREAM(logger_, "stop streams");
RCLCPP_DEBUG_STREAM(logger_, "Stop streams");
stopStreams();
stopIMU();
RCLCPP_WARN_STREAM(logger_, "Destroy ~OBLidarNode DONE");
RCLCPP_DEBUG_STREAM(logger_, "OBLidarNode cleanup complete");
}
void OBLidarNode::setupTopics() {
@@ -95,8 +94,9 @@ void OBLidarNode::setupTopics() {
setupProfiles();
setupPublishers();
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.getMessage());
throw std::runtime_error(e.getMessage());
RCLCPP_ERROR_STREAM(logger_,
"Failed to setup topics: " << orbbec_camera::formatObErrorWithStatus(e));
throw std::runtime_error(orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.what());
throw std::runtime_error(e.what());
@@ -115,9 +115,9 @@ void OBLidarNode::getParameters() {
param_name = stream_name_[stream_index] + "_rate";
setAndGetNodeParameter(rate_int_[stream_index], param_name, 0);
rate_[stream_index] = OBScanRateFromInt(rate_int_[stream_index]);
RCLCPP_INFO_STREAM(logger_, "Input rate: " << magic_enum::enum_name(rate_[stream_index])
<< " Input format:"
<< magic_enum::enum_name(format_[stream_index]));
RCLCPP_DEBUG_STREAM(logger_, "Input rate: " << magic_enum::enum_name(rate_[stream_index])
<< " Input format:"
<< magic_enum::enum_name(format_[stream_index]));
param_name = stream_name_[stream_index] + "_frame_id";
std::string default_frame_id = camera_name_ + "_" + stream_name_[stream_index] + "_frame";
setAndGetNodeParameter(frame_id_[stream_index], param_name, default_frame_id);
@@ -181,6 +181,8 @@ void OBLidarNode::getParameters() {
}
void OBLidarNode::setupDevices() {
RCLCPP_INFO_STREAM(logger_, "Current time domain: " << time_domain_);
auto sensor_list = device_->getSensorList();
for (size_t i = 0; i < sensor_list->getCount(); i++) {
auto sensor = sensor_list->getSensor(i);
@@ -196,14 +198,13 @@ void OBLidarNode::setupDevices() {
}
for (const auto &[stream_index, enable] : enable_stream_) {
if (enable && sensors_.find(stream_index) == sensors_.end()) {
RCLCPP_INFO_STREAM(logger_,
magic_enum::enum_name(stream_index.first)
<< "sensor isn't supported by current device! -- Skipping...");
RCLCPP_WARN_STREAM(logger_, magic_enum::enum_name(stream_index.first)
<< " sensor not supported by current device, skipping");
enable_stream_[stream_index] = false;
}
}
if (device_->isPropertySupported(OB_PROP_HEARTBEAT_BOOL, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting heartbeat to " << (enable_heartbeat_ ? "ON" : "OFF"));
RCLCPP_INFO_STREAM(logger_, "Current heartbeat: " << (enable_heartbeat_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_);
}
if (!echo_mode_.empty() &&
@@ -214,9 +215,9 @@ void OBLidarNode::setupDevices() {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_SPECIFIC_MODE_INT, 1);
}
RCLCPP_INFO_STREAM(
logger_, "Setting echo mode to "
<< (device_->getIntProperty(OB_PROP_LIDAR_SPECIFIC_MODE_INT) ? "First Echo"
: "Last Echo"));
logger_, "Current echo mode: " << (device_->getIntProperty(OB_PROP_LIDAR_SPECIFIC_MODE_INT)
? "First Echo"
: "Last Echo"));
}
if (repetitive_scan_mode_ != -1 &&
device_->isPropertySupported(OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT,
@@ -229,7 +230,7 @@ void OBLidarNode::setupDevices() {
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT,
repetitive_scan_mode_);
RCLCPP_INFO_STREAM(logger_, "Setting repetitive scan mode to " << device_->getIntProperty(
RCLCPP_INFO_STREAM(logger_, "Current repetitive scan mode: " << device_->getIntProperty(
OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT));
}
}
@@ -242,7 +243,7 @@ void OBLidarNode::setupDevices() {
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT, filter_level_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_APPLY_CONFIGS_INT, 1);
RCLCPP_INFO_STREAM(logger_, "Setting filter level to " << device_->getIntProperty(
RCLCPP_INFO_STREAM(logger_, "Current filter level: " << device_->getIntProperty(
OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT));
}
}
@@ -255,7 +256,7 @@ void OBLidarNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT, vertical_fov_);
RCLCPP_INFO_STREAM(logger_, "Setting vertical fov to " << device_->getFloatProperty(
RCLCPP_INFO_STREAM(logger_, "Current vertical fov: " << device_->getFloatProperty(
OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT));
}
}
@@ -303,20 +304,20 @@ void OBLidarNode::setupProfiles() {
<< "Format:" << format_[elem]);
RCLCPP_INFO_STREAM(logger_, "Available profiles:");
printSensorProfiles(sensor);
RCLCPP_ERROR(logger_, "Because can not set this stream, so exit.");
RCLCPP_ERROR(logger_, "Failed to configure the requested stream profile, exiting.");
exit(-1);
}
if (!selected_profile) {
RCLCPP_WARN_STREAM(logger_, "Given stream configuration is not supported by the device! "
<< " Stream: " << magic_enum::enum_name(elem.first)
<< ", Stream Index: " << elem.second
<< ", Scan Rate: " << rate_[elem]);
RCLCPP_WARN_STREAM(
logger_, "Requested stream configuration is not supported by the device: "
<< "stream=" << magic_enum::enum_name(elem.first)
<< ", stream_index=" << elem.second << ", scan_rate=" << rate_[elem]);
if (default_profile) {
RCLCPP_WARN_STREAM(logger_, "Using default profile instead.");
RCLCPP_WARN_STREAM(logger_, "default scan Rate "
<< magic_enum::enum_name(default_profile->getScanRate())
<< "default format:"
<< magic_enum::enum_name(default_profile->getFormat()));
RCLCPP_WARN_STREAM(logger_, "Using the default profile instead");
RCLCPP_WARN_STREAM(
logger_, "Default profile: scan_rate="
<< magic_enum::enum_name(default_profile->getScanRate())
<< ", format=" << magic_enum::enum_name(default_profile->getFormat()));
selected_profile = default_profile;
} else {
RCLCPP_ERROR_STREAM(
@@ -329,11 +330,11 @@ void OBLidarNode::setupProfiles() {
stream_profile_[elem] = selected_profile;
rate_[elem] = selected_profile->getScanRate();
format_[elem] = selected_profile->getFormat();
RCLCPP_INFO_STREAM(logger_, " stream "
<< stream_name_[elem] << " is enabled - scan rate: "
<< magic_enum::enum_name(selected_profile->getScanRate())
<< " format:"
<< magic_enum::enum_name(selected_profile->getFormat()));
RCLCPP_DEBUG_STREAM(logger_, "stream "
<< stream_name_[elem] << " is enabled - scan rate: "
<< magic_enum::enum_name(selected_profile->getScanRate())
<< " format:"
<< magic_enum::enum_name(selected_profile->getFormat()));
}
}
// IMU
@@ -354,12 +355,12 @@ void OBLidarNode::setupProfiles() {
auto profile = profile_list->getGyroStreamProfile(full_scale_range, sample_rate);
stream_profile_[stream_index] = profile;
}
RCLCPP_INFO_STREAM(logger_, "stream " << stream_name_[stream_index] << " full scale range "
RCLCPP_INFO_STREAM(logger_, "Stream " << stream_name_[stream_index] << " full scale range: "
<< (stream_index == ACCEL ? accel_range_ : gyro_range_)
<< " sample rate " << imu_rate_);
<< ", sample rate: " << imu_rate_);
} catch (const ob::Error &e) {
RCLCPP_INFO_STREAM(logger_, "Failed to setup << " << stream_name_[stream_index]
<< " profile: " << e.getMessage());
RCLCPP_INFO_STREAM(logger_, "Failed to set up " << stream_name_[stream_index] << " profile: "
<< orbbec_camera::formatObErrorWithStatus(e));
enable_stream_[stream_index] = false;
stream_profile_[stream_index] = nullptr;
}
@@ -434,7 +435,8 @@ void OBLidarNode::startStreams() {
onNewFrameSetCallback(frame_set);
});
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline: " << e.getMessage());
RCLCPP_ERROR_STREAM(logger_,
"Failed to start pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
setupPipelineConfig();
pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
onNewFrameSetCallback(frame_set);
@@ -499,13 +501,14 @@ void OBLidarNode::startIMU() {
void OBLidarNode::stopStreams() {
if (!pipeline_started_ || !pipeline_) {
RCLCPP_INFO_STREAM(logger_, "pipeline not started or not exist, skip stop pipeline");
RCLCPP_DEBUG_STREAM(logger_, "Pipeline not started or not exist, skip stop pipeline");
return;
}
try {
pipeline_->stop();
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline: " << e.getMessage());
RCLCPP_ERROR_STREAM(logger_,
"Failed to stop pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline");
}
@@ -517,14 +520,16 @@ void OBLidarNode::stopIMU() {
}
if (!imu_sync_output_start_ || !imuPipeline_) {
RCLCPP_INFO_STREAM(logger_, "IMU pipeline not started or not exist, skip stop imu pipeline");
RCLCPP_DEBUG_STREAM(logger_,
"IMU pipeline not started or unavailable, skip stopping IMU pipeline");
return;
}
try {
imuPipeline_->stop();
imu_sync_output_start_ = false;
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop IMU pipeline: " << e.getMessage());
RCLCPP_ERROR_STREAM(
logger_, "Failed to stop IMU pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop IMU pipeline");
}
@@ -537,15 +542,15 @@ void OBLidarNode::setupPipelineConfig() {
pipeline_config_ = std::make_shared<ob::Config>();
for (const auto &stream_index : LIDAR_STREAMS) {
if (enable_stream_[stream_index]) {
RCLCPP_INFO_STREAM(logger_, "Enable " << stream_name_[stream_index] << " stream");
RCLCPP_DEBUG_STREAM(logger_, "Enable " << stream_name_[stream_index] << " stream");
auto profile = stream_profile_[stream_index]->as<ob::LiDARStreamProfile>();
if (enable_stream_[stream_index]) {
auto video_profile = profile;
RCLCPP_INFO_STREAM(logger_,
"lidar profile: " << magic_enum::enum_name(video_profile->getScanRate())
<< " "
<< magic_enum::enum_name(video_profile->getFormat()));
RCLCPP_DEBUG_STREAM(logger_,
"lidar profile: " << magic_enum::enum_name(video_profile->getScanRate())
<< " "
<< magic_enum::enum_name(video_profile->getFormat()));
}
RCLCPP_INFO_STREAM(
@@ -650,7 +655,8 @@ void OBLidarNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set)
}
}
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.getMessage());
RCLCPP_ERROR_STREAM(
logger_, "onNewFrameSetCallback error: " << orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.what());
} catch (...) {
@@ -1244,12 +1250,13 @@ void OBLidarNode::calcAndPublishStaticTransform() {
RCLCPP_DEBUG_STREAM(logger_, "ACCEL and GYRO extrinsics are identical");
}
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_,
"Could not get GYRO extrinsic for verification: " << e.getMessage());
RCLCPP_WARN_STREAM(logger_, "Could not get GYRO extrinsic for verification: "
<< orbbec_camera::formatObErrorWithStatus(e));
}
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_, "Failed to get ACCEL extrinsic, trying GYRO: " << e.getMessage());
RCLCPP_WARN_STREAM(logger_, "Failed to get ACCEL extrinsic, trying GYRO: "
<< orbbec_camera::formatObErrorWithStatus(e));
try {
ex = base_stream_profile->getExtrinsicTo(stream_profile_[GYRO]);
RCLCPP_INFO_STREAM(logger_, "Using GYRO extrinsic for IMU");
@@ -1272,11 +1279,12 @@ void OBLidarNode::calcAndPublishStaticTransform() {
auto timestamp = node_->now();
publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], accel_gyro_frame_id_);
RCLCPP_INFO_STREAM(logger_, "Publishing static transform from "
<< frame_id_[base_stream_] << " to " << accel_gyro_frame_id_);
RCLCPP_INFO_STREAM(logger_, "Translation " << trans[0] << ", " << trans[1] << ", " << trans[2]);
RCLCPP_INFO_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ()
<< ", " << Q.getW());
RCLCPP_DEBUG_STREAM(logger_, "Publishing static transform from "
<< frame_id_[base_stream_] << " to " << accel_gyro_frame_id_);
RCLCPP_DEBUG_STREAM(logger_,
"Translation " << trans[0] << ", " << trans[1] << ", " << trans[2]);
RCLCPP_DEBUG_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ()
<< ", " << Q.getW());
}
}
orbbec_camera/src/ros_service.cpp
+251 -74
View File
@@ -22,6 +22,37 @@
#include "orbbec_camera/utils.h"
namespace orbbec_camera {
namespace {
bool isGemini330SeriesForDisparity(uint32_t pid) {
return pid == GEMINI_335_PID || pid == GEMINI_336_PID || pid == GEMINI_330_PID ||
pid == GEMINI_335L_PID || pid == GEMINI_336L_PID || pid == GEMINI_330L_PID ||
pid == GEMINI_335LG_PID || pid == GEMINI_335LE_PID || pid == GEMINI_338_PID ||
pid == GEMINI_338LG_PID || pid == GEMINI_338LE_PID || pid == GEMINI_338L_PID ||
pid == GEMINI_331L_PID;
}
bool isSupportedDisparityResolutionForPid(uint32_t pid, int width, int height) {
if (pid == GEMINI_335LE_PID || pid == GEMINI_338LE_PID) {
return (width == 1280 && height == 800) || (width == 640 && height == 400) ||
(width == 424 && height == 266) || (width == 320 && height == 200);
}
return (width == 1280 && height == 800) || (width == 1280 && height == 720) ||
(width == 640 && height == 400) || (width == 424 && height == 266);
}
std::string getDisparityResolutionHintByPid(uint32_t pid) {
if (pid == GEMINI_335LE_PID || pid == GEMINI_338LE_PID) {
return "Supported resolutions for the current device: 1280x800/640x400/424x266/320x200";
}
return "Supported resolutions for the current device: "
"1280x800/1280x720/640x400/424x266";
}
} // namespace
void OBCameraNode::setupCameraCtrlServices() {
using std_srvs::srv::SetBool;
for (auto stream_index : IMAGE_STREAMS) {
@@ -253,15 +284,15 @@ void OBCameraNode::setupCameraCtrlServices() {
getUserCalibParamsCallback(request, response);
});
}
set_ae_mode_srv_ = node_->create_service<SetString>(
"set_ae_mode", [this](const std::shared_ptr<SetString::Request> request,
std::shared_ptr<SetString::Response> response) {
setAEModeCallback(request, response);
set_ae_reference_stream_srv_ = node_->create_service<SetString>(
"set_ae_reference_stream", [this](const std::shared_ptr<SetString::Request> request,
std::shared_ptr<SetString::Response> response) {
setAEReferenceStreamCallback(request, response);
});
set_sports_mode_srv_ = node_->create_service<SetBool>(
"set_sports_mode", [this](const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setSportsModeCallback(request, response);
set_ae_strategy_srv_ = node_->create_service<SetString>(
"set_ae_strategy", [this](const std::shared_ptr<SetString::Request> request,
std::shared_ptr<SetString::Response> response) {
setAEStrategyCallback(request, response);
});
set_streams_enable_srv_ = node_->create_service<SetBool>(
"set_streams_enable", [this](const std::shared_ptr<SetBool::Request> request,
@@ -283,6 +314,16 @@ void OBCameraNode::setupCameraCtrlServices() {
std::shared_ptr<GetInt32::Response> response) {
getPointCloudDecimationCallback(request, response);
});
set_disparity_range_mode_srv_ = node_->create_service<SetInt32>(
"set_disparity_range_mode", [this](const std::shared_ptr<SetInt32::Request> request,
std::shared_ptr<SetInt32::Response> response) {
setDisparityRangeModeCallback(request, response);
});
set_disparity_search_offset_srv_ = node_->create_service<SetInt32>(
"set_disparity_search_offset", [this](const std::shared_ptr<SetInt32::Request> request,
std::shared_ptr<SetInt32::Response> response) {
setDisparitySearchOffsetCallback(request, response);
});
}
void OBCameraNode::getPointCloudDecimationCallback(
@@ -330,6 +371,139 @@ void OBCameraNode::setPointCloudDecimationCallback(
}
}
void OBCameraNode::setDisparityRangeModeCallback(const std::shared_ptr<SetInt32::Request>& request,
std::shared_ptr<SetInt32::Response>& response) {
if (!request) {
response->success = false;
response->message = "Invalid request";
return;
}
try {
if (!device_->isPropertySupported(OB_PROP_DISP_SEARCH_RANGE_MODE_INT, OB_PERMISSION_WRITE)) {
response->success = false;
response->message = "Current device does not support disparity range mode";
return;
}
const bool allow_set = isGemini435LePID(pid_) || enable_stream_[DEPTH];
if (!allow_set) {
response->success = false;
response->message = "Disparity range mode can only be set when depth stream is enabled";
return;
}
if (isGemini330SeriesForDisparity(pid_) &&
!isSupportedDisparityResolutionForPid(pid_, width_[DEPTH], height_[DEPTH])) {
response->success = false;
response->message = "Current depth resolution " + std::to_string(width_[DEPTH]) + "x" +
std::to_string(height_[DEPTH]) + " is not supported. " +
getDisparityResolutionHintByPid(pid_);
return;
}
auto range = device_->getIntPropertyRange(OB_PROP_DISP_SEARCH_RANGE_MODE_INT);
const int requested_mode_value = request->data;
int hw_mode_index = -1;
if (requested_mode_value == 64) {
hw_mode_index = 0;
} else if (requested_mode_value == 128) {
hw_mode_index = 1;
} else if (requested_mode_value == 256) {
hw_mode_index = 2;
}
if (hw_mode_index < range.min || hw_mode_index > range.max) {
response->success = false;
std::string supported_mode;
for (int i = range.min; i <= range.max; ++i) {
supported_mode += (i == 0) ? "64"
: (i == 1) ? "/128"
: (i == 2) ? "/256"
: "/" + std::to_string(i);
}
response->message = "Invalid disparity range mode. Allowed values:" + supported_mode;
return;
}
device_->setIntProperty(OB_PROP_DISP_SEARCH_RANGE_MODE_INT, hw_mode_index);
auto current_mode_index = device_->getIntProperty(OB_PROP_DISP_SEARCH_RANGE_MODE_INT);
auto current_mode_value = (current_mode_index == 0) ? 64
: (current_mode_index == 1) ? 128
: (current_mode_index == 2) ? 256
: current_mode_index;
response->success = true;
response->message = "disparity_range_mode updated to " + std::to_string(current_mode_value);
} catch (const ob::Error& e) {
response->success = false;
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->success = false;
response->message = e.what();
} catch (...) {
response->success = false;
response->message = "unknown error";
}
}
void OBCameraNode::setDisparitySearchOffsetCallback(
const std::shared_ptr<SetInt32::Request>& request,
std::shared_ptr<SetInt32::Response>& response) {
if (!request) {
response->success = false;
response->message = "Invalid request";
return;
}
try {
if (!device_->isPropertySupported(OB_PROP_DISP_SEARCH_OFFSET_INT, OB_PERMISSION_WRITE)) {
response->success = false;
response->message = "Current device does not support disparity search offset";
return;
}
const bool allow_set = isGemini435LePID(pid_) || enable_stream_[DEPTH];
if (!allow_set) {
response->success = false;
response->message = "Disparity search offset can only be set when depth stream is enabled";
return;
}
if (isGemini330SeriesForDisparity(pid_) &&
!isSupportedDisparityResolutionForPid(pid_, width_[DEPTH], height_[DEPTH])) {
response->success = false;
response->message = "Current depth resolution " + std::to_string(width_[DEPTH]) + "x" +
std::to_string(height_[DEPTH]) + " is not supported. " +
getDisparityResolutionHintByPid(pid_);
return;
}
auto range = device_->getIntPropertyRange(OB_PROP_DISP_SEARCH_OFFSET_INT);
if (request->data < range.min || request->data > range.max) {
response->success = false;
response->message =
"Invalid disparity search offset. Allowed values:" + std::to_string(range.min) + " to " +
std::to_string(range.max);
return;
}
device_->setIntProperty(OB_PROP_DISP_SEARCH_OFFSET_INT, request->data);
auto current_offset = device_->getIntProperty(OB_PROP_DISP_SEARCH_OFFSET_INT);
RCLCPP_INFO_STREAM(logger_, "Set disparity_search_offset to " << current_offset);
response->success = true;
response->message = "disparity_search_offset updated to " + std::to_string(current_offset);
} catch (const ob::Error& e) {
response->success = false;
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->success = false;
response->message = e.what();
} catch (...) {
response->success = false;
response->message = "unknown error";
}
}
void OBCameraNode::setStreamsEnableCallback(
const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
std::shared_ptr<std_srvs::srv::SetBool::Response> response) {
@@ -345,7 +519,7 @@ void OBCameraNode::setStreamsEnableCallback(
}
} catch (const ob::Error& e) {
response->success = false;
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->success = false;
response->message = e.what();
@@ -394,7 +568,7 @@ void OBCameraNode::setExposureCallback(const std::shared_ptr<SetInt32::Request>&
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->success = false;
response->message = e.what();
@@ -432,7 +606,7 @@ void OBCameraNode::getGainCallback(const std::shared_ptr<GetInt32::Request>& req
response->success = true;
} catch (ob::Error& e) {
response->success = false;
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->success = false;
response->message = e.what();
@@ -471,7 +645,7 @@ void OBCameraNode::setGainCallback(const std::shared_ptr<SetInt32 ::Request>& re
auto range = device_->getIntPropertyRange(prop_id);
if (request->data < range.min || request->data > range.max) {
response->success = false;
RCLCPP_INFO_STREAM(logger_, "set gain value out of range");
RCLCPP_WARN_STREAM(logger_, "Gain value is out of range");
response->message = "value out of range";
return;
}
@@ -479,7 +653,7 @@ void OBCameraNode::setGainCallback(const std::shared_ptr<SetInt32 ::Request>& re
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->success = false;
response->message = e.what();
@@ -493,16 +667,17 @@ void OBCameraNode::setAeRoiCallback(const std::shared_ptr<SetArrays ::Request>&
std::shared_ptr<SetArrays::Response>& response,
const stream_index_pair& stream_index) {
auto stream = stream_index.first;
if (device_->getDeviceInfo()->getPid() == GEMINI_305_PID &&
(stream != OB_STREAM_COLOR && ae_mode_ == "colorbased")) {
if (isGemini305SeriesPID(device_->getDeviceInfo()->getPid()) &&
(stream != OB_STREAM_COLOR && ae_reference_stream_ == "color")) {
response->success = false;
response->message = "AE MODE is colorbased, other sensors setting is not supported";
response->message = "AE Reference Stream is color, other sensors setting is not supported";
return;
}
if (device_->getDeviceInfo()->getPid() == GEMINI_305_PID &&
(stream != OB_STREAM_DEPTH && ae_mode_ == "depthbased")) {
if (isGemini305SeriesPID(device_->getDeviceInfo()->getPid()) &&
(stream != OB_STREAM_DEPTH && ae_reference_stream_ == "depth")) {
response->success = false;
response->message = "AE MODE is depthbased, other sensors sensor setting is not supported";
response->message =
"AE Reference Stream is depth, other sensors sensor setting is not supported";
return;
}
auto config = OBRegionOfInterest();
@@ -542,9 +717,9 @@ void OBCameraNode::setAeRoiCallback(const std::shared_ptr<SetArrays ::Request>&
device_->getStructuredData(OB_STRUCT_DEPTH_AE_ROI, reinterpret_cast<uint8_t*>(&config),
&data_size);
RCLCPP_INFO_STREAM(
logger_, "set depth AE ROI : "
logger_, "Set depth AE ROI to "
<< "[Left: " << config.x0_left << ", Right: " << config.x1_right
<< ", Top: " << config.y0_top << ", Bottom: " << config.y1_bottom << " ]");
<< ", Top: " << config.y0_top << ", Bottom: " << config.y1_bottom << "]");
break;
case OB_STREAM_COLOR:
case OB_STREAM_COLOR_LEFT:
@@ -578,9 +753,9 @@ void OBCameraNode::setAeRoiCallback(const std::shared_ptr<SetArrays ::Request>&
device_->getStructuredData(OB_STRUCT_COLOR_AE_ROI, reinterpret_cast<uint8_t*>(&config),
&data_size);
RCLCPP_INFO_STREAM(
logger_, "set color AE ROI : "
logger_, "Set color AE ROI to "
<< "[Left: " << config.x0_left << ", Right: " << config.x1_right
<< ", Top: " << config.y0_top << ", Bottom: " << config.y1_bottom << " ]");
<< ", Top: " << config.y0_top << ", Bottom: " << config.y1_bottom << "]");
break;
default:
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
@@ -591,7 +766,7 @@ void OBCameraNode::setAeRoiCallback(const std::shared_ptr<SetArrays ::Request>&
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->success = false;
response->message = e.what();
@@ -609,7 +784,7 @@ void OBCameraNode::getWhiteBalanceCallback(const std::shared_ptr<GetInt32::Reque
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->success = false;
response->message = e.what();
@@ -625,13 +800,13 @@ void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr<SetInt32 ::Requ
auto range = device_->getIntPropertyRange(OB_PROP_COLOR_WHITE_BALANCE_INT);
if (request->data < range.min || request->data > range.max) {
response->success = false;
RCLCPP_INFO_STREAM(logger_, "set white balance value out of range");
RCLCPP_WARN_STREAM(logger_, "White balance value is out of range");
response->message = "value out of range";
return;
}
bool auto_white_balance = device_->getBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL);
if (auto_white_balance) {
RCLCPP_WARN(logger_, "auto white balance is enabled, set white balance will be ignored");
RCLCPP_WARN(logger_, "Auto white balance is enabled, set white balance will be ignored");
response->success = false;
response->message = "auto white balance is enabled";
return;
@@ -639,7 +814,7 @@ void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr<SetInt32 ::Requ
device_->setIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT, request->data);
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->message = e.what();
response->success = false;
@@ -657,7 +832,7 @@ void OBCameraNode::getAutoWhiteBalanceCallback(const std::shared_ptr<GetInt32::R
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->message = e.what();
} catch (...) {
@@ -673,7 +848,7 @@ void OBCameraNode::setAutoWhiteBalanceCallback(const std::shared_ptr<SetBool::Re
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->message = e.what();
} catch (...) {
@@ -712,7 +887,7 @@ void OBCameraNode::setAutoExposureCallback(
auto range = device_->getIntPropertyRange(prop_id);
if (request->data < range.min || request->data > range.max) {
response->success = false;
RCLCPP_INFO_STREAM(logger_, "set auto exposure value out of range");
RCLCPP_WARN_STREAM(logger_, "Auto exposure value is out of range");
response->message = "value out of range";
return;
}
@@ -720,7 +895,7 @@ void OBCameraNode::setAutoExposureCallback(
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->message = e.what();
} catch (...) {
@@ -738,7 +913,7 @@ void OBCameraNode::setFanWorkModeCallback(const std::shared_ptr<SetInt32::Reques
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->success = false;
response->message = e.what();
@@ -760,7 +935,7 @@ void OBCameraNode::setFloorEnableCallback(
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->success = false;
response->message = e.what();
@@ -785,7 +960,7 @@ void OBCameraNode::setLaserEnableCallback(
}
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
@@ -821,7 +996,7 @@ void OBCameraNode::setLdpEnableCallback(
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->success = false;
response->message = e.what();
@@ -857,7 +1032,7 @@ void OBCameraNode::getExposureCallback(const std::shared_ptr<GetInt32::Request>&
}
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
@@ -882,7 +1057,7 @@ void OBCameraNode::getDeviceInfoCallback(const std::shared_ptr<GetDeviceInfo::Re
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->success = false;
response->message = e.what();
@@ -906,7 +1081,7 @@ void OBCameraNode::getSDKVersion(const std::shared_ptr<GetString::Request>& requ
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->success = false;
response->message = e.what();
@@ -948,7 +1123,7 @@ void OBCameraNode::setMirrorCallback(const std::shared_ptr<SetBool::Request>& re
}
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
@@ -993,7 +1168,7 @@ void OBCameraNode::setFlipCallback(const std::shared_ptr<SetBool::Request>& requ
}
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
@@ -1038,7 +1213,7 @@ void OBCameraNode::setRotationCallback(const std::shared_ptr<SetInt32::Request>&
}
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
@@ -1056,7 +1231,7 @@ void OBCameraNode::getLdpStatusCallback(const std::shared_ptr<GetBool::Request>&
response->data = device_->getBoolProperty(OB_PROP_LDP_STATUS_BOOL);
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
@@ -1078,7 +1253,7 @@ void OBCameraNode::getLaserStatusCallback(const std::shared_ptr<GetBool::Request
}
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
@@ -1099,14 +1274,14 @@ void OBCameraNode::setPtpConfigCallback(
if (!device_->isPropertySupported(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL,
OB_PERMISSION_READ_WRITE)) {
response->success = false;
RCLCPP_ERROR(logger_, "OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL not supported or not writable");
RCLCPP_ERROR(logger_, "PTP clock sync property is not supported or not writable");
return;
}
device_->setBoolProperty(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL, request->data);
response->success = true;
} catch (const ob::Error& e) {
response->success = false;
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
} catch (const std::exception& e) {
response->success = false;
response->message = e.what();
@@ -1123,7 +1298,7 @@ void OBCameraNode::getPtpConfigCallback(const std::shared_ptr<GetBool::Request>&
response->data = device_->getBoolProperty(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL);
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
@@ -1141,7 +1316,7 @@ void OBCameraNode::getLrmMeasureDistanceCallback(const std::shared_ptr<GetInt32:
response->data = device_->getIntProperty(OB_PROP_LDP_MEASURE_DISTANCE_INT);
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
@@ -1158,18 +1333,19 @@ void OBCameraNode::toggleSensorCallback(const std::shared_ptr<SetBool::Request>&
std::string msg;
if (request->data) {
if (enable_stream_[stream_index]) {
msg = stream_name_[stream_index] + " Already ON";
msg = stream_name_[stream_index] + " is already enabled";
}
RCLCPP_INFO_STREAM(logger_, "toggling sensor " << stream_name_[stream_index] << " ON");
RCLCPP_INFO_STREAM(logger_, "Request to set sensor " << stream_name_[stream_index] << " to ON");
} else {
if (!enable_stream_[stream_index]) {
msg = stream_name_[stream_index] + " Already OFF";
msg = stream_name_[stream_index] + " is already disabled";
}
RCLCPP_INFO_STREAM(logger_, "toggling sensor " << stream_name_[stream_index] << " OFF");
RCLCPP_INFO_STREAM(logger_,
"Request to set sensor " << stream_name_[stream_index] << " to OFF");
}
if (!msg.empty()) {
RCLCPP_ERROR_STREAM(logger_, msg);
RCLCPP_WARN_STREAM(logger_, msg);
response->success = true;
response->message = msg;
return;
@@ -1187,7 +1363,7 @@ bool OBCameraNode::toggleSensor(const stream_index_pair& stream_index, bool enab
startStreams();
return true;
} catch (const ob::Error& e) {
msg = e.getMessage();
msg = orbbec_camera::formatObErrorWithStatus(e);
return false;
} catch (const std::exception& e) {
msg = e.what();
@@ -1236,7 +1412,7 @@ void OBCameraNode::switchIRCameraCallback(const std::shared_ptr<SetString::Reque
response->success = true;
return;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
@@ -1253,7 +1429,7 @@ void OBCameraNode::setIRLongExposureCallback(
device_->setBoolProperty(OB_PROP_IR_LONG_EXPOSURE_BOOL, request->data);
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
@@ -1273,7 +1449,7 @@ void OBCameraNode::setRESETTimestampCallback(
device_->setBoolProperty(OB_PROP_TIMER_RESET_SIGNAL_BOOL, true);
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
@@ -1291,7 +1467,7 @@ void OBCameraNode::setSYNCInterleaveLaserCallback(
device_->setIntProperty(OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT, request->data);
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
@@ -1309,7 +1485,7 @@ void OBCameraNode::setSYNCHostimeCallback(
device_->timerSyncWithHost();
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
@@ -1329,7 +1505,7 @@ void OBCameraNode::sendSoftwareTriggerCallback(
}
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->message = orbbec_camera::formatObErrorWithStatus(e);
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
@@ -1477,35 +1653,36 @@ void OBCameraNode::getUserCalibParamsCallback(
response->message = "exception occurred";
}
}
void OBCameraNode::setAEModeCallback(const std::shared_ptr<SetString::Request>& request,
std::shared_ptr<SetString::Response>& response) {
void OBCameraNode::setAEReferenceStreamCallback(const std::shared_ptr<SetString::Request>& request,
std::shared_ptr<SetString::Response>& response) {
try {
if (device_->isPropertySupported(OB_PROP_DEVICE_AE_REFERENCE_INT, OB_PERMISSION_WRITE) &&
(request->data == "depthbased" || request->data == "colorbased")) {
device_->setIntProperty(OB_PROP_DEVICE_AE_REFERENCE_INT,
request->data == "depthbased" ? 0 : 1);
ae_mode_ = request->data;
(request->data == "depth" || request->data == "color")) {
device_->setIntProperty(OB_PROP_DEVICE_AE_REFERENCE_INT, request->data == "depth" ? 0 : 1);
ae_reference_stream_ = request->data;
response->success = true;
response->message = "set AE mode success";
response->message = "set AE reference stream success";
} else {
response->success = false;
response->message = "set AE mode failed";
response->message = "set AE reference stream failed";
}
} catch (...) {
response->success = false;
response->message = "exception occurred";
}
}
void OBCameraNode::setSportsModeCallback(const std::shared_ptr<SetBool::Request>& request,
std::shared_ptr<SetBool::Response>& response) {
void OBCameraNode::setAEStrategyCallback(const std::shared_ptr<SetString::Request>& request,
std::shared_ptr<SetString::Response>& response) {
try {
if (device_->isPropertySupported(OB_PROP_DEVICE_AE_STRATEGY_INT, OB_PERMISSION_WRITE)) {
device_->setIntProperty(OB_PROP_DEVICE_AE_STRATEGY_INT, request->data ? 1 : 0);
if (device_->isPropertySupported(OB_PROP_DEVICE_AE_STRATEGY_INT, OB_PERMISSION_WRITE) &&
(request->data == "default" || request->data == "motion")) {
device_->setIntProperty(OB_PROP_DEVICE_AE_STRATEGY_INT, request->data == "motion" ? 1 : 0);
ae_strategy_ = request->data;
response->success = true;
response->message = "set sports mode success";
response->message = "set AE strategy success";
} else {
response->success = false;
response->message = "set sports mode failed";
response->message = "set AE strategy failed";
}
} catch (...) {
response->success = false;
orbbec_camera/src/utils.cpp
+77 -7
View File
@@ -540,6 +540,38 @@ float depthPrecisionFromString(const std::string &depth_precision_level_str) {
return std::stof(depth_precision_level_str_num);
}
std::string colorPowerLineFrequencyToString(int value) {
switch (value) {
case 0:
return "disable";
case 1:
return "50hz";
case 2:
return "60hz";
case 3:
return "auto";
default:
return "unknown";
}
}
std::string depthPrecisionLevelToString(int value) {
switch (static_cast<OB_DEPTH_PRECISION_LEVEL>(value)) {
case OB_PRECISION_1MM:
return "1mm";
case OB_PRECISION_0MM8:
return "0.8mm";
case OB_PRECISION_0MM4:
return "0.4mm";
case OB_PRECISION_0MM2:
return "0.2mm";
case OB_PRECISION_0MM1:
return "0.1mm";
default:
return "unknown";
}
}
OBMultiDeviceSyncMode OBSyncModeFromString(const std::string &mode) {
if (mode == "FREE_RUN") {
return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN;
@@ -560,6 +592,43 @@ OBMultiDeviceSyncMode OBSyncModeFromString(const std::string &mode) {
}
}
std::string disparityRangeModeToString(int value) {
switch (value) {
case 0:
return "64";
case 1:
return "128";
case 2:
return "256";
default:
return "unknown";
}
}
std::string exposureRangeModeToString(int value) {
switch (value) {
case 0:
return "regular";
case 1:
return "ultimate";
default:
return "unknown";
}
}
std::string intraCameraSyncReferenceToString(int value) {
switch (value) {
case 0:
return "Start";
case 1:
return "Middle";
case 2:
return "End";
default:
return "unknown";
}
}
OB_SAMPLE_RATE sampleRateFromString(std::string &sample_rate) {
// covert to lower case
std::transform(sample_rate.begin(), sample_rate.end(), sample_rate.begin(), ::tolower);
@@ -941,16 +1010,17 @@ UndistortedImageResult undistortImage(const cv::Mat &image, const OBCameraIntrin
cv::Mat dist_coeffs = (cv::Mat_<float>(8, 1) << distortion.k1, distortion.k2, distortion.p1,
distortion.p2, distortion.k3, distortion.k4, distortion.k5, distortion.k6);
cv::Size image_size(image.cols, image.rows);
cv::Mat new_camera_matrix =
cv::getOptimalNewCameraMatrix(camera_matrix, dist_coeffs, image_size, 0.0, image_size);
// cv::Mat new_camera_matrix =
// cv::getOptimalNewCameraMatrix(camera_matrix, dist_coeffs, image_size, 0.0, image_size);
// Undistort the image using the new camera matrix
cv::undistort(image, result.image, camera_matrix, dist_coeffs, new_camera_matrix);
// cv::undistort(image, result.image, camera_matrix, dist_coeffs, new_camera_matrix);
cv::undistort(image, result.image, camera_matrix, dist_coeffs);
// Update the intrinsic parameters with the new camera matrix
result.new_intrinsic = intrinsic; // Copy original values first
result.new_intrinsic.fx = new_camera_matrix.at<double>(0, 0);
result.new_intrinsic.fy = new_camera_matrix.at<double>(1, 1);
result.new_intrinsic.cx = new_camera_matrix.at<double>(0, 2);
result.new_intrinsic.cy = new_camera_matrix.at<double>(1, 2);
// result.new_intrinsic.fx = new_camera_matrix.at<double>(0, 0);
// result.new_intrinsic.fy = new_camera_matrix.at<double>(1, 1);
// result.new_intrinsic.cx = new_camera_matrix.at<double>(0, 2);
// result.new_intrinsic.cy = new_camera_matrix.at<double>(1, 2);
result.new_intrinsic.width = image.cols;
result.new_intrinsic.height = image.rows;
return result;