Include pose detection node in ROS

people_tracking_v2/CMakeLists.txt

@@ -7,6 +7,8 @@ find_package(catkin REQUIRED COMPONENTS
   rospy
   std_msgs
   sensor_msgs
+  image_recognition_msgs
+  cv_bridge
 )
 
 catkin_python_setup()
@@ -24,7 +26,7 @@ generate_messages(
 )
 
 catkin_package(
-  CATKIN_DEPENDS message_runtime roscpp rospy std_msgs sensor_msgs
+  CATKIN_DEPENDS message_runtime roscpp rospy std_msgs sensor_msgs image_recognition_msgs cv_bridge
 )
 
 include_directories(

people_tracking_v2/package.xml

@@ -29,8 +29,9 @@
   <build_depend>roscpp</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>visualization_msgs</build_depend>
-
   <build_depend>message_generation</build_depend>
+  <build_depend>image_recognition_msgs</build_depend>
+  <build_depend>cv_bridge</build_depend>
 
   <exec_depend>cv_bridge</exec_depend>
   <exec_depend>image_recognition_util</exec_depend>
@@ -42,6 +43,8 @@
   <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>visualization_msgs</exec_depend>
+  <exec_depend>image_recognition_msgs</exec_depend>
+  <exec_depend>cv_bridge</exec_depend>
 
   <test_depend>catkin_lint_cmake</test_depend>
 

people_tracking_v2/scripts/HoC.py

@@ -30,7 +30,7 @@ def detection_callback(self, msg):
             x1, y1, x2, y2 = detection.x1, detection.y1, detection.x2, detection.y2
             roi = self.current_image[int(y1):int(y2), int(x1):int(x2)]
             hoc = self.compute_hoc(roi)
-            rospy.loginfo(f'HoC for detection #{i + 1}: {hoc}')
+            #rospy.loginfo(f'HoC for detection #{i + 1}: {hoc}')
 
     def compute_hoc(self, roi):
         hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)

people_tracking_v2/scripts/memory_node.py

@@ -0,0 +1,73 @@
+#!/usr/bin/env python
+import rospy
+from people_tracking_v2.msg import Detection, DetectionArray
+from std_msgs.msg import String
+from collections import namedtuple
+
+# Named tuple for storing target information
+Target = namedtuple("Target", ["nr_batch", "time", "colour_vector"])
+
+class TargetMemoryNode:
+    def __init__(self):
+        # Initialize the ROS node
+        rospy.init_node('target_memory', anonymous=True)
+
+        # Subscriber to HoC detections
+        self.subscriber_hoc = rospy.Subscriber('/hero/HoC', DetectionArray, self.hoc_callback)
+
+        # Publisher for debug information or status updates
+        self.publisher_debug = rospy.Publisher('/hero/target_memory/debug', String, queue_size=10)
+
+        # Dictionary to store targets with their batch number as key
+        self.targets = {}
+
+        # Initialize with some example data
+        self.initialize_targets()
+
+        rospy.spin()
+
+    def initialize_targets(self):
+        """Initialize the target memory with some example data."""
+        initial_targets = [
+            Target(nr_batch=0, time=rospy.Time.now(), colour_vector=[0.1] * 96),
+            Target(nr_batch=1, time=rospy.Time.now(), colour_vector=[0.2] * 96),
+            Target(nr_batch=2, time=rospy.Time.now(), colour_vector=[0.3] * 96)
+        ]
+
+        for target in initial_targets:
+            self.targets[target.nr_batch] = target
+
+        rospy.loginfo("Initialized target memory with example data.")
+
+    def hoc_callback(self, msg):
+        """Callback function to handle new HoC detections."""
+        for detection in msg.detections:
+            self.update_target(detection)
+
+    def update_target(self, detection):
+        """Update or add a target based on the detection."""
+        batch_number = detection.nr_batch
+        colour_vector = detection.colour_vector
+
+        if batch_number in self.targets:
+            # Update existing target
+            existing_target = self.targets[batch_number]
+            updated_target = Target(nr_batch=batch_number, time=existing_target.time, colour_vector=colour_vector)
+            self.targets[batch_number] = updated_target
+            rospy.loginfo(f"Updated target batch {batch_number}")
+        else:
+            # Add new target
+            new_target = Target(nr_batch=batch_number, time=rospy.Time.now(), colour_vector=colour_vector)
+            self.targets[batch_number] = new_target
+            rospy.loginfo(f"Added new target batch {batch_number}")
+
+        # Publish debug information
+        debug_msg = String()
+        debug_msg.data = f"Total targets: {len(self.targets)}"
+        self.publisher_debug.publish(debug_msg)
+
+if __name__ == '__main__':
+    try:
+        TargetMemoryNode()
+    except rospy.ROSInterruptException:
+        pass

