From 1610e020bb2d44d2dd82f61febd1a27219fc9b65 Mon Sep 17 00:00:00 2001 From: Miguelmelon Date: Fri, 28 Jun 2024 15:08:09 +0200 Subject: [PATCH] Testing --- people_tracking_v2/CMakeLists.txt | 2 + people_tracking_v2/scripts/decision_node.py | 2 +- people_tracking_v2/testing/depth_publisher.py | 53 ++++++++++++++++ .../testing/depth_publisher_vid.py | 38 ++++++++++++ .../testing/launchers/test_videos.launch | 62 +++++++++++++++++++ people_tracking_v2/testing/rgb_publisher.py | 38 ++++++++++++ people_tracking_v2/tools/image2video.py | 61 ++++++++++++++++++ people_tracking_v2/tools/image2video_depth.py | 51 +++++++++++++++ people_tracking_v2/tools/realsense.launch | 7 --- people_tracking_v2/tools/recorder.py | 6 +- 10 files changed, 309 insertions(+), 11 deletions(-) create mode 100755 people_tracking_v2/testing/depth_publisher.py create mode 100755 people_tracking_v2/testing/depth_publisher_vid.py create mode 100644 people_tracking_v2/testing/launchers/test_videos.launch create mode 100755 people_tracking_v2/testing/rgb_publisher.py create mode 100644 people_tracking_v2/tools/image2video.py create mode 100644 people_tracking_v2/tools/image2video_depth.py delete mode 100644 people_tracking_v2/tools/realsense.launch diff --git a/people_tracking_v2/CMakeLists.txt b/people_tracking_v2/CMakeLists.txt index 826b06f..8762266 100644 --- a/people_tracking_v2/CMakeLists.txt +++ b/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} ) diff --git a/people_tracking_v2/scripts/decision_node.py b/people_tracking_v2/scripts/decision_node.py index e2cf85f..b15bef7 100755 --- a/people_tracking_v2/scripts/decision_node.py +++ b/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 = [] diff --git a/people_tracking_v2/testing/depth_publisher.py b/people_tracking_v2/testing/depth_publisher.py new file mode 100755 index 0000000..fd9e4be --- /dev/null +++ b/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 diff --git a/people_tracking_v2/testing/depth_publisher_vid.py b/people_tracking_v2/testing/depth_publisher_vid.py new file mode 100755 index 0000000..1f9e27d --- /dev/null +++ b/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 diff --git a/people_tracking_v2/testing/launchers/test_videos.launch b/people_tracking_v2/testing/launchers/test_videos.launch new file mode 100644 index 0000000..0dedd8d --- /dev/null +++ b/people_tracking_v2/testing/launchers/test_videos.launch @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/people_tracking_v2/testing/rgb_publisher.py b/people_tracking_v2/testing/rgb_publisher.py new file mode 100755 index 0000000..b0c0259 --- /dev/null +++ b/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 diff --git a/people_tracking_v2/tools/image2video.py b/people_tracking_v2/tools/image2video.py new file mode 100644 index 0000000..5e55b17 --- /dev/null +++ b/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() diff --git a/people_tracking_v2/tools/image2video_depth.py b/people_tracking_v2/tools/image2video_depth.py new file mode 100644 index 0000000..9c5fbfe --- /dev/null +++ b/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) diff --git a/people_tracking_v2/tools/realsense.launch b/people_tracking_v2/tools/realsense.launch deleted file mode 100644 index 09be2ea..0000000 --- a/people_tracking_v2/tools/realsense.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/people_tracking_v2/tools/recorder.py b/people_tracking_v2/tools/recorder.py index 5820b07..67e7594 100755 --- a/people_tracking_v2/tools/recorder.py +++ b/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)