add resize function

src/camera/camera/camera_controller.py

@@ -14,9 +14,11 @@ class CameraController(Node):
     def __init__(self):
         super().__init__('camera_controller')
         self.capture = cv2.VideoCapture(0,cv2.CAP_DSHOW)
-        
-        self.capture.set(cv2.CAP_PROP_FRAME_WIDTH, RES_4K_W)
-        self.capture.set(cv2.CAP_PROP_FRAME_HEIGHT, RES_4K_H)
+
+        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 controller started. Waiting for service call...")
         self.srv = self.create_service(TakePicture, 'drone/picture', self.take_picture_callback)
@@ -28,13 +30,42 @@ class CameraController(Node):
                 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"
+                image = self.maintain_aspect_ratio_resize(image, width=RES_4K_W, height=RES_4K_H)
                 cv2.imwrite(imagename, image)
                 response.filename = imagename
             else:
                 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)