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

Alignment Closed Source

Browse Source
This commit is contained in:
jj
2024-09-23 20:22:23 +08:00
parent 8b6bdfcd5d
commit bbed84961a
42 changed files with 909 additions and 385 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/image_publisher.cpp
+48
View File
@@ -0,0 +1,48 @@
// Copyright 2023 Intel Corporation. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "orbbec_camera/image_publisher.h"
namespace orbbec_camera {
// --- image_rcl_publisher implementation ---
image_rcl_publisher::image_rcl_publisher(rclcpp::Node& node, const std::string& topic_name,
const rmw_qos_profile_t& qos) {
image_publisher_impl = node.create_publisher<sensor_msgs::msg::Image>(
topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos));
}
void image_rcl_publisher::publish(sensor_msgs::msg::Image::UniquePtr image_ptr) {
image_publisher_impl->publish(std::move(image_ptr));
}
size_t image_rcl_publisher::get_subscription_count() const {
return image_publisher_impl->get_subscription_count();
}
// --- image_transport_publisher implementation ---
image_transport_publisher::image_transport_publisher(rclcpp::Node& node,
const std::string& topic_name,
const rmw_qos_profile_t& qos) {
image_publisher_impl = std::make_shared<image_transport::Publisher>(
image_transport::create_publisher(&node, topic_name, qos));
}
void image_transport_publisher::publish(sensor_msgs::msg::Image::UniquePtr image_ptr) {
image_publisher_impl->publish(*image_ptr);
}
size_t image_transport_publisher::get_subscription_count() const {
return image_publisher_impl->getNumSubscribers();
}
} // namespace orbbec_camera
orbbec_camera/src/ob_camera_node.cpp
+259 -107
View File
@@ -35,11 +35,14 @@ namespace orbbec_camera {
using namespace std::chrono_literals;
OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> device,
std::shared_ptr<Parameters> parameters)
std::shared_ptr<Parameters> parameters, bool use_intra_process)
: node_(node),
device_(std::move(device)),
parameters_(std::move(parameters)),
logger_(node->get_logger()) {
logger_(node->get_logger()),
use_intra_process_(use_intra_process) {
RCLCPP_INFO_STREAM(logger_,
"OBCameraNode: use_intra_process: " << (use_intra_process ? "ON" : "OFF"));
is_running_.store(true);
stream_name_[COLOR] = "color";
stream_name_[DEPTH] = "depth";
@@ -94,9 +97,11 @@ void OBCameraNode::setAndGetNodeParameter(
OBCameraNode::~OBCameraNode() noexcept { clean(); }
void OBCameraNode::rebootDevice() {
RCLCPP_WARN_STREAM(logger_, "Reboot device");
clean();
if (device_) {
device_->reboot();
RCLCPP_WARN_STREAM(logger_, "Reboot device DONE");
}
}
@@ -104,10 +109,11 @@ void OBCameraNode::clean() noexcept {
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBCameraNode");
is_running_.store(false);
RCLCPP_WARN_STREAM(logger_, "Stop tf thread");
if (tf_thread_ && tf_thread_->joinable()) {
tf_thread_->join();
}
RCLCPP_WARN_STREAM(logger_, "Stop color frame thread");
if (colorFrameThread_ && colorFrameThread_->joinable()) {
color_frame_queue_cv_.notify_all();
colorFrameThread_->join();
@@ -116,7 +122,6 @@ void OBCameraNode::clean() noexcept {
RCLCPP_WARN_STREAM(logger_, "stop streams");
stopStreams();
stopIMU();
RCLCPP_WARN_STREAM(logger_, "Destroy ~OBCameraNode DONE");
if (rgb_buffer_) {
delete[] rgb_buffer_;
rgb_buffer_ = nullptr;
@@ -129,9 +134,9 @@ void OBCameraNode::clean() noexcept {
delete[] xy_table_data_;
xy_table_data_ = nullptr;
}
RCLCPP_WARN_STREAM(logger_, "Destroy ~OBCameraNode DONE");
}
void OBCameraNode::setupDevices() {
auto sensor_list = device_->getSensorList();
for (size_t i = 0; i < sensor_list->getCount(); i++) {
@@ -162,20 +167,23 @@ void OBCameraNode::setupDevices() {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DEVICE_USB3_REPEAT_IDENTIFY_BOOL,
retry_on_usb3_detection_failure_);
}
// if (device_->isPropertySupported(OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_BOOL,
// OB_PERMISSION_READ_WRITE)) {
// TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_BOOL,
// enable_noise_removal_filter_);
// }
// if (device_->isPropertySupported(OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_BOOL,
// OB_PERMISSION_READ_WRITE)) {
// TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_BOOL,
// enable_noise_removal_filter_);
// }
if (device_->isPropertySupported(OB_PROP_HEARTBEAT_BOOL, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting heartbeat to " << (enable_heartbeat_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_);
}
if (max_depth_limit_ > 0 &&
device_->isPropertySupported(OB_PROP_MAX_DEPTH_INT, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting max depth limit to " << max_depth_limit_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_MAX_DEPTH_INT, max_depth_limit_);
}
if (min_depth_limit_ > 0 &&
device_->isPropertySupported(OB_PROP_MIN_DEPTH_INT, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting min depth limit to " << min_depth_limit_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_MIN_DEPTH_INT, min_depth_limit_);
}
if (laser_energy_level_ != -1 &&
@@ -193,6 +201,7 @@ void OBCameraNode::setupDevices() {
}
}
if (depth_registration_) {
RCLCPP_INFO_STREAM(logger_, "Create align filter");
align_filter_ = std::make_unique<ob::Align>(align_target_stream_);
}
if (device_->isPropertySupported(OB_PROP_DISPARITY_TO_DEPTH_BOOL, OB_PERMISSION_READ_WRITE)) {
@@ -202,12 +211,15 @@ void OBCameraNode::setupDevices() {
RCLCPP_INFO_STREAM(logger_, "Depth process is " << d2d_mode);
}
if (device_->isPropertySupported(OB_PROP_LDP_BOOL, OB_PERMISSION_READ_WRITE)) {
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)) {
RCLCPP_INFO_STREAM(logger_, "Setting laser control to " << enable_laser_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_CONTROL_INT, enable_laser_);
}
if (device_->isPropertySupported(OB_PROP_LASER_ON_OFF_MODE_INT, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting laser on off mode to " << laser_on_off_mode_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_ON_OFF_MODE_INT, laser_on_off_mode_);
}
if (!device_preset_.empty()) {
@@ -218,7 +230,7 @@ void OBCameraNode::setupDevices() {
RCLCPP_INFO_STREAM(logger_, "Preset " << i << ": " << preset_list->getName(i));
}
RCLCPP_INFO_STREAM(logger_, "Load device preset: " << device_preset_);
device_->loadPreset(device_preset_.c_str());
TRY_EXECUTE_BLOCK(device_->loadPreset(device_preset_.c_str()));
RCLCPP_INFO_STREAM(logger_, "Device preset " << device_->getCurrentPresetName() << " loaded");
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to load device preset: " << e.getMessage());
@@ -230,7 +242,7 @@ void OBCameraNode::setupDevices() {
}
if (!depth_work_mode_.empty()) {
RCLCPP_INFO_STREAM(logger_, "Set depth work mode: " << depth_work_mode_);
device_->switchDepthWorkMode(depth_work_mode_.c_str());
TRY_EXECUTE_BLOCK(device_->switchDepthWorkMode(depth_work_mode_.c_str()));
}
if (!sync_mode_str_.empty() && device_->isPropertySupported(OB_PROP_SYNC_SIGNAL_TRIGGER_OUT_BOOL,
OB_PERMISSION_READ_WRITE)) {
@@ -246,15 +258,15 @@ void OBCameraNode::setupDevices() {
sync_config.triggerOutDelayUs = trigger_out_delay_us_;
sync_config.triggerOutEnable = trigger_out_enabled_;
sync_config.framesPerTrigger = frames_per_trigger_;
device_->setMultiDeviceSyncConfig(sync_config);
TRY_EXECUTE_BLOCK(device_->setMultiDeviceSyncConfig(sync_config));
sync_config = device_->getMultiDeviceSyncConfig();
RCLCPP_INFO_STREAM(logger_, "Set sync mode: " << magic_enum::enum_name(sync_config.syncMode));
if (sync_mode_ == OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING) {
RCLCPP_INFO_STREAM(logger_, "Frames per trigger: " << sync_config.framesPerTrigger);
RCLCPP_INFO_STREAM(logger_,
"Software trigger period " << software_trigger_period_.count() << " ms");
software_trigger_timer_ = node_->create_wall_timer(software_trigger_period_,
[this]() { device_->triggerCapture(); });
software_trigger_timer_ = node_->create_wall_timer(
software_trigger_period_, [this]() { TRY_EXECUTE_BLOCK(device_->triggerCapture()); });
}
}
@@ -277,8 +289,8 @@ void OBCameraNode::setupDevices() {
logger_, "depth unit flexible adjustment value is out of range, please check the value");
} else {
RCLCPP_INFO_STREAM(logger_, "set depth unit to " << depth_unit_flexible_adjustment << "mm");
device_->setFloatProperty(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT,
depth_unit_flexible_adjustment);
TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT,
depth_unit_flexible_adjustment);
}
}
@@ -299,73 +311,107 @@ void OBCameraNode::setupDevices() {
}
if (device_->isPropertySupported(mirrorPropertyID, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(mirrorPropertyID, flip_stream_[stream_index]);
RCLCPP_INFO_STREAM(logger_, "Setting " << stream_name_[stream_index] << " mirror to "
<< (flip_stream_[stream_index] ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, mirrorPropertyID, flip_stream_[stream_index]);
}
}
}
if (!depth_filter_config_.empty() && enable_depth_filter_) {
RCLCPP_INFO_STREAM(logger_, "Load depth filter config: " << depth_filter_config_);
device_->loadDepthFilterConfig(depth_filter_config_.c_str());
TRY_EXECUTE_BLOCK(device_->loadDepthFilterConfig(depth_filter_config_.c_str()));
} else {
if (device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) {
device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_);
RCLCPP_INFO_STREAM(logger_,
"Setting depth soft filter to " << (enable_soft_filter_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_);
}
}
if (device_->isPropertySupported(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, enable_color_auto_exposure_);
RCLCPP_INFO_STREAM(
logger_, "Setting color auto exposure to " << (enable_color_auto_exposure_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL,
enable_color_auto_exposure_);
}
if (device_->isPropertySupported(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL,
enable_color_auto_white_balance_);
RCLCPP_INFO_STREAM(logger_, "Setting color auto white balance to "
<< (enable_color_auto_white_balance_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL,
enable_color_auto_white_balance_);
}
if (color_exposure_ != -1 &&
device_->isPropertySupported(OB_PROP_COLOR_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, false);
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, false);
auto range = device_->getIntPropertyRange(OB_PROP_COLOR_EXPOSURE_INT);
if (color_exposure_ < range.min || color_exposure_ > range.max) {
RCLCPP_ERROR(logger_, "color exposure value is out of range[%d,%d], please check the value",
range.min, range.max);
} else {
device_->setIntProperty(OB_PROP_COLOR_EXPOSURE_INT, color_exposure_);
RCLCPP_INFO_STREAM(logger_, "Setting color exposure to " << color_exposure_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_EXPOSURE_INT, color_exposure_);
}
}
if (color_gain_ != -1 &&
device_->isPropertySupported(OB_PROP_COLOR_GAIN_INT, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, false);
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, false);
auto range = device_->getIntPropertyRange(OB_PROP_COLOR_GAIN_INT);
if (color_gain_ < range.min || color_gain_ > range.max) {
RCLCPP_ERROR(logger_, "color gain value is out of range[%d,%d], please check the value",
range.min, range.max);
} else {
device_->setIntProperty(OB_PROP_COLOR_GAIN_INT, color_gain_);
RCLCPP_INFO_STREAM(logger_, "Setting color gain to " << color_gain_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_GAIN_INT, color_gain_);
}
}
if (color_white_balance_ != -1 &&
device_->isPropertySupported(OB_PROP_COLOR_WHITE_BALANCE_INT, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, false);
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, false);
auto range = device_->getIntPropertyRange(OB_PROP_COLOR_WHITE_BALANCE_INT);
if (color_white_balance_ < range.min || color_white_balance_ > range.max) {
RCLCPP_ERROR(logger_,
"color white balance value is out of range[%d,%d], please check the value",
range.min, range.max);
} else {
device_->setIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT, color_white_balance_);
RCLCPP_INFO_STREAM(logger_, "Setting color white balance to " << color_white_balance_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_WHITE_BALANCE_INT, color_white_balance_);
}
}
if (device_->isPropertySupported(OB_PROP_COLOR_AE_MAX_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting color AE max exposure to " << color_ae_max_exposure_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_AE_MAX_EXPOSURE_INT, color_ae_max_exposure_);
}
if (device_->isPropertySupported(OB_PROP_COLOR_BRIGHTNESS_INT, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting color brightness to " << color_brightness_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_BRIGHTNESS_INT, color_brightness_);
}
// ir ae max
if (device_->isPropertySupported(OB_PROP_IR_AE_MAX_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting IR AE max exposure to " << ir_ae_max_exposure_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_AE_MAX_EXPOSURE_INT, ir_ae_max_exposure_);
}
// ir brightness
if (device_->isPropertySupported(OB_PROP_IR_BRIGHTNESS_INT, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting IR brightness to " << ir_brightness_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_BRIGHTNESS_INT, ir_brightness_);
}
if (device_->isPropertySupported(OB_PROP_IR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
RCLCPP_INFO_STREAM(logger_,
"Setting IR auto exposure to " << (enable_ir_auto_exposure_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
}
if (ir_exposure_ != -1 &&
device_->isPropertySupported(OB_PROP_IR_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
device_->setBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL, false);
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, false);
auto range = device_->getIntPropertyRange(OB_PROP_IR_EXPOSURE_INT);
if (ir_exposure_ < range.min || ir_exposure_ > range.max) {
RCLCPP_ERROR(logger_, "ir exposure value is out of range[%d,%d], please check the value",
range.min, range.max);
} else {
RCLCPP_INFO_STREAM(logger_, "Setting IR exposure to " << ir_exposure_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_EXPOSURE_INT, ir_exposure_);
}
}
@@ -376,11 +422,14 @@ void OBCameraNode::setupDevices() {
RCLCPP_ERROR(logger_, "ir gain value is out of range[%d,%d], please check the value",
range.min, range.max);
} else {
RCLCPP_INFO_STREAM(logger_, "Setting IR gain to " << ir_gain_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_GAIN_INT, ir_gain_);
}
}
if (device_->isPropertySupported(OB_PROP_IR_LONG_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_,
"Setting IR long exposure to " << (enable_ir_long_exposure_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_LONG_EXPOSURE_BOOL, enable_ir_long_exposure_);
}
@@ -401,7 +450,8 @@ void OBCameraNode::setupDevices() {
"default_soft_filter_speckle_size: " << default_soft_filter_speckle_size);
if (soft_filter_speckle_size_ != -1 &&
default_soft_filter_speckle_size != soft_filter_speckle_size_) {
device_->setIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, soft_filter_speckle_size_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT,
soft_filter_speckle_size_);
auto new_soft_filter_speckle_size =
device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
RCLCPP_INFO_STREAM(logger_,
@@ -551,19 +601,19 @@ void OBCameraNode::printSensorProfiles(const std::shared_ptr<ob::Sensor> &sensor
auto origin_profile = profiles->getProfile(i);
if (sensor->getType() == OB_SENSOR_COLOR) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "color profile: " << profile->getWidth() << "x" << profile->getHeight()
<< " " << profile->getFps() << "fps "
<< profile->getFormat());
RCLCPP_INFO_STREAM(
logger_, "color profile: " << profile->getWidth() << "x" << profile->getHeight() << " "
<< profile->getFps() << "fps " << profile->getFormat());
} else if (sensor->getType() == OB_SENSOR_DEPTH) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "depth profile: " << profile->getWidth() << "x" << profile->getHeight()
<< " " << profile->getFps() << "fps "
<< profile->getFormat());
RCLCPP_INFO_STREAM(
logger_, "depth profile: " << profile->getWidth() << "x" << profile->getHeight() << " "
<< profile->getFps() << "fps " << profile->getFormat());
} else if (sensor->getType() == OB_SENSOR_IR) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "ir profile: " << profile->getWidth() << "x" << profile->getHeight()
<< " " << profile->getFps() << "fps "
<< profile->getFormat());
RCLCPP_INFO_STREAM(logger_, "ir profile: " << profile->getWidth() << "x"
<< profile->getHeight() << " " << profile->getFps()
<< "fps " << profile->getFormat());
} else if (sensor->getType() == OB_SENSOR_ACCEL) {
auto profile = origin_profile->as<ob::AccelStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "accel profile: sampleRate " << profile->getSampleRate()
@@ -590,12 +640,19 @@ void OBCameraNode::setupProfiles() {
CHECK_NOTNULL(profiles.get());
CHECK(profiles->getCount() > 0);
for (size_t i = 0; i < profiles->getCount(); i++) {
auto profile = profiles->getProfile(i)->as<ob::VideoStreamProfile>();
auto base_profile = profiles->getProfile(i)->as<ob::VideoStreamProfile>();
if (base_profile == nullptr) {
throw std::runtime_error("Failed to get profile " + std::to_string(i));
}
auto profile = base_profile->as<ob::VideoStreamProfile>();
if (profile == nullptr) {
throw std::runtime_error("Failed cast profile to VideoStreamProfile");
}
RCLCPP_DEBUG_STREAM(
logger_,
"Sensor profile: " << "stream_type: " << magic_enum::enum_name(profile->getType())
<< "Format: " << profile->getFormat() << ", Width: " << profile->getWidth()
<< ", Height: " << profile->getHeight() << ", FPS: " << profile->getFps());
logger_, "Sensor profile: "
<< "stream_type: " << magic_enum::enum_name(profile->getType())
<< "Format: " << profile->getFormat() << ", Width: " << profile->getWidth()
<< ", Height: " << profile->getHeight() << ", FPS: " << profile->getFps());
supported_profiles_[elem].emplace_back(profile);
}
std::shared_ptr<ob::VideoStreamProfile> selected_profile;
@@ -622,6 +679,7 @@ void OBCameraNode::setupProfiles() {
"configuration and try again. The current process will now exit.");
RCLCPP_INFO_STREAM(logger_, "Available profiles:");
printSensorProfiles(sensor);
RCLCPP_ERROR(logger_, "Because can not set this stream, so exit.");
exit(-1);
}
@@ -653,12 +711,13 @@ void OBCameraNode::setupProfiles() {
updateImageConfig(elem);
images_[elem] =
cv::Mat(height_[elem], width_[elem], image_format_[elem], cv::Scalar(0, 0, 0));
RCLCPP_INFO_STREAM(
logger_, " stream " << stream_name_[elem]
<< " is enabled - width: " << selected_profile->getWidth()
<< ", height: " << selected_profile->getHeight()
<< ", fps: " << selected_profile->getFps() << ", "
<< "Format: " << magic_enum::enum_name(selected_profile->getFormat()));
RCLCPP_INFO_STREAM(logger_,
" stream "
<< stream_name_[elem]
<< " is enabled - width: " << selected_profile->getWidth()
<< ", height: " << selected_profile->getHeight()
<< ", fps: " << selected_profile->getFps() << ", "
<< "Format: " << magic_enum::enum_name(selected_profile->getFormat()));
}
}
// IMU
@@ -740,10 +799,10 @@ void OBCameraNode::startStreams() {
}
if (enable_frame_sync_) {
RCLCPP_INFO_STREAM(logger_, "Enable frame sync");
pipeline_->enableFrameSync();
TRY_EXECUTE_BLOCK(pipeline_->enableFrameSync());
} else {
RCLCPP_INFO_STREAM(logger_, "Disable frame sync");
pipeline_->disableFrameSync();
TRY_EXECUTE_BLOCK(pipeline_->disableFrameSync());
}
pipeline_started_.store(true);
}
@@ -771,7 +830,7 @@ void OBCameraNode::startIMUSyncStream() {
std::shared_ptr<ob::Config> imuConfig = std::make_shared<ob::Config>();
imuConfig->enableStream(accelProfile);
imuConfig->enableStream(gyroProfile);
imuPipeline_->enableFrameSync();
TRY_EXECUTE_BLOCK(imuPipeline_->enableFrameSync());
imuPipeline_->start(imuConfig, [&](std::shared_ptr<ob::Frame> frame) {
auto frameSet = frame->as<ob::FrameSet>();
auto aFrame = frameSet->getFrame(OB_FRAME_ACCEL);
@@ -802,8 +861,9 @@ void OBCameraNode::startIMU() {
for (const auto &stream_index : HID_STREAMS) {
if (enable_stream_[stream_index] && !imu_started_[stream_index]) {
auto imu_profile = stream_profile_[stream_index];
CHECK(imu_profile);
CHECK_NOTNULL(imu_profile);
RCLCPP_INFO_STREAM(logger_, "start " << stream_name_[stream_index] << " stream");
CHECK_NOTNULL(sensors_[stream_index]);
sensors_[stream_index]->start(
imu_profile, [this, stream_index](const std::shared_ptr<ob::Frame> &frame) {
onNewIMUFrameCallback(frame, stream_index);
@@ -815,31 +875,42 @@ void OBCameraNode::startIMU() {
void OBCameraNode::stopStreams() {
if (!pipeline_started_ || !pipeline_) {
RCLCPP_INFO_STREAM(logger_, "pipeline not started or not exist, skip stop pipeline");
return;
}
try {
pipeline_->stop();
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline: " << e.getMessage());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline");
}
}
void OBCameraNode::stopIMU() {
if (enable_sync_output_accel_gyro_) {
if (!imu_sync_output_start_ || !imuPipeline_) {
RCLCPP_INFO_STREAM(logger_, "imu pipeline not started or not exist, skip stop imu pipeline");
return;
}
try {
imuPipeline_->stop();
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop imu pipeline: " << e.getMessage());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop imu pipeline");
}
} else {
for (const auto &stream_index : HID_STREAMS) {
if (imu_started_[stream_index]) {
CHECK(sensors_.count(stream_index));
RCLCPP_INFO_STREAM(logger_, "stop " << stream_name_[stream_index] << " stream");
sensors_[stream_index]->stop();
try {
sensors_[stream_index]->stop();
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop " << stream_name_[stream_index]
<< " stream: " << e.getMessage());
}
imu_started_[stream_index] = false;
}
}
@@ -886,7 +957,11 @@ void OBCameraNode::getParameters() {
param_name = stream_name_[stream_index] + "_fps";
setAndGetNodeParameter(fps_[stream_index], param_name, 0);
param_name = "enable_" + stream_name_[stream_index];
setAndGetNodeParameter(enable_stream_[stream_index], param_name, false);
if (stream_index == DEPTH) {
setAndGetNodeParameter(enable_stream_[stream_index], param_name, true);
} else {
setAndGetNodeParameter(enable_stream_[stream_index], param_name, false);
}
param_name = "flip_" + stream_name_[stream_index];
setAndGetNodeParameter(flip_stream_[stream_index], param_name, false);
param_name = camera_name_ + "_" + stream_name_[stream_index] + "_frame_id";
@@ -956,9 +1031,13 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<int>(color_exposure_, "color_exposure", -1);
setAndGetNodeParameter<int>(color_gain_, "color_gain", -1);
setAndGetNodeParameter<int>(color_white_balance_, "color_white_balance", -1);
setAndGetNodeParameter<int>(color_ae_max_exposure_, "color_ae_max_exposure", -1);
setAndGetNodeParameter<int>(color_brightness_, "color_brightness", -1);
setAndGetNodeParameter(enable_ir_auto_exposure_, "enable_ir_auto_exposure", true);
setAndGetNodeParameter<int>(ir_exposure_, "ir_exposure", -1);
setAndGetNodeParameter<int>(ir_gain_, "ir_gain", -1);
setAndGetNodeParameter<int>(ir_ae_max_exposure_, "ir_ae_max_exposure", -1);
setAndGetNodeParameter<int>(ir_brightness_, "ir_brightness", -1);
setAndGetNodeParameter(enable_ir_long_exposure_, "enable_ir_long_exposure", true);
setAndGetNodeParameter<std::string>(depth_work_mode_, "depth_work_mode", "");
setAndGetNodeParameter<std::string>(sync_mode_str_, "sync_mode", "");
@@ -968,10 +1047,11 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter(trigger_out_delay_us_, "trigger_out_delay_us", 0);
setAndGetNodeParameter(trigger_out_enabled_, "trigger_out_enabled", false);
setAndGetNodeParameter<std::string>(depth_precision_str_, "depth_precision", "");
setAndGetNodeParameter<std::string>(cloud_frame_id_, "cloud_frame_id", "");
if (!depth_precision_str_.empty()) {
depth_precision_ = depthPrecisionLevelFromString(depth_precision_str_);
}
if (enable_colored_point_cloud_) {
if (enable_colored_point_cloud_ || enable_d2c_viewer_) {
depth_registration_ = true;
}
if (!enable_stream_[COLOR]) {
@@ -1058,10 +1138,13 @@ void OBCameraNode::setupTopics() {
setupDiagnosticUpdater();
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.getMessage());
throw std::runtime_error(e.getMessage());
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.what());
throw std::runtime_error(e.what());
} catch (...) {
RCLCPP_ERROR(logger_, "Failed to setup topics");
throw std::runtime_error("Failed to setup topics");
}
}
@@ -1125,20 +1208,22 @@ void OBCameraNode::setupPipelineConfig() {
if (enable_stream_[stream_index]) {
RCLCPP_INFO_STREAM(logger_, "Enable " << stream_name_[stream_index] << " stream");
auto profile = stream_profile_[stream_index]->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(logger_,
"Stream " << stream_name_[stream_index] << " width: " << profile->getWidth()
<< " height: " << profile->getHeight() << " fps: " << profile->getFps()
<< " format: " << profile->getFormat());
RCLCPP_INFO_STREAM(
logger_, "Stream " << stream_name_[stream_index] << " width: " << profile->getWidth()
<< " height: " << profile->getHeight() << " fps: " << profile->getFps()
<< " format: " << profile->getFormat());
pipeline_config_->enableStream(stream_profile_[stream_index]);
}
}
}
}
void OBCameraNode::setupPublishers() {
using PointCloud2 = sensor_msgs::msg::PointCloud2;
using CameraInfo = sensor_msgs::msg::CameraInfo;
auto point_cloud_qos_profile = getRMWQosProfileFromString(point_cloud_qos_);
if (use_intra_process_) {
point_cloud_qos_profile = rmw_qos_profile_default;
}
if (enable_colored_point_cloud_) {
depth_registration_cloud_pub_ = node_->create_publisher<PointCloud2>(
"depth_registered/points",
@@ -1159,13 +1244,24 @@ void OBCameraNode::setupPublishers() {
}
std::string name = stream_name_[stream_index];
std::string topic = name + "/image_raw";
auto image_qos = image_qos_[stream_index];
auto image_qos_profile = getRMWQosProfileFromString(image_qos);
image_publishers_[stream_index] =
image_transport::create_publisher(node_, topic, image_qos_profile);
auto image_qos_profile = image_qos_[stream_index];
// if (use_intra_process_) {
// image_qos_profile = rmw_qos_profile_default;
// }
// if (use_intra_process_) {
// image_publishers_[stream_index] =
// std::make_shared<image_rcl_publisher>(*node_, topic, image_qos_profile);
// } else {
// image_publishers_[stream_index] =
// std::make_shared<image_transport_publisher>(*node_, topic, image_qos_profile);
// }
topic = name + "/camera_info";
auto camera_info_qos = camera_info_qos_[stream_index];
auto camera_info_qos_profile = getRMWQosProfileFromString(camera_info_qos);
if (use_intra_process_) {
camera_info_qos_profile = rmw_qos_profile_default;
}
camera_info_publishers_[stream_index] = node_->create_publisher<CameraInfo>(
topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
camera_info_qos_profile));
@@ -1176,15 +1272,23 @@ void OBCameraNode::setupPublishers() {
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
camera_info_qos_profile));
}
if (stream_index == COLOR && enable_color_undistortion_) {
color_undistortion_publisher_ =
image_transport::create_publisher(node_, "color/image_undistorted", image_qos_profile);
}
// if (stream_index == COLOR && enable_color_undistortion_) {
// if (use_intra_process_) {
// color_undistortion_publisher_ = std::make_shared<image_rcl_publisher>(
// *node_, "color/image_undistorted", image_qos_profile);
// } else {
// color_undistortion_publisher_ = std::make_shared<image_transport_publisher>(
// *node_, "color/image_undistorted", image_qos_profile);
// }
// }
}
if (enable_sync_output_accel_gyro_) {
std::string topic_name = stream_name_[GYRO] + "_" + stream_name_[ACCEL] + "/sample";
auto data_qos = getRMWQosProfileFromString(imu_qos_[GYRO]);
if (use_intra_process_) {
data_qos = rmw_qos_profile_default;
}
imu_gyro_accel_publisher_ = node_->create_publisher<sensor_msgs::msg::Imu>(
topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
topic_name = stream_name_[GYRO] + "/imu_info";
@@ -1200,6 +1304,9 @@ void OBCameraNode::setupPublishers() {
}
std::string data_topic_name = stream_name_[stream_index] + "/sample";
auto data_qos = getRMWQosProfileFromString(imu_qos_[stream_index]);
if (use_intra_process_) {
data_qos = rmw_qos_profile_default;
}
imu_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Imu>(
data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
data_topic_name = stream_name_[stream_index] + "/imu_info";
@@ -1209,38 +1316,42 @@ void OBCameraNode::setupPublishers() {
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
}
}
auto extrinsics_qos = rclcpp::QoS(1).transient_local();
if (use_intra_process_) {
extrinsics_qos = rclcpp::QoS(1);
}
if (enable_stream_[DEPTH] && enable_stream_[INFRA0]) {
depth_to_other_extrinsics_publishers_[INFRA0] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_ir", rclcpp::QoS(1).transient_local());
"/" + camera_name_ + "/depth_to_ir", extrinsics_qos);
}
if (enable_stream_[DEPTH] && enable_stream_[COLOR]) {
depth_to_other_extrinsics_publishers_[COLOR] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_color", rclcpp::QoS(1).transient_local());
"/" + camera_name_ + "/depth_to_color", extrinsics_qos);
}
if (enable_stream_[DEPTH] && enable_stream_[INFRA1]) {
depth_to_other_extrinsics_publishers_[INFRA1] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_left_ir", rclcpp::QoS(1).transient_local());
"/" + camera_name_ + "/depth_to_left_ir", extrinsics_qos);
}
if (enable_stream_[DEPTH] && enable_stream_[INFRA2]) {
depth_to_other_extrinsics_publishers_[INFRA2] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_right_ir", rclcpp::QoS(1).transient_local());
"/" + camera_name_ + "/depth_to_right_ir", extrinsics_qos);
}
if (enable_stream_[DEPTH] && enable_stream_[ACCEL]) {
depth_to_other_extrinsics_publishers_[ACCEL] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_accel", rclcpp::QoS(1).transient_local());
"/" + camera_name_ + "/depth_to_accel", extrinsics_qos);
}
if (enable_stream_[DEPTH] && enable_stream_[GYRO]) {
depth_to_other_extrinsics_publishers_[GYRO] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_gyro", rclcpp::QoS(1).transient_local());
"/" + camera_name_ + "/depth_to_gyro", extrinsics_qos);
}
filter_status_pub_ = node_->create_publisher<std_msgs::msg::String>(
"depth_filter_status", rclcpp::QoS(1).transient_local());
filter_status_pub_ =
node_->create_publisher<std_msgs::msg::String>("depth_filter_status", extrinsics_qos);
std_msgs::msg::String msg;
msg.data = filter_status_.dump(2);
filter_status_pub_->publish(msg);
@@ -1249,15 +1360,12 @@ void OBCameraNode::setupPublishers() {
void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
try {
if (depth_registration_ || enable_colored_point_cloud_) {
if (frame_set->depthFrame()!= nullptr && frame_set->colorFrame()!= nullptr) {
if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
publishColoredPointCloud(frame_set);
}
}
if (enable_point_cloud_ && frame_set->depthFrame() != nullptr) {
publishDepthPointCloud(frame_set);
}
@@ -1277,6 +1385,10 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
return;
}
std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
if (!depth_frame_) {
RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
return;
}
auto depth_frame = depth_frame_->as<ob::DepthFrame>();
if (!depth_frame) {
RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
@@ -1284,8 +1396,11 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
}
CHECK_NOTNULL(pipeline_);
auto camera_params = pipeline_->getCameraParam();
auto device_info = device_->getDeviceInfo();
CHECK_NOTNULL(device_info.get());
auto pid = device_info->getPid();
if (depth_registration_) {
if (depth_registration_ || pid == DABAI_MAX_PID) {
camera_params.depthIntrinsic = camera_params.rgbIntrinsic;
}
depth_point_cloud_filter_.setCameraParam(camera_params);
@@ -1343,6 +1458,9 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
auto frame_timestamp = getFrameTimestampUs(depth_frame);
auto timestamp = fromUsToROSTime(frame_timestamp);
std::string frame_id = depth_registration_ ? optical_frame_id_[COLOR] : optical_frame_id_[DEPTH];
if (!cloud_frame_id_.empty()) {
frame_id = cloud_frame_id_;
}
point_cloud_msg->header.stamp = timestamp;
point_cloud_msg->header.frame_id = frame_id;
if (save_point_cloud_) {
@@ -1373,6 +1491,10 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
}
CHECK_NOTNULL(depth_frame_.get());
std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
if (!depth_frame_) {
RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
return;
}
auto depth_frame = depth_frame_->as<ob::DepthFrame>();
auto color_frame = frame_set->colorFrame();
@@ -1391,7 +1513,6 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
}
if (!xy_tables_.has_value()) {
calibration_param_ = pipeline_->getCalibrationParam(pipeline_config_);
uint32_t table_size =
@@ -1471,9 +1592,13 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
modifier.resize(valid_count);
}
auto frame_timestamp = getFrameTimestampUs(depth_frame);
std::string frame_id = optical_frame_id_[COLOR];
if (!cloud_frame_id_.empty()) {
frame_id = cloud_frame_id_;
}
auto timestamp = fromUsToROSTime(frame_timestamp);
point_cloud_msg->header.stamp = timestamp;
point_cloud_msg->header.frame_id = optical_frame_id_[COLOR];
point_cloud_msg->header.frame_id = frame_id;
if (save_colored_point_cloud_) {
save_colored_point_cloud_ = false;
auto now = std::time(nullptr);
@@ -1501,6 +1626,9 @@ std::shared_ptr<ob::Frame> OBCameraNode::processDepthFrameFilter(
if (frame == nullptr || frame->getType() != OB_FRAME_DEPTH) {
return nullptr;
}
auto sensor = device_->getSensor(OB_SENSOR_DEPTH);
CHECK_NOTNULL(sensor.get());
auto filter_list = sensor->getRecommendedFilters();
for (size_t i = 0; i < filter_list_.size(); i++) {
auto filter = filter_list_[i];
CHECK_NOTNULL(filter.get());
@@ -1553,10 +1681,10 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
CHECK_NOTNULL(device_info.get());
auto pid = device_info->getPid();
auto color_frame = frame_set->getFrame(OB_FRAME_COLOR);
has_first_color_frame_ = has_first_color_frame_ || color_frame;
if (isGemini335PID(pid)) {
depth_frame_ = processDepthFrameFilter(depth_frame_);
if (depth_registration_ && align_filter_ && depth_frame_ && color_frame) {
if (depth_registration_ && align_filter_ && depth_frame_ && has_first_color_frame_) {
auto new_frame = align_filter_->process(frame_set);
if (new_frame) {
auto new_frame_set = new_frame->as<ob::FrameSet>();
@@ -1569,6 +1697,10 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
RCLCPP_DEBUG(logger_,
"Depth registration is disabled or align filter is null or depth frame is "
"null or color frame is null");
return;
}
if (depth_registration_ && align_filter_ && depth_frame_ && !has_first_color_frame_) {
return;
}
}
@@ -1675,7 +1807,8 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
if (!rgb_buffer_) {
return false;
}
bool has_subscriber = image_publishers_[COLOR].getNumSubscribers() > 0;
CHECK_NOTNULL(image_publishers_[COLOR]);
bool has_subscriber = image_publishers_[COLOR]->get_subscription_count() > 0;
if (enable_colored_point_cloud_ && depth_registration_cloud_pub_->get_subscription_count() > 0) {
has_subscriber = true;
}
@@ -1738,8 +1871,9 @@ std::shared_ptr<ob::Frame> OBCameraNode::decodeIRMJPGFrame(
cv::Mat mjpgMat(1, video_frame->getDataSize(), CV_8UC1, video_frame->getData());
cv::Mat irRawMat = cv::imdecode(mjpgMat, cv::IMREAD_GRAYSCALE);
std::shared_ptr<ob::Frame> irFrame = ob::FrameFactory::createVideoFrame(
video_frame->getType(), video_frame->getFormat(), video_frame->getWidth(), video_frame->getHeight(), 0);
std::shared_ptr<ob::Frame> irFrame =
ob::FrameFactory::createVideoFrame(video_frame->getType(), video_frame->getFormat(),
video_frame->getWidth(), video_frame->getHeight(), 0);
uint32_t buffer_size = irRawMat.rows * irRawMat.cols * irRawMat.channels();
@@ -1764,7 +1898,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
if (frame == nullptr) {
return;
}
bool has_subscriber = image_publishers_[stream_index].getNumSubscribers() > 0;
CHECK_NOTNULL(image_publishers_[stream_index]);
bool has_subscriber = image_publishers_[stream_index]->get_subscription_count() > 0;
has_subscriber =
has_subscriber || camera_info_publishers_[stream_index]->get_subscription_count() > 0;
has_subscriber =
@@ -1811,11 +1946,19 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
: camera_params.depthIntrinsic;
distortion = stream_index.first == OB_STREAM_COLOR ? camera_params.rgbDistortion
: camera_params.depthDistortion;
if (pid == DABAI_MAX_PID) {
// use color param
intrinsic = camera_params.rgbIntrinsic;
distortion = camera_params.rgbDistortion;
}
}
std::string frame_id = optical_frame_id_[stream_index];
if (depth_registration_ && stream_index == DEPTH) {
frame_id = depth_aligned_frame_id_[stream_index];
}
if (stream_index == COLOR && enable_color_undistortion_) {
memset(&distortion, 0, sizeof(distortion));
}
auto camera_info = convertToCameraInfo(intrinsic, distortion, width);
camera_info.header.stamp = timestamp;
camera_info.header.frame_id = frame_id;
@@ -1839,7 +1982,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
if (isGemini335PID(pid)) {
publishMetadata(frame, stream_index, camera_info.header);
}
if (image_publishers_[stream_index].getNumSubscribers() == 0) {
CHECK_NOTNULL(image_publishers_[stream_index]);
if (image_publishers_[stream_index]->get_subscription_count() == 0) {
return;
}
auto &image = images_[stream_index];
@@ -1859,28 +2003,30 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
auto depth_scale = video_frame->as<ob::DepthFrame>()->getValueScale();
image = image * depth_scale;
}
auto image_msg =
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image).toImageMsg();
sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image)
.toImageMsg(*image_msg);
CHECK_NOTNULL(image_msg.get());
image_msg->header.stamp = timestamp;
image_msg->is_bigendian = false;
image_msg->step = width * unit_step_size_[stream_index];
image_msg->header.frame_id = frame_id;
CHECK(image_publishers_.count(stream_index) > 0);
saveImageToFile(stream_index, image, image_msg);
image_publishers_[stream_index].publish(std::move(image_msg));
saveImageToFile(stream_index, image, *image_msg);
image_publishers_[stream_index]->publish(std::move(image_msg));
if (stream_index == COLOR && enable_color_undistortion_ &&
color_undistortion_publisher_.getNumSubscribers() > 0) {
color_undistortion_publisher_->get_subscription_count() > 0) {
auto undistorted_image = undistortImage(image, intrinsic, distortion);
auto undistorted_image_msg =
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], undistorted_image)
.toImageMsg();
sensor_msgs::msg::Image::UniquePtr undistorted_image_msg(new sensor_msgs::msg::Image());
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], undistorted_image)
.toImageMsg(*undistorted_image_msg);
CHECK_NOTNULL(undistorted_image_msg.get());
undistorted_image_msg->header.stamp = timestamp;
undistorted_image_msg->is_bigendian = false;
undistorted_image_msg->step = width * unit_step_size_[stream_index];
undistorted_image_msg->header.frame_id = frame_id;
color_undistortion_publisher_.publish(std::move(undistorted_image_msg));
color_undistortion_publisher_->publish(std::move(undistorted_image_msg));
}
}
@@ -1912,7 +2058,7 @@ void OBCameraNode::publishMetadata(const std::shared_ptr<ob::Frame> &frame,
}
void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const cv::Mat &image,
const sensor_msgs::msg::Image::SharedPtr &image_msg) {
const sensor_msgs::msg::Image &image_msg) {
if (save_images_[stream_index]) {
auto now = time(nullptr);
std::stringstream ss;
@@ -1922,8 +2068,8 @@ void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const
int index = save_images_count_[stream_index];
std::string file_suffix = stream_index == COLOR ? ".png" : ".raw";
std::string filename = current_path + "/image/" + stream_name_[stream_index] + "_" +
std::to_string(image_msg->width) + "x" +
std::to_string(image_msg->height) + "_" + std::to_string(fps) + "hz_" +
std::to_string(image_msg.width) + "x" +
std::to_string(image_msg.height) + "_" + std::to_string(fps) + "hz_" +
ss.str() + "_" + std::to_string(index) + file_suffix;
if (!std::filesystem::exists(current_path + "/image")) {
std::filesystem::create_directory(current_path + "/image");
@@ -2231,6 +2377,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
}
depth_to_other_extrinsics_[COLOR] = ex;
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[COLOR]);
depth_to_other_extrinsics_publishers_[COLOR]->publish(ex_msg);
}
@@ -2246,6 +2393,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
}
depth_to_other_extrinsics_[INFRA0] = ex;
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA0]);
depth_to_other_extrinsics_publishers_[INFRA0]->publish(ex_msg);
}
if (enable_stream_[DEPTH] && enable_stream_[INFRA1]) {
@@ -2260,6 +2408,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
}
depth_to_other_extrinsics_[INFRA1] = ex;
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA1]);
depth_to_other_extrinsics_publishers_[INFRA1]->publish(ex_msg);
}
if (enable_stream_[DEPTH] && enable_stream_[INFRA2]) {
@@ -2275,6 +2424,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
ex.trans[0] = -std::abs(ex.trans[0]);
depth_to_other_extrinsics_[INFRA2] = ex;
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA2]);
depth_to_other_extrinsics_publishers_[INFRA2]->publish(ex_msg);
}
if (enable_stream_[DEPTH] && enable_stream_[ACCEL]) {
@@ -2289,6 +2439,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
}
depth_to_other_extrinsics_[ACCEL] = ex;
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[ACCEL]);
depth_to_other_extrinsics_publishers_[ACCEL]->publish(ex_msg);
}
if (enable_stream_[DEPTH] && enable_stream_[GYRO]) {
@@ -2303,6 +2454,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
}
depth_to_other_extrinsics_[GYRO] = ex;
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[GYRO]);
depth_to_other_extrinsics_publishers_[GYRO]->publish(ex_msg);
}
}
orbbec_camera/src/ob_camera_node_driver.cpp
+74 -31
View File
@@ -15,6 +15,7 @@
*******************************************************************************/
#include "orbbec_camera/ob_camera_node_driver.h"
#include "orbbec_camera/utils.h"
#include <fcntl.h>
#include <semaphore.h>
#include <sys/shm.h>
@@ -38,7 +39,7 @@ void signalHandler(int sig) {
// get current time
std::time_t now = std::time(nullptr);
std::tm* local_time = std::localtime(&now);
std::tm *local_time = std::localtime(&now);
// format date and time to string, format as "2024_05_20_12_34_56"
std::ostringstream time_stream;
@@ -69,24 +70,27 @@ void signalHandler(int sig) {
}
namespace orbbec_camera {
OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions& node_options)
backward::SignalHandling OBCameraNodeDriver::sh;
OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
: Node("orbbec_camera_node", "/", node_options),
node_options_(node_options),
config_path_(ament_index_cpp::get_package_share_directory("orbbec_camera") +
"/config/OrbbecSDKConfig_v1.0.xml"),
ctx_(std::make_unique<ob::Context>(config_path_.c_str())),
logger_(this->get_logger()),
extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") +
"/lib/extensions") {
extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") + "/lib/extensions") {
init();
}
OBCameraNodeDriver::OBCameraNodeDriver(const std::string& node_name, const std::string& ns,
const rclcpp::NodeOptions& node_options)
OBCameraNodeDriver::OBCameraNodeDriver(const std::string &node_name, const std::string &ns,
const rclcpp::NodeOptions &node_options)
: Node(node_name, ns, node_options),
node_options_(node_options),
config_path_(ament_index_cpp::get_package_share_directory("orbbec_camera") +
"/config/OrbbecSDKConfig_v1.0.xml"),
ctx_(std::make_unique<ob::Context>()),
logger_(this->get_logger()),
extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") +
"/lib/extensions") {
extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") + "/lib/extensions") {
init();
}
@@ -128,8 +132,8 @@ void OBCameraNodeDriver::init() {
return;
}
orb_device_lock_shm_addr_ =
static_cast<uint8_t*>(mmap(NULL, sizeof(pthread_mutex_t), PROT_READ | PROT_WRITE, MAP_SHARED,
orb_device_lock_shm_fd_, 0));
static_cast<uint8_t *>(mmap(NULL, sizeof(pthread_mutex_t), PROT_READ | PROT_WRITE, MAP_SHARED,
orb_device_lock_shm_fd_, 0));
if (orb_device_lock_shm_addr_ == MAP_FAILED) {
RCLCPP_ERROR_STREAM(logger_, "Failed to map shared memory " << ORB_DEFAULT_LOCK_NAME);
return;
@@ -139,7 +143,7 @@ void OBCameraNodeDriver::init() {
std::placeholders::_1, std::placeholders::_2));
pthread_mutexattr_init(&orb_device_lock_attr_);
pthread_mutexattr_setpshared(&orb_device_lock_attr_, PTHREAD_PROCESS_SHARED);
orb_device_lock_ = (pthread_mutex_t*)orb_device_lock_shm_addr_;
orb_device_lock_ = (pthread_mutex_t *)orb_device_lock_shm_addr_;
pthread_mutex_init(orb_device_lock_, &orb_device_lock_attr_);
is_alive_.store(true);
parameters_ = std::make_shared<Parameters>(this);
@@ -150,8 +154,8 @@ void OBCameraNodeDriver::init() {
net_device_port_ = static_cast<int>(declare_parameter<int>("net_device_port", 0));
enumerate_net_device_ = declare_parameter<bool>("enumerate_net_device", false);
ctx_->enableNetDeviceEnumeration(enumerate_net_device_);
ctx_->setDeviceChangedCallback([this](const std::shared_ptr<ob::DeviceList>& removed_list,
const std::shared_ptr<ob::DeviceList>& added_list) {
ctx_->setDeviceChangedCallback([this](const std::shared_ptr<ob::DeviceList> &removed_list,
const std::shared_ptr<ob::DeviceList> &added_list) {
onDeviceConnected(added_list);
onDeviceDisconnected(removed_list);
});
@@ -162,7 +166,7 @@ void OBCameraNodeDriver::init() {
reset_device_thread_ = std::make_shared<std::thread>([this]() { resetDevice(); });
}
void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList>& device_list) {
void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {
CHECK_NOTNULL(device_list);
if (device_list->getCount() == 0) {
return;
@@ -172,7 +176,7 @@ void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList>
}
}
void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceList>& device_list) {
void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceList> &device_list) {
CHECK_NOTNULL(device_list);
if (device_list->getCount() == 0) {
return;
@@ -194,7 +198,7 @@ void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceLi
}
}
OBLogSeverity OBCameraNodeDriver::obLogSeverityFromString(const std::string_view& log_level) {
OBLogSeverity OBCameraNodeDriver::obLogSeverityFromString(const std::string_view &log_level) {
if (log_level == "debug") {
return OBLogSeverity::OB_LOG_SEVERITY_DEBUG;
} else if (log_level == "info") {
@@ -276,7 +280,7 @@ void OBCameraNodeDriver::rebootDeviceCallback(
}
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
const std::shared_ptr<ob::DeviceList>& list) {
const std::shared_ptr<ob::DeviceList> &list) {
if (device_num_ == 1) {
RCLCPP_INFO_STREAM(logger_, "Connecting to the default device");
return list->getDevice(0);
@@ -300,7 +304,7 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
}
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
const std::shared_ptr<ob::DeviceList>& list, const std::string& serial_number) {
const std::shared_ptr<ob::DeviceList> &list, const std::string &serial_number) {
std::string lower_sn;
std::transform(serial_number.begin(), serial_number.end(), std::back_inserter(lower_sn),
[](auto ch) { return isalpha(ch) ? tolower(ch) : static_cast<int>(ch); });
@@ -327,10 +331,10 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
return list->getDevice(i);
}
}
} catch (ob::Error& e) {
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 1000,
"Failed to get device info " << e.getMessage());
} catch (std::exception& e) {
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.what());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info");
@@ -340,17 +344,24 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
}
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
const std::shared_ptr<ob::DeviceList>& list, const std::string& usb_port) {
const std::shared_ptr<ob::DeviceList> &list, const std::string &usb_port) {
try {
RCLCPP_INFO_STREAM(logger_, "Before lock: Select device usb port: " << usb_port);
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_INFO_STREAM(logger_, "After lock: Select device usb port: " << usb_port);
auto device = list->getDeviceByUid(usb_port.c_str());
RCLCPP_INFO_STREAM(logger_, "Device usb port " << usb_port << " done");
if (device) {
RCLCPP_INFO_STREAM(logger_, "getDeviceByUid device usb port " << usb_port << " done");
} else {
RCLCPP_ERROR_STREAM(logger_, "getDeviceByUid device usb port " << usb_port << " failed");
RCLCPP_ERROR_STREAM(logger_,
"Please use script to get usb port: "
"ros2 run orbbec_camera list_devices_node");
}
return device;
} catch (ob::Error& e) {
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.getMessage());
} catch (std::exception& e) {
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.what());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info");
@@ -359,14 +370,46 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
return nullptr;
}
void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device>& device) {
void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &device) {
device_ = device;
CHECK_NOTNULL(device_);
CHECK_NOTNULL(device_.get());
if (ob_camera_node_) {
ob_camera_node_.reset();
}
ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_);
int retry_count = 0;
constexpr int max_retries = 3;
bool initialized = false;
device_info_ = device_->getDeviceInfo();
RCLCPP_INFO_STREAM(logger_, "Try to connect device via " << device_info_->connectionType());
while (retry_count < max_retries && !initialized) {
try {
ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_,
node_options_.use_intra_process_comms());
initialized = true;
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt "
<< retry_count + 1 << " of " << max_retries
<< "): " << e.getMessage());
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt " << retry_count + 1
<< " of " << max_retries
<< "): " << e.what());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device (Attempt "
<< retry_count + 1 << " of " << max_retries << ")");
}
retry_count++;
}
if (!initialized) {
RCLCPP_ERROR_STREAM(logger_,
"Device initialization failed after " << max_retries << " attempts.");
throw std::runtime_error("Device initialization failed after " + std::to_string(max_retries) +
" attempts.");
}
ob_camera_node_->startIMU();
ob_camera_node_->startStreams();
device_connected_ = true;
@@ -397,7 +440,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device>& dev
RCLCPP_INFO_STREAM(logger_, "Start device cost " << time_cost.count() << " ms");
} // namespace orbbec_camera
void OBCameraNodeDriver::connectNetDevice(const std::string& net_device_ip, int net_device_port) {
void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int net_device_port) {
if (net_device_ip.empty() || net_device_port == 0) {
RCLCPP_ERROR_STREAM(logger_, "Invalid net device ip or port");
return;
@@ -411,7 +454,7 @@ void OBCameraNodeDriver::connectNetDevice(const std::string& net_device_ip, int
initializeDevice(device);
}
void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList>& list) {
void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list) {
if (device_connected_) {
return;
}
@@ -426,7 +469,7 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList>& list
std::this_thread::sleep_for(std::chrono::milliseconds(connection_delay_));
pthread_mutex_lock(orb_device_lock_);
std::shared_ptr<int> lock_holder(nullptr,
[this](int*) { pthread_mutex_unlock(orb_device_lock_); });
[this](int *) { pthread_mutex_unlock(orb_device_lock_); });
bool start_device_failed = false;
try {
auto start_time = std::chrono::high_resolution_clock::now();
@@ -445,10 +488,10 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList>& list
end_time = std::chrono::high_resolution_clock::now();
time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
RCLCPP_INFO_STREAM(logger_, "Initialize device cost " << time_cost.count() << " ms");
} catch (ob::Error& e) {
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device " << e.getMessage());
start_device_failed = true;
} catch (std::exception& e) {
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device " << e.what());
start_device_failed = true;
} catch (...) {