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

fixed get camera params

Browse Source
This commit is contained in:
Joe Dong
2023-02-15 19:50:30 +08:00
parent 9b79d6696b
commit daff270011
4 changed files with 45 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+34 -2
View File
@@ -326,8 +326,10 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& f
point_cloud_publisher_->get_subscription_count() == 0) {
return;
}
if (!camera_param_) {
if (!camera_param_ && depth_registration_) {
camera_param_ = pipeline_->getCameraParam();
} else if (!camera_param_) {
camera_param_ = getDepthCameraParam();
}
point_cloud_filter_.setCameraParam(*camera_param_);
point_cloud_filter_.setCreatePointFormat(OB_FORMAT_POINT);
@@ -494,8 +496,12 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame>& frame,
}
image.data = (uchar*)video_frame->data();
auto timestamp = frameTimeStampToROSTime(video_frame->systemTimeStamp());
if (!camera_param_) {
if (!camera_param_ && depth_registration_) {
camera_param_ = pipeline_->getCameraParam();
} else if (!camera_param_ && stream_index == COLOR) {
camera_param_ = getColorCameraParam();
} else if (!camera_param_) {
camera_param_ = getDepthCameraParam();
}
auto& intrinsic =
stream_index == COLOR ? camera_param_->rgbIntrinsic : camera_param_->depthIntrinsic;
@@ -531,6 +537,32 @@ std::optional<OBCameraParam> OBCameraNode::findDefaultCameraParam() {
return {};
}
std::optional<OBCameraParam> OBCameraNode::getDepthCameraParam() {
auto camera_params = device_->getCalibrationCameraParamList();
for (size_t i = 0; i < camera_params->count(); i++) {
auto param = camera_params->getCameraParam(i);
int depth_w = param.depthIntrinsic.width;
int depth_h = param.depthIntrinsic.height;
if (depth_w * height_[DEPTH] == depth_h * width_[DEPTH]) {
return param;
}
}
return {};
}
std::optional<OBCameraParam> OBCameraNode::getColorCameraParam() {
auto camera_params = device_->getCalibrationCameraParamList();
for (size_t i = 0; i < camera_params->count(); i++) {
auto param = camera_params->getCameraParam(i);
int color_w = param.rgbIntrinsic.width;
int color_h = param.rgbIntrinsic.height;
if (color_w * height_[COLOR] == color_h * width_[COLOR]) {
return param;
}
}
return {};
}
void OBCameraNode::publishStaticTF(const rclcpp::Time& t, const std::vector<float>& trans,
const tf2::Quaternion& q, const std::string& from,
const std::string& to) {
orbbec_camera/src/ob_camera_node_factory.cpp
+6 -4
View File
@@ -88,13 +88,14 @@ void OBCameraNodeFactory::onDeviceDisconnected(const std::shared_ptr<ob::DeviceL
}
RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected");
for (size_t i = 0; i < device_list->deviceCount(); i++) {
std::string serial_number = device_list->serialNumber(i);
std::string uid = device_list->uid(i);
std::scoped_lock<decltype(device_lock_)> lock(device_lock_);
RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected: " << serial_number);
if (device_info_ && device_info_->serialNumber() == serial_number) {
RCLCPP_INFO_STREAM(logger_, "device with" << uid << "disconnected");
if (uid == device_unique_id_) {
ob_camera_node_.reset();
device_.reset();
device_connected_ = false;
device_unique_id_.clear();
break;
}
}
@@ -214,7 +215,7 @@ void OBCameraNodeFactory::startDevice(const std::shared_ptr<ob::DeviceList> &lis
}
if (device_ == nullptr) {
// RCLCPP_WARN(logger_, "Device with serial number %s not found", serial_number_.c_str());
// RCLCPP_WARN(logger_, "Device with serial number %s not found", serial_number_.c_str());
device_connected_ = false;
return;
} else {
@@ -251,6 +252,7 @@ void OBCameraNodeFactory::startDevice(const std::shared_ptr<ob::DeviceList> &lis
device_connected_ = true;
device_info_ = device_->getDeviceInfo();
CHECK_NOTNULL(device_info_.get());
device_unique_id_ = device_info_->uid();
RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->name() << " connected");
RCLCPP_INFO_STREAM(logger_, "Serial number: " << device_info_->serialNumber());
RCLCPP_INFO_STREAM(logger_, "Firmware version: " << device_info_->firmwareVersion());