Change what distance is taken in Pose

people_tracking_v2/scripts/HoC.py

@@ -65,7 +65,7 @@ def segmented_images_callback(self, msg):
                 hoc_vectors.vectors.append(hoc_vector)
 
                 # Log the resulting values
-                rospy.loginfo(f"Detection ID {detection_id}: HoC Hue Vector: {hoc_vector.hue_vector[1]}, HoC Saturation Vector: {hoc_vector.sat_vector[1]}")
+                #rospy.loginfo(f"Detection ID {detection_id}: HoC Hue Vector: {hoc_vector.hue_vector[1]}, HoC Saturation Vector: {hoc_vector.sat_vector[1]}")
 
             except CvBridgeError as e:
                 rospy.logerr(f"Failed to convert segmented image: {e}") 

people_tracking_v2/scripts/comparison_node.py

@@ -92,6 +92,10 @@ def sync_callback(self, hoc_array, pose_array):
             comparison_scores_msg.id = hoc_msg.id
             comparison_scores_msg.hoc_distance_score = hoc_distance_score
             comparison_scores_msg.pose_distance_score = distance_score  # Save head-feet distance as pose_distance_score
+
+            # Log the scores
+            rospy.loginfo(f"Publishing scores - Detection ID {comparison_scores_msg.id}: HoC Distance score: {comparison_scores_msg.hoc_distance_score:.2f}, Pose Distance score: {comparison_scores_msg.pose_distance_score:.2f}")
+
             comparison_scores_array.scores.append(comparison_scores_msg)
 
         # Publish the comparison scores as a batch

people_tracking_v2/scripts/pose_estimation_node.py

@@ -51,12 +51,37 @@ def image_callback(self, image_msg):
                 detection_id = pose_distance_msg.id
 
                 pose_distance_msg.header.stamp = image_msg.header.stamp  # Use the timestamp from the incoming YOLO image
-                if "Nose" in pose and "LAnkle" in pose and "RAnkle" in pose:
-                    nose_to_left_ankle = self._wrapper.compute_distance(pose["Nose"], pose["LAnkle"])
-                    nose_to_right_ankle = self._wrapper.compute_distance(pose["Nose"], pose["RAnkle"])
-                    pose_distance_msg.head_feet_distance = (nose_to_left_ankle + nose_to_right_ankle) / 2
+
+                if "LEar" in pose and "LAnkle" in pose:
+                    left_ear_to_left_ankle = self._wrapper.compute_distance(pose["LEar"], pose["LAnkle"])
+                else:
+                    left_ear_to_left_ankle = float('inf')
+
+                if "LEar" in pose and "RAnkle" in pose:
+                    left_ear_to_right_ankle = self._wrapper.compute_distance(pose["LEar"], pose["RAnkle"])
+                else:
+                    left_ear_to_right_ankle = float('inf')
+
+                if "REar" in pose and "LAnkle" in pose:
+                    right_ear_to_left_ankle = self._wrapper.compute_distance(pose["REar"], pose["LAnkle"])
+                else:
+                    right_ear_to_left_ankle = float('inf')
+
+                if "REar" in pose and "RAnkle" in pose:
+                    right_ear_to_right_ankle = self._wrapper.compute_distance(pose["REar"], pose["RAnkle"])
+                else:
+                    right_ear_to_right_ankle = float('inf')
+
+                distances = [left_ear_to_left_ankle, left_ear_to_right_ankle, right_ear_to_left_ankle, right_ear_to_right_ankle]
+                min_distance = min(distances)
+
+                if min_distance == float('inf'):
+                    rospy.logwarn("No valid ear-to-ankle distance found")
+                else:
+                    pose_distance_msg.head_feet_distance = min_distance
                     rospy.loginfo(f"For ID {detection_id} Head-Feet Distance: {pose_distance_msg.head_feet_distance:.2f}")
 
+
                 # Find the corresponding detection ID and use depth value to normalize the size
                 for detection in self.current_detections:
                     if self.is_pose_within_detection(pose, detection):

people_tracking_v2/scripts/yolo_seg.py

@@ -68,7 +68,7 @@ def __init__(self):
     def operator_id_callback(self, msg):
         """Callback function to update the operator ID."""
         self.operator_id = msg.data
-        rospy.loginfo(f"Updated operator ID to: {self.operator_id}")
+        #rospy.loginfo(f"Updated operator ID to: {self.operator_id}")
 
     def set_operator(self, operator_id):
         """Set the ID of the operator to track."""