Revert "Drawing operator"

Solved problem

people_tracking_v2/scripts/comparison_node.py

@@ -44,11 +44,23 @@ def __init__(self):
         self.operator_data_set = False
 
         # Define the file paths for saving the data
-        self.operator_npz_file_path = os.path.expanduser('~/hoc_data/operator_data.npz')
-        self.detection_npz_file_path = os.path.expanduser('~/hoc_data/latest_detection_1_data.npz')
+        self.data_directory = '/home/miguel/ros/noetic/system/src/people_tracking_v2/operator_data'
+        self.operator_npz_file_path = os.path.join(self.data_directory, 'operator_data.npz')
+        self.detection_npz_file_path = os.path.join(self.data_directory, 'latest_detection_1_data.npz')
+
+        # Ensure the directories exist
+        self.ensure_directories_exist([self.operator_npz_file_path, self.detection_npz_file_path])
 
         rospy.spin()
 
+    def ensure_directories_exist(self, paths):
+        """Ensure that the directories for the given paths exist."""
+        for path in paths:
+            directory = os.path.dirname(path)
+            if not os.path.exists(directory):
+                os.makedirs(directory)
+                rospy.loginfo(f"Created directory: {directory}")
+
     def sync_callback(self, hoc_array, pose_array):
         """Callback function to handle synchronized HoC and pose data."""
         rospy.loginfo("sync_callback invoked")
@@ -147,17 +159,17 @@ def compute_distance(self, vector1, vector2):
         """Compute the Euclidean distance between two vectors (General)."""
         return np.linalg.norm(vector1 - vector2)
 
-    #def save_operator_data(self):
+    def save_operator_data(self):
         """Save the accumulated operator data to an .npz file."""
         np.savez(self.operator_npz_file_path, hue=self.operator_hue_avg, sat=self.operator_sat_avg, val=self.operator_val_avg, pose=self.operator_pose_median)
         rospy.loginfo(f"Saved accumulated operator data to {self.operator_npz_file_path}")
 
-    #def save_latest_detection_data(self, hue_vector, sat_vector, val_vector, head_feet_distance):
+    def save_latest_detection_data(self, hue_vector, sat_vector, val_vector, head_feet_distance):
         """Save the latest data for detection ID 1 to an .npz file."""
         np.savez(self.detection_npz_file_path, hue=hue_vector, sat=sat_vector, val=val_vector, pose=head_feet_distance)
         rospy.loginfo(f"Saved latest detection 1 data to {self.detection_npz_file_path}")
 
-    #def publish_debug_info(self, hoc_distance_score, pose_distance_score, detection_id):
+    def publish_debug_info(self, hoc_distance_score, pose_distance_score, detection_id):
         """Publish debug information about the current comparison (General)."""
         debug_msg = String()
         debug_msg.data = f"Detection ID {detection_id}: HoC Distance score: {hoc_distance_score:.2f}, Pose Distance score: {pose_distance_score:.2f}"

people_tracking_v2/scripts/decision_node.py

@@ -33,7 +33,7 @@ def __init__(self):
 
         # Publisher for decision results and marked images
         self.decision_pub = rospy.Publisher('/decision/result', DecisionResult, queue_size=10)
-        #self.marked_image_pub = rospy.Publisher('/marked_image', Image, queue_size=10)
+        self.marked_image_pub = rospy.Publisher('/marked_image', Image, queue_size=10)
 
         # Initialize variables for saving data
         self.save_data = save_data

people_tracking_v2/scripts/yolo_seg.py

@@ -19,8 +19,8 @@
 
 laptop = sys.argv[1]
 name_subscriber_RGB = 'Webcam/image_raw' if laptop == "True" else '/hero/head_rgbd_sensor/rgb/image_raw'
-depth_camera = False #if sys.argv[2] == "False" else True
-save_data = False #if sys.argv[3] == "False" else True
+depth_camera = False if sys.argv[2] == "False" else True
+save_data = False if sys.argv[3] == "False" else True
 
 class YoloSegNode:
     def __init__(self):