diff --git a/camera_data/README.md b/camera_data/README.md new file mode 100644 index 00000000..58a0cc07 --- /dev/null +++ b/camera_data/README.md @@ -0,0 +1,19 @@ +# Camera Data Module + +This module read and writes data from available camera feeds in the `/camera_frames` topic. + +### Messages + +- `v_angle`: Vertical orientation tilt of the camera enclosed in [-90, 90] degrees (The pitch axis of rotation). +- `h_angle`: Horizontal orientation tilt of the camera enclosed in [-90, 90] degrees (The Yaw axis of rotation). + +### Scripts + +- `camera_feeds.py`: Collects available camera ports and outputs them all in distinct windows. +- `camera_subscriber.py`: Subscriber to the `/camera_frames` topic that displays the images to screen. +- `camera_publisher.py`: Publishes frames from the selected camera (from the `camera_selection` topic) to the + `/camera_frames` topic at a rate of __30fps__. + +### Tests + +- `test_get_cameras.py`: Tests the camera feed collection from the `camera_feeds.py` script. diff --git a/camera_data/src/camera_feeds.py b/camera_data/src/camera_feeds.py new file mode 100644 index 00000000..30dd6f9c --- /dev/null +++ b/camera_data/src/camera_feeds.py @@ -0,0 +1,40 @@ +import cv2 + + +class CameraHandler: + available_cameras = [] + + def __init__(self): + # Collect all found cameras + # Assumes sequential indices + for i in range (25): + newCap = cv2.VideoCapture(i, cv2.CAP_V4L2) + + if newCap is not None and newCap.open(i): + self.available_cameras.append(newCap) + + def get_all_feeds(self): + ret_frame = [] + for camera in self.available_cameras: + newFeed = camera.read() + ret_frame.append((newFeed[0], newFeed[1])) + + return ret_frame + + def run_feeds(self): + print("Press 'q' to exit") + while True: + for index, (ret, frame) in enumerate(self.get_all_feeds()): + if ret: + cv2.imshow(f"frame {index}", frame) + + if cv2.waitKey(1) & 0xFF == ord('q'): + for vid in self.available_cameras: + vid.release() + cv2.destroyAllWindows() + break + +# Runnable to display camera feeds +if __name__ == '__main__': + camera_handler = CameraHandler() + camera_handler.run_feeds() diff --git a/camera_data/src/camera_publisher.py b/camera_data/src/camera_publisher.py index 26de6420..ed3eec1b 100755 --- a/camera_data/src/camera_publisher.py +++ b/camera_data/src/camera_publisher.py @@ -1,16 +1,13 @@ #!/usr/bin/env python3 import os, sys -from pydoc_data.topics import topics currentdir = os.path.dirname(os.path.realpath(__file__)) sys.path.append(currentdir) import rospy import cv2 -import time from cv_bridge import CvBridge from sensor_msgs.msg import Image from std_msgs.msg import Int16 -from PyQt5 import QtGui import sys stop = False @@ -24,23 +21,20 @@ def __init__(self): rospy.init_node('camera_frame_publisher') self.camera_select_subscriber = rospy.Subscriber("camera_selection", Int16, self.select_camera) - self.timer = rospy.Timer(rospy.Duration(0.03), self.timer_callback) + _30fps_ns = 30000000 + self.timer = rospy.Timer(rospy.Duration(0, _30fps_ns), self.timer_callback) self.camera_frame_publisher = rospy.Publisher('/camera_frames', Image, queue_size=10) - def timer_callback(self, event): - try: - ret, frame = self.video_capture.read() - # these code cause error - encode_params = [int(cv2.IMWRITE_JPEG_QUALITY), 90] - result, frame = cv2.imencode(".jpg", frame, encode_params) + def timer_callback(self, _): + ret, frame = self.video_capture.read() + if ret: + encode_params = [cv2.IMWRITE_JPEG_QUALITY, 90] + _, frame = cv2.imencode(".jpg", frame, encode_params) self.frames = self.openCV_to_ros_image(frame) - if ret: - print("Publishing frame") - self.camera_frame_publisher.publish(self.frames) - except: + print("Publishing frame") + self.camera_frame_publisher.publish(self.frames) + else: print("No camera found") - finally: - pass def openCV_to_ros_image(self, cv_image): try: diff --git a/camera_data/src/camera_subscriber.py b/camera_data/src/camera_subscriber.py index 14b2e918..e068c544 100644 --- a/camera_data/src/camera_subscriber.py +++ b/camera_data/src/camera_subscriber.py @@ -1,10 +1,8 @@ import os, sys -from pydoc_data.topics import topics currentdir = os.path.dirname(os.path.realpath(__file__)) sys.path.append(currentdir) + import rospy -currentdir = os.path.dirname(os.path.realpath(__file__)) -sys.path.append(currentdir) import cv2 from sensor_msgs.msg import Image from cv_bridge import CvBridge @@ -34,4 +32,4 @@ def ros_to_openCV_image(self, ros_image): if __name__ == "__main__": driver = Node_CameraFrameSub() - rospy.spin() \ No newline at end of file + rospy.spin() diff --git a/camera_data/src/getCameraFeeds.py b/camera_data/src/getCameraFeeds.py deleted file mode 100644 index 43bafd84..00000000 --- a/camera_data/src/getCameraFeeds.py +++ /dev/null @@ -1,45 +0,0 @@ -import cv2 - - -class CameraHandler: - vids = [] # available cameras - - def __init__(self): - # Loop 25 times and collect all found cameras - # Assumes sequential indices - for i in range (25): - newCap = cv2.VideoCapture(i, cv2.CAP_V4L2) - - if newCap is not None: - newCap.open(i) - - # check if object is not null - if newCap is not None and newCap.isOpened(): - # add the camera found - self.vids.append(newCap) - - def get_all_feeds(self): - retAndFrame = [] - # read each capture object in vids and add as a tuple to retAndFrame - for i in range(len(self.vids)): - newFeed = self.vids[i].read() - retAndFrame.append((newFeed[0], newFeed[1], i)) - - return retAndFrame - -# # Runnable for displaying camera feeds -if __name__ == '__main__': - camHandler = CameraHandler() - while True: - retAndFrame = camHandler.get_all_feeds() - # display each frame (camera feed) in the list - for ret, frame, index in retAndFrame: - if ret: - cv2.imshow(f"frame {index}", frame) - - # press 'q' to stop displaying - if cv2.waitKey(1) & 0xFF == ord('q'): - for vid in camHandler.vids: - vid.release() - cv2.destroyAllWindows() - break diff --git a/camera_data/test/test_getCam.py b/camera_data/test/test_getCam.py deleted file mode 100644 index 8e268970..00000000 --- a/camera_data/test/test_getCam.py +++ /dev/null @@ -1,26 +0,0 @@ -import cv2 -import sys -import os - -# gets location of getCameraFeeds to be recognized as a module -dir = os.path.dirname(__file__) -filename = os.path.join(dir, '../src') -sys.path.insert(0,filename) - -import getCameraFeeds as gcf - -# Runnable for displaying camera feeds -camHandler = gcf.CameraHandler() -while True: - retAndFrame = camHandler.get_all_feeds() - # display each frame (camera feed) in the list - for ret, frame, index in retAndFrame: - cv2.imshow(f"frame {index}", frame) - - # press 'q' to stop displaying - if cv2.waitKey(1) & 0xFF == ord('q'): - for vid in camHandler.vids: - vid.release() - cv2.destroyAllWindows() - break - diff --git a/camera_data/test/test_get_cameras.py b/camera_data/test/test_get_cameras.py new file mode 100644 index 00000000..79323dff --- /dev/null +++ b/camera_data/test/test_get_cameras.py @@ -0,0 +1,13 @@ +import sys +import os + +# gets location of getCameraFeeds to be recognized as a module +dir = os.path.dirname(__file__) +filename = os.path.join(dir, '../src') +sys.path.insert(0,filename) + +import getCameraFeeds as gcf + +# Runnable to display camera feeds +camera_handler = gcf.CameraHandler() +camera_handler.run_feeds()