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

catch publish point cloud exception

Browse Source
This commit is contained in:
Joe Dong
2022-06-09 17:24:59 +08:00
parent 208cc84503
commit 61bf2dd7b2
1 changed files with 17 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+17 -6
View File
@@ -37,7 +37,7 @@ OBCameraNode::OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> devic
stream_name_[OB_STREAM_IR] = "ir"; stream_name_[OB_STREAM_IR] = "ir";
unit_step_size_[INFRA0] = sizeof(uint8_t); unit_step_size_[INFRA0] = sizeof(uint8_t);
format_[COLOR] = OB_FORMAT_I420; format_[COLOR] = OB_FORMAT_YUYV;
image_format_[OB_STREAM_COLOR] = CV_8UC3; image_format_[OB_STREAM_COLOR] = CV_8UC3;
encoding_[COLOR] = sensor_msgs::image_encodings::BGR8; encoding_[COLOR] = sensor_msgs::image_encodings::BGR8;
stream_name_[OB_STREAM_COLOR] = "color"; stream_name_[OB_STREAM_COLOR] = "color";
@@ -222,11 +222,22 @@ void OBCameraNode::setupPublishers() {
} }
void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) { void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
if (publish_rgb_point_cloud_ && frame_set->depthFrame() != nullptr && if (point_cloud_publisher_->get_subscription_count() == 0) {
frame_set->colorFrame() != nullptr) { return;
publishColorPointCloud(frame_set); }
} else if (frame_set->depthFrame() != nullptr) { try {
publishDepthPointCloud(frame_set); if (publish_rgb_point_cloud_ && frame_set->depthFrame() != nullptr &&
frame_set->colorFrame() != nullptr) {
publishColorPointCloud(frame_set);
} else if (frame_set->depthFrame() != nullptr) {
publishDepthPointCloud(frame_set);
}
} catch (const ob::Error& e) {
RCLCPP_ERROR_STREAM(logger_, e.getMessage());
} catch (const std::exception& e) {
RCLCPP_ERROR_STREAM(logger_, e.what());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "publishPointCloud with unknown error");
} }
} }