mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-09-10 10:20:19 +08:00
Merge branch 'feature/runtime-image-registration-mode-service' into merge/ros_2.9.2
This commit is contained in:
slz
@@ -18,6 +18,7 @@
|
||||
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
#include <cstddef>
|
||||
#include <memory>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <string>
|
||||
@@ -294,6 +295,8 @@ class OBCameraNode {
|
||||
|
||||
void setupPublishers();
|
||||
|
||||
void syncSoftwareAlignment();
|
||||
|
||||
void publishDepthFiltersStatus();
|
||||
|
||||
void publishLrmObstacleDistance();
|
||||
@@ -331,6 +334,8 @@ class OBCameraNode {
|
||||
|
||||
void setStreamsEnableCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
|
||||
std::shared_ptr<std_srvs::srv::SetBool::Response> response);
|
||||
void setImageRegistrationModeCallback(const std::shared_ptr<SetString::Request> request,
|
||||
std::shared_ptr<SetString::Response> response);
|
||||
|
||||
void getStreamsEnableCallback(
|
||||
const std::shared_ptr<orbbec_camera_msgs::srv::GetBool::Request> request,
|
||||
@@ -702,6 +707,7 @@ class OBCameraNode {
|
||||
rclcpp::Service<SetInt32>::SharedPtr set_sync_io_voltage_level_srv_;
|
||||
rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_streams_enable_srv_;
|
||||
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_streams_enable_srv_;
|
||||
rclcpp::Service<SetString>::SharedPtr set_image_registration_mode_srv_;
|
||||
rclcpp::Service<GetUserCalibParams>::SharedPtr get_user_calib_params_srv_;
|
||||
rclcpp::Service<SetUserCalibParams>::SharedPtr set_user_calib_params_srv_;
|
||||
rclcpp::Service<SetString>::SharedPtr set_ae_reference_stream_srv_;
|
||||
@@ -835,6 +841,9 @@ class OBCameraNode {
|
||||
uint8_t* rgb_buffer_ = nullptr;
|
||||
uint8_t* rgb_buffer_left_ = nullptr;
|
||||
uint8_t* rgb_buffer_right_ = nullptr;
|
||||
size_t rgb_buffer_size_ = 0;
|
||||
size_t rgb_buffer_left_size_ = 0;
|
||||
size_t rgb_buffer_right_size_ = 0;
|
||||
bool is_left_color_frame_decoded_ = false;
|
||||
bool is_right_color_frame_decoded_ = false;
|
||||
bool is_color_frame_decoded_ = false;
|
||||
|
||||
@@ -739,13 +739,16 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
|
||||
d2c_viewer_ = std::make_unique<D2CViewer>(node_, rgb_qos, depth_qos, use_intra_process_);
|
||||
}
|
||||
if (enable_stream_[COLOR]) {
|
||||
rgb_buffer_ = new uint8_t[width_[COLOR] * height_[COLOR] * 4];
|
||||
rgb_buffer_size_ = static_cast<size_t>(width_[COLOR]) * height_[COLOR] * 4;
|
||||
rgb_buffer_ = new uint8_t[rgb_buffer_size_];
|
||||
}
|
||||
if (enable_stream_[COLOR_LEFT]) {
|
||||
rgb_buffer_left_ = new uint8_t[width_[COLOR_LEFT] * height_[COLOR_LEFT] * 4];
|
||||
rgb_buffer_left_size_ = static_cast<size_t>(width_[COLOR_LEFT]) * height_[COLOR_LEFT] * 4;
|
||||
rgb_buffer_left_ = new uint8_t[rgb_buffer_left_size_];
|
||||
}
|
||||
if (enable_stream_[COLOR_RIGHT]) {
|
||||
rgb_buffer_right_ = new uint8_t[width_[COLOR_RIGHT] * height_[COLOR_RIGHT] * 4];
|
||||
rgb_buffer_right_size_ = static_cast<size_t>(width_[COLOR_RIGHT]) * height_[COLOR_RIGHT] * 4;
|
||||
rgb_buffer_right_ = new uint8_t[rgb_buffer_right_size_];
|
||||
}
|
||||
if (enable_colored_point_cloud_ && enable_stream_[DEPTH] && enable_stream_[COLOR]) {
|
||||
rgb_point_cloud_buffer_size_ = width_[COLOR] * height_[COLOR] * sizeof(OBColorPoint);
|
||||
@@ -903,10 +906,13 @@ void OBCameraNode::clean() noexcept {
|
||||
try {
|
||||
delete[] rgb_buffer_;
|
||||
rgb_buffer_ = nullptr;
|
||||
rgb_buffer_size_ = 0;
|
||||
delete[] rgb_buffer_left_;
|
||||
rgb_buffer_left_ = nullptr;
|
||||
rgb_buffer_left_size_ = 0;
|
||||
delete[] rgb_buffer_right_;
|
||||
rgb_buffer_right_ = nullptr;
|
||||
rgb_buffer_right_size_ = 0;
|
||||
|
||||
if (jpeg_decoder_) {
|
||||
jpeg_decoder_.reset();
|
||||
@@ -1082,6 +1088,7 @@ void OBCameraNode::setupDevices() {
|
||||
if (depth_registration_ && align_mode_ == "SW") {
|
||||
RCLCPP_DEBUG_STREAM(logger_, "Create align filter");
|
||||
align_filter_ = std::make_unique<ob::Align>(align_target_stream_);
|
||||
RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
|
||||
}
|
||||
if (should_apply_launch_config("disparity_to_depth_mode") && !disparity_to_depth_mode_.empty() &&
|
||||
sensors_.find(DEPTH) != sensors_.end() &&
|
||||
@@ -4588,7 +4595,7 @@ void OBCameraNode::setupPipelineConfig() {
|
||||
if (depth_registration_ && enable_stream_[COLOR] && enable_stream_[DEPTH] &&
|
||||
align_mode_ == "HW") {
|
||||
OBAlignMode align_mode = ALIGN_D2C_HW_MODE;
|
||||
RCLCPP_INFO_STREAM(logger_, "set align mode to " << magic_enum::enum_name(align_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_);
|
||||
@@ -4701,19 +4708,7 @@ void OBCameraNode::setupPublishers() {
|
||||
}
|
||||
}
|
||||
|
||||
if (depth_registration_ && align_mode_ == "SW") {
|
||||
auto depth_image_qos_profile = getRMWQosProfileFromString(image_qos_[DEPTH]);
|
||||
if (use_intra_process_) {
|
||||
depth_image_qos_profile = rmw_qos_profile_default;
|
||||
}
|
||||
if (use_intra_process_) {
|
||||
depth_unaligned_publisher_ = std::make_shared<image_rcl_publisher>(
|
||||
*node_, "depth/image_unaligned", depth_image_qos_profile);
|
||||
} else {
|
||||
depth_unaligned_publisher_ = std::make_shared<image_transport_publisher>(
|
||||
*node_, "depth/image_unaligned", depth_image_qos_profile);
|
||||
}
|
||||
}
|
||||
syncSoftwareAlignment();
|
||||
|
||||
if (enable_sync_output_accel_gyro_) {
|
||||
std::string topic_name = stream_name_[GYRO] + "_" + stream_name_[ACCEL] + "/sample";
|
||||
@@ -4805,6 +4800,32 @@ void OBCameraNode::setupPublishers() {
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::syncSoftwareAlignment() {
|
||||
if (depth_registration_ && align_mode_ == "SW") {
|
||||
if (!align_filter_) {
|
||||
align_filter_ = std::make_unique<ob::Align>(align_target_stream_);
|
||||
RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
|
||||
}
|
||||
if (!depth_unaligned_publisher_) {
|
||||
auto depth_image_qos_profile = getRMWQosProfileFromString(image_qos_[DEPTH]);
|
||||
if (use_intra_process_) {
|
||||
depth_image_qos_profile = rmw_qos_profile_default;
|
||||
}
|
||||
if (use_intra_process_) {
|
||||
depth_unaligned_publisher_ = std::make_shared<image_rcl_publisher>(
|
||||
*node_, "depth/image_unaligned", depth_image_qos_profile);
|
||||
} else {
|
||||
depth_unaligned_publisher_ = std::make_shared<image_transport_publisher>(
|
||||
*node_, "depth/image_unaligned", depth_image_qos_profile);
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
align_filter_.reset();
|
||||
depth_unaligned_publisher_.reset();
|
||||
}
|
||||
|
||||
void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
|
||||
try {
|
||||
if (depth_registration_ || enable_colored_point_cloud_) {
|
||||
@@ -5442,13 +5463,49 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
|
||||
}
|
||||
if (depth_registration_ && align_filter_ && depth_frame) {
|
||||
publishRawDepthImage(depth_frame);
|
||||
if (auto new_frame = align_filter_->process(frame_set)) {
|
||||
auto new_frame_set = new_frame->as<ob::FrameSet>();
|
||||
CHECK_NOTNULL(new_frame_set.get());
|
||||
frame_set = new_frame_set;
|
||||
auto target_frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(align_target_stream_);
|
||||
if (!frame_set->getFrame(target_frame_type) || !color_frame) {
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
logger_, "Depth registration requires depth and color frames, skip software alignment");
|
||||
} else {
|
||||
RCLCPP_ERROR(logger_, "Failed to align depth frame to color frame");
|
||||
return;
|
||||
auto align_color_frame = color_frame;
|
||||
if (align_target_stream_ == OB_STREAM_DEPTH) {
|
||||
ob::FormatConvertFilter align_color_format_convert_filter;
|
||||
switch (color_frame->getFormat()) {
|
||||
case OB_FORMAT_YUYV:
|
||||
align_color_format_convert_filter.setFormatConvertType(FORMAT_YUYV_TO_RGB888);
|
||||
align_color_frame = align_color_format_convert_filter.process(color_frame);
|
||||
break;
|
||||
case OB_FORMAT_UYVY:
|
||||
align_color_format_convert_filter.setFormatConvertType(FORMAT_UYVY_TO_RGB888);
|
||||
align_color_frame = align_color_format_convert_filter.process(color_frame);
|
||||
break;
|
||||
case OB_FORMAT_MJPG:
|
||||
align_color_format_convert_filter.setFormatConvertType(FORMAT_MJPEG_TO_RGB888);
|
||||
align_color_frame = align_color_format_convert_filter.process(color_frame);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
if (!align_color_frame) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to convert color frame for C2D alignment");
|
||||
} else if (align_color_frame != color_frame) {
|
||||
color_frame = align_color_frame;
|
||||
frame_set->pushFrame(color_frame);
|
||||
}
|
||||
}
|
||||
if (align_color_frame) {
|
||||
if (auto new_frame = align_filter_->process(frame_set)) {
|
||||
auto new_frame_set = new_frame->as<ob::FrameSet>();
|
||||
CHECK_NOTNULL(new_frame_set.get());
|
||||
frame_set = new_frame_set;
|
||||
depth_frame = frame_set->getFrame(OB_FRAME_DEPTH);
|
||||
color_frame = frame_set->getFrame(OB_FRAME_COLOR);
|
||||
} else {
|
||||
RCLCPP_ERROR(logger_, "Failed to align depth frame to color frame");
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
RCLCPP_DEBUG_ONCE(logger_,
|
||||
@@ -5755,6 +5812,24 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to convert frame to video frame");
|
||||
return false;
|
||||
}
|
||||
uint8_t **target_buffer = nullptr;
|
||||
size_t *target_buffer_size = nullptr;
|
||||
if (stream_index == COLOR_LEFT) {
|
||||
target_buffer = &rgb_buffer_left_;
|
||||
target_buffer_size = &rgb_buffer_left_size_;
|
||||
} else if (stream_index == COLOR_RIGHT) {
|
||||
target_buffer = &rgb_buffer_right_;
|
||||
target_buffer_size = &rgb_buffer_right_size_;
|
||||
} else {
|
||||
target_buffer = &rgb_buffer_;
|
||||
target_buffer_size = &rgb_buffer_size_;
|
||||
}
|
||||
if (video_frame->getDataSize() > *target_buffer_size) {
|
||||
delete[](*target_buffer);
|
||||
*target_buffer_size = video_frame->getDataSize();
|
||||
*target_buffer = new uint8_t[*target_buffer_size];
|
||||
buffer = *target_buffer;
|
||||
}
|
||||
CHECK_NOTNULL(buffer);
|
||||
memcpy(buffer, video_frame->getData(), video_frame->getDataSize());
|
||||
return true;
|
||||
|
||||
@@ -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>
|
||||
@@ -351,6 +353,11 @@ void OBCameraNode::setupCameraCtrlServices() {
|
||||
std::shared_ptr<SetBool::Response> response) {
|
||||
setStreamsEnableCallback(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);
|
||||
});
|
||||
get_streams_enable_srv_ = node_->create_service<GetBool>(
|
||||
"get_streams_enable", [this](const std::shared_ptr<GetBool::Request> request,
|
||||
std::shared_ptr<GetBool::Response> response) {
|
||||
@@ -632,6 +639,116 @@ void OBCameraNode::setStreamsEnableCallback(
|
||||
}
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
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(orbbec_camera::formatObErrorWithStatus(e));
|
||||
} catch (const std::exception& e) {
|
||||
rollback_after_error(e.what());
|
||||
} catch (...) {
|
||||
rollback_after_error("unknown error");
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::getStreamsEnableCallback(
|
||||
const std::shared_ptr<orbbec_camera_msgs::srv::GetBool::Request> request,
|
||||
std::shared_ptr<orbbec_camera_msgs::srv::GetBool::Response> response) {
|
||||
|
||||
Reference in New Issue
Block a user