new plot

colcon_ws/src/my_controller/my_controller/latency_plotter.py

@@ -14,12 +14,13 @@ class LatencyPlotter(Node):
         super().__init__('latency_plotter')
         
         # Subscriptions
-        self.create_subscription(
+        self.subscription = self.create_subscription(
             FrameOfMocapData,
             '/optitrack_wrapper_node/frame_data',
             self.latency_callback,
             10
         )
+        self.get_logger().info("Subscribed to /optitrack_wrapper_node/frame_data")
         
         # Data storage
         self.timestamps = deque(maxlen=1000)  # Stores timestamps
@@ -33,6 +34,9 @@ class LatencyPlotter(Node):
         timestamp = msg.time_publishing.sec + msg.time_publishing.nanosec * 1e-9
         latency = msg.system_latency_ms
 
+        # Log for debugging
+        self.get_logger().info(f"Received data: Timestamp={timestamp}, Latency={latency}")
+
         # Append data to deques
         with self.data_lock:
             self.timestamps.append(timestamp)
@@ -42,6 +46,7 @@ class LatencyPlotter(Node):
         with self.data_lock:
             return list(self.timestamps), list(self.latencies)
 
+
 def plot_animation(node):
     # Initialize the plot
     fig, ax = plt.subplots()
@@ -61,8 +66,9 @@ def plot_animation(node):
             ax.autoscale_view()
         return line,
 
-    ani = FuncAnimation(fig, update, interval=100, blit=True)
-    plt.show()
+    ani = FuncAnimation(fig, update, interval=100, blit=False)
+    plt.show(block=False)  # Non-blocking to allow ROS to keep running
+
 
 def main(args=None):
     rclpy.init(args=args)
@@ -75,11 +81,16 @@ def main(args=None):
     try:
         # Start the plot animation
         plot_animation(latency_plotter)
+
+        # Keep the script alive for ROS and plotting
+        while plt.fignum_exists(1):
+            plt.pause(0.1)
     except KeyboardInterrupt:
         pass
     finally:
         latency_plotter.destroy_node()
         rclpy.shutdown()
 
+
 if __name__ == '__main__':
     main()