Fix diagnostic

src/soft_biometrics_estimation.py

@@ -77,7 +77,10 @@ class SoftBiometricsEstimation:
         # Initialize the diagnostics
         self.last_diagnostic_stamp = None
         self.send_diagnostic_every = 5  # seconds
-        self._init_diagnostic()
+        # Initialize the diagnostics publisher
+        self.diagnostic_pub = rospy.Publisher(
+            "/diagnostics", DiagnosticArray, queue_size=1
+        )
 
     @property
     def empty_database(self):
@@ -111,6 +114,15 @@ class SoftBiometricsEstimation:
             elif id not in face_ids and id in subscribed_ids:
                 self._unsubscribe_face(id)
 
+        # Send diagnostic if needed
+        if (
+            self.last_diagnostic_stamp is None
+            or (rospy.Time.now() - self.last_diagnostic_stamp).to_sec()
+            > self.send_diagnostic_every
+        ):
+            self._send_diagnostic()
+            self.last_diagnostic_stamp = rospy.Time.now()
+
     def _unsubscribe_face(self, face_id):
         print(f"Unsubscribing from face {face_id} topics")
 
@@ -162,15 +174,6 @@ class SoftBiometricsEstimation:
         # Find matchings
         self._find_matchings(embeddings, id)
 
-        # Send diagnostic if needed
-        if (
-            self.last_diagnostic_stamp is None
-            or (rospy.Time.now() - self.last_diagnostic_stamp).to_sec()
-            > self.send_diagnostic_every
-        ):
-            self._send_diagnostic()
-            self.last_diagnostic_stamp = rospy.Time.now()
-
     def _find_matchings(self, embeddings, face_id):
         # Evaluate the similarity with the gallery.
         if embeddings is not None and not self.empty_database:
@@ -286,27 +289,10 @@ class SoftBiometricsEstimation:
         processed_tensor = (image_tensor - 127.5) / 128.0
         return processed_tensor
 
-    def _init_diagnostic(self):
-        # Initialize the diagnostics publisher
-        self.diagnostic_pub = rospy.Publisher(
-            "/diagnostics", DiagnosticArray, queue_size=1
-        )
-
-        # Send a diagnostic message
-        diagnostic_msgs = DiagnosticArray(
-            status=[
-                DiagnosticStatus(
-                    level=DiagnosticStatus.OK,
-                    name="Social perception: Face detection: Soft biometrics",
-                    message="Soft biometrics is running",
-                )
-            ]
-        )
-        diagnostic_msgs.header.stamp = rospy.Time.now()
-        self.diagnostic_pub.publish(diagnostic_msgs)
-
     def _send_diagnostic(self):
-        diagnostic_values = []
+        diagnostic_values = [
+            KeyValue(key="Faces tracked currently", value=f"{len(self.faces.keys())}")
+        ]
         for face_id, face in self.faces.items():
             if "data" not in face.keys():
                 continue
@@ -339,7 +325,7 @@ class SoftBiometricsEstimation:
             status=[
                 DiagnosticStatus(
                     level=DiagnosticStatus.OK,
-                    name="Social perception: Face detection: Soft biometrics",
+                    name="Social perception: Soft biometrics",
                     values=diagnostic_values,
                 )
             ]