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

Add enable_3d_reconstruction_mode params for 3d reconstruction scenario

Browse Source
This commit is contained in:
Joe Dong
2024-07-09 13:55:34 +08:00
parent 51bf9ecb33
commit a5a81d1791
7 changed files with 110 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+31 -8
View File
@@ -996,6 +996,11 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<bool>(retry_on_usb3_detection_failure_, "retry_on_usb3_detection_failure",
false);
setAndGetNodeParameter<int>(laser_energy_level_, "laser_energy_level", -1);
setAndGetNodeParameter<bool>(enable_3d_reconstruction_mode_, "enable_3d_reconstruction_mode",
false);
if (enable_3d_reconstruction_mode_) {
laser_on_off_mode_ = 1; // 0 off, 1 on-off, 1 off-on
}
}
void OBCameraNode::setupTopics() {
@@ -1460,6 +1465,10 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
tf_published_ = true;
}
depth_frame_ = frame_set->getFrame(OB_FRAME_DEPTH);
bool depth_laser_status = false;
if (depth_frame_ && depth_frame_->hasMetadata(OB_FRAME_METADATA_TYPE_LASER_STATUS)) {
depth_laser_status = depth_frame_->getMetadataValue(OB_FRAME_METADATA_TYPE_LASER_STATUS) == 1;
}
auto device_info = device_->getDeviceInfo();
CHECK_NOTNULL(device_info.get());
@@ -1484,8 +1493,10 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
}
if (enable_stream_[COLOR] && color_frame) {
std::unique_lock<std::mutex> lock(color_frame_queue_lock_);
color_frame_queue_.push(frame_set);
color_frame_queue_cv_.notify_all();
if (!enable_3d_reconstruction_mode_ || depth_laser_status) {
color_frame_queue_.push(frame_set);
color_frame_queue_cv_.notify_all();
}
} else {
publishPointCloud(frame_set);
}
@@ -1502,12 +1513,21 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
continue;
}
if (stream_index == DEPTH) {
frame = depth_frame_;
frame = depth_laser_status ? depth_frame_ : nullptr;
}
std::shared_ptr<ob::Frame> irFrame = decodeIRMJPGFrame(frame);
if (irFrame) {
onNewFrameCallback(irFrame, stream_index);
} else {
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->format() == 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);
}
}
@@ -1629,6 +1649,9 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
std::shared_ptr<ob::Frame> OBCameraNode::decodeIRMJPGFrame(
const std::shared_ptr<ob::Frame> &frame) {
if (frame == nullptr) {
return nullptr;
}
if (frame->format() == OB_FORMAT_MJPEG &&
(frame->type() == OB_FRAME_IR || frame->type() == OB_FRAME_IR_LEFT ||
frame->type() == OB_FRAME_IR_RIGHT)) {
@@ -1655,7 +1678,7 @@ std::shared_ptr<ob::Frame> OBCameraNode::decodeIRMJPGFrame(
return irFrame;
}
return nullptr;
return frame;
}
void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
orbbec_camera/src/ob_camera_node_driver.cpp
+1 -15
View File
@@ -24,7 +24,6 @@
#include <sys/mman.h>
#include <unistd.h>
namespace orbbec_camera {
OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
: Node("orbbec_camera_node", "/", node_options),
@@ -48,9 +47,6 @@ OBCameraNodeDriver::~OBCameraNodeDriver() {
if (device_count_update_thread_ && device_count_update_thread_->joinable()) {
device_count_update_thread_->join();
}
if (sync_time_thread_ && sync_time_thread_->joinable()) {
sync_time_thread_->join();
}
if (query_thread_ && query_thread_->joinable()) {
query_thread_->join();
}
@@ -104,7 +100,6 @@ void OBCameraNodeDriver::init() {
this->create_wall_timer(std::chrono::milliseconds(1000), [this]() { checkConnectTimer(); });
CHECK_NOTNULL(check_connect_timer_);
query_thread_ = std::make_shared<std::thread>([this]() { queryDevice(); });
sync_time_thread_ = std::make_shared<std::thread>([this]() { syncTime(); });
reset_device_thread_ = std::make_shared<std::thread>([this]() { resetDevice(); });
}
@@ -183,15 +178,6 @@ void OBCameraNodeDriver::queryDevice() {
}
}
void OBCameraNodeDriver::syncTime() {
while (is_alive_ && rclcpp::ok()) {
if (device_ && device_info_ && !isOpenNIDevice(device_info_->pid())) {
ctx_->enableDeviceClockSync(0);
}
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
}
}
void OBCameraNodeDriver::resetDevice() {
while (is_alive_ && rclcpp::ok()) {
std::unique_lock<decltype(reset_device_mutex_)> lock(reset_device_mutex_);
@@ -315,7 +301,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
CHECK_NOTNULL(device_info_.get());
device_unique_id_ = device_info_->uid();
if (!isOpenNIDevice(device_info_->pid())) {
ctx_->enableDeviceClockSync(0); // sync time stamp
ctx_->enableDeviceClockSync(30000); // every 30s sync time
}
RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->name() << " connected");
RCLCPP_INFO_STREAM(logger_, "Serial number: " << device_info_->serialNumber());