Add detection score to service response

scripts/pose_estimation.py

@@ -81,20 +81,21 @@ class PredictionService:
         pose_predictions = self.predictor.pose_predictions
 
         # transform cosypose objects to ros msg
-        objects_array = CosyObjectArray()
-        objects_array.header = req.image.header
+        scores = []
+        objects_array = CosyObjectArray(header=req.image.header)
         for i in range(len(pose_predictions.poses)):
             label = pose_predictions.infos['label'][i]
             poseKDL = pm.fromMatrix(pose_predictions.poses[i])
             poseROS = Pose(position=Point(*poseKDL.p) , orientation=Quaternion(*poseKDL.M.GetQuaternion()))
             objects_array.objects.append(CosyObject(label=label, pose=poseROS))
+            if pose_estimation_prior == None: # Detection score only if not warm-started
+                scores.append(pose_predictions.infos['score'][i])
 
         # Publish debug objects
         if(self.debug_converter != None):
             self.debug_converter.pub(objects_array)
 
-
-        return objects_array
+        return [objects_array, scores]
 
 
 if __name__ == '__main__':

srv/GetPrediction.srv

@@ -9,5 +9,8 @@ CosyObject[] objects_prior
 
 ---
 
-# Prediction with additionnal data
+# Prediction (with additionnal data)
 CosyObjectArray prediction
+
+# Detection scores (Empty if warm-started)
+float32[] scores