|
| 1 | +#!/usr/bin/env python3 |
| 2 | +import cv2 |
| 3 | +from cv_bridge import CvBridge |
| 4 | +import pathlib |
| 5 | +import rclpy |
| 6 | +from rclpy.node import Node |
| 7 | +from sensor_msgs.msg import Image |
| 8 | +import threading |
| 9 | + |
| 10 | + |
| 11 | +class DummyRealSense(Node): |
| 12 | + """ |
| 13 | + Reads in a video file and publishes the frames as ROS2 messages. |
| 14 | + """ |
| 15 | + |
| 16 | + def __init__(self, video_path, fps=30, topic="/camera/color/image_raw"): |
| 17 | + super().__init__("dummy_real_sense") |
| 18 | + |
| 19 | + # Read in the video file |
| 20 | + self.video = cv2.VideoCapture(video_path) |
| 21 | + self.fps = fps |
| 22 | + |
| 23 | + # Create the publisher |
| 24 | + self.topic = topic |
| 25 | + self.publisher_ = self.create_publisher(Image, topic, 1) |
| 26 | + self.num_frames = 0 |
| 27 | + self.bridge = CvBridge() |
| 28 | + |
| 29 | + # Launch the publisher in a separate thread |
| 30 | + self.thread = threading.Thread(target=self.publish_frames, daemon=True) |
| 31 | + self.thread.start() |
| 32 | + |
| 33 | + def publish_frames(self): |
| 34 | + # Maintain the rate |
| 35 | + rate = self.create_rate(self.fps) |
| 36 | + |
| 37 | + while True: |
| 38 | + # Capture frame-by-frame |
| 39 | + ret, frame = self.video.read() |
| 40 | + self.num_frames += 1 |
| 41 | + # If the last frame is reached, reset the capture and the frame_counter |
| 42 | + if self.num_frames == self.video.get(cv2.CAP_PROP_FRAME_COUNT): |
| 43 | + self.num_frames = 0 |
| 44 | + self.video.set(cv2.CAP_PROP_POS_FRAMES, self.num_frames) |
| 45 | + # Convert to a ROS2 msg |
| 46 | + frame_msg = self.bridge.cv2_to_imgmsg(frame, "bgr8") |
| 47 | + frame_msg.header.stamp = self.get_clock().now().to_msg() |
| 48 | + self.publisher_.publish(frame_msg) |
| 49 | + # # Our operations on the frame come here |
| 50 | + # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) |
| 51 | + # # Display the resulting frame |
| 52 | + # cv2.imshow('frame',gray) |
| 53 | + # if cv2.waitKey(1) & 0xFF == ord('q'): |
| 54 | + # break |
| 55 | + rate.sleep() |
| 56 | + |
| 57 | + |
| 58 | +def main(args=None): |
| 59 | + rclpy.init(args=args) |
| 60 | + |
| 61 | + video_name = "2022_11_01_ada_picks_up_carrots_camera_compressed_ft_tf.mp4" |
| 62 | + video_path = str( |
| 63 | + ( |
| 64 | + pathlib.Path(__file__).parent.parent.parent.parent.parent |
| 65 | + / "share/data" |
| 66 | + / video_name |
| 67 | + ).resolve() |
| 68 | + ) |
| 69 | + |
| 70 | + dummy_real_sense = DummyRealSense(video_path) |
| 71 | + |
| 72 | + rclpy.spin(dummy_real_sense) |
| 73 | + |
| 74 | + # Destroy the node explicitly |
| 75 | + # (optional - otherwise it will be done automatically |
| 76 | + # when the garbage collector destroys the node object) |
| 77 | + dummy_real_sense.destroy_node() |
| 78 | + rclpy.shutdown() |
| 79 | + |
| 80 | + |
| 81 | +if __name__ == "__main__": |
| 82 | + main() |
0 commit comments