add metric to on_dynamic_joint_states

Author: Nicolas Torres

dynamic_state_router/dynamic_state_router/dynamic_state_router.py

@@ -37,6 +37,7 @@
 
 from .forward_controller import ForwardControllersPool
 import reachy2_monitoring as rm
+import prometheus_client as pc
 
 class TracedCommands:
     def __init__(self, traceparent="") -> None:
@@ -55,6 +56,9 @@ def __init__(self, node_name, controllers_file):
         self.logger = self.get_logger()
         self.tracer = rm.tracer(node_name)
         self.on_dynamic_joint_commands_counter = 0
+        pc.start_http_server(10002)
+        self.sum_joint_states = pc.Summary("dynamic_state_router_on_dynamic_joint_states_time",
+                                           "Time spent during on_dynamic_joint_states callback")
 
         self.freq, self.forward_controllers = ForwardControllersPool.parse(
             self.logger, controllers_file
@@ -335,26 +339,27 @@ def handle_regular_commands(self, commands):
     # Subscription: DynamicJointState cb
     def on_dynamic_joint_states(self, state: DynamicJointState):
         """Retreive the joint state from /dynamic_joint_states."""
-        if not self.joint_state_ready.is_set():
-            for uid, name in enumerate(state.joint_names):
-                self.joint_state[name] = {}
-                self.joint_state[name]["name"] = name
-                self.joint_state[name]["uid"] = uid
-
-                self.joint_command[name] = {}
-
-        for uid, (name, kv) in enumerate(
-            zip(state.joint_names, state.interface_values)
-        ):
-            for k, v in zip(kv.interface_names, kv.values):
-                self.joint_state[name][k] = v
-
-        if not self.joint_state_ready.is_set():
-            for name, state in self.joint_state.items():
-                if "position" in state:
-                    state["target_position"] = state["position"]
-
-            self.joint_state_ready.set()
+        with self.sum_joint_states.time():
+            if not self.joint_state_ready.is_set():
+                for uid, name in enumerate(state.joint_names):
+                    self.joint_state[name] = {}
+                    self.joint_state[name]["name"] = name
+                    self.joint_state[name]["uid"] = uid
+
+                    self.joint_command[name] = {}
+
+            for uid, (name, kv) in enumerate(
+                zip(state.joint_names, state.interface_values)
+            ):
+                for k, v in zip(kv.interface_names, kv.values):
+                    self.joint_state[name][k] = v
+
+            if not self.joint_state_ready.is_set():
+                for name, state in self.joint_state.items():
+                    if "position" in state:
+                        state["target_position"] = state["position"]
+
+                self.joint_state_ready.set()
 
     def on_forward_position_controller_update(
         self, msg: Float64MultiArray, controller_name: str