Trying to get video from HERO

people_tracking_v2/CMakeLists.txt

@@ -49,6 +49,7 @@ install(PROGRAMS
   scripts/decision_node.py
   scripts/central_node.py
   src/kalman_filter.py
+  tools/recorder.py
   tools/save_HoC_data.py
   tools/save_pose_data.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

people_tracking_v2/scripts/decision_node.py

@@ -26,8 +26,8 @@ def __init__(self):
     def sync_callback(self, comparison_msg, detection_msg):
         """Callback function to handle synchronized comparison scores and detection info."""
         # Define thresholds
-        iou_threshold = 0.9
-        hoc_threshold = 1.5
+        iou_threshold = 0.8
+        hoc_threshold = 0.05
         pose_threshold = 0.5  # Example threshold for pose distance
 
         iou_detections = []

people_tracking_v2/scripts/yolo_seg.py

@@ -44,7 +44,7 @@ def __init__(self):
         self.kalman_filter_operator = KalmanFilterCV()
         self.operator_id = None  # To be set by an external node
 
-        self.iou_threshold = 0.7  # Default threshold value
+        self.iou_threshold = 0.8  # Default threshold value
         self.matching_threshold = 50  # Threshold for matching the prediction with detections
 
         # Timer to reset operator ID

people_tracking_v2/tools/recorder.py

@@ -0,0 +1,81 @@
+#!/usr/bin/env python
+
+import rospy
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+import cv2
+
+class VideoRecorder:
+    def __init__(self):
+        rospy.init_node('video_recorder', anonymous=True)
+
+        # Subscribe to the RGB and depth image topics
+        self.rgb_image_sub = rospy.Subscriber('/hero/head_rgbd_sensor/rgb/image_raw', Image, self.rgb_image_callback)
+        self.depth_image_sub = rospy.Subscriber('/hero/head_rgbd_sensor/depth_registered/image_raw', Image, self.depth_image_callback)
+
+        # Create a CvBridge object
+        self.bridge = CvBridge()
+
+        # Initialize the video writers
+        self.rgb_out = None
+        self.depth_out = None
+        self.fourcc = cv2.VideoWriter_fourcc(*'XVID')
+        self.fps = 30  # You can adjust this based on your requirements
+
+    def rgb_image_callback(self, data):
+        try:
+            # Convert the ROS Image message to an OpenCV image
+            rgb_image = self.bridge.imgmsg_to_cv2(data, 'bgr8')
+
+            # Initialize the RGB video writer once we get the first image
+            if self.rgb_out is None:
+                self.rgb_frame_size = (rgb_image.shape[1], rgb_image.shape[0])
+                self.rgb_out = cv2.VideoWriter('rgb_output.avi', self.fourcc, self.fps, self.rgb_frame_size)
+
+            # Write the RGB image to the video file
+            self.rgb_out.write(rgb_image)
+
+            # Optionally, display the RGB image
+            cv2.imshow('RGB Camera Feed', rgb_image)
+            cv2.waitKey(1)
+        except CvBridgeError as e:
+            rospy.logerr(f'CvBridge Error: {e}')
+
+    def depth_image_callback(self, data):
+        try:
+            # Convert the ROS Image message to an OpenCV image
+            depth_image = self.bridge.imgmsg_to_cv2(data, '16UC1')
+
+            # Normalize depth image to 8-bit for visualization
+            depth_image_normalized = cv2.normalize(depth_image, None, 0, 255, cv2.NORM_MINMAX, cv2.CV_8UC1)
+            depth_image_colored = cv2.applyColorMap(depth_image_normalized, cv2.COLORMAP_JET)
+
+            # Initialize the depth video writer once we get the first image
+            if self.depth_out is None:
+                self.depth_frame_size = (depth_image_colored.shape[1], depth_image_colored.shape[0])
+                self.depth_out = cv2.VideoWriter('depth_output.avi', self.fourcc, self.fps, self.depth_frame_size)
+
+            # Write the depth image to the video file
+            self.depth_out.write(depth_image_colored)
+
+            # Optionally, display the depth image
+            cv2.imshow('Depth Camera Feed', depth_image_colored)
+            cv2.waitKey(1)
+        except CvBridgeError as e:
+            rospy.logerr(f'CvBridge Error: {e}')
+
+    def cleanup(self):
+        # Release the video writers and close OpenCV windows
+        if self.rgb_out is not None:
+            self.rgb_out.release()
+        if self.depth_out is not None:
+            self.depth_out.release()
+        cv2.destroyAllWindows()
+
+if __name__ == '__main__':
+    recorder = VideoRecorder()
+    try:
+        rospy.spin()
+    except KeyboardInterrupt:
+        recorder.cleanup()
+        print('Shutting down')