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

fix point cloud color

Browse Source
This commit is contained in:
Joe Dong
2022-06-07 11:58:59 +08:00
parent a56f7dd859
commit e498db77b3
1 changed files with 15 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+15 -15
View File
@@ -208,12 +208,12 @@ void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set,
}
}
if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
// publishColorPointCloud(frame_set, t);
publishDepthPointCloud(frame_set, t);
} else if (frame_set->depthFrame() != nullptr) {
publishDepthPointCloud(frame_set, t);
publishColorPointCloud(frame_set, t);
// publishDepthPointCloud(frame_set, t);
}
// } else if (frame_set->depthFrame() != nullptr) {
// publishDepthPointCloud(frame_set, t);
// }
}
void OBCameraNode::publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_set,
@@ -267,7 +267,7 @@ void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_se
modifier.resize(point_size);
point_cloud_msg_.width = width_[DEPTH];
point_cloud_msg_.height = height_[DEPTH];
std::string format_str = "intensity";
std::string format_str = "rgb";
point_cloud_msg_.point_step =
addPointField(point_cloud_msg_, format_str.c_str(), 1, sensor_msgs::msg::PointField::FLOAT32,
@@ -277,19 +277,19 @@ void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_se
sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud_msg_, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(point_cloud_msg_, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(point_cloud_msg_, "z");
sensor_msgs::PointCloud2Iterator<float> iter_r(point_cloud_msg_, "r");
sensor_msgs::PointCloud2Iterator<float> iter_g(point_cloud_msg_, "g");
sensor_msgs::PointCloud2Iterator<float> iter_b(point_cloud_msg_, "b");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(point_cloud_msg_, "r");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(point_cloud_msg_, "g");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(point_cloud_msg_, "b");
size_t valid_count = 0;
for (size_t point_idx = 0; point_idx < point_size; point_idx++, points++) {
bool valid_pixel(points->z > 0);
if (valid_pixel) {
*iter_x = points->x;
*iter_y = points->y;
*iter_z = points->z;
*iter_r = points->r;
*iter_g = points->g;
*iter_b = points->b;
*iter_x = static_cast<float>(points->x / 1000.0);
*iter_y = static_cast<float>(points->y / 1000.0);
*iter_z = static_cast<float>(points->z / 1000.0);
*iter_r = static_cast<uint8_t>(points->r);
*iter_g = static_cast<uint8_t>(points->g);
*iter_b = static_cast<uint8_t>(points->b);
++iter_x;
++iter_y;