Testing

people_tracking_v2/CMakeLists.txt

@@ -53,6 +53,8 @@ install(PROGRAMS
   tools/recorder.py
   tools/save_HoC_data.py
   tools/save_pose_data.py
+  testing/rgb_publisher.py
+  testing/depth_publisher.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

people_tracking_v2/scripts/decision_node.py

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

people_tracking_v2/testing/depth_publisher.py

@@ -0,0 +1,53 @@
+#!/usr/bin/env python
+import rospy
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+import cv2
+import os
+import numpy as np
+
+def publish_depth_images_from_folder(folder_path, topic_name, video_duration):
+    pub = rospy.Publisher(topic_name, Image, queue_size=10)
+    bridge = CvBridge()
+
+    # List all files in the folder and sort them
+    image_files = sorted([f for f in os.listdir(folder_path) if f.endswith(('.png', '.jpg', '.jpeg'))])
+    total_frames = len(image_files)
+
+    # Calculate frame rate
+    fps = total_frames / video_duration
+    rospy.loginfo(f"Calculated frame rate: {fps} fps")
+
+    rate = rospy.Rate(fps)
+
+    for image_file in image_files:
+        if rospy.is_shutdown():
+            break
+
+        image_path = os.path.join(folder_path, image_file)
+        frame = cv2.imread(image_path, cv2.IMREAD_UNCHANGED)  # Read as is (e.g., 16-bit depth)
+
+        if frame is None:
+            rospy.logerr(f"Error reading image file {image_path}")
+            continue
+
+        try:
+            if frame.dtype == np.uint16:
+                image_message = bridge.cv2_to_imgmsg(frame, "16UC1")
+            else:
+                rospy.logwarn(f"Unexpected image format for depth image: {image_file}")
+                continue
+            pub.publish(image_message)
+            rate.sleep()
+        except CvBridgeError as e:
+            rospy.logerr(e)
+
+if __name__ == '__main__':
+    rospy.init_node('depth_publisher_node', anonymous=True)
+    folder_path = '/home/miguel/Documents/BEP-Testing/TestCase1/Frames Tue Jun 25 Test case 1/depth'
+    topic_name = '/hero/head_rgbd_sensor/depth_registered/image_raw'
+    video_duration = 122  # Duration of the video in seconds, modify as needed
+    try:
+        publish_depth_images_from_folder(folder_path, topic_name, video_duration)
+    except rospy.ROSInterruptException:
+        pass

people_tracking_v2/testing/depth_publisher_vid.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+import rospy
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+import cv2
+
+def publish_video(video_path):
+    pub = rospy.Publisher("/hero/head_rgbd_sensor/depth_registered/image_raw", Image, queue_size=10)
+    bridge = CvBridge()
+    cap = cv2.VideoCapture(video_path)
+
+    if not cap.isOpened():
+        rospy.logerr(f"Error opening video file {video_path}")
+        return
+
+    rate = rospy.Rate(30.095)  # Modify based on the specific fps for depth video
+
+    while not rospy.is_shutdown() and cap.isOpened():
+        ret, frame = cap.read()
+        if ret:
+            try:
+                image_message = bridge.cv2_to_imgmsg(frame, "bgr8")
+                pub.publish(image_message)
+                rate.sleep()
+            except CvBridgeError as e:
+                rospy.logerr(e)
+        else:
+            break
+
+    cap.release()
+
+if __name__ == '__main__':
+    rospy.init_node('depth_video_publisher_node', anonymous=True)
+    video_path = '/home/miguel/Documents/BEP-Testing/TestCase1/TestCase1_depth.mp4'
+    try:
+        publish_video(video_path)
+    except rospy.ROSInterruptException:
+        pass

people_tracking_v2/testing/launchers/test_videos.launch

@@ -0,0 +1,62 @@
+<launch>
+
+  <arg name="laptop" default="False" />
+  <arg name="depth_camera" default="True" />
+  <arg name="save_data" default="False"/>
+
+<node
+    pkg="people_tracking_v2"
+    type="rgb_publisher.py"
+    name="RGB"
+    output="screen"
+    args="$(arg laptop) $(arg depth_camera) $(arg save_data)"
+  />
+
+  <node
+    pkg="people_tracking_v2"
+    type="depth_publisher.py"
+    name="depth"
+    output="screen"
+    args="$(arg laptop) $(arg depth_camera) $(arg save_data)"
+  />
+
+  <node
+    pkg="people_tracking_v2"
+    type="yolo_seg.py"
+    name="Image_seg"
+    output="screen"
+    args="$(arg laptop) $(arg depth_camera) $(arg save_data)"
+  />
+
+  <node
+    pkg="people_tracking_v2"
+    type="HoC.py"
+    name="HoC"
+    output="screen"
+    args="$(arg laptop) $(arg depth_camera) $(arg save_data)"
+  />
+
+  <node
+    pkg="people_tracking_v2"
+    type="pose_estimation_node.py"
+    name="Pose_estimation"
+    output="screen"
+    args="$(arg laptop) $(arg depth_camera) $(arg save_data)"
+  />
+
+  <node
+    pkg="people_tracking_v2"
+    type="comparison_node.py"
+    name="Comparison"
+    output="screen"
+    args="$(arg laptop) $(arg depth_camera) $(arg save_data)"
+  />
+
+    <node
+    pkg="people_tracking_v2"
+    type="decision_node.py"
+    name="Decision"
+    output="screen"
+    args="$(arg laptop) $(arg depth_camera) $(arg save_data)"
+  />
+</launch>

people_tracking_v2/testing/rgb_publisher.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+import rospy
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+import cv2
+
+def publish_video(video_path):
+    pub = rospy.Publisher("/hero/head_rgbd_sensor/rgb/image_raw", Image, queue_size=10)
+    bridge = CvBridge()
+    cap = cv2.VideoCapture(video_path)
+
+    if not cap.isOpened():
+        rospy.logerr(f"Error opening video file {video_path}")
+        return
+
+    rate = rospy.Rate(20)  # Modify based on your video's fps
+
+    while not rospy.is_shutdown() and cap.isOpened():
+        ret, frame = cap.read()
+        if ret:
+            try:
+                image_message = bridge.cv2_to_imgmsg(frame, "bgr8")
+                pub.publish(image_message)
+                rate.sleep()
+            except CvBridgeError as e:
+                rospy.logerr(e)
+        else:
+            break
+
+    cap.release()
+
+if __name__ == '__main__':
+    rospy.init_node('rgb_video_publisher_node', anonymous=True)
+    video_path = '/home/miguel/Documents/BEP-Testing/TestCase1/TestCase1_rgb.mp4'
+    try:
+        publish_video(video_path)
+    except rospy.ROSInterruptException:
+        pass

people_tracking_v2/tools/image2video.py

@@ -0,0 +1,61 @@
+import cv2
+import os
+import glob
+
+def create_video_from_images(image_folder, output_video_file, fps=30, target_size=None):
+    print(f"Processing images from: {image_folder}")
+
+    # Verify the directory exists and is readable
+    abs_image_folder = os.path.abspath(image_folder)
+    if not os.path.exists(abs_image_folder):
+        raise Exception(f"The specified image folder does not exist: {abs_image_folder}")
+    if not os.access(abs_image_folder, os.R_OK):
+        raise Exception(f"The script does not have permission to read the directory: {abs_image_folder}")
+
+    # Get all the image paths with correct extension and sorted
+    images = sorted(glob.glob(os.path.join(abs_image_folder, '*.png')))
+    if not images:
+        raise Exception("No images found. Check your folder path and image extensions.")
+
+    print(f"Found {len(images)} images in {image_folder}")
+
+    # Load the first image to get video properties if target_size is not specified
+    frame = cv2.imread(images[0])
+    if frame is None:
+        raise Exception("Could not read the first image. Ensure the image is accessible and not corrupted.")
+    if target_size is None:
+        target_size = (frame.shape[1], frame.shape[0])
+
+    # Define the codec and create VideoWriter object
+    fourcc = cv2.VideoWriter_fourcc(*'mp4v')
+    out = cv2.VideoWriter(output_video_file, fourcc, fps, target_size)
+
+    # Write each image (resized if necessary) as a frame in the video
+    for image in images:
+        frame = cv2.imread(image)
+        if frame is None:
+            print(f"Warning: Could not read image {image}. Skipping.")
+            continue
+        if target_size:
+            frame = cv2.resize(frame, target_size, interpolation=cv2.INTER_AREA)
+        out.write(frame)
+
+    out.release()  # Release everything when job is finished
+    print(f"Video created at {output_video_file}")
+
+def main():
+    base_folder = '/home/miguel/Documents/BEP-Testing/TestCase4/Frames Tue Jun 25 Test case 4'
+
+    # RGB Video Processing
+    rgb_folder = os.path.join(base_folder, 'rgb')
+    rgb_output = '/home/miguel/Documents/BEP-Testing/TestCase4/TestCase4_rgb.mp4'
+    create_video_from_images(rgb_folder, rgb_output, fps=20)
+
+    # Depth Video Processing
+    depth_folder = os.path.join(base_folder, 'depth')
+    depth_output = '/home/miguel/Documents/BEP-Testing/TestCase4/TestCase4_depth.mp4'
+    depth_fps = 20 * len(glob.glob(os.path.join(depth_folder, '*.png'))) / len(glob.glob(os.path.join(rgb_folder, '*.png')))
+    create_video_from_images(depth_folder, depth_output, fps=depth_fps, target_size=(1280, 720))
+
+if __name__ == '__main__':
+    main()

people_tracking_v2/tools/image2video_depth.py

@@ -0,0 +1,51 @@
+import cv2
+import os
+import glob
+
+def create_video_from_images(image_folder, output_video_file, fps=30, target_size=(1280, 720)):
+    # Print the absolute path to verify correctness
+    abs_image_folder = os.path.abspath(image_folder)
+    print(f"Absolute path to images: {abs_image_folder}")
+
+    # Verify the directory exists
+    if not os.path.exists(abs_image_folder):
+        raise Exception(f"The specified image folder does not exist: {abs_image_folder}")
+
+    # Check if directory is readable (permission check)
+    if not os.access(abs_image_folder, os.R_OK):
+        raise Exception(f"The script does not have permission to read the directory: {abs_image_folder}")
+
+    # Get all the image paths with correct extension and sorted
+    images = sorted(glob.glob(os.path.join(abs_image_folder, '*.png')))
+    print(f"Found images: {len(images)}")  # Print count of found images for debugging
+
+    # Proceed if images are found
+    if not images:
+        raise Exception("No images found. Check your folder path and image extensions.")
+
+    # Define the codec and create VideoWriter object
+    fourcc = cv2.VideoWriter_fourcc(*'mp4v')
+    out = cv2.VideoWriter(output_video_file, fourcc, fps, target_size)
+
+    # Write each resized image as a frame in the video
+    for image in images:
+        frame = cv2.imread(image)
+        if frame is None:
+            print(f"Warning: Could not read image {image}. Skipping.")
+            continue
+
+        # Resize the frame
+        resized_frame = cv2.resize(frame, target_size, interpolation=cv2.INTER_AREA)
+
+        # Write the resized frame to the video
+        out.write(resized_frame)
+
+    out.release()  # Release everything when job is finished
+    print("Video processing complete.")
+
+# Usage
+image_folder = '/home/miguel/Documents/BEP-Testing/TestCase2/Frames Tue Jun 25 Test case 2/depth'  # Corrected path
+output_video_file = '/home/miguel/Documents/BEP-Testing/TestCase2/TestCase2_depth.mp4'  # Desired output file
+fps = 32.095  # Frames per second of the output video
+
+create_video_from_images(image_folder, output_video_file, fps)

people_tracking_v2/tools/recorder.py

@@ -14,7 +14,7 @@ def save_image(cv_image, image_type, count):
         os.makedirs(base_dir)
 
     # Create the directory for today's date if it doesn't exist
-    date_str = datetime.now().strftime('%a %b %d 1')
+    date_str = datetime.now().strftime('%a %b %d Test case 4')
     output_dir = os.path.join(base_dir, f'Frames {date_str}')
     if not os.path.exists(output_dir):
         os.makedirs(output_dir)
@@ -59,8 +59,8 @@ def depth_callback(msg):
     depth_count = 0
 
     # Define the topics
-    rgb_topic = '/hero/head_rgbd_sensor/rgb/image_raw'  # Topic for RGB images
-    depth_topic = '/hero/head_rgbd_sensor/depth_registered/image_raw' # Topic for Depth images
+    rgb_topic = '/camera/color/image_raw'  # Topic for RGB images
+    depth_topic = '/camera/depth/image_rect_raw' # Topic for Depth images
 
     # Subscribe to the topics
     rospy.Subscriber(rgb_topic, Image, rgb_callback)