From 4d362e98b97a4c548ffe0f004fa86e795f66eb15 Mon Sep 17 00:00:00 2001 From: Cameron Date: Sun, 26 Jul 2026 17:10:15 -0700 Subject: [PATCH 1/2] Initial mvp for a subsection of image in python. --- .../rover_vision/camera_enhancer.py | 82 +++++++++++++++++++ src/rover_vision/setup.py | 1 + 2 files changed, 83 insertions(+) create mode 100644 src/rover_vision/rover_vision/camera_enhancer.py diff --git a/src/rover_vision/rover_vision/camera_enhancer.py b/src/rover_vision/rover_vision/camera_enhancer.py new file mode 100644 index 00000000..e4758200 --- /dev/null +++ b/src/rover_vision/rover_vision/camera_enhancer.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 + +import cv2 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from cv_bridge import CvBridge + +# Default frame dimensions this node is tuned for (standard 1080p feed) +FRAME_WIDTH = 480 +FRAME_HEIGHT = 640 + +# ROI size for the bottom-right corner box +ROI_SIZE = 240 + +# Digital zoom factor applied to the extracted ROI +ZOOM_FACTOR = 2 + + +class CameraEnhancerNode(Node): + def __init__(self): + super().__init__('camera_enhancer') + + self.bridge = CvBridge() + + self.declare_parameter('image_topic', '/camera/color/image_raw') + self.declare_parameter('enhanced_topic', '/camera/roi_enhanced') + + image_topic = self.get_parameter('image_topic').value + enhanced_topic = self.get_parameter('enhanced_topic').value + self.enhanced_pub = self.create_publisher(Image, enhanced_topic, 10) + + self.image_sub = self.create_subscription( + Image, image_topic, self.image_callback, 10) + + self.get_logger().info( + f'Camera enhancer subscribed to {image_topic}, publishing to {enhanced_topic}') + + def image_callback(self, msg): + try: + # Zero-copy conversion: cv_bridge hands back a view into msg.data + frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') + except Exception as e: + self.get_logger().error(f'Failed to convert image: {e}') + return + + height, width = frame.shape[:2] + + # Bottom-right corner box, clamped to the actual frame size + x2, y2 = width, height + x1 = max(0, x2 - ROI_SIZE) + y1 = max(0, y2 - ROI_SIZE) + + # NumPy slicing yields a view, not a copy - no full-frame duplication + roi = frame[y1:y2, x1:x2] + enhanced_roi = cv2.resize( + roi, None, fx=ZOOM_FACTOR, fy=ZOOM_FACTOR, interpolation=cv2.INTER_LINEAR) + + try: + enhanced_msg = self.bridge.cv2_to_imgmsg(enhanced_roi, encoding='bgr8') + except Exception as e: + self.get_logger().error(f'Failed to convert enhanced ROI: {e}') + return + + enhanced_msg.header = msg.header + self.enhanced_pub.publish(enhanced_msg) + + +def main(args=None): + rclpy.init(args=args) + node = CameraEnhancerNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/rover_vision/setup.py b/src/rover_vision/setup.py index 261dd6a5..c21d0278 100644 --- a/src/rover_vision/setup.py +++ b/src/rover_vision/setup.py @@ -34,6 +34,7 @@ 'waypoint_node = rover_vision.waypoint_node:main', 'morse_camera_pub_node = rover_vision.morse_camera_pub_node:main', 'morse_decoder_node = rover_vision.morse_decoder_node:main', + 'camera_enhancer = rover_vision.camera_enhancer:main', ], }, ) From f0145b8fa254dbc0e939b18a1f60e32fae40aa66 Mon Sep 17 00:00:00 2001 From: Pranav Rajesh Krishnan Date: Wed, 5 Aug 2026 18:28:03 -0400 Subject: [PATCH 2/2] Add ROI selector for camera zoom --- .gitignore | 1 + .../rover_vision/camera_enhancer.py | 103 ++++++++-- src/rover_vision/rover_vision/roi_selector.py | 185 ++++++++++++++++++ src/rover_vision/setup.py | 1 + 4 files changed, 271 insertions(+), 19 deletions(-) create mode 100644 src/rover_vision/rover_vision/roi_selector.py diff --git a/.gitignore b/.gitignore index e3d507a9..94576040 100644 --- a/.gitignore +++ b/.gitignore @@ -72,3 +72,4 @@ log/ .vscode/settings.json .venv +test_files/ diff --git a/src/rover_vision/rover_vision/camera_enhancer.py b/src/rover_vision/rover_vision/camera_enhancer.py index e4758200..de10fd51 100644 --- a/src/rover_vision/rover_vision/camera_enhancer.py +++ b/src/rover_vision/rover_vision/camera_enhancer.py @@ -3,17 +3,12 @@ import cv2 import rclpy from rclpy.node import Node -from sensor_msgs.msg import Image +from rclpy.qos import QoSProfile, DurabilityPolicy +from sensor_msgs.msg import Image, RegionOfInterest from cv_bridge import CvBridge -# Default frame dimensions this node is tuned for (standard 1080p feed) -FRAME_WIDTH = 480 -FRAME_HEIGHT = 640 - -# ROI size for the bottom-right corner box +# Fallback crop used before a dynamic ROI is selected. ROI_SIZE = 240 - -# Digital zoom factor applied to the extracted ROI ZOOM_FACTOR = 2 @@ -23,18 +18,53 @@ def __init__(self): self.bridge = CvBridge() + self.roi = None + self.last_valid_crop = None + self.declare_parameter('image_topic', '/camera/color/image_raw') self.declare_parameter('enhanced_topic', '/camera/roi_enhanced') + self.declare_parameter('roi_topic', '/camera/roi_select') + self.declare_parameter('output_width', 640) + self.declare_parameter('output_height', 480) image_topic = self.get_parameter('image_topic').value enhanced_topic = self.get_parameter('enhanced_topic').value + roi_topic = self.get_parameter('roi_topic').value + self.output_width = self.get_parameter('output_width').value + self.output_height = self.get_parameter('output_height').value + self.enhanced_pub = self.create_publisher(Image, enhanced_topic, 10) self.image_sub = self.create_subscription( Image, image_topic, self.image_callback, 10) + # Must match roi_selector.py so late-starting nodes receive the last ROI. + roi_qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + self.roi_sub = self.create_subscription( + RegionOfInterest, roi_topic, self.roi_callback, roi_qos) + self.get_logger().info( - f'Camera enhancer subscribed to {image_topic}, publishing to {enhanced_topic}') + f'Camera enhancer subscribed to {image_topic} (image) and ' + f'{roi_topic} (ROI), publishing to {enhanced_topic}') + + def roi_callback(self, msg): + # All-zero ROI is the reset signal shared with roi_selector.py. + if (msg.x_offset == 0 and msg.y_offset == 0 + and msg.width == 0 and msg.height == 0): + self.roi = None + self.last_valid_crop = None + self.get_logger().info('ROI reset - reverting to default corner crop') + return + + # Keep the previous ROI rather than passing an empty crop to cv2.resize. + if msg.width <= 0 or msg.height <= 0: + self.get_logger().warn( + f'Ignoring invalid ROI (width={msg.width}, height={msg.height})') + return + self.roi = msg def image_callback(self, msg): try: @@ -46,15 +76,49 @@ def image_callback(self, msg): height, width = frame.shape[:2] - # Bottom-right corner box, clamped to the actual frame size - x2, y2 = width, height - x1 = max(0, x2 - ROI_SIZE) - y1 = max(0, y2 - ROI_SIZE) - - # NumPy slicing yields a view, not a copy - no full-frame duplication - roi = frame[y1:y2, x1:x2] - enhanced_roi = cv2.resize( - roi, None, fx=ZOOM_FACTOR, fy=ZOOM_FACTOR, interpolation=cv2.INTER_LINEAR) + if self.roi is None: + x2, y2 = width, height + x1 = max(0, x2 - ROI_SIZE) + y1 = max(0, y2 - ROI_SIZE) + + roi = frame[y1:y2, x1:x2] + enhanced_roi = cv2.resize( + roi, None, fx=ZOOM_FACTOR, fy=ZOOM_FACTOR, interpolation=cv2.INTER_LINEAR) + else: + # Clamp against the current frame size, which can change at runtime. + x1 = min(max(0, self.roi.x_offset), width) + y1 = min(max(0, self.roi.y_offset), height) + x2 = min(x1 + self.roi.width, width) + y2 = min(y1 + self.roi.height, height) + + if x2 <= x1 or y2 <= y1: + if self.last_valid_crop is not None: + px1, py1, px2, py2 = self.last_valid_crop + px1, py1 = min(px1, width), min(py1, height) + px2, py2 = min(px2, width), min(py2, height) + if px2 > px1 and py2 > py1: + x1, y1, x2, y2 = px1, py1, px2, py2 + else: + x1, y1, x2, y2 = None, None, None, None + else: + x1, y1, x2, y2 = None, None, None, None + + if x1 is None: + self.get_logger().warn( + 'ROI fully outside current frame bounds and no valid ' + 'previous crop to retain; using fallback corner crop') + x2f, y2f = width, height + x1f = max(0, x2f - ROI_SIZE) + y1f = max(0, y2f - ROI_SIZE) + roi = frame[y1f:y2f, x1f:x2f] + else: + roi = frame[y1:y2, x1:x2] + else: + roi = frame[y1:y2, x1:x2] + self.last_valid_crop = (x1, y1, x2, y2) + + enhanced_roi = cv2.resize( + roi, (self.output_width, self.output_height), interpolation=cv2.INTER_LINEAR) try: enhanced_msg = self.bridge.cv2_to_imgmsg(enhanced_roi, encoding='bgr8') @@ -75,7 +139,8 @@ def main(args=None): pass finally: node.destroy_node() - rclpy.shutdown() + if rclpy.ok(): + rclpy.shutdown() if __name__ == '__main__': diff --git a/src/rover_vision/rover_vision/roi_selector.py b/src/rover_vision/rover_vision/roi_selector.py new file mode 100644 index 00000000..e54fbc78 --- /dev/null +++ b/src/rover_vision/rover_vision/roi_selector.py @@ -0,0 +1,185 @@ +#!/usr/bin/env python3 +""" +roi_selector.py + +Prototype OpenCV node for selecting a camera ROI with the mouse. + +The selected box is published as sensor_msgs/RegionOfInterest on roi_topic. +Pressing 'r' publishes an all-zero ROI, which camera_enhancer.py treats as a +reset signal. +""" + +import cv2 +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, DurabilityPolicy +from sensor_msgs.msg import Image, RegionOfInterest +from cv_bridge import CvBridge + +MIN_BOX_SIZE = 10 +COLOR_IN_PROGRESS = (0, 255, 255) +COLOR_COMMITTED = (0, 255, 0) + + +class RoiSelectorNode(Node): + def __init__(self): + super().__init__('roi_selector') + + self.bridge = CvBridge() + + self.declare_parameter('image_topic', '/camera/color/image_raw') + self.declare_parameter('roi_topic', '/camera/roi_select') + self.declare_parameter('window_name', 'ROI Selector') + + self.image_topic = self.get_parameter('image_topic').value + self.roi_topic = self.get_parameter('roi_topic').value + self.window_name = self.get_parameter('window_name').value + + # Must match camera_enhancer.py or the ROI topic will not connect. + roi_qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + self.roi_pub = self.create_publisher(RegionOfInterest, self.roi_topic, roi_qos) + + self.dragging = False + self.anchor = None + self.current = None + self.committed_box = None + + self.gui_ready = self._setup_gui() + if not self.gui_ready: + return + + self.image_sub = self.create_subscription( + Image, self.image_topic, self.image_callback, 10) + + self.get_logger().info( + f'ROI selector subscribed to {self.image_topic}, publishing ROIs to {self.roi_topic}. ' + f"Drag with the left mouse button to select a box, 'r' to reset, 'q'/ESC to quit.") + + def _setup_gui(self): + try: + cv2.namedWindow(self.window_name, cv2.WINDOW_AUTOSIZE) + cv2.setMouseCallback(self.window_name, self._on_mouse) + except cv2.error as e: + self.get_logger().error( + 'Failed to create an OpenCV display window. This node needs a ' + 'reachable display/X server (e.g. run natively on a Linux ' + 'desktop, or forward X11 to a host X server such as VcXsrv ' + 'when running in a container) and an OpenCV build with GUI ' + f'support (not opencv-python-headless). Underlying error: {e}') + return False + except Exception as e: + self.get_logger().error( + f'Unexpected error setting up the ROI selector GUI: {e}') + return False + return True + + def _on_mouse(self, event, x, y, flags, param): + # Window pixels map to image pixels because the frame is shown 1:1. + if event == cv2.EVENT_LBUTTONDOWN: + self.dragging = True + self.anchor = (x, y) + self.current = (x, y) + elif event == cv2.EVENT_MOUSEMOVE: + if self.dragging: + self.current = (x, y) + elif event == cv2.EVENT_LBUTTONUP: + if self.dragging: + self.dragging = False + self.current = (x, y) + self._commit_drag() + + def _commit_drag(self): + if self.anchor is None or self.current is None: + return + + ax, ay = self.anchor + cx, cy = self.current + + x1, x2 = min(ax, cx), max(ax, cx) + y1, y2 = min(ay, cy), max(ay, cy) + + if (x2 - x1) < MIN_BOX_SIZE or (y2 - y1) < MIN_BOX_SIZE: + return + + self.committed_box = (x1, y1, x2, y2) + + roi = RegionOfInterest() + roi.x_offset = int(x1) + roi.y_offset = int(y1) + roi.width = int(x2 - x1) + roi.height = int(y2 - y1) + roi.do_rectify = False + self.roi_pub.publish(roi) + self.get_logger().info( + f'Published ROI: x={roi.x_offset} y={roi.y_offset} ' + f'w={roi.width} h={roi.height}') + + def _reset(self): + self.committed_box = None + roi = RegionOfInterest() + roi.x_offset = 0 + roi.y_offset = 0 + roi.width = 0 + roi.height = 0 + roi.do_rectify = False + self.roi_pub.publish(roi) + self.get_logger().info('ROI reset - camera_enhancer will revert to its default corner crop') + + def image_callback(self, msg): + try: + frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') + except Exception as e: + self.get_logger().error(f'Failed to convert image: {e}') + return + + display = frame.copy() + + if self.dragging and self.anchor is not None and self.current is not None: + ax, ay = self.anchor + cx, cy = self.current + x1, x2 = min(ax, cx), max(ax, cx) + y1, y2 = min(ay, cy), max(ay, cy) + cv2.rectangle(display, (x1, y1), (x2, y2), COLOR_IN_PROGRESS, 2) + + if self.committed_box is not None: + x1, y1, x2, y2 = self.committed_box + cv2.rectangle(display, (x1, y1), (x2, y2), COLOR_COMMITTED, 2) + + try: + cv2.imshow(self.window_name, display) + key = cv2.waitKey(1) & 0xFF + except cv2.error as e: + self.get_logger().error( + f'Lost the ability to display frames (display/X server gone?): {e}') + return + + if key == ord('r'): + self._reset() + elif key == ord('q') or key == 27: # 27 == ESC + self.get_logger().info('Quit key pressed, shutting down ROI selector') + cv2.destroyWindow(self.window_name) + raise SystemExit + + +def main(args=None): + rclpy.init(args=args) + node = RoiSelectorNode() + try: + if node.gui_ready: + rclpy.spin(node) + else: + node.get_logger().error('ROI selector GUI unavailable - shutting down without spinning') + except (KeyboardInterrupt, SystemExit): + pass + finally: + cv2.destroyAllWindows() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/rover_vision/setup.py b/src/rover_vision/setup.py index c21d0278..531211ec 100644 --- a/src/rover_vision/setup.py +++ b/src/rover_vision/setup.py @@ -35,6 +35,7 @@ 'morse_camera_pub_node = rover_vision.morse_camera_pub_node:main', 'morse_decoder_node = rover_vision.morse_decoder_node:main', 'camera_enhancer = rover_vision.camera_enhancer:main', + 'roi_selector = rover_vision.roi_selector:main', ], }, )