diff --git a/people_tracking_v2/scripts/HoC.py b/people_tracking_v2/scripts/HoC.py index 99e4a63..0eddf1d 100755 --- a/people_tracking_v2/scripts/HoC.py +++ b/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}") diff --git a/people_tracking_v2/scripts/comparison_node.py b/people_tracking_v2/scripts/comparison_node.py index 3247ee8..7282b09 100755 --- a/people_tracking_v2/scripts/comparison_node.py +++ b/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 diff --git a/people_tracking_v2/scripts/pose_estimation_node.py b/people_tracking_v2/scripts/pose_estimation_node.py index 056d5f5..fa6fdaf 100755 --- a/people_tracking_v2/scripts/pose_estimation_node.py +++ b/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): diff --git a/people_tracking_v2/scripts/yolo_seg.py b/people_tracking_v2/scripts/yolo_seg.py index cf4cc25..1b250a0 100755 --- a/people_tracking_v2/scripts/yolo_seg.py +++ b/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."""