diff --git a/ros/angel_utils/scripts/convert_video_to_ros_bag.py b/ros/angel_utils/scripts/convert_video_to_ros_bag.py new file mode 100644 index 000000000..a1db51d68 --- /dev/null +++ b/ros/angel_utils/scripts/convert_video_to_ros_bag.py @@ -0,0 +1,49 @@ +import os +import time +import sys +import cv2 +import datetime +#import roslib +import rospy +#roslib.load_manifest('sensor_msgs') + +from ros import rosbag +from sensor_msgs.msg import Image +from cv_bridge import CvBridge + + +video_fn = "/data/PTG/medical/bbn_data/M2_Lab_data/skills_by_frame/tq_10/20230412_123220_HoloLens.mp4" +bag_fn = "rosbag2_tq_10.db3" +def convert_video_to_bag(video_fn, bag_fn, topic="PVFramesBGR"): + bag = rosbag.Bag(bag_fn, 'w') + cb = CvBridge() + + # Read video + cam = cv2.VideoCapture(video_fn) + + prop_fps = cam.get(cv2.CAP_PROP_FPS) + if prop_fps != prop_fps or prop_fps <= 1e-2: + print("Warning: can't get FPS. Assuming 24") + prop_fps = 24 + + ret = True + frame_id = 0 + start_ts = rospy.Time.now() + while(True): + ret, frame = cam.read() + if not ret: + break + + #stamp = rospy.rostime.Time.from_sec(float(frame_id) / prop_fps) + frame_ms = cam.get(cv2.CAP_PROP_POS_MSEC) + frame_ts = start_ts + rospy.Duration(seconds=frame_ms/1000) + print(frame_ts) + frame_id += 1 + + image = cb.cv2_to_compressed_imgmsg(frame) + image.header.stamp = stamp + image.header.frame_id = topic + bag.write(topic, image, stamp) + + cap.release() + bag.close()