diff --git a/people_tracking_v2/scripts/HoC.py b/people_tracking_v2/scripts/HoC.py index 2b3ef71..4af3e66 100755 --- a/people_tracking_v2/scripts/HoC.py +++ b/people_tracking_v2/scripts/HoC.py @@ -30,6 +30,7 @@ def segmented_images_callback(self, msg): # Extract the ID from the incoming message detection_id = msg.ids[i] + rospy.loginfo(f"Received Detection ID: {detection_id} for segmented image #{i + 1}") # Publish the HoC vectors with the detection ID self.publish_hoc_vectors(hoc_hue, hoc_sat, detection_id) diff --git a/people_tracking_v2/scripts/pose_estimation_node.py b/people_tracking_v2/scripts/pose_estimation_node.py index 703de08..c23d2b5 100755 --- a/people_tracking_v2/scripts/pose_estimation_node.py +++ b/people_tracking_v2/scripts/pose_estimation_node.py @@ -70,8 +70,8 @@ def __init__( 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", 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) + #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) self.last_master_check = rospy.get_time() @@ -95,6 +95,7 @@ def _image_callback(self, image_msg): pose_distance_msg = BodySize() pose_distance_msg.header.stamp = rospy.Time.now() pose_distance_msg.id = i + 1 # Assigning the same sequential ID + rospy.loginfo(f"Processing Pose with ID: {pose_distance_msg.id}") 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/scripts/yolo_seg.py b/people_tracking_v2/scripts/yolo_seg.py index 27bdbea..81c9a9d 100755 --- a/people_tracking_v2/scripts/yolo_seg.py +++ b/people_tracking_v2/scripts/yolo_seg.py @@ -20,6 +20,7 @@ def __init__(self): self.model = YOLO("yolov8n-seg.pt") # Ensure the model supports segmentation self.image_sub = rospy.Subscriber("/Webcam/image_raw", Image, self.image_callback) + #self.depth_sub = rospy.Subscriber("/depth_registered/image", Image, self.image_callback) self.segmented_images_pub = rospy.Publisher("/segmented_images", SegmentedImages, queue_size=10) self.individual_segmented_image_pub = rospy.Publisher("/individual_segmented_images", Image, queue_size=10) self.bounding_box_image_pub = rospy.Publisher("/bounding_box_image", Image, queue_size=10) @@ -65,6 +66,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 + rospy.loginfo(f"Detection ID: {id} for box: {box}") detection.x1 = float(box[0]) detection.y1 = float(box[1]) detection.x2 = float(box[2]) @@ -131,6 +133,7 @@ def image_callback(self, data): segmented_images_msg.ids.append(i + 1) # Add the ID to the ids list # Publish segmented images as a batch + rospy.loginfo(f"Publishing Segmented Images with IDs: {segmented_images_msg.ids}") self.segmented_images_pub.publish(segmented_images_msg) # Publish bounding box image