clean up

Signed-off-by: Ken Haagh <[email protected]>
tue-robotics · Dec 19, 2023 · 7858410 · 7858410
1 parent ab8a9d4
commit 7858410
Show file tree

Hide file tree

Showing 10 changed files with 61 additions and 109 deletions.
diff --git a/people_tracking/src/people_tracking/colour_check.py b/people_tracking/src/people_tracking/colour_check.py
@@ -5,14 +5,9 @@
 from cv_bridge import CvBridge
 
 # MSGS
-from sensor_msgs.msg import Image
-from people_tracking.msg import ColourCheckedTarget
 from people_tracking.msg import DetectedPerson
 from people_tracking.msg import ColourTarget
 
-from rospy.numpy_msg import numpy_msg
-
-
 NODE_NAME = 'HoC'
 TOPIC_PREFIX = '/hero/'
 
@@ -56,47 +51,6 @@ def get_vector(image, bins=32):
         vector = np.concatenate(histograms, axis=0).reshape(-1)  # Create colour histogram vector
         return vector.tolist()
 
-    @staticmethod
-    def euclidean(a, b):
-        """ Euclidean distance between two vectors. Closer to 0 means better match."""
-        return np.linalg.norm(a - b)
-
-    @staticmethod
-    def cosine(a, b):
-        """ Cosine distance between two vectors. Closer to 1 means a better match."""
-        return np.dot(a, b) / (np.linalg.norm(a) * np.linalg.norm(b))
-
-    def compare_hoc(self, detected_persons):
-        """ Compare newly detected persons to previously detected target."""
-        bridge = CvBridge()
-        match = False
-        idx_person = None
-
-        person_vectors = [self.get_vector(bridge.imgmsg_to_cv2(person, desired_encoding='passthrough')) for person in
-                          detected_persons]
-
-        if len(self.HoC_detections) < 1:
-            self.HoC_detections.append(person_vectors[0])
-            idx_person = 0
-            match = True
-        else:
-            flag = False
-            for Hoc_detection in self.HoC_detections:
-                for idx_person, vector in enumerate(person_vectors):
-                    distance = self.euclidean(vector, Hoc_detection)
-                    if distance < 0.25:
-                        # rospy.loginfo(str(idx_person) + " " + str(distance))
-                        if len(self.HoC_detections) < 5:
-                            self.HoC_detections.append(vector)
-                        else:
-                            self.HoC_detections = self.HoC_detections[1:] + [vector]
-                        flag = True
-                        match = True
-                        break
-                if flag:
-                    break
-        return match, idx_person
-
     def callback(self, data):
         time = data.time
         nr_batch = data.nr_batch
@@ -106,18 +60,13 @@ def callback(self, data):
         y_positions = data.y_positions
         z_positions = data.z_positions
 
-        match = False
-        idx_match = None
-
         if nr_batch <= self.last_batch_processed:
             return
         if nr_persons < 1:
             return
         bridge = CvBridge()
-        # match, idx_match = self.compare_hoc(detected_persons)
         colour_vectors = [self.get_vector(bridge.imgmsg_to_cv2(person, desired_encoding='passthrough')) for person in
                           detected_persons]
-        # if match:
 
         msg = ColourTarget()
         msg.time = time
@@ -127,15 +76,10 @@ def callback(self, data):
         msg.y_positions = y_positions
         msg.z_positions = z_positions
         msg.colour_vectors = [item for sublist in colour_vectors for item in sublist]
-        # msg.detected_person = detected_persons[idx_match]
 
         self.publisher.publish(msg)
         self.last_batch_processed = nr_batch
 
-        # if nr_persons > 0 and match:
-        #     self.publisher_debug.publish(detected_persons[idx_match])
-
-
 if __name__ == '__main__':
     try:
         node_hoc = HOC()

diff --git a/people_tracking/src/people_tracking/people_tracker.py b/people_tracking/src/people_tracking/people_tracker.py
diff --git a/people_tracking/test/webcam.py → ...le_tracking/src/people_tracking/webcam.py b/people_tracking/test/webcam.py → ...le_tracking/src/people_tracking/webcam.py
diff --git a/...acking/src/people_tracking/depth_image.py → ..._tracking/test/experiments/depth_image.py b/...acking/src/people_tracking/depth_image.py → ..._tracking/test/experiments/depth_image.py
diff --git a/..._tracking/src/people_tracking/listener.py → people_tracking/test/experiments/listener.py b/..._tracking/src/people_tracking/listener.py → people_tracking/test/experiments/listener.py
diff --git a/people_tracking/src/people_tracker.cpp → ...cking/test/experiments/people_tracker.cpp b/people_tracking/src/people_tracker.cpp → ...cking/test/experiments/people_tracker.cpp
diff --git a/...le_tracking/src/people_tracking/skelly.py → people_tracking/test/experiments/skelly.py b/...le_tracking/src/people_tracking/skelly.py → people_tracking/test/experiments/skelly.py
diff --git a/...e_tracking/test/test_script+webcam.launch → ...est/experiments/test_script+webcam.launch b/...e_tracking/test/test_script+webcam.launch → ...est/experiments/test_script+webcam.launch
diff --git a/people_tracking/src/people_tracking/yolo.py → people_tracking/test/experiments/yolo.py b/people_tracking/src/people_tracking/yolo.py → people_tracking/test/experiments/yolo.py
diff --git a/people_tracking/test/people_tracking.launch b/people_tracking/test/people_tracking.launch
@@ -7,13 +7,13 @@
   <arg name="depth_camera" default="False" />
   <arg name="save_data" default="False"/>
 
-  <node
-    pkg="people_tracking"
-    type="depth_image.py"
-    name="depth"
-    output="screen"
-    args="$(arg laptop) $(arg depth_camera) $(arg save_data)"
-  />
+<!--   <node -->
+<!--     pkg="people_tracking" -->
+<!--     type="depth_image.py" -->
+<!--     name="depth" -->
+<!--     output="screen" -->
+<!--     args="$(arg laptop) $(arg depth_camera) $(arg save_data)" -->
+<!--   /> -->
 
   <node
     pkg="people_tracking"