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

fix drop bad mjpeg frame

Browse Source
This commit is contained in:
Joe Dong
2023-03-24 19:09:01 +08:00
parent 5ceb870714
commit 9dfdcbe100
1 changed files with 22 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+22
View File
@@ -420,9 +420,20 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& f
point_cloud_filter_.setCameraParam(*camera_param_);
point_cloud_filter_.setCreatePointFormat(OB_FORMAT_POINT);
auto depth_frame = frame_set->depthFrame();
if (!depth_frame) {
return;
}
auto frame = point_cloud_filter_.process(frame_set);
if (!frame) {
RCLCPP_ERROR_STREAM(logger_, "point cloud filter process failed");
return;
}
size_t point_size = frame->dataSize() / sizeof(OBPoint);
auto* points = (OBPoint*)frame->data();
if (!points) {
RCLCPP_ERROR_STREAM(logger_, "point cloud data is null");
return;
}
CHECK_NOTNULL(points);
sensor_msgs::PointCloud2Modifier modifier(point_cloud_msg_);
modifier.setPointCloud2FieldsByString(1, "xyz");
@@ -479,6 +490,9 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>&
}
auto depth_frame = frame_set->depthFrame();
auto color_frame = frame_set->colorFrame();
if (!depth_frame || !color_frame) {
return;
}
if (!camera_param_) {
camera_param_ = pipeline_->getCameraParam();
}
@@ -486,8 +500,16 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>&
point_cloud_filter_.setCreatePointFormat(OB_FORMAT_POINT);
point_cloud_filter_.setCreatePointFormat(OB_FORMAT_RGB_POINT);
auto frame = point_cloud_filter_.process(frame_set);
if (!frame) {
RCLCPP_ERROR_STREAM(logger_, "point cloud filter process failed");
return;
}
size_t point_size = frame->dataSize() / sizeof(OBColorPoint);
auto* points = (OBColorPoint*)frame->data();
if (!points) {
RCLCPP_ERROR_STREAM(logger_, "point cloud data is null");
return;
}
CHECK_NOTNULL(points);
sensor_msgs::PointCloud2Modifier modifier(point_cloud_msg_);
modifier.setPointCloud2FieldsByString(1, "xyz");