From 6c4050742b55475a4063232ae38fa587c1c047d2 Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Wed, 10 May 2023 11:27:38 +0200
Subject: [PATCH] add taking picture with fswebcam

---
 src/camera/camera/camera_controller.py | 62 +++++++++++++-------------
 src/px4_msgs                           |  2 +-
 2 files changed, 33 insertions(+), 31 deletions(-)

diff --git a/src/camera/camera/camera_controller.py b/src/camera/camera/camera_controller.py
index b138f9b6..04edf4ad 100644
--- a/src/camera/camera/camera_controller.py
+++ b/src/camera/camera/camera_controller.py
@@ -2,10 +2,11 @@ import rclpy
 from rclpy.node import Node
 
 from drone_services.srv import TakePicture
-
-import cv2
+import os
 from datetime import datetime
 
+# import cv2
+
 RES_4K_H = 3496
 RES_4K_W = 4656
 
@@ -13,13 +14,13 @@ RES_4K_W = 4656
 class CameraController(Node):
     def __init__(self):
         super().__init__('camera_controller')
-        self.capture = cv2.VideoCapture(0,cv2.CAP_DSHOW)
+        # 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)))
+        # 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)
@@ -31,12 +32,13 @@ 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)
+                # 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)
+                # cv2.imwrite(request.input_name, image)
                 response.filename = request.input_name
+            os.system('fswebcam -r 4656x3496 ' + response.filename)
             self.get_logger().info("Picture saved as " + response.filename)
         else:
             self.get_logger().error("Could not take picture")
@@ -44,28 +46,28 @@ class CameraController(Node):
 
         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]
+    # 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
+    #     # 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))
+    #     # 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)
+    #     # Return the resized image
+    #     return cv2.resize(image, dim, interpolation=inter)
             
 
 def main(args=None):
diff --git a/src/px4_msgs b/src/px4_msgs
index ffc3a4cd..b64ef047 160000
--- a/src/px4_msgs
+++ b/src/px4_msgs
@@ -1 +1 @@
-Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266
+Subproject commit b64ef0475c1d44605688f4770899fe453d532be4