add picamera

src/camera/camera/camera_controller.py

@@ -3,7 +3,7 @@ from rclpy.node import Node
 
 from drone_services.srv import TakePicture
 
-import cv2
+from picamera import PiCamera
 from datetime import datetime
 
 RES_4K_H = 2160
@@ -13,14 +13,7 @@ RES_4K_W = 3840
 class CameraController(Node):
     def __init__(self):
         super().__init__('camera_controller')
-        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.camera = PiCamera()
         self.get_logger().info("Camera controller started. Waiting for service call...")
         self.srv = self.create_service(TakePicture, 'drone/picture', self.take_picture_callback)
 
@@ -31,11 +24,10 @@ 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)
+                self.camera.capture(imagename)
                 response.filename = imagename
             else:
-                cv2.imwrite(request.input_name, image)
+                self.camera.capture(request.input_name)
                 response.filename = request.input_name
             self.get_logger().info("Picture saved as " + response.filename)
         else:
@@ -43,30 +35,6 @@ class CameraController(Node):
             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)