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

feat: add runtime image registration mode service

Browse Source
This commit is contained in:
slz
2026-07-08 16:03:06 +08:00
parent 3e58c45f0d
commit b887b3a8e9
3 changed files with 199 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+7
View File
@@ -169,6 +169,8 @@ class OBCameraNode {
void setupProfiles();
void syncSoftwareAlignment();
void updateImageConfig(const stream_index_pair& stream_index);
void printSensorProfiles(const std::shared_ptr<ob::Sensor>& sensor);
@@ -269,6 +271,9 @@ class OBCameraNode {
std::shared_ptr<SetBool::Response>& response,
const stream_index_pair& stream_index);
void setImageRegistrationModeCallback(const std::shared_ptr<SetString::Request> request,
std::shared_ptr<SetString::Response> response);
void setMirrorCallback(const std::shared_ptr<SetBool::Request>& request,
std::shared_ptr<SetBool::Response>& response,
const stream_index_pair& stream_index);
@@ -424,6 +429,7 @@ class OBCameraNode {
rclcpp::Service<SetBool>::SharedPtr set_auto_white_balance_srv_;
rclcpp::Service<GetString>::SharedPtr get_sdk_version_srv_;
rclcpp::Service<SetString>::SharedPtr switch_ir_camera_srv_;
rclcpp::Service<SetString>::SharedPtr set_image_registration_mode_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ir_long_exposure_srv_;
std::map<stream_index_pair, rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr>
set_auto_exposure_srv_;
@@ -530,6 +536,7 @@ class OBCameraNode {
// mjpeg decoder
std::shared_ptr<JPEGDecoder> jpeg_decoder_ = nullptr;
uint8_t* rgb_buffer_ = nullptr;
size_t rgb_buffer_size_ = 0;
bool is_color_frame_decoded_ = false;
std::mutex device_lock_;
// For color
orbbec_camera/src/ob_camera_node.cpp
+74 -11
View File
@@ -92,7 +92,8 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
d2c_viewer_ = std::make_unique<D2CViewer>(node_, rgb_qos, depth_qos);
}
if (enable_stream_[COLOR]) {
rgb_buffer_ = new uint8_t[width_[COLOR] * height_[COLOR] * 3];
rgb_buffer_size_ = static_cast<size_t>(width_[COLOR]) * height_[COLOR] * 3;
rgb_buffer_ = new uint8_t[rgb_buffer_size_];
}
if (enable_colored_point_cloud_ && enable_stream_[DEPTH] && enable_stream_[COLOR]) {
rgb_point_cloud_buffer_size_ = width_[COLOR] * height_[COLOR] * sizeof(OBColorPoint);
@@ -153,6 +154,7 @@ void OBCameraNode::clean() noexcept {
if (rgb_buffer_) {
delete[] rgb_buffer_;
rgb_buffer_ = nullptr;
rgb_buffer_size_ = 0;
}
if (rgb_point_cloud_buffer_) {
delete[] rgb_point_cloud_buffer_;
@@ -278,9 +280,12 @@ void OBCameraNode::setupDevices() {
"Laser energy level set to " << new_laser_energy_level << " (new value)");
}
}
if (depth_registration_) {
if (depth_registration_ && (align_mode_ == "SW" || isGemini335PID(pid))) {
RCLCPP_INFO_STREAM(logger_, "Create align filter");
align_filter_ = std::make_unique<ob::Align>(align_target_stream_);
if (align_mode_ == "SW") {
RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
}
}
if (device_->isPropertySupported(OB_PROP_DISPARITY_TO_DEPTH_BOOL, OB_PERMISSION_READ_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DISPARITY_TO_DEPTH_BOOL, enable_hardware_d2d_);
@@ -885,6 +890,26 @@ void OBCameraNode::setupProfiles() {
}
}
}
void OBCameraNode::syncSoftwareAlignment() {
bool use_software_alignment = depth_registration_ && align_mode_ == "SW";
if (depth_registration_ && align_mode_ == "HW" && device_) {
auto device_info = device_->getDeviceInfo();
CHECK_NOTNULL(device_info.get());
use_software_alignment = isGemini335PID(device_info->pid());
}
if (use_software_alignment) {
if (!align_filter_) {
align_filter_ = std::make_unique<ob::Align>(align_target_stream_);
if (align_mode_ == "SW") {
RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
}
}
return;
}
align_filter_.reset();
}
void OBCameraNode::updateImageConfig(const stream_index_pair &stream_index) {
if (format_[stream_index] == OB_FORMAT_Y8) {
image_format_[stream_index] = CV_8UC1;
@@ -1374,9 +1399,9 @@ void OBCameraNode::setupPipelineConfig() {
CHECK_NOTNULL(device_info.get());
auto pid = device_info->pid();
if (depth_registration_ && enable_stream_[COLOR] && enable_stream_[DEPTH] &&
!isGemini335PID(pid)) {
OBAlignMode align_mode = align_mode_ == "HW" ? ALIGN_D2C_HW_MODE : ALIGN_D2C_SW_MODE;
RCLCPP_INFO_STREAM(logger_, "set align mode to " << magic_enum::enum_name(align_mode));
!isGemini335PID(pid) && align_mode_ == "HW") {
OBAlignMode align_mode = ALIGN_D2C_HW_MODE;
RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
pipeline_config_->setAlignMode(align_mode);
RCLCPP_INFO_STREAM(logger_, "enable depth scale " << (enable_depth_scale_ ? "ON" : "OFF"));
pipeline_config_->setDepthScaleRequire(enable_depth_scale_);
@@ -1900,19 +1925,51 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
has_first_color_frame_ = has_first_color_frame_ || color_frame;
if (isGemini335PID(pid) && depth_frame_) {
depth_frame_ = processDepthFrameFilter(depth_frame_);
if (depth_registration_ && align_filter_ && depth_frame_ && has_first_color_frame_) {
}
if (depth_registration_ && align_filter_ && depth_frame_ && color_frame) {
auto target_frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(align_target_stream_);
if (frame_set->getFrame(target_frame_type)) {
if (align_target_stream_ == OB_STREAM_DEPTH) {
ob::FormatConvertFilter align_color_format_convert_filter;
bool need_convert = true;
switch (color_frame->format()) {
case OB_FORMAT_YUYV:
align_color_format_convert_filter.setFormatConvertType(FORMAT_YUYV_TO_RGB888);
break;
case OB_FORMAT_UYVY:
align_color_format_convert_filter.setFormatConvertType(FORMAT_UYVY_TO_RGB888);
break;
case OB_FORMAT_MJPG:
align_color_format_convert_filter.setFormatConvertType(FORMAT_MJPEG_TO_RGB888);
break;
default:
need_convert = false;
break;
}
if (need_convert) {
auto converted_color_frame = align_color_format_convert_filter.process(color_frame);
if (converted_color_frame) {
ob::FrameHelper::pushFrame(frame_set, OB_FRAME_COLOR, converted_color_frame);
color_frame = converted_color_frame;
} else {
RCLCPP_ERROR_SKIPFIRST_THROTTLE(logger_, *(node_->get_clock()), 1000,
"Failed to convert color frame for C2D alignment");
}
}
}
auto new_frame = align_filter_->process(frame_set);
if (new_frame) {
auto new_frame_set = new_frame->as<ob::FrameSet>();
CHECK_NOTNULL(new_frame_set.get());
depth_frame_ = new_frame_set->getFrame(OB_FRAME_DEPTH);
frame_set = new_frame_set;
depth_frame_ = frame_set->getFrame(OB_FRAME_DEPTH);
color_frame = frame_set->getFrame(OB_FRAME_COLOR);
has_first_color_frame_ = has_first_color_frame_ || color_frame;
} else {
RCLCPP_ERROR(logger_, "Failed to align depth frame to color frame");
RCLCPP_ERROR(logger_, "Failed to align frame set");
}
} else {
RCLCPP_DEBUG(logger_,
"Depth registration is disabled or align filter is null or depth frame is "
"null or color frame is null");
RCLCPP_DEBUG(logger_, "Depth registration target frame is null, skip software alignment");
}
}
if (enable_stream_[COLOR] && color_frame) {
@@ -2052,6 +2109,12 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
RCLCPP_ERROR_STREAM(logger_, "Failed to convert frame to video frame");
return false;
}
if (video_frame->dataSize() > rgb_buffer_size_) {
delete[] rgb_buffer_;
rgb_buffer_size_ = video_frame->dataSize();
rgb_buffer_ = new uint8_t[rgb_buffer_size_];
buffer = rgb_buffer_;
}
CHECK_NOTNULL(buffer);
memcpy(buffer, video_frame->data(), video_frame->dataSize());
return true;
orbbec_camera/src/ros_service.cpp
+118
View File
@@ -15,6 +15,8 @@
*******************************************************************************/
#include "orbbec_camera/ob_camera_node.h"
#include <algorithm>
#include <cctype>
#include <rclcpp/rclcpp.hpp>
#include <nlohmann/json.hpp>
#include <thread>
@@ -172,6 +174,11 @@ void OBCameraNode::setupCameraCtrlServices() {
std::shared_ptr<SetString::Response> response) {
switchIRCameraCallback(request, response);
});
set_image_registration_mode_srv_ = node_->create_service<SetString>(
"set_image_registration_mode", [this](const std::shared_ptr<SetString::Request> request,
std::shared_ptr<SetString::Response> response) {
setImageRegistrationModeCallback(request, response);
});
set_ir_long_exposure_srv_ = node_->create_service<SetBool>(
"set_ir_long_exposure", [this](const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
@@ -756,6 +763,117 @@ bool OBCameraNode::toggleSensor(const stream_index_pair& stream_index, bool enab
}
}
void OBCameraNode::setImageRegistrationModeCallback(
const std::shared_ptr<SetString::Request> request,
std::shared_ptr<SetString::Response> response) {
auto mode = request->data;
std::transform(mode.begin(), mode.end(), mode.begin(),
[](unsigned char ch) { return static_cast<char>(std::toupper(ch)); });
if (mode != "OFF" && mode != "HW_D2C" && mode != "SW_D2C" && mode != "SW_C2D") {
response->success = false;
response->message = "Invalid image registration mode '" + request->data +
"'. Valid values: OFF, HW_D2C, SW_D2C, SW_C2D";
return;
}
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
if (mode != "OFF" && (!enable_stream_[COLOR] || !enable_stream_[DEPTH])) {
response->success = false;
response->message =
"Image registration mode " + mode + " requires both color and depth streams to be enabled";
return;
}
const bool old_depth_registration = depth_registration_;
const std::string old_align_mode = align_mode_;
const OBStreamType old_align_target_stream = align_target_stream_;
const bool was_running = pipeline_started_.load();
auto mode_from_state = [](bool depth_registration, const std::string& align_mode,
OBStreamType align_target_stream) {
if (!depth_registration) {
return std::string("OFF");
}
if (align_mode == "HW") {
return std::string("HW_D2C");
}
return align_target_stream == OB_STREAM_DEPTH ? std::string("SW_C2D") : std::string("SW_D2C");
};
const auto old_mode =
mode_from_state(old_depth_registration, old_align_mode, old_align_target_stream);
auto apply_image_registration_mode = [this](const std::string& mode) {
if (mode == "OFF") {
depth_registration_ = false;
align_mode_ = "HW";
align_target_stream_ = OB_STREAM_COLOR;
} else if (mode == "HW_D2C") {
depth_registration_ = true;
align_mode_ = "HW";
align_target_stream_ = OB_STREAM_COLOR;
} else {
depth_registration_ = true;
align_mode_ = "SW";
align_target_stream_ = mode == "SW_C2D" ? OB_STREAM_DEPTH : OB_STREAM_COLOR;
}
align_filter_.reset();
syncSoftwareAlignment();
};
auto restore_old_mode = [this, old_depth_registration, old_align_mode,
old_align_target_stream]() {
depth_registration_ = old_depth_registration;
align_mode_ = old_align_mode;
align_target_stream_ = old_align_target_stream;
align_filter_.reset();
syncSoftwareAlignment();
};
auto rollback_after_error = [&](const std::string& error_message) {
try {
restore_old_mode();
if (was_running && !pipeline_started_.load()) {
startStreams();
}
response->message = "Failed to set image registration mode to " + mode + ": " +
error_message + ". Rolled back to " + old_mode;
} catch (const std::exception& rollback_error) {
response->message = "Failed to set image registration mode to " + mode + ": " +
error_message + ". Rollback to " + old_mode +
" also failed: " + rollback_error.what();
} catch (...) {
response->message = "Failed to set image registration mode to " + mode + ": " +
error_message + ". Rollback to " + old_mode + " also failed";
}
response->success = false;
};
try {
if (was_running) {
stopStreams();
pipeline_started_.store(false);
}
apply_image_registration_mode(mode);
if (was_running) {
startStreams();
response->message = "Image registration mode changed from " + old_mode + " to " + mode +
"; streams restarted";
} else {
response->message = "Image registration mode set to " + mode + "; streams remain stopped";
}
response->success = true;
} catch (const ob::Error& e) {
rollback_after_error(e.getMessage());
} catch (const std::exception& e) {
rollback_after_error(e.what());
} catch (...) {
rollback_after_error("unknown error");
}
}
void OBCameraNode::saveImageCallback(const std::shared_ptr<std_srvs::srv::Empty::Request>& request,
std::shared_ptr<std_srvs::srv::Empty::Response>& response) {
(void)request;