diff --git a/include/tos_supervoxels_alg.h b/include/tos_supervoxels_alg.h index b7919fc58ff82b3f71dbe4b48cd3213795a268e8..074022a2e0db31de10b9b8bff8a955b56e4fd063 100644 --- a/include/tos_supervoxels_alg.h +++ b/include/tos_supervoxels_alg.h @@ -201,6 +201,8 @@ class TosSupervoxelsAlgorithm */ std::vector<pcl::PointCloud<pcl::PointXYZRGBA> > get_segmented_objects_simple(); + pcl::PointCloud<pcl::PointXYZRGBA>::Ptr get_plane_cloud(); + /*! \brief Print the parameters of the algorithm in the shell */ void print_parameters(); diff --git a/src/tos_supervoxels_alg.cpp b/src/tos_supervoxels_alg.cpp index 9eed65d5600cdd7da09e90b6f67fb168c98f70a1..ca375cadb9582a4338f0070be15e2db14b50c1ed 100644 --- a/src/tos_supervoxels_alg.cpp +++ b/src/tos_supervoxels_alg.cpp @@ -62,6 +62,11 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGBA> > TosSupervoxelsAlgorithm::get_se return this->obj_segment.get_segmented_objects_simple(); } +pcl::PointCloud<pcl::PointXYZRGBA>::Ptr TosSupervoxelsAlgorithm::get_plane_cloud() +{ + return this->obj_segment.get_plane_cloud(); +} + void TosSupervoxelsAlgorithm::print_parameters() { this->obj_segment.print_parameters(); diff --git a/src/tos_supervoxels_alg_node.cpp b/src/tos_supervoxels_alg_node.cpp index 389ab234f2128c1b9e3ec812b7909aa4f206096f..dcc9fe5f742e01f0dfa6628b13ca0b8acf851525 100644 --- a/src/tos_supervoxels_alg_node.cpp +++ b/src/tos_supervoxels_alg_node.cpp @@ -119,7 +119,8 @@ bool TosSupervoxelsAlgNode::object_segmentationCallback(iri_tos_supervoxels::obj } res.objects = seg_objs_msg; - + // get the table point cloud + pcl::toROSMsg(*(this->alg_.get_plane_cloud()),res.plane_cloud); //unlock previously blocked shared variables this->object_segmentation_mutex_exit(); diff --git a/srv/object_segmentation.srv b/srv/object_segmentation.srv index 9fb72e453297e7a4c6790cfeb8ea93d52caec7b6..e60447c07a69defb39ad41fcb65e8ab40b4b6b35 100644 --- a/srv/object_segmentation.srv +++ b/srv/object_segmentation.srv @@ -1,4 +1,5 @@ sensor_msgs/PointCloud2 point_cloud --- iri_tos_supervoxels/segmented_objects objects -iri_tos_supervoxels/plane_coefficients plane_coeff \ No newline at end of file +iri_tos_supervoxels/plane_coefficients plane_coeff +sensor_msgs/PointCloud2 plane_cloud