Initial Commit

src/vision/camera_processor.py

@@ -0,0 +1,227 @@
+#!/usr/bin/env python3
+"""
+Camera Processor Node
+Handles camera feed processing for GPS-denied navigation
+"""
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image, CameraInfo
+from std_msgs.msg import Header
+from cv_bridge import CvBridge
+import cv2
+import numpy as np
+
+
+class CameraProcessor(Node):
+    """
+    Processes camera feeds for visual odometry and obstacle detection.
+    Outputs processed images for downstream nodes.
+    """
+    
+    def __init__(self):
+        super().__init__('camera_processor')
+        
+        self.bridge = CvBridge()
+        
+        # Camera parameters
+        self.camera_matrix = None
+        self.dist_coeffs = None
+        self.image_size = None
+        
+        # Image processing parameters
+        self.declare_parameter('undistort', True)
+        self.declare_parameter('grayscale_output', True)
+        self.declare_parameter('histogram_equalization', True)
+        self.declare_parameter('resize_factor', 1.0)
+        
+        self.undistort = self.get_parameter('undistort').value
+        self.grayscale_output = self.get_parameter('grayscale_output').value
+        self.histogram_eq = self.get_parameter('histogram_equalization').value
+        self.resize_factor = self.get_parameter('resize_factor').value
+        
+        # Undistort maps (computed once)
+        self.map1 = None
+        self.map2 = None
+        
+        # Subscribers - Forward camera
+        self.forward_image_sub = self.create_subscription(
+            Image,
+            '/uav/camera/forward/image_raw',
+            self.forward_image_callback,
+            10
+        )
+        
+        self.forward_info_sub = self.create_subscription(
+            CameraInfo,
+            '/uav/camera/forward/camera_info',
+            self.camera_info_callback,
+            10
+        )
+        
+        # Subscribers - Downward camera
+        self.downward_image_sub = self.create_subscription(
+            Image,
+            '/uav/camera/downward/image_raw',
+            self.downward_image_callback,
+            10
+        )
+        
+        # Publishers - Processed images
+        self.forward_processed_pub = self.create_publisher(
+            Image,
+            '/uav/camera/forward/image_processed',
+            10
+        )
+        
+        self.downward_processed_pub = self.create_publisher(
+            Image,
+            '/uav/camera/downward/image_processed',
+            10
+        )
+        
+        # Debug visualization
+        self.debug_pub = self.create_publisher(
+            Image,
+            '/uav/camera/debug',
+            10
+        )
+        
+        self.get_logger().info('Camera Processor Node Started')
+    
+    def camera_info_callback(self, msg: CameraInfo):
+        """Extract and store camera calibration parameters."""
+        if self.camera_matrix is None:
+            self.camera_matrix = np.array(msg.k).reshape(3, 3)
+            self.dist_coeffs = np.array(msg.d)
+            self.image_size = (msg.width, msg.height)
+            
+            # Compute undistortion maps
+            if self.undistort and self.dist_coeffs is not None:
+                new_camera_matrix, _ = cv2.getOptimalNewCameraMatrix(
+                    self.camera_matrix,
+                    self.dist_coeffs,
+                    self.image_size,
+                    alpha=0
+                )
+                self.map1, self.map2 = cv2.initUndistortRectifyMap(
+                    self.camera_matrix,
+                    self.dist_coeffs,
+                    None,
+                    new_camera_matrix,
+                    self.image_size,
+                    cv2.CV_16SC2
+                )
+            
+            self.get_logger().info(
+                f'Camera calibration received: {self.image_size[0]}x{self.image_size[1]}'
+            )
+    
+    def process_image(self, image: np.ndarray) -> np.ndarray:
+        """
+        Apply image processing pipeline.
+        
+        Args:
+            image: Input BGR image
+            
+        Returns:
+            Processed image
+        """
+        processed = image.copy()
+        
+        # 1. Undistort if calibration available
+        if self.undistort and self.map1 is not None:
+            processed = cv2.remap(processed, self.map1, self.map2, cv2.INTER_LINEAR)
+        
+        # 2. Resize if needed
+        if self.resize_factor != 1.0:
+            new_size = (
+                int(processed.shape[1] * self.resize_factor),
+                int(processed.shape[0] * self.resize_factor)
+            )
+            processed = cv2.resize(processed, new_size, interpolation=cv2.INTER_AREA)
+        
+        # 3. Convert to grayscale if requested
+        if self.grayscale_output:
+            if len(processed.shape) == 3:
+                processed = cv2.cvtColor(processed, cv2.COLOR_BGR2GRAY)
+        
+        # 4. Histogram equalization for better feature detection
+        if self.histogram_eq:
+            if len(processed.shape) == 2:
+                # CLAHE for better results than standard histogram equalization
+                clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
+                processed = clahe.apply(processed)
+            else:
+                # For color images, apply to L channel in LAB
+                lab = cv2.cvtColor(processed, cv2.COLOR_BGR2LAB)
+                clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
+                lab[:, :, 0] = clahe.apply(lab[:, :, 0])
+                processed = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR)
+        
+        return processed
+    
+    def forward_image_callback(self, msg: Image):
+        """Process forward camera images for visual odometry."""
+        try:
+            # Convert ROS Image to OpenCV
+            cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
+            
+            # Process image
+            processed = self.process_image(cv_image)
+            
+            # Convert back to ROS Image
+            if len(processed.shape) == 2:
+                encoding = 'mono8'
+            else:
+                encoding = 'bgr8'
+            
+            processed_msg = self.bridge.cv2_to_imgmsg(processed, encoding)
+            processed_msg.header = msg.header
+            
+            # Publish processed image
+            self.forward_processed_pub.publish(processed_msg)
+            
+        except Exception as e:
+            self.get_logger().error(f'Forward image processing error: {e}')
+    
+    def downward_image_callback(self, msg: Image):
+        """Process downward camera images for optical flow."""
+        try:
+            # Convert ROS Image to OpenCV
+            cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
+            
+            # Process image
+            processed = self.process_image(cv_image)
+            
+            # Convert back to ROS Image
+            if len(processed.shape) == 2:
+                encoding = 'mono8'
+            else:
+                encoding = 'bgr8'
+            
+            processed_msg = self.bridge.cv2_to_imgmsg(processed, encoding)
+            processed_msg.header = msg.header
+            
+            # Publish processed image
+            self.downward_processed_pub.publish(processed_msg)
+            
+        except Exception as e:
+            self.get_logger().error(f'Downward image processing error: {e}')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = CameraProcessor()
+    
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()