add camera service

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()

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>
+    <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>

src/drone_services/srv/TakePicture.srv

@@ -0,0 +1,3 @@
+wstring input_name "default" # name of the input file
+---
+wstring filename # output file name

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"

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"