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

replace service enable_laser connector

Browse Source
This commit is contained in:
jj
2024-10-19 16:33:42 +08:00
parent e581d4334a
commit 1a84b65f7b
2 changed files with 51 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+49 -53
View File
@@ -472,7 +472,7 @@ void OBCameraNode::setupDepthPostProcessFilter() {
// set depth sensor to filter
filter_list_ = depth_sensor->getRecommendedFilters();
if (!filter_list_.empty()) {
RCLCPP_ERROR(logger_, "Failed to get depth sensor filter list");
// RCLCPP_ERROR(logger_, "Failed to get depth sensor filter list");
return;
}
for (size_t i = 0; i < filter_list_.size(); i++) {
@@ -1742,69 +1742,65 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
auto new_frame_set = new_frame->as<ob::FrameSet>();
CHECK_NOTNULL(new_frame_set.get());
frame_set = new_frame_set;
} else {
RCLCPP_ERROR(logger_, "Failed to align depth frame to color frame");
return;
}
} else {
RCLCPP_ERROR(logger_, "Failed to align depth frame to color frame");
return;
RCLCPP_DEBUG(logger_,
"Depth registration is disabled or align filter is null or depth frame is "
"null or color frame is null");
}
}
if (enable_stream_[COLOR] && color_frame) {
std::unique_lock<std::mutex> lock(color_frame_queue_lock_);
if (!enable_3d_reconstruction_mode_ || depth_laser_status) {
color_frame_queue_.push(frame_set);
color_frame_queue_cv_.notify_all();
}
} else {
RCLCPP_DEBUG(logger_,
"Depth registration is disabled or align filter is null or depth frame is "
"null or color frame is null");
publishPointCloud(frame_set);
}
}
if (enable_stream_[COLOR] && color_frame) {
std::unique_lock<std::mutex> lock(color_frame_queue_lock_);
if (!enable_3d_reconstruction_mode_ || depth_laser_status) {
color_frame_queue_.push(frame_set);
color_frame_queue_cv_.notify_all();
}
} else {
publishPointCloud(frame_set);
}
for (const auto &stream_index : IMAGE_STREAMS) {
if (enable_stream_[stream_index]) {
auto frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(stream_index.first);
if (frame_type == OB_FRAME_COLOR) {
continue;
}
auto frame = frame_set->getFrame(frame_type);
if (frame == nullptr) {
continue;
}
if (stream_index == DEPTH) {
frame = (enable_3d_reconstruction_mode_ && !depth_laser_status) ? nullptr : frame;
}
auto is_ir_frame = frame_type == OB_FRAME_IR_LEFT || frame_type == OB_FRAME_IR_RIGHT ||
frame_type == OB_FRAME_IR;
if (is_ir_frame) {
std::shared_ptr<ob::Frame> ir_frame =
frame->getFormat() == OB_FORMAT_MJPG ? decodeIRMJPGFrame(frame) : frame;
bool ir_laser_status = false;
if (ir_frame && ir_frame->hasMetadata(OB_FRAME_METADATA_TYPE_LASER_STATUS)) {
ir_laser_status = ir_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_LASER_STATUS) == 1;
for (const auto &stream_index : IMAGE_STREAMS) {
if (enable_stream_[stream_index]) {
auto frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(stream_index.first);
if (frame_type == OB_FRAME_COLOR) {
continue;
}
if (ir_frame && (!enable_3d_reconstruction_mode_ || !ir_laser_status)) {
onNewFrameCallback(ir_frame, stream_index);
auto frame = frame_set->getFrame(frame_type);
if (frame == nullptr) {
continue;
}
if (stream_index == DEPTH) {
frame = (enable_3d_reconstruction_mode_ && !depth_laser_status) ? nullptr : frame;
}
auto is_ir_frame = frame_type == OB_FRAME_IR_LEFT || frame_type == OB_FRAME_IR_RIGHT ||
frame_type == OB_FRAME_IR;
if (is_ir_frame) {
std::shared_ptr<ob::Frame> ir_frame =
frame->getFormat() == OB_FORMAT_MJPG ? decodeIRMJPGFrame(frame) : frame;
bool ir_laser_status = false;
if (ir_frame && ir_frame->hasMetadata(OB_FRAME_METADATA_TYPE_LASER_STATUS)) {
ir_laser_status = ir_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_LASER_STATUS) == 1;
}
if (ir_frame && (!enable_3d_reconstruction_mode_ || !ir_laser_status)) {
onNewFrameCallback(ir_frame, stream_index);
}
} else if (frame_type == OB_FRAME_DEPTH) {
onNewFrameCallback(frame, stream_index);
}
} else if (frame_type == OB_FRAME_DEPTH) {
onNewFrameCallback(frame, stream_index);
}
}
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.getMessage());
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.what());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: unknown error");
}
}
catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.getMessage());
}
catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.what());
}
catch (...) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: unknown error");
}
}
void OBCameraNode::onNewColorFrameCallback() {
while (enable_stream_[COLOR] && rclcpp::ok() && is_running_.load()) {
orbbec_camera/src/ros_service.cpp
+2 -2
View File
@@ -459,9 +459,9 @@ void OBCameraNode::setLaserEnableCallback(
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
(void)request_header;
(void)response;
bool laser_enable = request->data;
int laser_enable = request->data? 1 : 0;
try {
device_->setBoolProperty(OB_PROP_LASER_BOOL, laser_enable);
device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, laser_enable);
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();