Frequency measurement

people_tracking_v2/CMakeLists.txt

@@ -55,6 +55,7 @@ install(PROGRAMS
   tools/save_pose_data.py
   testing/rgb_publisher.py
   testing/depth_publisher.py
+  testing/frequency_measure.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

people_tracking_v2/testing/depth_publisher.py

@@ -42,7 +42,7 @@ def publish_depth_images_from_folder(folder_path, topic_name):
 
 if __name__ == '__main__':
     rospy.init_node('depth_publisher_node', anonymous=True)
-    folder_path = '/home/miguel/Documents/BEP-Testing/Test Case 2/Frames Sat Jun 29 Test Case 2/depth' #/home/miguel/Documents/BEP-Testing/Test Case 1/Frames Sat Jun 29 Test Case 1/depth
+    folder_path = '/home/miguel/Documents/BEP-Testing/Test Case 1/Frames Sat Jun 29 Test Case 1/depth' #/home/miguel/Documents/BEP-Testing/Test Case 1/Frames Sat Jun 29 Test Case 1/depth
     topic_name = '/hero/head_rgbd_sensor/depth_registered/image_raw'
     try:
         publish_depth_images_from_folder(folder_path, topic_name)

people_tracking_v2/testing/frequency_measure.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python
+
+import rospy
+from people_tracking_v2.msg import HoCVectorArray, BodySizeArray, DetectionArray, DecisionResult  # Replace with actual package name
+import numpy as np
+import csv
+import os
+import threading
+
+class FrequencyMonitor:
+    def __init__(self, topic_name, msg_type):
+        self.topic_name = topic_name
+        self.times = []
+        self.subscriber = rospy.Subscriber(topic_name, msg_type, self.callback)
+
+    def callback(self, msg):
+        current_time = rospy.get_time()
+        self.times.append(current_time)
+
+    def compute_frequencies(self):
+        if len(self.times) < 2:
+            rospy.loginfo(f"Not enough data to compute frequencies for {self.topic_name}.")
+            return None, None
+
+        time_intervals = np.diff(self.times)
+        mean_interval = np.mean(time_intervals)
+        max_interval = np.max(time_intervals)
+
+        mean_frequency = 1.0 / mean_interval if mean_interval > 0 else 0
+        worst_case_frequency = 1.0 / max_interval if max_interval > 0 else 0
+
+        return mean_frequency, worst_case_frequency
+
+def save_frequencies(monitors, output_file):
+    os.makedirs(os.path.dirname(output_file), exist_ok=True)
+
+    with open(output_file, 'w', newline='') as csvfile:
+        fieldnames = ['topic', 'mean_frequency', 'worst_case_frequency']
+        writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
+        writer.writeheader()
+
+        for monitor in monitors:
+            mean_frequency, worst_case_frequency = monitor.compute_frequencies()
+            if mean_frequency is not None and worst_case_frequency is not None:
+                writer.writerow({
+                    'topic': monitor.topic_name,
+                    'mean_frequency': mean_frequency,
+                    'worst_case_frequency': worst_case_frequency
+                })
+
+    rospy.loginfo(f"Frequency data saved to {output_file}")
+
+def timer_callback(monitors, output_file):
+    save_frequencies(monitors, output_file)
+    rospy.signal_shutdown("Data collection complete.")
+
+def main():
+    rospy.init_node('frequency_monitor', anonymous=True)
+
+    # Define the topics and their corresponding message types
+    topics = [
+        ('/hoc_vectors', HoCVectorArray),
+        ('/pose_distances', BodySizeArray),
+        ('/detections_info', DetectionArray),
+        ('/decision/result', DecisionResult)
+    ]
+
+    monitors = [FrequencyMonitor(topic, msg_type) for topic, msg_type in topics]
+
+    # Set up a timer to save data after 1 minute (60 seconds)
+    output_file = '/home/miguel/Documents/BEP-Testing/Test Case 1/Frequency Measurement/frequency_measurement.csv'
+    timer = threading.Timer(60, timer_callback, [monitors, output_file])
+    timer.start()
+
+    rospy.spin()
+
+if __name__ == '__main__':
+    main()

people_tracking_v2/testing/launchers/test_videos.launch

@@ -1,8 +1,8 @@
 <launch>
 
   <arg name="laptop" default="False" />
-  <arg name="depth_camera" default="False" />
-  <arg name="save_data" default="True"/>
+  <arg name="depth_camera" default="True" />
+  <arg name="save_data" default="False"/>
 
 <node
     pkg="people_tracking_v2"
@@ -58,5 +58,16 @@
     name="Decision"
     output="screen"
     args="$(arg laptop) $(arg depth_camera) $(arg save_data)"
+    />
+
+  <node
+    pkg="people_tracking_v2"
+    type="frequency_measure.py"
+    name="Freq_measurer"
+    output="screen"
+    args="$(arg laptop) $(arg depth_camera) $(arg save_data)"
   />
-</launch>
+
+
+</launch>
+

people_tracking_v2/testing/rgb_publisher.py

@@ -38,7 +38,7 @@ def publish_images_from_folder(folder_path):
 
 if __name__ == '__main__':
     rospy.init_node('rgb_image_folder_publisher_node', anonymous=True)
-    folder_path = '/home/miguel/Documents/BEP-Testing/Test Case 2/Frames Sat Jun 29 Test Case 2/rgb' #/home/miguel/Documents/BEP-Testing/Test Case 1/Frames Sat Jun 29 Test Case 1/rgb
+    folder_path = '/home/miguel/Documents/BEP-Testing/Test Case 1/Frames Sat Jun 29 Test Case 1/rgb' #/home/miguel/Documents/BEP-Testing/Test Case 1/Frames Sat Jun 29 Test Case 1/rgb
     try:
         publish_images_from_folder(folder_path)
     except rospy.ROSInterruptException: