From f83520c29abc1ae475274aa2e977d5591f81c5e8 Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Mon, 8 May 2023 16:20:37 +0200
Subject: [PATCH] add camera service

---
 src/camera/camera/camera_controller.py        | 39 +++++++++++++++++++
 src/camera/package.xml                        | 29 +++++++-------
 src/drone_services/srv/TakePicture.srv        |  3 ++
 src/object_detection/src/lidar_reader.cpp     |  4 --
 src/object_detection/src/multiflex_reader.cpp |  4 --
 src/px4_msgs                                  |  2 +-
 6 files changed, 59 insertions(+), 22 deletions(-)
 create mode 100644 src/camera/camera/camera_controller.py
 create mode 100644 src/drone_services/srv/TakePicture.srv

diff --git a/src/camera/camera/camera_controller.py b/src/camera/camera/camera_controller.py
new file mode 100644
index 00000000..e65852f4
--- /dev/null
+++ b/src/camera/camera/camera_controller.py
@@ -0,0 +1,39 @@
+import rclpy
+from rclpy.node import Node
+
+from drone_services.srv import TakePicture
+
+from picamera2 import Picamera2
+
+
+class CameraController(Node):
+    def __init__(self):
+        super().__init__('camera_controller')
+        self.picam2 = Picamera2()
+        self.srv = self.create_service(TakePicture, 'drone/picture', self.take_picture_callback)
+
+    def take_picture_callback(self, request, response):
+        if (request.input_name == "default"):
+            self.get_logger().info("Taking picture with default filename")
+            self.picam2.start_and_capture_file("/home/ubuntu/image.jpg")
+            response.filename = "/home/ubuntu/image.jpg"
+        else:
+            self.picam2.start_and_capture_file(request.input_name)
+            response.filename = request.input_name
+        return response
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    test_controller = CameraController()
+    rclpy.spin(test_controller)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    test_controller.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file
diff --git a/src/camera/package.xml b/src/camera/package.xml
index 6c6a617e..e9065b1e 100644
--- a/src/camera/package.xml
+++ b/src/camera/package.xml
@@ -1,18 +1,21 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>camera</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
-  <license>TODO: License declaration</license>
+    <name>camera</name>
+    <version>0.0.0</version>
+    <description>Package for controlling the camera of the drone</description>
+    <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
+    <license>Apache License 2.0</license>
 
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
+    <exec_depend>rclpy</exec_depend>
+    <exec_depend>drone_services</exec_depend>
 
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
-</package>
+    <test_depend>ament_copyright</test_depend>
+    <test_depend>ament_flake8</test_depend>
+    <test_depend>ament_pep257</test_depend>
+    <test_depend>python3-pytest</test_depend>
+
+    <export>
+        <build_type>ament_python</build_type>
+    </export>
+</package>
\ No newline at end of file
diff --git a/src/drone_services/srv/TakePicture.srv b/src/drone_services/srv/TakePicture.srv
new file mode 100644
index 00000000..e043283c
--- /dev/null
+++ b/src/drone_services/srv/TakePicture.srv
@@ -0,0 +1,3 @@
+wstring input_name "default" # name of the input file
+---
+wstring filename # output file name
\ No newline at end of file
diff --git a/src/object_detection/src/lidar_reader.cpp b/src/object_detection/src/lidar_reader.cpp
index 265d35a2..85faa854 100644
--- a/src/object_detection/src/lidar_reader.cpp
+++ b/src/object_detection/src/lidar_reader.cpp
@@ -1,8 +1,4 @@
 #include <chrono>
-#include <functional>
-#include <memory>
-#include <string>
-#include <iostream>
 
 #include "rclcpp/rclcpp.hpp"
 #include "object_detection/msg/lidar_reading.hpp"
diff --git a/src/object_detection/src/multiflex_reader.cpp b/src/object_detection/src/multiflex_reader.cpp
index f2e92272..83528502 100644
--- a/src/object_detection/src/multiflex_reader.cpp
+++ b/src/object_detection/src/multiflex_reader.cpp
@@ -1,8 +1,4 @@
 #include <chrono>
-#include <functional>
-#include <memory>
-#include <string>
-#include <iostream>
 
 #include "rclcpp/rclcpp.hpp"
 #include "object_detection/msg/multiflex_reading.hpp"
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