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)