From 125981c0d2849560f3538a47cb05e88bf41f250e Mon Sep 17 00:00:00 2001
From: Miguelmelon <m.colera.borras@student.tue.nl>
Date: Tue, 28 May 2024 13:02:40 +0200
Subject: [PATCH] Debugging pose data recorder

---
 .../scripts/pose_estimation_node.py           |  8 +++---
 ...e-HoC-data.launch => save_HoC_data.launch} |  2 +-
 .../launchers/save_pose_data.launch           | 27 +++++++++++++++++++
 people_tracking_v2/tools/save_pose_data.py    |  4 +--
 4 files changed, 34 insertions(+), 7 deletions(-)
 rename people_tracking_v2/src/people_tracking/launchers/{save-HoC-data.launch => save_HoC_data.launch} (95%)
 create mode 100644 people_tracking_v2/src/people_tracking/launchers/save_pose_data.launch
 mode change 100644 => 100755 people_tracking_v2/tools/save_pose_data.py

diff --git a/people_tracking_v2/scripts/pose_estimation_node.py b/people_tracking_v2/scripts/pose_estimation_node.py
index b91b635..715784b 100755
--- a/people_tracking_v2/scripts/pose_estimation_node.py
+++ b/people_tracking_v2/scripts/pose_estimation_node.py
@@ -18,7 +18,7 @@
 from sensor_msgs.msg import Image
 
 from people_tracking.yolo_pose_wrapper import YoloPoseWrapper
-from people_tracking_v2.msg import PoseDistance
+from people_tracking_v2.msg import BodySize
 
 class PoseEstimationNode:
     def __init__(
@@ -69,7 +69,7 @@ def __init__(
         self._recognize_srv = rospy.Service("recognize", Recognize, self._recognize_srv)
         self._image_subscriber = rospy.Subscriber("/Webcam/image_raw", Image, self._image_callback)
         self._recognitions_publisher = rospy.Publisher("/pose_recognitions", Recognitions, queue_size=10)
-        self._pose_distance_publisher = rospy.Publisher("/pose_distances", PoseDistance, queue_size=10)
+        self._pose_distance_publisher = rospy.Publisher("/pose_distances", BodySize, queue_size=10)
         if self._topic_publish_result_image or self._service_publish_result_image:
             self._result_image_publisher = rospy.Publisher("/pose_result_image", Image, queue_size=10)
 
@@ -92,7 +92,7 @@ def _image_callback(self, image_msg):
         # Calculate distances and publish them
         for pose in pose_details:
             try:
-                pose_distance_msg = PoseDistance()
+                pose_distance_msg = BodySize()
                 pose_distance_msg.header.stamp = rospy.Time.now()
                 if "LShoulder" in pose and "LHip" in pose:
                     pose_distance_msg.left_shoulder_hip_distance = self._wrapper.compute_distance(pose["LShoulder"], pose["LHip"])
@@ -148,7 +148,7 @@ def _get_recognitions(self, image_msg, save_images, publish_images):
         # Calculate distances and log them
         for pose in pose_details:
             try:
-                pose_distance_msg = PoseDistance()
+                pose_distance_msg = BodySize()
                 pose_distance_msg.header.stamp = rospy.Time.now()
                 if "LShoulder" in pose and "LHip" in pose:
                     pose_distance_msg.left_shoulder_hip_distance = self._wrapper.compute_distance(pose["LShoulder"], pose["LHip"])
diff --git a/people_tracking_v2/src/people_tracking/launchers/save-HoC-data.launch b/people_tracking_v2/src/people_tracking/launchers/save_HoC_data.launch
similarity index 95%
rename from people_tracking_v2/src/people_tracking/launchers/save-HoC-data.launch
rename to people_tracking_v2/src/people_tracking/launchers/save_HoC_data.launch
index 45716e1..673d42e 100644
--- a/people_tracking_v2/src/people_tracking/launchers/save-HoC-data.launch
+++ b/people_tracking_v2/src/people_tracking/launchers/save_HoC_data.launch
@@ -26,7 +26,7 @@
 
   <node
     pkg="people_tracking_v2"
-    type="save-HoC-data.py"
+    type="save_HoC_data.py"
     name="Operator_HoC"
     output="screen"
   />
diff --git a/people_tracking_v2/src/people_tracking/launchers/save_pose_data.launch b/people_tracking_v2/src/people_tracking/launchers/save_pose_data.launch
new file mode 100644
index 0000000..1184550
--- /dev/null
+++ b/people_tracking_v2/src/people_tracking/launchers/save_pose_data.launch
@@ -0,0 +1,27 @@
+<!--  <arg name="laptop" default="True" />  -->
+<!--   <arg name="depth_camera" default="False" /> -->
+<!--   <arg name="save_data" default="False"/> -->
+
+<launch>
+  <node
+    pkg="cv_camera"
+    type="cv_camera_node"
+    name="Webcam"
+    output="screen"
+  />
+
+  <node
+    pkg="people_tracking_v2"
+    type="pose_estimation_node.py"
+    name="Pose"
+    output="screen"
+  />
+
+  <node
+    pkg="people_tracking_v2"
+    type="save_pose_data.py"
+    name="Operator_pose"
+    output="screen"
+  />
+
+</launch>
\ No newline at end of file
diff --git a/people_tracking_v2/tools/save_pose_data.py b/people_tracking_v2/tools/save_pose_data.py
old mode 100644
new mode 100755
index aefee88..2892328
--- a/people_tracking_v2/tools/save_pose_data.py
+++ b/people_tracking_v2/tools/save_pose_data.py
@@ -2,7 +2,7 @@
 
 import rospy
 import numpy as np
-from people_tracking_v2.msg import PoseDistance
+from people_tracking_v2.msg import BodySize
 import os
 
 class SavePoseDataNode:
@@ -11,7 +11,7 @@ def __init__(self):
         rospy.init_node('save_pose_data_node', anonymous=True)
         
         # Subscriber to pose distance data
-        self.pose_sub = rospy.Subscriber('/pose_distances', PoseDistance, self.pose_callback)
+        self.pose_sub = rospy.Subscriber('/pose_distances', BodySize, self.pose_callback)
         
         # File to save pose data
         self.pose_data_file = os.path.expanduser('~/pose_data/pose_data.npz')