i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 12:07:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix Gemini2L laser shutdown caused by LDP shutdown

Browse Source
This commit is contained in:
jj
2025-03-24 16:50:06 +08:00
parent 052846f9ef
commit 5e46ced56e
2 changed files with 44 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+30 -17
View File
@@ -573,14 +573,14 @@ void OBCameraNode::setupDepthPostProcessFilter() {
} else if (filter_name == "NoiseRemovalFilter" && enable_noise_removal_filter_) { } else if (filter_name == "NoiseRemovalFilter" && enable_noise_removal_filter_) {
auto noise_removal_filter = filter->as<ob::NoiseRemovalFilter>(); auto noise_removal_filter = filter->as<ob::NoiseRemovalFilter>();
OBNoiseRemovalFilterParams params = noise_removal_filter->getFilterParams(); OBNoiseRemovalFilterParams params = noise_removal_filter->getFilterParams();
RCLCPP_INFO_STREAM(logger_, "Default noise removal filter params: " RCLCPP_INFO_STREAM(
<< "disp_diff: " << params.disp_diff logger_, "Default noise removal filter params: " << "disp_diff: " << params.disp_diff
<< ", max_size: " << params.max_size); << ", max_size: " << params.max_size);
params.disp_diff = noise_removal_filter_min_diff_; params.disp_diff = noise_removal_filter_min_diff_;
params.max_size = noise_removal_filter_max_size_; params.max_size = noise_removal_filter_max_size_;
RCLCPP_INFO_STREAM(logger_, "Set noise removal filter params: " RCLCPP_INFO_STREAM(logger_,
<< "disp_diff: " << params.disp_diff "Set noise removal filter params: " << "disp_diff: " << params.disp_diff
<< ", max_size: " << params.max_size); << ", max_size: " << params.max_size);
if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) { if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) {
noise_removal_filter->setFilterParams(params); noise_removal_filter->setFilterParams(params);
} }
@@ -589,11 +589,11 @@ void OBCameraNode::setupDepthPostProcessFilter() {
hdr_merge_gain_2_ != -1) { hdr_merge_gain_2_ != -1) {
auto hdr_merge_filter = filter->as<ob::HdrMerge>(); auto hdr_merge_filter = filter->as<ob::HdrMerge>();
hdr_merge_filter->enable(true); hdr_merge_filter->enable(true);
RCLCPP_INFO_STREAM(logger_, "Set HDR merge filter params: " RCLCPP_INFO_STREAM(
<< "exposure_1: " << hdr_merge_exposure_1_ logger_, "Set HDR merge filter params: " << "exposure_1: " << hdr_merge_exposure_1_
<< ", gain_1: " << hdr_merge_gain_1_ << ", gain_1: " << hdr_merge_gain_1_
<< ", exposure_2: " << hdr_merge_exposure_2_ << ", exposure_2: " << hdr_merge_exposure_2_
<< ", gain_2: " << hdr_merge_gain_2_); << ", gain_2: " << hdr_merge_gain_2_);
auto config = OBHdrConfig(); auto config = OBHdrConfig();
config.enable = true; config.enable = true;
config.exposure_1 = hdr_merge_exposure_1_; config.exposure_1 = hdr_merge_exposure_1_;
@@ -676,10 +676,10 @@ void OBCameraNode::setupProfiles() {
throw std::runtime_error("Failed cast profile to VideoStreamProfile"); throw std::runtime_error("Failed cast profile to VideoStreamProfile");
} }
RCLCPP_DEBUG_STREAM( RCLCPP_DEBUG_STREAM(
logger_, "Sensor profile: " logger_,
<< "stream_type: " << magic_enum::enum_name(profile->type()) "Sensor profile: " << "stream_type: " << magic_enum::enum_name(profile->type())
<< "Format: " << profile->format() << ", Width: " << profile->width() << "Format: " << profile->format() << ", Width: " << profile->width()
<< ", Height: " << profile->height() << ", FPS: " << profile->fps()); << ", Height: " << profile->height() << ", FPS: " << profile->fps());
supported_profiles_[elem].emplace_back(profile); supported_profiles_[elem].emplace_back(profile);
} }
std::shared_ptr<ob::VideoStreamProfile> selected_profile; std::shared_ptr<ob::VideoStreamProfile> selected_profile;
@@ -830,9 +830,22 @@ void OBCameraNode::startStreams() {
RCLCPP_INFO_STREAM(logger_, "Disable frame sync"); RCLCPP_INFO_STREAM(logger_, "Disable frame sync");
TRY_EXECUTE_BLOCK(pipeline_->disableFrameSync()); TRY_EXECUTE_BLOCK(pipeline_->disableFrameSync());
} }
if (device_->isPropertySupported(OB_PROP_LDP_BOOL, OB_PERMISSION_WRITE)) { if (device_->isPropertySupported(OB_PROP_LDP_BOOL, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting LDP to " << (enable_ldp_ ? "ON" : "OFF")); RCLCPP_INFO_STREAM(logger_, "Setting LDP to " << (enable_ldp_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_LDP_BOOL, enable_ldp_); if (device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
auto laser_enable = device_->getIntProperty(OB_PROP_LASER_CONTROL_INT);
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_LDP_BOOL, enable_ldp_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_CONTROL_INT, laser_enable);
} else if (device_->isPropertySupported(OB_PROP_LASER_BOOL, OB_PERMISSION_READ_WRITE)) {
if (!enable_ldp_) {
auto laser_enable = device_->getIntProperty(OB_PROP_LASER_BOOL);
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_LDP_BOOL, enable_ldp_);
std::this_thread::sleep_for(std::chrono::milliseconds(3));
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_BOOL, laser_enable);
} else {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_LDP_BOOL, enable_ldp_);
}
}
} }
pipeline_started_.store(true); pipeline_started_.store(true);
} }
orbbec_camera/src/ros_service.cpp
+14 -1
View File
@@ -487,7 +487,20 @@ void OBCameraNode::setLdpEnableCallback(
(void)response; (void)response;
bool ldp_enable = request->data; bool ldp_enable = request->data;
try { try {
device_->setBoolProperty(OB_PROP_LDP_BOOL, ldp_enable); if (device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
auto laser_enable = device_->getIntProperty(OB_PROP_LASER_CONTROL_INT);
device_->setBoolProperty(OB_PROP_LDP_BOOL, ldp_enable);
device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, laser_enable);
} else if (device_->isPropertySupported(OB_PROP_LASER_BOOL, OB_PERMISSION_READ_WRITE)) {
if (!ldp_enable) {
auto laser_enable = device_->getIntProperty(OB_PROP_LASER_BOOL);
device_->setBoolProperty(OB_PROP_LDP_BOOL, ldp_enable);
std::this_thread::sleep_for(std::chrono::milliseconds(3));
device_->setIntProperty(OB_PROP_LASER_BOOL, laser_enable);
} else {
device_->setBoolProperty(OB_PROP_LDP_BOOL, ldp_enable);
}
}
response->success = true; response->success = true;
} catch (const ob::Error& e) { } catch (const ob::Error& e) {
response->success = false; response->success = false;