people_tracking_v2/scripts/pose_estimation_node.py

@@ -67,10 +67,10 @@ def __init__(
         # ROS IO
         self._bridge = CvBridge()
         self._recognize_srv = rospy.Service("recognize", Recognize, self._recognize_srv)
-        self._image_subscriber = rospy.Subscriber("image", Image, self._image_callback)
-        self._recognitions_publisher = rospy.Publisher("recognitions", Recognitions, queue_size=10)
+        self._image_subscriber = rospy.Subscriber("/Webcam/image_raw", Image, self._image_callback)
+        self._recognitions_publisher = rospy.Publisher("/pose_recognitions", Recognitions, queue_size=10)
         if self._topic_publish_result_image or self._service_publish_result_image:
-            self._result_image_publisher = rospy.Publisher("result_image", Image, queue_size=10)
+            self._result_image_publisher = rospy.Publisher("/pose_result_image", Image, queue_size=10)
 
         self.last_master_check = rospy.get_time()
 
@@ -96,7 +96,7 @@ def _recognize_srv(self, req):
 
     def _get_recognitions(self, image_msg, save_images, publish_images):
         """
-        Handles the recognition and publishes and stores the debug images (should be called in the main thread)
+         Handles the recognition and publishes and stores the debug images (should be called in the main thread)
 
         :param image_msg: Incoming image
         :param save_images: Whether to store the images
@@ -111,6 +111,12 @@ def _get_recognitions(self, image_msg, save_images, publish_images):
 
         recognitions, result_image = self._wrapper.detect_poses(bgr_image)
 
+        # Log the number of poses detected
+        if recognitions:
+            rospy.loginfo(f"Detected {len(recognitions)} poses")
+        else:
+            rospy.loginfo("No poses detected")
+
         # Write images
         if save_images:
             image_writer.write_raw(self._save_images_folder, bgr_image)

people_tracking_v2/scripts/yolo.py

@@ -63,8 +63,8 @@ def image_callback(self, data):
         self.detection_pub.publish(detection_array)
 
         # Display the frame (optional)
-        cv2.imshow("YOLOv8", cv_image)
-        cv2.waitKey(3)
+        #cv2.imshow("YOLOv8", cv_image)
+        #cv2.waitKey(3)
 
 def main():
     rospy.init_node('yolo_node', anonymous=True)

people_tracking_v2/src/people_tracking/launchers/yolo_launcher.launch

@@ -32,6 +32,13 @@
     output="screen"
   />
 
+  <node
+    pkg="people_tracking_v2"
+    type="pose_estimation_node.py"
+    name="Pose_estimation"
+    output="screen"
+  />
+
   <!-- Uncomment these nodes when ready -->
   <!--
   <node
@@ -41,11 +48,5 @@
     output="screen"
   />
   
-  <node
-    pkg="people_tracking_v2"
-    type="pose_estimation_node.py"
-    name="Pose_estimation"
-    output="screen"
-  />
   -->
 </launch>