Assigning individual IDs to each detection

people_tracking_v2/msg/BodySize.msg

@@ -1,3 +1,4 @@
 Header header
+int32 id
 float32 left_shoulder_hip_distance
 float32 right_shoulder_hip_distance

people_tracking_v2/msg/Detection.msg

@@ -1,6 +1,8 @@
+# Detection.msg
+int32 id
 float32 x1
 float32 y1
 float32 x2
 float32 y2
 float32 score
-int32 label
+int32 label

people_tracking_v2/msg/HoCVector.msg

@@ -1,3 +1,5 @@
+# HoCVector.msg
 Header header
+int32 id
 float32[] hue_vector
 float32[] sat_vector

people_tracking_v2/msg/SegmentedImages.msg

@@ -1,2 +1,3 @@
-std_msgs/Header header
+Header header
 sensor_msgs/Image[] images
+int32[] ids

people_tracking_v2/scripts/HoC.py

@@ -27,9 +27,16 @@ def segmented_images_callback(self, msg):
                 segmented_image = self.bridge.imgmsg_to_cv2(segmented_image_msg, "bgr8")
                 hoc_hue, hoc_sat = self.compute_hoc(segmented_image)
                 rospy.loginfo(f'Computed HoC for segmented image #{i + 1}')
-                self.publish_hoc_vectors(hoc_hue, hoc_sat)
+
+                # Extract the ID from the incoming message
+                detection_id = msg.ids[i]
+
+                # Publish the HoC vectors with the detection ID
+                self.publish_hoc_vectors(hoc_hue, hoc_sat, detection_id)
             except CvBridgeError as e:
-                rospy.logerr(f"Failed to convert segmented image: {e}")
+                rospy.logerr(f"Failed to convert segmented image: {e}") 
+            except IndexError as e:
+                rospy.logerr(f"IndexError: {e}. This might happen if there are more segmented images than detections.")
 
     def compute_hoc(self, segmented_image):
         # Convert to HSV
@@ -51,10 +58,11 @@ def compute_hoc(self, segmented_image):
         # Flatten the histograms
         return hist_hue.flatten(), hist_sat.flatten()
 
-    def publish_hoc_vectors(self, hue_vector, sat_vector):
+    def publish_hoc_vectors(self, hue_vector, sat_vector, detection_id):
         """Publish the computed HoC vectors."""
         hoc_msg = HoCVector()
         hoc_msg.header.stamp = rospy.Time.now()
+        hoc_msg.id = detection_id
         hoc_msg.hue_vector = hue_vector.tolist()
         hoc_msg.sat_vector = sat_vector.tolist()
         self.hoc_vector_pub.publish(hoc_msg)

people_tracking_v2/scripts/comparison_node.py

@@ -23,6 +23,11 @@ def __init__(self):
         self.pose_data_file = os.path.expanduser('~/pose_data/pose_data.npz')
         self.load_hoc_data()
         self.load_pose_data()
+
+        # Initialize storage for the latest incoming data
+        self.latest_hoc_vectors = {}
+        self.latest_pose_data = {}
+
 
         # Initialize storage for the latest incoming data
         self.latest_hoc_vectors = []

people_tracking_v2/scripts/pose_estimation_node.py

@@ -90,10 +90,12 @@ def _image_callback(self, image_msg):
         recognitions, result_image, pose_details = self._wrapper.detect_poses(self._bridge.imgmsg_to_cv2(image_msg, "bgr8"))
 
         # Calculate distances and publish them
-        for pose in pose_details:
+        for i, pose in enumerate(pose_details):
             try:
                 pose_distance_msg = BodySize()
                 pose_distance_msg.header.stamp = rospy.Time.now()
+                pose_distance_msg.id = i + 1  # Assigning the same sequential ID
+
                 if "LShoulder" in pose and "LHip" in pose:
                     pose_distance_msg.left_shoulder_hip_distance = self._wrapper.compute_distance(pose["LShoulder"], pose["LHip"])
                     rospy.loginfo(f"Left Shoulder-Hip Distance: {pose_distance_msg.left_shoulder_hip_distance:.2f}")
@@ -146,10 +148,11 @@ def _get_recognitions(self, image_msg, save_images, publish_images):
             self._result_image_publisher.publish(self._bridge.cv2_to_imgmsg(result_image, "bgr8"))
 
         # Calculate distances and log them
-        for pose in pose_details:
+        for i, pose in enumerate(pose_details):
             try:
                 pose_distance_msg = BodySize()
                 pose_distance_msg.header.stamp = rospy.Time.now()
+                pose_distance_msg.id = i + 1  # Assigning the same sequential ID
                 if "LShoulder" in pose and "LHip" in pose:
                     pose_distance_msg.left_shoulder_hip_distance = self._wrapper.compute_distance(pose["LShoulder"], pose["LHip"])
                     rospy.loginfo(f"Left Shoulder-Hip Distance: {pose_distance_msg.left_shoulder_hip_distance:.2f}")

people_tracking_v2/scripts/yolo_seg.py

@@ -47,6 +47,9 @@ def image_callback(self, data):
         rospy.loginfo(f"Total Detections: {len(labels)}")  # Log the total number of detections
 
         detection_array = DetectionArray()
+        segmented_images_msg = SegmentedImages()
+        segmented_images_msg.header.stamp = rospy.Time.now()
+        segmented_images_msg.ids = []  # Initialize the ids list
 
         # Create a copy of the image for bounding box visualization
         bounding_box_image = cv_image.copy()
@@ -61,6 +64,7 @@ def image_callback(self, data):
 
         for i, (box, score, label, mask) in enumerate(human_detections):
             detection = Detection()
+            detection.id = i + 1  # Assigning sequential ID starting from 1
             detection.x1 = float(box[0])
             detection.y1 = float(box[1])
             detection.x2 = float(box[2])
@@ -124,6 +128,7 @@ def image_callback(self, data):
 
                 # Publish individual segmented images
                 self.individual_segmented_image_pub.publish(segmented_image_msg)
+                segmented_images_msg.ids.append(i + 1)  # Add the ID to the ids list
 
         # Publish segmented images as a batch
         self.segmented_images_pub.publish(segmented_images_msg)