Skip to content
Snippets Groups Projects
Commit 8df7bc26 authored by Bruno Galdos's avatar Bruno Galdos
Browse files

new implementations

parent 69183a6b
No related branches found
No related tags found
No related merge requests found
Pipeline #506502 canceled
#!/usr/bin/env python3
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from collections import deque
import rclpy
from rclpy.node import Node
from optitrack_wrapper_ros2_msgs.msg import FrameOfMocapData
import threading
class LatencyPlotter(Node):
def __init__(self):
super().__init__('latency_plotter')
# Subscriptions
self.create_subscription(
FrameOfMocapData,
'/optitrack_wrapper_node/frame_data',
self.latency_callback,
10
)
# Data storage
self.timestamps = deque(maxlen=1000) # Stores timestamps
self.latencies = deque(maxlen=1000) # Stores system latencies
# Thread lock for data synchronization
self.data_lock = threading.Lock()
def latency_callback(self, msg):
# Extract timestamp and latency data
timestamp = msg.time_publishing.sec + msg.time_publishing.nanosec * 1e-9
latency = msg.system_latency_ms
# Append data to deques
with self.data_lock:
self.timestamps.append(timestamp)
self.latencies.append(latency)
def get_plot_data(self):
with self.data_lock:
return list(self.timestamps), list(self.latencies)
def plot_animation(node):
# Initialize the plot
fig, ax = plt.subplots()
line, = ax.plot([], [], label="System Latency (ms)", color="blue")
ax.set_title("System Latency Over Time")
ax.set_xlabel("Timestamp [s]")
ax.set_ylabel("System Latency [ms]")
ax.grid(True)
ax.legend()
# Update function for animation
def update(frame):
timestamps, latencies = node.get_plot_data()
if timestamps and latencies:
line.set_data(timestamps, latencies)
ax.relim()
ax.autoscale_view()
return line,
ani = FuncAnimation(fig, update, interval=100, blit=True)
plt.show()
def main(args=None):
rclpy.init(args=args)
latency_plotter = LatencyPlotter()
# Run ROS 2 node in a separate thread
ros_thread = threading.Thread(target=rclpy.spin, args=(latency_plotter,), daemon=True)
ros_thread.start()
try:
# Start the plot animation
plot_animation(latency_plotter)
except KeyboardInterrupt:
pass
finally:
latency_plotter.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
...@@ -36,6 +36,7 @@ setup( ...@@ -36,6 +36,7 @@ setup(
"evaluation_node_odom_final=my_controller.evaluation_node_odom_final:main", "evaluation_node_odom_final=my_controller.evaluation_node_odom_final:main",
"evaluation_node_slam=my_controller.evaluation_node_slam:main", "evaluation_node_slam=my_controller.evaluation_node_slam:main",
"latency_analysis=my_controller.latency_analysis:main", "latency_analysis=my_controller.latency_analysis:main",
"latency_plotter=my_controller.latency_plotter:main",
"optitrack_node=my_controller.optitrack_node:main" "optitrack_node=my_controller.optitrack_node:main"
], ],
}, },
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment