diff --git a/people_tracking_v2/msg/BodySize.msg b/people_tracking_v2/msg/BodySize.msg index 51d7da8..7b0abf2 100644 --- a/people_tracking_v2/msg/BodySize.msg +++ b/people_tracking_v2/msg/BodySize.msg @@ -1,3 +1,4 @@ Header header +int32 id float32 left_shoulder_hip_distance float32 right_shoulder_hip_distance diff --git a/people_tracking_v2/msg/Detection.msg b/people_tracking_v2/msg/Detection.msg index 12791ef..3960fd8 100644 --- a/people_tracking_v2/msg/Detection.msg +++ b/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 \ No newline at end of file +int32 label diff --git a/people_tracking_v2/msg/HoCVector.msg b/people_tracking_v2/msg/HoCVector.msg index c80df51..a498598 100644 --- a/people_tracking_v2/msg/HoCVector.msg +++ b/people_tracking_v2/msg/HoCVector.msg @@ -1,3 +1,5 @@ +# HoCVector.msg Header header +int32 id float32[] hue_vector float32[] sat_vector diff --git a/people_tracking_v2/msg/SegmentedImages.msg b/people_tracking_v2/msg/SegmentedImages.msg index f9024d3..b2ed0c7 100644 --- a/people_tracking_v2/msg/SegmentedImages.msg +++ b/people_tracking_v2/msg/SegmentedImages.msg @@ -1,2 +1,3 @@ -std_msgs/Header header +Header header sensor_msgs/Image[] images +int32[] ids \ No newline at end of file diff --git a/people_tracking_v2/scripts/HoC.py b/people_tracking_v2/scripts/HoC.py index 3398d08..2b3ef71 100755 --- a/people_tracking_v2/scripts/HoC.py +++ b/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) diff --git a/people_tracking_v2/scripts/comparison_node.py b/people_tracking_v2/scripts/comparison_node.py index 271f9c6..00b1e07 100755 --- a/people_tracking_v2/scripts/comparison_node.py +++ b/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 = [] diff --git a/people_tracking_v2/scripts/pose_estimation_node.py b/people_tracking_v2/scripts/pose_estimation_node.py index 715784b..703de08 100755 --- a/people_tracking_v2/scripts/pose_estimation_node.py +++ b/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}") diff --git a/people_tracking_v2/scripts/yolo_seg.py b/people_tracking_v2/scripts/yolo_seg.py index e023f58..27bdbea 100755 --- a/people_tracking_v2/scripts/yolo_seg.py +++ b/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)