Cameron/camera enhance

.gitignore

@@ -72,3 +72,4 @@ log/
 
 .vscode/settings.json
 .venv
+test_files/

src/rover_vision/rover_vision/camera_enhancer.py

@@ -0,0 +1,147 @@
+#!/usr/bin/env python3
+
+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
+
+# Fallback crop used before a dynamic ROI is selected.
+ROI_SIZE = 240
+ZOOM_FACTOR = 2
+
+
+class CameraEnhancerNode(Node):
+    def __init__(self):
+        super().__init__('camera_enhancer')
+
+        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} (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:
+            # 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]
+
+        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')
+        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()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

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()

src/rover_vision/setup.py

@@ -34,6 +34,8 @@
             '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',
+            'roi_selector = rover_vision.roi_selector:main',
         ],
     },
 )