Draft: Pi-camera UDP Client + boost

CMakeLists.txt

@@ -72,6 +72,7 @@ include(${DEPS_DIRECTORY}/matplot/matplot.cmake)
 include(${DEPS_DIRECTORY}/protobuf/protobuf.cmake)
 include(${DEPS_DIRECTORY}/loguru/loguru.cmake)
 include(${DEPS_DIRECTORY}/imagemagick/imagemagick.cmake)
+include(${DEPS_DIRECTORY}/boost/boost.cmake)
 # =============================
 
 # =============================

configs/dev.json

@@ -60,7 +60,7 @@
     },
     "camera": {
         "_comment": "See CameraConfig struct in datatypes.hpp for detailed explanations",
-        "type": "mock",
+        "type": "PiCamera",
         "save_dir": "/workspaces/obcpp/images/mock",
         "save_images_to_file": true,
         "mock": {

deps/boost/boost.cmake

@@ -0,0 +1,16 @@
+function(target_add_boost target_name)
+    include(FetchContent)
+
+    FetchContent_Declare(boost 
+        URL https://github.com/boostorg/boost/releases/download/boost-1.87.0/boost-1.87.0-b2-nodocs.tar.gz
+        URL_HASH SHA256=d6c69e4459eb5d6ec208250291221e7ff4a2affde9af6e49c9303b89c687461f
+        DOWNLOAD_EXTRACT_TIMESTAMP true
+    )
+    FetchContent_MakeAvailable(boost)
+    # target_link_libraries(${target_name} PRIVATE
+    #     boost
+    # )
+    target_include_directories(${target_name} PRIVATE ${boost_SOURCE_DIR})
+
+
+endfunction()

include/camera/interface.hpp

@@ -101,4 +101,4 @@ class CameraInterface {
         std::shared_ptr<MavlinkClient> mavlinkClient) = 0;
 };
 
-#endif  // INCLUDE_CAMERA_INTERFACE_HPP_
+#endif  // INCLUDE_CAMERA_INTERFACE_HPP_

include/camera/rpi.hpp

@@ -0,0 +1,61 @@
+#ifndef INCLUDE_CAMERA_RPI_HPP_
+#define INCLUDE_CAMERA_RPI_HPP_
+
+#include <string>
+#include <memory>
+#include <optional>
+#include <shared_mutex>
+#include <thread>
+#include <deque>
+#include <vector>
+
+#include <nlohmann/json.hpp>
+#include "camera/interface.hpp"
+#include "network/mavlink.hpp"
+#include "network/udp_client.hpp"
+
+using json = nlohmann::json;
+using namespace std::chrono_literals; // NOLINT
+
+namespace asio = boost::asio;
+
+const std::uint8_t START_REQUEST = 's';
+const std::uint8_t PICTURE_REQUEST = 'I';
+const std::uint8_t END_REQUEST = 'e';
+const std::uint8_t LOCK_REQUEST = 'l';
+
+class RPICamera : public CameraInterface {
+    private:
+        UDPClient client;
+        asio::io_context io_context_;
+        std::atomic_bool connected;
+
+        /**
+         * Converts the 3-plane raw data to BGR cv::Mat, handling stride/padding
+         */
+        std::optional<cv::Mat> imgConvert(const std::vector<std::vector<uint8_t>>& planes);
+
+        /**
+         * Reads the 3 planes (Y, U, V) from the camera
+         */
+        std::vector<std::vector<uint8_t>> readImage();
+
+    public:
+        explicit RPICamera(CameraConfig config, asio::io_context* io_context_);
+        ~RPICamera();
+
+        void connect() override;
+        bool isConnected() override;
+
+        std::optional<ImageData> getLatestImage() override {return std::nullopt;}
+        std::deque<ImageData> getAllImages() override {return std::deque<ImageData>();}
+
+        std::optional<ImageData> takePicture(const std::chrono::milliseconds& timeout, std::shared_ptr<MavlinkClient> mavlinkClient) override;
+
+        void startTakingPictures(const std::chrono::milliseconds& interval, std::shared_ptr<MavlinkClient> mavlinkClient) override;
+        void stopTakingPictures() override;
+        void startStreaming() override;
+        void ping();
+};
+
+#endif

include/core/mission_state.hpp

@@ -11,6 +11,7 @@
 #include <queue>
 #include <unordered_set>
 #include <vector>
+#include <boost/asio.hpp>
 
 #include "camera/interface.hpp"
 #include "core/mission_parameters.hpp"
@@ -113,6 +114,7 @@ class MissionState {
     OBCConfig config;
 
     std::optional<airdrop_t> next_airdrop_to_drop;
+    boost::asio::io_context raspy_io;
 
  private:
     std::mutex converter_mut;
@@ -143,6 +145,8 @@ class MissionState {
     // with the detected_target specified by the index
     std::array<size_t, NUM_AIRDROPS> cv_matches;
 
+    // boost::asio::io_context io_testies;
+
     bool mappingIsDone;
 
     void _setTick(Tick* newTick);  // does not acquire the tick_mut

include/network/camera_data.hpp

@@ -0,0 +1,82 @@
+#ifndef INCLUDE_NETWORK_CAMERA_DATA_HPP_
+#define INCLUDE_NETWORK_CAMERA_DATA_HPP_
+
+#include <iostream>
+#include <boost/serialization/serialization.hpp>
+#include <boost/serialization/vector.hpp>
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/archive/binary_oarchive.hpp>
+
+struct ImageData_t
+{
+    int width;
+    int height;
+    size_t imageSizeBytes;
+    std::vector<std::uint8_t> imgBuffer;
+
+    template<class Archive>
+	void serialize(Archive & ar, const unsigned int version){
+		ar & width;
+		ar & height;
+		ar & imageSizeBytes;
+		ar & imgBuffer;
+	}
+};
+
+// What does OBC needs from camera
+enum class RequestType_t
+{
+    SENDIMAGE
+};
+
+// TODO: unsure if these need to be in a namespace or not
+template<class Archive>
+void serialize(Archive & ar, RequestType_t & request, const unsigned int version) {
+	ar & static_cast<int>(request); // have to type cast enum class
+}
+
+// What is the status of the request (debugging + error handling)
+enum class ResponseType_t
+{
+    SUCC,
+    ERROR
+};
+
+// TODO: unsure if these need to be in a namespace or not
+template<class Archive>
+void serialize(Archive & ar, ResponseType_t & response, const unsigned int version) {
+	ar & static_cast<int>(response); // have to type cast enum class
+}
+
+// OBC requesting camera for something 
+struct CameraRequest_t
+{
+    // device id that requests (for debugging)
+    int pid;
+
+    // type of request, we should figure out edge cases
+    RequestType_t requestType;
+
+    template<class Archive>
+	void serialize(Archive & ar, const unsigned int version) {
+		ar & pid;
+		ar & requestType;
+	}
+};
+
+// Response back from camera to OBC
+struct CameraResponse_t
+{
+    int pid; 
+    ResponseType_t responseType;
+    ImageData_t imageData;
+
+    template<class Archive>
+	void serialize(Archive & ar, const unsigned int version) {
+		ar & pid;
+		ar & responseType;
+		ar & imageData;
+	}
+};
+
+#endif

include/network/rpi_connection.hpp

@@ -0,0 +1,31 @@
+#ifndef INCLUDE_NETWORK_RPI_CONNECTION_HPP_
+#define INCLUDE_NETWORK_RPI_CONNECTION_HPP_
+
+#include <iostream>
+#pragma once 
+#include <cstdint>
+
+// Image Config
+inline uint32_t IMG_WIDTH = 1456;
+inline uint32_t IMG_HEIGHT = 1088;
+inline uint32_t IMG_BUFFER = IMG_WIDTH * IMG_HEIGHT * 3 / 2;
+
+// Libcamera Strides/Padding
+const uint32_t STRIDE_Y = 1472;
+const uint32_t STRIDE_UV = 736;
+
+// Network Config
+const std::string SERVER_IP = "192.168.77.2";
+const int SERVER_PORT = 25565;
+
+const int headerSize = 12;
+const uint32_t EXPECTED_MAGIC = 0x12345678;
+const size_t CHUNK_SIZE = 1024;
+
+struct Header {
+    uint32_t magic;
+    uint32_t total_chunks;
+    uint32_t mem_size;
+};
+
+#endif

include/network/serialize.hpp

@@ -0,0 +1,28 @@
+#ifndef INCLUDE_NETWORK_SERIALIZE_HPP_
+#define INCLUDE_NETWORK_SERIALIZE_HPP_
+
+#include <boost/asio.hpp>
+
+/*
+    Includes some reference on how to serialize and deserialize structs
+*/
+
+namespace serialh {
+    // TODO: make this so that it returns any struct
+    template <typename T>
+    void serialize(T* response, boost::asio::streambuf* buf) {
+        std::ostream os(buf);
+        boost::archive::binary_oarchive oa(os);
+        oa << *response;
+    }
+
+    // TODO: make this so that it returns any struct
+    template <typename T>
+    void deserialize(T* response, boost::asio::streambuf* buf) {
+        std::istream is(buf);
+        boost::archive::binary_iarchive ia(is);
+        ia >> *response;
+    }
+}
+
+#endif

include/network/udp_client.hpp

@@ -0,0 +1,31 @@
+#ifndef INCLUDE_NETWORK_UDP_CLIENT_HPP_
+#define INCLUDE_NETWORK_UDP_CLIENT_HPP_
+
+#include <iostream>
+#include <boost/asio.hpp>
+#include <opencv2/opencv.hpp>
+#include <loguru.hpp>
+#include "rpi_connection.hpp"
+
+namespace asio = boost::asio;
+
+class UDPClient {
+    private:
+        asio::ip::udp::socket socket_;
+        std::string ip;
+        int port;
+
+    public:
+        UDPClient(asio::io_context* io_context_, std::string ip, int port);
+
+        // there isnt really a notion of connect with connectionless udp sockets
+        bool connect();
+
+        bool send(std::uint8_t request);
+
+        Header recvHeader();
+
+        std::vector<std::uint8_t> recvBody(const int mem_size, const int total_chunks);
+};
+
+#endif

include/network/udp_server.hpp

@@ -0,0 +1,35 @@
+#ifndef INCLUDE_NETWORK_UDP_SERVER_HPP_
+#define INCLUDE_NETWORK_UDP_SERVER_HPP_
+
+#include <iostream>
+#include <boost/asio.hpp>
+#include <opencv2/opencv.hpp>
+#include "network/rpi_connection.hpp"
+
+namespace asio = boost::asio;
+
+class UDPServer {
+    private:
+        std::string ip;
+        int port;
+        asio::ip::udp::socket socket_;
+
+        cv::Mat createBGR();
+
+        cv::Mat createYUV();
+
+    public:
+        UDPServer(asio::io_context* io_context_, std::string ip, int port);
+
+        bool start();
+
+        void send(asio::ip::udp::endpoint & endpoint);
+
+        void recv();
+
+        void handleRequest(char request, asio::ip::udp::endpoint & endpoint);
+
+        void shutdown();
+};
+
+#endif  

src/CMakeLists.txt

@@ -43,7 +43,9 @@ target_link_libraries(obcpp_lib_mock INTERFACE
 
 add_executable(${PROJECT_NAME} main.cpp)
 target_include_directories(${PROJECT_NAME} PRIVATE ${INCLUDE_DIRECTORY} ${GEN_PROTOS_DIRECTORY})
-target_link_libraries(${PROJECT_NAME} PRIVATE obcpp_lib)
+target_link_libraries(${PROJECT_NAME} PRIVATE 
+    obcpp_lib
+)
 target_add_protobuf(${PROJECT_NAME})
 target_add_torch(${PROJECT_NAME})
 target_add_torchvision(${PROJECT_NAME})
@@ -53,6 +55,7 @@ target_add_mavsdk(${PROJECT_NAME})
 target_add_opencv(${PROJECT_NAME})
 target_add_httplib(${PROJECT_NAME})
 target_add_loguru(${PROJECT_NAME})
+target_add_boost(${PROJECT_NAME})
 target_add_onnxruntime(${PROJECT_NAME})
 # for some reason calling target_add_imagemagick here conflicts with, so we are including/linking without the function call
 # target_add_imagemagick(${PROJECT_NAME})

src/camera/CMakeLists.txt

@@ -3,12 +3,13 @@ set(LIB_NAME obcpp_camera)
 set(FILES
     interface.cpp
     mock.cpp
+    rpi.cpp
     picamera.cpp
 )
 
 SET(LIB_DEPS
     obcpp_protos
-    # obcpp_network # circular dependency
+    obcpp_network # circular dependency - resolved at executable level
 )
 
 add_library(${LIB_NAME} STATIC
@@ -30,6 +31,7 @@ target_add_httplib(${LIB_NAME})
 target_add_mavsdk(${LIB_NAME})
 target_add_matplot(${LIB_NAME})
 target_add_loguru(${LIB_NAME})
+target_add_boost(${LIB_NAME})
 
 target_include_directories(${LIB_NAME} PRIVATE ${ImageMagick_INCLUDE_DIRS})
-target_link_libraries(${LIB_NAME} PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES})
+target_link_libraries(${LIB_NAME} PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES})