From c59e5f97ee04ed758a5f6f6ec4a797122adb009c Mon Sep 17 00:00:00 2001
From: Nicola Covallero <ncovallero@iri.upc.edu>
Date: Fri, 8 Apr 2016 12:16:37 +0200
Subject: [PATCH] Added plane coefficients msg

---
 CMakeLists.txt                          |  1 +
 include/tos_supervoxels_alg.h           |  3 +++
 include/tos_supervoxels_alg_node.h      |  2 ++
 msg/plane_coefficients.msg              |  4 ++++
 msg/segmented_objects.msg               |  2 +-
 src/tos_supervoxels_alg.cpp             |  6 ++++++
 src/tos_supervoxels_alg_client_node.cpp |  1 +
 src/tos_supervoxels_alg_node.cpp        | 10 ++++++++++
 srv/object_segmentation.srv             |  1 +
 9 files changed, 29 insertions(+), 1 deletion(-)
 create mode 100644 msg/plane_coefficients.msg

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ffb02fc..4bcc230 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -30,6 +30,7 @@ ENDIF (PCL_FOUND)
 add_message_files(
    FILES
    segmented_objects.msg
+   plane_coefficients.msg
 )
 
 ## Generate services in the 'srv' folder
diff --git a/include/tos_supervoxels_alg.h b/include/tos_supervoxels_alg.h
index 5610087..7de7418 100644
--- a/include/tos_supervoxels_alg.h
+++ b/include/tos_supervoxels_alg.h
@@ -255,6 +255,9 @@ class TosSupervoxelsAlgorithm
     double get_zmin();
     double get_zmax();
     int get_th_points();
+
+    pcl::ModelCoefficients get_plane_coefficients();
+
 };
 
 #endif
diff --git a/include/tos_supervoxels_alg_node.h b/include/tos_supervoxels_alg_node.h
index 703c6b4..42d9c32 100644
--- a/include/tos_supervoxels_alg_node.h
+++ b/include/tos_supervoxels_alg_node.h
@@ -30,6 +30,8 @@
 #include <pcl_conversions/pcl_conversions.h>
 #include <iri_tos_supervoxels/object_segmentation.h>
 #include <iri_tos_supervoxels/segmented_objects.h>
+#include <iri_tos_supervoxels/plane_coefficients.h>
+
 
 // [publisher subscriber headers]
 
diff --git a/msg/plane_coefficients.msg b/msg/plane_coefficients.msg
new file mode 100644
index 0000000..0c8f8c7
--- /dev/null
+++ b/msg/plane_coefficients.msg
@@ -0,0 +1,4 @@
+float32 a
+float32 b
+float32 c
+float32 d
\ No newline at end of file
diff --git a/msg/segmented_objects.msg b/msg/segmented_objects.msg
index 474846f..ee20477 100644
--- a/msg/segmented_objects.msg
+++ b/msg/segmented_objects.msg
@@ -1 +1 @@
-sensor_msgs/PointCloud2[] objects
+sensor_msgs/PointCloud2[] objects
\ No newline at end of file
diff --git a/src/tos_supervoxels_alg.cpp b/src/tos_supervoxels_alg.cpp
index 2bd6a99..7ad9505 100644
--- a/src/tos_supervoxels_alg.cpp
+++ b/src/tos_supervoxels_alg.cpp
@@ -227,4 +227,10 @@ int TosSupervoxelsAlgorithm::get_th_points()
   return this->obj_segment.get_th_points();
 }
 
+pcl::ModelCoefficients TosSupervoxelsAlgorithm::get_plane_coefficients()
+{
+  return this->obj_segment.get_plane_coefficients();
+}
+
+
 // TosSupervoxelsAlgorithm Public API
diff --git a/src/tos_supervoxels_alg_client_node.cpp b/src/tos_supervoxels_alg_client_node.cpp
index 227d761..5e26fee 100644
--- a/src/tos_supervoxels_alg_client_node.cpp
+++ b/src/tos_supervoxels_alg_client_node.cpp
@@ -13,6 +13,7 @@
 
 // msg
 #include "iri_tos_supervoxels/segmented_objects.h"
+#include "iri_tos_supervoxels/plane_coefficients.h"
 
 ros::ServiceClient client;
 ros::Publisher pub,pub_segmented_objs;
diff --git a/src/tos_supervoxels_alg_node.cpp b/src/tos_supervoxels_alg_node.cpp
index c0cd3da..b6ef3bc 100644
--- a/src/tos_supervoxels_alg_node.cpp
+++ b/src/tos_supervoxels_alg_node.cpp
@@ -88,6 +88,14 @@ bool TosSupervoxelsAlgNode::object_segmentationCallback(iri_tos_supervoxels::obj
   std::vector<pcl::PointCloud<pcl::PointXYZRGBA> > objects;
   objects = alg_.get_segmented_objects_simple();
   
+  //fill the plane coefficients field
+  pcl::ModelCoefficients plane_coefficients;
+  plane_coefficients = alg_.get_plane_coefficients();
+  res.plane_coeff.a = plane_coefficients.values[0];
+  res.plane_coeff.b = plane_coefficients.values[1];
+  res.plane_coeff.c = plane_coefficients.values[2];
+  res.plane_coeff.d = plane_coefficients.values[3];
+
   // free the memory of the algorithm
   alg_.reset();
 
@@ -104,6 +112,8 @@ bool TosSupervoxelsAlgNode::object_segmentationCallback(iri_tos_supervoxels::obj
   }
   res.objects = seg_objs_msg; 
 
+
+
   //unlock previously blocked shared variables
   this->object_segmentation_mutex_exit();
   this->alg_.unlock();
diff --git a/srv/object_segmentation.srv b/srv/object_segmentation.srv
index 28afd51..9fb72e4 100644
--- a/srv/object_segmentation.srv
+++ b/srv/object_segmentation.srv
@@ -1,3 +1,4 @@
 sensor_msgs/PointCloud2 point_cloud
 ---
 iri_tos_supervoxels/segmented_objects objects
+iri_tos_supervoxels/plane_coefficients plane_coeff
\ No newline at end of file
-- 
GitLab