diff --git a/include/tos_supervoxels_alg_node.h b/include/tos_supervoxels_alg_node.h
index 089a48af8fefc471a437ca5b4eba469bb21d9c67..703c6b4ed58cb892f740e786cc86bb2fd28f3cb8 100644
--- a/include/tos_supervoxels_alg_node.h
+++ b/include/tos_supervoxels_alg_node.h
@@ -29,6 +29,7 @@
 #include "tos_supervoxels_alg.h"
 #include <pcl_conversions/pcl_conversions.h>
 #include <iri_tos_supervoxels/object_segmentation.h>
+#include <iri_tos_supervoxels/segmented_objects.h>
 
 // [publisher subscriber headers]
 
@@ -68,7 +69,7 @@ class TosSupervoxelsAlgNode : public algorithm_base::IriBaseAlgorithm<TosSupervo
     * Is updated everytime function config_update() is called.
     */
     Config config_;
-    
+
   public:
    /**
     * \brief Constructor
diff --git a/src/tos_supervoxels_alg_client_node.cpp b/src/tos_supervoxels_alg_client_node.cpp
index 92e8b39d2b9dbf49711ffc10edec523ad5155dc3..e7be51809e44b84daa2c0ebb2f04f778884e2c68 100644
--- a/src/tos_supervoxels_alg_client_node.cpp
+++ b/src/tos_supervoxels_alg_client_node.cpp
@@ -31,13 +31,13 @@ void callback(const sensor_msgs::PointCloud2 cloud_msg)
     sensor_msgs::PointCloud2 detected_objects_msg;
     pcl::PointCloud<pcl::PointXYZRGB> detected_objects_cloud;
 
-    iri_tos_supervoxels::segmented_objects segmented_objects_msg;
+    //iri_tos_supervoxels::segmented_objects segmented_objects_msg;
 
-    for (int i = 0; i < srv.response.objects.size(); ++i)
+    for (int i = 0; i < srv.response.objects.objects.size(); ++i)
     {
       // convert it to pcl
       pcl::PointCloud<pcl::PointXYZRGB> tmp;
-      pcl::fromROSMsg(srv.response.objects[i],tmp);
+      pcl::fromROSMsg(srv.response.objects.objects[i],tmp);
 
       //we now construct manually the point cloud
       //choose a random color for each segmented object
@@ -57,7 +57,7 @@ void callback(const sensor_msgs::PointCloud2 cloud_msg)
         detected_objects_cloud.points.push_back(temp_point);
       }
 
-      segmented_objects_msg.objects.push_back(srv.response.objects[i]);
+      //segmented_objects_msg.objects.push_back(srv.response.objects[i]);
     }
 
     detected_objects_cloud.width = detected_objects_cloud.points.size();
@@ -74,7 +74,7 @@ void callback(const sensor_msgs::PointCloud2 cloud_msg)
     //std::cout << "frame of point cloud: " << detected_objects_msg.header.frame_id << "\n";
     pub.publish(detected_objects_msg);
 
-    pub_segmented_objs.publish(segmented_objects_msg);
+    pub_segmented_objs.publish(srv.response.objects);
   }
   else
     ROS_ERROR("Failed to call service %s",service_name.c_str());
diff --git a/src/tos_supervoxels_alg_node.cpp b/src/tos_supervoxels_alg_node.cpp
index 3b37a5c993c1fa3329699d5126ec9677bfba9004..c0cd3da32cc553dbecf10d1080f933f30ae251db 100644
--- a/src/tos_supervoxels_alg_node.cpp
+++ b/src/tos_supervoxels_alg_node.cpp
@@ -92,7 +92,7 @@ bool TosSupervoxelsAlgNode::object_segmentationCallback(iri_tos_supervoxels::obj
   alg_.reset();
 
   //create msg
-  std::vector<sensor_msgs::PointCloud2> objects_msg;
+  iri_tos_supervoxels::segmented_objects seg_objs_msg;
   for (uint i = 0; i < objects.size(); ++i)
   {
     sensor_msgs::PointCloud2 object_msg;
@@ -100,9 +100,9 @@ bool TosSupervoxelsAlgNode::object_segmentationCallback(iri_tos_supervoxels::obj
     object_msg.header.seq = i;
     object_msg.header.frame_id = req.point_cloud.header.frame_id;
     object_msg.header.stamp = ros::Time::now();
-    objects_msg.push_back(object_msg);
+    seg_objs_msg.objects.push_back(object_msg);
   }
-  res.objects = objects_msg; 
+  res.objects = seg_objs_msg; 
 
   //unlock previously blocked shared variables
   this->object_segmentation_mutex_exit();
diff --git a/srv/object_segmentation.srv b/srv/object_segmentation.srv
index e70c6fa95acc39736cb84d76faf44f0851e1708b..28afd513a7073db8bf0f4adc53ab061a7eac4467 100644
--- a/srv/object_segmentation.srv
+++ b/srv/object_segmentation.srv
@@ -1,3 +1,3 @@
 sensor_msgs/PointCloud2 point_cloud
 ---
-sensor_msgs/PointCloud2[] objects
+iri_tos_supervoxels/segmented_objects objects