Calculating right distances

people_tracking_v2/scripts/pose_estimation_node.py

@@ -136,15 +136,15 @@ def _get_recognitions(self, image_msg, save_images, publish_images):
         if publish_images:
             self._result_image_publisher.publish(self._bridge.cv2_to_imgmsg(result_image, "bgr8"))
 
-        # Calculate distances and log them
+        # Calculate distances
         for pose in pose_details:
-            if "LShoulder" in pose and "RShoulder" in pose:
-                shoulder_distance = self._wrapper.compute_distance(pose["LShoulder"], pose["RShoulder"])
-                rospy.loginfo(f"Shoulder Distance: {shoulder_distance:.2f}")
+            if "LShoulder" in pose and "LHip" in pose:
+                left_shoulder_hip_distance = self._wrapper.compute_distance(pose["LShoulder"], pose["LHip"])
+                rospy.loginfo(f"Left Shoulder-Hip Distance: {left_shoulder_hip_distance:.2f}")
 
-            if "LHip" in pose and "RHip" in pose:
-                hip_distance = self._wrapper.compute_distance(pose["LHip"], pose["RHip"])
-                rospy.loginfo(f"Hip Distance: {hip_distance:.2f}")
+            if "RShoulder" in pose and "RHip" in pose:
+                right_shoulder_hip_distance = self._wrapper.compute_distance(pose["RShoulder"], pose["RHip"])
+                rospy.loginfo(f"Right Shoulder-Hip Distance: {right_shoulder_hip_distance:.2f}")
 
         return recognitions
 

people_tracking_v2/src/people_tracking/yolo_pose_wrapper.py

@@ -89,6 +89,6 @@ def detect_poses(self, image: np.ndarray, conf: float = 0.25) -> Tuple[List[Reco
         return recognitions, overlayed_image, pose_details
 
     @staticmethod
-    def compute_distance(point1: Tuple[float, float], point2: Tuple[float, float]) -> float:
+    def compute_distance(self, point1, point2):
         """Compute the Euclidean distance between two points."""
-        return np.linalg.norm(np.array(point1) - np.array(point2))
+        return np.sqrt((point1[0] - point2[0]) ** 2 + (point1[1] - point2[1]) ** 2)