From b887b3a8e9ff159bc1967e2f2b94bdb555ce7da8 Mon Sep 17 00:00:00 2001 From: slz Date: Wed, 8 Jul 2026 16:03:06 +0800 Subject: [PATCH] feat: add runtime image registration mode service --- .../include/orbbec_camera/ob_camera_node.h | 7 ++ orbbec_camera/src/ob_camera_node.cpp | 85 +++++++++++-- orbbec_camera/src/ros_service.cpp | 118 ++++++++++++++++++ 3 files changed, 199 insertions(+), 11 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index cd906450..096329a1 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -169,6 +169,8 @@ class OBCameraNode { void setupProfiles(); + void syncSoftwareAlignment(); + void updateImageConfig(const stream_index_pair& stream_index); void printSensorProfiles(const std::shared_ptr& sensor); @@ -269,6 +271,9 @@ class OBCameraNode { std::shared_ptr& response, const stream_index_pair& stream_index); + void setImageRegistrationModeCallback(const std::shared_ptr request, + std::shared_ptr response); + void setMirrorCallback(const std::shared_ptr& request, std::shared_ptr& response, const stream_index_pair& stream_index); @@ -424,6 +429,7 @@ class OBCameraNode { rclcpp::Service::SharedPtr set_auto_white_balance_srv_; rclcpp::Service::SharedPtr get_sdk_version_srv_; rclcpp::Service::SharedPtr switch_ir_camera_srv_; + rclcpp::Service::SharedPtr set_image_registration_mode_srv_; rclcpp::Service::SharedPtr set_ir_long_exposure_srv_; std::map::SharedPtr> set_auto_exposure_srv_; @@ -530,6 +536,7 @@ class OBCameraNode { // mjpeg decoder std::shared_ptr 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 diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 7f83acd5..b99672a5 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -92,7 +92,8 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr devic d2c_viewer_ = std::make_unique(node_, rgb_qos, depth_qos); } if (enable_stream_[COLOR]) { - rgb_buffer_ = new uint8_t[width_[COLOR] * height_[COLOR] * 3]; + rgb_buffer_size_ = static_cast(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(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(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 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(); 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 &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; diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index e675f936..bae1b78c 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -15,6 +15,8 @@ *******************************************************************************/ #include "orbbec_camera/ob_camera_node.h" +#include +#include #include #include #include @@ -172,6 +174,11 @@ void OBCameraNode::setupCameraCtrlServices() { std::shared_ptr response) { switchIRCameraCallback(request, response); }); + set_image_registration_mode_srv_ = node_->create_service( + "set_image_registration_mode", [this](const std::shared_ptr request, + std::shared_ptr response) { + setImageRegistrationModeCallback(request, response); + }); set_ir_long_exposure_srv_ = node_->create_service( "set_ir_long_exposure", [this](const std::shared_ptr request, std::shared_ptr response) { @@ -756,6 +763,117 @@ bool OBCameraNode::toggleSensor(const stream_index_pair& stream_index, bool enab } } +void OBCameraNode::setImageRegistrationModeCallback( + const std::shared_ptr request, + std::shared_ptr response) { + auto mode = request->data; + std::transform(mode.begin(), mode.end(), mode.begin(), + [](unsigned char ch) { return static_cast(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 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& request, std::shared_ptr& response) { (void)request;