From 5d07a5975fbb40fcbef29243c7e0581ca84f81f5 Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Mon, 8 May 2023 18:14:24 +0200
Subject: [PATCH] add opencv back

---
 src/camera/camera/camera_controller.py | 53 ++++++++++++++++++++++----
 1 file changed, 45 insertions(+), 8 deletions(-)

diff --git a/src/camera/camera/camera_controller.py b/src/camera/camera/camera_controller.py
index 3427be8f..b138f9b6 100644
--- a/src/camera/camera/camera_controller.py
+++ b/src/camera/camera/camera_controller.py
@@ -3,33 +3,70 @@ from rclpy.node import Node
 
 from drone_services.srv import TakePicture
 
-from picamera import PiCamera
+import cv2
 from datetime import datetime
 
-RES_4K_H = 2160
-RES_4K_W = 3840
+RES_4K_H = 3496
+RES_4K_W = 4656
 
 
 class CameraController(Node):
     def __init__(self):
         super().__init__('camera_controller')
-        self.camera = PiCamera()
+        self.capture = cv2.VideoCapture(0,cv2.CAP_DSHOW)
+
+        CAMERA_PROP_WIDTH = 3
+        CAMERA_PROP_HEIGHT = 4
+        self.capture.set(CAMERA_PROP_WIDTH, RES_4K_W)
+        self.capture.set(CAMERA_PROP_HEIGHT, RES_4K_H)
+        self.get_logger().info("Camera resolution set to " + str(self.capture.get(CAMERA_PROP_WIDTH)) + "x" + str(self.capture.get(CAMERA_PROP_HEIGHT)))
+
         self.get_logger().info("Camera controller started. Waiting for service call...")
         self.srv = self.create_service(TakePicture, 'drone/picture', self.take_picture_callback)
 
     def take_picture_callback(self, request, response):
+        result, image = self.capture.read()
+        if (result):
             if (request.input_name == "default"):
                 self.get_logger().info("Taking picture with default filename")
                 now = datetime.now().strftime("droneimage_%Y-%m-%d_%H-%M-%S")
                 imagename = "/home/ubuntu/drone_img" + now + ".jpg"
-                self.camera.capture(imagename)
+                image = self.maintain_aspect_ratio_resize(image, width=RES_4K_W, height=RES_4K_H)
+                cv2.imwrite(imagename, image)
                 response.filename = imagename
             else:
-                self.camera.capture(request.input_name)
+                cv2.imwrite(request.input_name, image)
                 response.filename = request.input_name
-                
             self.get_logger().info("Picture saved as " + response.filename)
-            return response
+        else:
+            self.get_logger().error("Could not take picture")
+            response.filename = "/dev/null"
+
+        return response
+    
+    def maintain_aspect_ratio_resize(self, image, width=None, height=None, inter=cv2.INTER_AREA):
+        # Grab the image size and initialize dimensions
+        dim = None
+        (h, w) = image.shape[:2]
+
+        # Return original image if no need to resize
+        if width is None and height is None:
+            return image
+
+        # We are resizing height if width is none
+        if width is None:
+            # Calculate the ratio of the height and construct the dimensions
+            r = height / float(h)
+            dim = (int(w * r), height)
+        # We are resizing width if height is none
+        else:
+            # Calculate the ratio of the 0idth and construct the dimensions
+            r = width / float(w)
+            dim = (width, int(h * r))
+
+        # Return the resized image
+        return cv2.resize(image, dim, interpolation=inter)
+            
 
 def main(args=None):
     rclpy.init(args=args)