diff --git a/CMakeLists.txt b/CMakeLists.txt index da38ed91..4a50cef3 100644 --- a/CMakeLists.txt +++ b/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) # ============================= # ============================= diff --git a/deps/boost/boost.cmake b/deps/boost/boost.cmake new file mode 100644 index 00000000..556ea5cb --- /dev/null +++ b/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() diff --git a/include/camera/interface.hpp b/include/camera/interface.hpp index f53b9416..bffbf204 100644 --- a/include/camera/interface.hpp +++ b/include/camera/interface.hpp @@ -101,4 +101,4 @@ class CameraInterface { std::shared_ptr mavlinkClient) = 0; }; -#endif // INCLUDE_CAMERA_INTERFACE_HPP_ +#endif // INCLUDE_CAMERA_INTERFACE_HPP_ \ No newline at end of file diff --git a/include/camera/rpi.hpp b/include/camera/rpi.hpp new file mode 100644 index 00000000..18022651 --- /dev/null +++ b/include/camera/rpi.hpp @@ -0,0 +1,88 @@ +#ifndef INCLUDE_CAMERA_RPI_HPP_ +#define INCLUDE_CAMERA_RPI_HPP_ + + +#include +#include +#include +#include +#include +#include + +#include + +#include "camera/interface.hpp" +#include "network/mavlink.hpp" + +using json = nlohmann::json; + +using namespace std::chrono_literals; // NOLINT + +#include "interface.hpp" +#include "network/udp_client.hpp" + +namespace asio = boost::asio; + +// const std::string SERVER_IP = "192.168.68.1"; +// const int SERVER_PORT = 25565; +// const std::string SERVER_IP = "127.0.0.1"; +// const int SERVER_PORT = 5000; +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; + + std::deque imageQueue; // TODO: unsure if we actually need this if we're just gonna directly save images to disk + + // lock for obc client? + // lock for imageQueue? + + /** + * Converts the image taken from the camera to a suitable format for the CV pipeline + */ + std::optional imgConvert(std::vector imgbuf); + + /** + * Reads in the image data when taking a picture + */ + std::vector readImage(); + + /** + * Reads in the telemetry data when taking a picture + */ + std::optional readTelemetry(); + + std::vector readPacket(); + + public: + + explicit RPICamera(CameraConfig config, asio::io_context* io_context_); + + // TODO: destructor? + ~RPICamera(); + + void connect() override; + bool isConnected() override; + + std::optional getLatestImage() override {return std::nullopt;} + std::deque getAllImages() override {return std::deque();} + std::optional takePicture(const std::chrono::milliseconds& timeout, std::shared_ptr mavlinkClient) override; + + void startTakingPictures(const std::chrono::milliseconds& interval, + std::shared_ptr mavlinkClient) override; + void stopTakingPictures() override; + + void startStreaming() override; + + // TODO: unsure how to implement + void ping(); +}; + +#endif diff --git a/include/network/camera_data.hpp b/include/network/camera_data.hpp new file mode 100644 index 00000000..87e95150 --- /dev/null +++ b/include/network/camera_data.hpp @@ -0,0 +1,82 @@ +#ifndef INCLUDE_NETWORK_CAMERA_DATA_HPP_ +#define INCLUDE_NETWORK_CAMERA_DATA_HPP_ + +#include +#include +#include +#include +#include + +struct ImageData_t +{ + int width; + int height; + size_t imageSizeBytes; + std::vector imgBuffer; + + template + 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 +void serialize(Archive & ar, RequestType_t & request, const unsigned int version) { + ar & static_cast(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 +void serialize(Archive & ar, ResponseType_t & response, const unsigned int version) { + ar & static_cast(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 + 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 + void serialize(Archive & ar, const unsigned int version) { + ar & pid; + ar & responseType; + ar & imageData; + } +}; + +#endif diff --git a/include/network/rpi_connection.hpp b/include/network/rpi_connection.hpp new file mode 100644 index 00000000..f1440807 --- /dev/null +++ b/include/network/rpi_connection.hpp @@ -0,0 +1,27 @@ +#ifndef INCLUDE_NETWORK_RPI_CONNECTION_HPP_ +#define INCLUDE_NETWORK_RPI_CONNECTION_HPP_ + +#include + +const uint32_t IMG_WIDTH = 2028; +const uint32_t IMG_HEIGHT = 1520; +const uint32_t BUFFER_SIZE = IMG_WIDTH * IMG_HEIGHT * 3 / 2; + +// const std::string SERVER_IP = "192.168.68.1"; +// const int SERVER_PORT = 25565; + +// local testing only +const std::string SERVER_IP = "0.0.0.0"; +const int SERVER_PORT = 5000; + +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 diff --git a/include/network/serialize.hpp b/include/network/serialize.hpp new file mode 100644 index 00000000..e05621d2 --- /dev/null +++ b/include/network/serialize.hpp @@ -0,0 +1,28 @@ +#ifndef INCLUDE_NETWORK_SERIALIZE_HPP_ +#define INCLUDE_NETWORK_SERIALIZE_HPP_ + +#include + +/* + Includes some reference on how to serialize and deserialize structs +*/ + +namespace serialh { + // TODO: make this so that it returns any struct + template + 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 + void deserialize(T* response, boost::asio::streambuf* buf) { + std::istream is(buf); + boost::archive::binary_iarchive ia(is); + ia >> *response; + } +} + +#endif diff --git a/include/network/server.hpp b/include/network/server.hpp deleted file mode 100644 index 8a88bd50..00000000 --- a/include/network/server.hpp +++ /dev/null @@ -1,4 +0,0 @@ -#ifndef INCLUDE_NETWORK_SERVER_HPP_ -#define INCLUDE_NETWORK_SERVER_HPP_ - -#endif // INCLUDE_NETWORK_SERVER_HPP_ diff --git a/include/network/tcp_client.hpp b/include/network/tcp_client.hpp new file mode 100644 index 00000000..7962cb6c --- /dev/null +++ b/include/network/tcp_client.hpp @@ -0,0 +1,50 @@ +#ifndef INCLUDE_NETWORK_CLIENT_HPP_ +#define INCLUDE_NETWORK_CLIENT_HPP_ + +#include +#include +#include +#include "rpi_connection.hpp" + +namespace asio = boost::asio; + +class Client { + private: + asio::ip::tcp::socket socket_; + std::string ip; + int port; + + public: + + // TODO: should probably make the io_context a unique_ptr + Client(asio::io_context* io_context_, std::string ip, int port); + + // TODO: do we need a destructor? probably to free the socket right? + // ~Client(); + + /** + * Connects to the specified ip and port + */ + bool connect(); + + /** + * + */ + // TODO: so send is supposed to send a char but im unsure what the type should be uchar, uint8_t, etc. + bool send(std::uint8_t request); + + /** + * Reads in the header and fills a Header struct + */ + // std::optional
+ Header recvHeader(); + + /** + * Reads the actual data specified by the header + */ + // std::optional> + std::vector recvBody(const int bufSize); + +}; + +#endif diff --git a/include/network/tcp_server.hpp b/include/network/tcp_server.hpp new file mode 100644 index 00000000..61c90851 --- /dev/null +++ b/include/network/tcp_server.hpp @@ -0,0 +1,37 @@ +#ifndef INCLUDE_NETWORK_TCP_SERVER_HPP_ +#define INCLUDE_NETWORK_TCP_SERVER_HPP_ + +#include +#include +#include + +namespace asio = boost::asio; + +class Server { + private: + std::string ip; + int port; + asio::ip::tcp::socket socket_; + asio::ip::tcp::acceptor acceptor_; + + void takePicture(); + + cv::Mat createBGR(); + + cv::Mat createYUV(); + + public: + Server(asio::io_context* io_context_, std::string ip, int port); + + void start(); + + void send(); + + void recv(); + + void handleRequest(char request); + + void shutdown(); +}; + +#endif diff --git a/include/network/udp_client.hpp b/include/network/udp_client.hpp new file mode 100644 index 00000000..246d218c --- /dev/null +++ b/include/network/udp_client.hpp @@ -0,0 +1,31 @@ +#ifndef INCLUDE_NETWORK_UDP_CLIENT_HPP_ +#define INCLUDE_NETWORK_UDP_CLIENT_HPP_ + +#include +#include +#include +#include +#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 recvBody(const int mem_size, const int total_chunks); +}; + +#endif diff --git a/include/network/udp_server.hpp b/include/network/udp_server.hpp new file mode 100644 index 00000000..4e53f9f1 --- /dev/null +++ b/include/network/udp_server.hpp @@ -0,0 +1,35 @@ +#ifndef INCLUDE_NETWORK_UDP_SERVER_HPP_ +#define INCLUDE_NETWORK_UDP_SERVER_HPP_ + +#include +#include +#include +#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 diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index b1e78b8d..2f83e7ee 100644 --- a/src/CMakeLists.txt +++ b/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}) diff --git a/src/camera/CMakeLists.txt b/src/camera/CMakeLists.txt index 789c6821..943efaf4 100644 --- a/src/camera/CMakeLists.txt +++ b/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}) \ No newline at end of file +target_link_libraries(${LIB_NAME} PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) diff --git a/src/camera/interface.cpp b/src/camera/interface.cpp index 21b91d9a..6fff55cf 100644 --- a/src/camera/interface.cpp +++ b/src/camera/interface.cpp @@ -1,13 +1,12 @@ #include "camera/interface.hpp" -#include #include -#include - -#include "nlohmann/json.hpp" #include +#include +#include #include "network/mavlink.hpp" // imported here but not in the header due to circular dependency +#include "nlohmann/json.hpp" #include "utilities/base64.hpp" using json = nlohmann::json; @@ -17,62 +16,52 @@ CameraInterface::CameraInterface(const CameraConfig& config) : config(config) {} std::string cvMatToBase64(cv::Mat image) { std::vector buf; cv::imencode(".jpg", image, buf); - auto *enc_msg = reinterpret_cast(buf.data()); + auto* enc_msg = reinterpret_cast(buf.data()); return base64_encode(enc_msg, buf.size()); } void saveImageToFile(cv::Mat image, const std::filesystem::path& filepath) { - cv::imwrite(filepath, image); + cv::imwrite(filepath, image); } void saveImageTelemetryToFile(const ImageTelemetry& telemetry, const std::filesystem::path& filepath) { - json telemetry_json = { - {"latitude_deg", telemetry.latitude_deg }, - {"longitude_deg", telemetry.longitude_deg }, - {"altitude_agl_m", telemetry.altitude_agl_m }, - {"airspeed_m_s", telemetry.airspeed_m_s }, - {"heading_deg", telemetry.heading_deg }, - {"yaw_deg", telemetry.yaw_deg }, - {"pitch_deg", telemetry.pitch_deg }, - {"roll_deg", telemetry.roll_deg } - }; - std::ofstream telemetry_file(filepath); - if (!telemetry_file.is_open()) { - LOG_F(ERROR, "Failed to save telemetry json to %s", filepath.string().c_str()); - return; - } - telemetry_file << to_string(telemetry_json); + json telemetry_json = { + {"latitude_deg", telemetry.latitude_deg}, {"longitude_deg", telemetry.longitude_deg}, + {"altitude_agl_m", telemetry.altitude_agl_m}, {"airspeed_m_s", telemetry.airspeed_m_s}, + {"heading_deg", telemetry.heading_deg}, {"yaw_deg", telemetry.yaw_deg}, + {"pitch_deg", telemetry.pitch_deg}, {"roll_deg", telemetry.roll_deg}}; + std::ofstream telemetry_file(filepath); + if (!telemetry_file.is_open()) { + LOG_F(ERROR, "Failed to save telemetry json to %s", filepath.string().c_str()); + // std::cout << "Failed to save telemetry json to " << filepath.string().c_str() << '\n'; + return; + } + telemetry_file << to_string(telemetry_json); } std::optional queryMavlinkImageTelemetry( - std::shared_ptr mavlinkClient) { - if (mavlinkClient == nullptr) { - return {}; - } - - auto [lat_deg, lon_deg] = mavlinkClient->latlng_deg(); - double altitude_agl_m = mavlinkClient->altitude_agl_m(); - double airspeed_m_s = mavlinkClient->airspeed_m_s(); - double heading_deg = mavlinkClient->heading_deg(); - double yaw_deg = mavlinkClient->yaw_deg(); - double pitch_deg = mavlinkClient->pitch_deg(); - double roll_deg = mavlinkClient->roll_deg(); + std::shared_ptr mavlinkClient) { + if (mavlinkClient == nullptr) { + return {}; + } - std::cout << "lat_deg" << lat_deg << std::endl; - std::cout << "long deg" << lon_deg << std::endl; - std::cout << "altitude" << altitude_agl_m << std::endl; + auto [lat_deg, lon_deg] = mavlinkClient->latlng_deg(); + double altitude_agl_m = mavlinkClient->altitude_agl_m(); + double airspeed_m_s = mavlinkClient->airspeed_m_s(); + double heading_deg = mavlinkClient->heading_deg(); + double yaw_deg = mavlinkClient->yaw_deg(); + double pitch_deg = mavlinkClient->pitch_deg(); + double roll_deg = mavlinkClient->roll_deg(); - return ImageTelemetry { - .latitude_deg = lat_deg, - .longitude_deg = lon_deg, - .altitude_agl_m = altitude_agl_m, - .airspeed_m_s = airspeed_m_s, - .heading_deg = heading_deg, - .yaw_deg = yaw_deg, - .pitch_deg = pitch_deg, - .roll_deg = roll_deg - }; + return ImageTelemetry{.latitude_deg = lat_deg, + .longitude_deg = lon_deg, + .altitude_agl_m = altitude_agl_m, + .airspeed_m_s = airspeed_m_s, + .heading_deg = heading_deg, + .yaw_deg = yaw_deg, + .pitch_deg = pitch_deg, + .roll_deg = roll_deg}; } bool ImageData::saveToFile(std::string directory) const { @@ -90,6 +79,7 @@ bool ImageData::saveToFile(std::string directory) const { } } catch (std::exception& e) { LOG_F(ERROR, "Failed to save image and telemetry to file"); + // std::cout << "Failed to save image and telemetry to file" << '\n'; return false; } diff --git a/src/camera/rpi.cpp b/src/camera/rpi.cpp new file mode 100644 index 00000000..bcb00066 --- /dev/null +++ b/src/camera/rpi.cpp @@ -0,0 +1,159 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "camera/interface.hpp" +#include "network/mavlink.hpp" +#include "utilities/locks.hpp" +#include "utilities/datatypes.hpp" +#include "utilities/common.hpp" + +#include "camera/rpi.hpp" + +// const uint32_t IMG_WIDTH = 2028; +// const uint32_t IMG_HEIGHT = 1520; +// const uint32_t IMG_BUFFER = IMG_WIDTH * IMG_HEIGHT * 3 / 2; // normal image size +const uint32_t IMG_SIZE = 4668440; + + +RPICamera::RPICamera(CameraConfig config, asio::io_context* io_context_) : CameraInterface(config), client(io_context_, SERVER_IP, SERVER_PORT) { + this->connected = false; +} + +void RPICamera::connect() { + if (this->connected == true) { + return; + } + + while (!this->connected) { + this->connected = client.connect(); + } + + // tells the camera to start the camera thread + client.send(START_REQUEST); +} + +RPICamera::~RPICamera() { + // TODO: probably have to shutdown/free the socket and free the iocontext if that's a thing +} + +std::optional RPICamera::takePicture(const std::chrono::milliseconds& timeout, std::shared_ptr mavlinkClient) { + + // client sends a request to take a pictures + client.send(PICTURE_REQUEST); + + // TODO: get the imgbuf + std::vector imgbuf = readImage(); + + // give the image buffer to imgConvert + std::optional mat = imgConvert(imgbuf); + + if (!mat.has_value()) { + return {}; + } + + uint64_t timestamp = getUnixTime_s().count(); + + return ImageData { + .DATA = mat.value(), + .TIMESTAMP = timestamp, + .TELEMETRY = {} // TODO: nullopt for now + }; +} + +std::optional RPICamera::imgConvert(std::vector buf) { + + // TODO: how to check the expected buffer size idk, counter? + // if (buf.size() != BUFFER_SIZE) { + // return {}; + // } + + // put raw bytes in cv::Mat + cv::Mat yuv420_img(IMG_HEIGHT * 3 / 2, IMG_WIDTH, CV_8UC1, buf.data()); + + cv::Mat bgr_img(IMG_HEIGHT, IMG_WIDTH, CV_8UC3); + cv::cvtColor(yuv420_img, bgr_img, cv::COLOR_YUV2BGR_I420); // TODO: not sure if this is the right color space + + return bgr_img; +} + +void RPICamera::startTakingPictures(const std::chrono::milliseconds& timeout, std::shared_ptr mavlinkClient) { + +} + +void RPICamera::stopTakingPictures() { + +} + +void RPICamera::ping() { + +} + +std::vector RPICamera::readImage() { + // 3 separate reads for 3 separate files + + // std::vector imgbuf; + + // for (int i = 0; i < 3; i++) { + // std::vector packet = readPacket(); + + // // have to concatenate the packets since there are 3 "image files" + // imgbuf.reserve(imgbuf.size() + packet.size()); + // imgbuf.insert(imgbuf.end(), packet.begin(), packet.end()); + // } + + std::vector packet = readPacket(); + + return packet; + // return imgbuf; +} + +std::optional RPICamera::readTelemetry() { + + ImageTelemetry telemetry; + // asio::read() + + return telemetry; +} + +std::vector RPICamera::readPacket() { + + std::vector packet; + + Header header = client.recvHeader(); + + // TODO: ntohl or ntohs? + header.magic = ntohl(header.magic); + header.mem_size = ntohl(header.mem_size); + header.total_chunks = ntohl(header.total_chunks); + + LOG_F(INFO, "mem_size: %d, total_chunks: %d", header.mem_size, header.total_chunks); + + // check the magic number, sort of like a checksum ig + if (header.magic != EXPECTED_MAGIC) { + // TODO: how do we even handle this, after we read the corrupted header we don't even know how many bytes to throw away to read the next header + } + + packet = client.recvBody(header.mem_size, header.total_chunks); + + io_context_.run(); + + return packet; +} + +void RPICamera::startStreaming() { + +} + +bool RPICamera::isConnected() { + return true; +} diff --git a/src/camera/rpi.cpp~ b/src/camera/rpi.cpp~ new file mode 100644 index 00000000..c17943cc --- /dev/null +++ b/src/camera/rpi.cpp~ @@ -0,0 +1,360 @@ +#ifdef ARENA_SDK_INSTALLED + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "camera/interface.hpp" +#include "network/mavlink.hpp" +#include "utilities/locks.hpp" +#include "utilities/datatypes.hpp" +#include "utilities/common.hpp" + +using json = nlohmann::json; + +raspCam::raspCam(CameraConfig config) : + CameraInterface(config) { + this->connect(); + this->startStreaming(); +} + +void raspCam::connect() { + if (this->isConnected()) { + return; + } + + // aquire locks to Arena System and Device + WriteLock systemLock(this->arenaSystemLock); + WriteLock deviceLock(this->arenaDeviceLock); + + CATCH_ARENA_EXCEPTION("opening Arena System", + this->system = Arena::OpenSystem();); + + while (true) { + CATCH_ARENA_EXCEPTION("attempting to connect to LUCID camera", + // ArenaSystem broadcasts a discovery packet on the network to discover + // any cameras on the network. + // We provide a timeout in milliseconds for this broadcasting sequence. + this->system->UpdateDevices(this->connectionTimeout.count()); + + std::vector deviceInfos = this->system->GetDevices(); + if (deviceInfos.size() != 0) { + LOG_F(INFO, "Lucid camera connection succeeded!"); + this->device = this->system->CreateDevice(deviceInfos[0]); + break; + }); + + LOG_F(INFO, "Lucid camera connection failed! Retrying in %ld ms", + this->connectionRetry.count()); + std::this_thread::sleep_for(this->connectionRetry); + } + + this->configureSettings(); +} + +raspCam::~raspCam() { + // aquire locks to Arena System and Device + WriteLock systemLock(this->arenaSystemLock); + WriteLock deviceLock(this->arenaDeviceLock); + + CATCH_ARENA_EXCEPTION("closing Arena System", + this->system->DestroyDevice(this->device); + Arena::CloseSystem(this->system);); +} + +void raspCam::startTakingPictures(const std::chrono::milliseconds& interval, + std::shared_ptr mavlinkClient) { + if (this->isTakingPictures) { + return; + } + + this->isTakingPictures = true; + + this->captureThread = std::thread(&raspCam::captureEvery, this, interval); +} + +void raspCam::stopTakingPictures() { + if (!this->isTakingPictures) { + return; + } + + this->isTakingPictures = false; + + this->captureThread.join(); +} + +void raspCam::configureSettings() { + const std::string sensor_shutter_mode_name = "SensorShutterMode"; + CATCH_ARENA_EXCEPTION((std::string("setting ") + sensor_shutter_mode_name).c_str(), + Arena::SetNodeValue( + device->GetNodeMap(), + sensor_shutter_mode_name.c_str(), + this->config.lucid.sensor_shutter_mode.c_str());); + + const std::string acquisition_frame_rate_enable_name = "AcquisitionFrameRateEnable"; + CATCH_ARENA_EXCEPTION((std::string("setting ") + acquisition_frame_rate_enable_name).c_str(), + Arena::SetNodeValue( + device->GetNodeMap(), + acquisition_frame_rate_enable_name.c_str(), + this->config.lucid.acquisition_frame_rate_enable);); + + // Note that this modifies the TLStreamNodeMap and not the standard NodeMap + const std::string stream_auto_negotiate_packet_size_name = "StreamAutoNegotiatePacketSize"; + CATCH_ARENA_EXCEPTION(( + std::string("setting ") + stream_auto_negotiate_packet_size_name).c_str(), + Arena::SetNodeValue( + device->GetTLStreamNodeMap(), + stream_auto_negotiate_packet_size_name.c_str(), + this->config.lucid.stream_auto_negotiate_packet_size);); + + // Note that this modifies the TLStreamNodeMap and not the standard NodeMap + const std::string stream_packet_resend_enable_name = "StreamPacketResendEnable"; + CATCH_ARENA_EXCEPTION((std::string("setting ") + stream_packet_resend_enable_name).c_str(), + Arena::SetNodeValue( + device->GetTLStreamNodeMap(), + stream_packet_resend_enable_name.c_str(), + this->config.lucid.stream_packet_resend_enable);); + + const std::string target_brightness_name = "TargetBrightness"; + CATCH_ARENA_EXCEPTION((std::string("setting ") + target_brightness_name).c_str(), + Arena::SetNodeValue( + device->GetNodeMap(), + target_brightness_name.c_str(), + this->config.lucid.target_brightness);); + + const std::string gamma_enable_name = "GammaEnable"; + CATCH_ARENA_EXCEPTION((std::string("setting ") + gamma_enable_name).c_str(), + Arena::SetNodeValue( + device->GetNodeMap(), + gamma_enable_name.c_str(), + this->config.lucid.gamma_enable);); + + const std::string gamma_name = "Gamma"; + CATCH_ARENA_EXCEPTION((std::string("setting ") + gamma_name).c_str(), + Arena::SetNodeValue( + device->GetNodeMap(), + gamma_name.c_str(), + this->config.lucid.gamma);); + + const std::string gain_auto_name = "GainAuto"; + CATCH_ARENA_EXCEPTION((std::string("setting ") + gain_auto_name).c_str(), + Arena::SetNodeValue( + device->GetNodeMap(), + gain_auto_name.c_str(), + this->config.lucid.gain_auto.c_str());); + + const std::string gain_auto_upper_limit_name = "GainAutoUpperLimit"; + CATCH_ARENA_EXCEPTION((std::string("setting ") + gain_auto_upper_limit_name).c_str(), + Arena::SetNodeValue( + device->GetNodeMap(), + gain_auto_upper_limit_name.c_str(), + this->config.lucid.gain_auto_upper_limit);); + + const std::string gain_auto_lower_limit_name = "GainAutoLowerLimit"; + CATCH_ARENA_EXCEPTION((std::string("setting ") + gain_auto_lower_limit_name).c_str(), + Arena::SetNodeValue( + device->GetNodeMap(), + gain_auto_lower_limit_name.c_str(), + this->config.lucid.gain_auto_lower_limit);); + + const std::string exposure_auto_name = "ExposureAuto"; + CATCH_ARENA_EXCEPTION((std::string("setting ") + exposure_auto_name).c_str(), + Arena::SetNodeValue( + device->GetNodeMap(), + exposure_auto_name.c_str(), + this->config.lucid.exposure_auto.c_str());); + + const std::string exposure_time_name = "ExposureTime"; + CATCH_ARENA_EXCEPTION((std::string("setting ") + exposure_time_name).c_str(), + Arena::SetNodeValue( + device->GetNodeMap(), + exposure_time_name.c_str(), + this->config.lucid.exposure_time);); + + const std::string exposure_auto_damping_name = "ExposureAutoDamping"; + CATCH_ARENA_EXCEPTION((std::string("setting ") + exposure_auto_damping_name).c_str(), + Arena::SetNodeValue( + device->GetNodeMap(), + exposure_auto_damping_name.c_str(), + this->config.lucid.exposure_auto_damping);); + + const std::string exposure_auto_algorithm_name = "ExposureAutoAlgorithm"; + CATCH_ARENA_EXCEPTION((std::string("setting ") + exposure_auto_algorithm_name).c_str(), + Arena::SetNodeValue( + device->GetNodeMap(), + exposure_auto_algorithm_name.c_str(), + this->config.lucid.exposure_auto_algorithm.c_str());); + + const std::string exposure_auto_upper_limit_name = "ExposureAutoUpperLimit"; + CATCH_ARENA_EXCEPTION((std::string("setting ") + exposure_auto_upper_limit_name).c_str(), + Arena::SetNodeValue( + device->GetNodeMap(), + exposure_auto_upper_limit_name.c_str(), + this->config.lucid.exposure_auto_upper_limit);); + + const std::string exposure_auto_lower_limit_name = "ExposureAutoLowerLimit"; + CATCH_ARENA_EXCEPTION((std::string("setting ") + exposure_auto_lower_limit_name).c_str(), + Arena::SetNodeValue( + device->GetNodeMap(), + exposure_auto_lower_limit_name.c_str(), + this->config.lucid.exposure_auto_lower_limit);); +} + +std::optional raspCam::getLatestImage() { + ReadLock lock(this->imageQueueLock); + ImageData lastImage = this->imageQueue.front(); + this->imageQueue.pop_front(); + return lastImage; +} + +std::deque raspCam::getAllImages() { + ReadLock lock(this->imageQueueLock); + std::deque outputQueue = this->imageQueue; + this->imageQueue = std::deque(); + return outputQueue; +} + +bool raspCam::isConnected() { + if (this->device == nullptr) { + return false; + } + + ReadLock lock(this->arenaDeviceLock); + CATCH_ARENA_EXCEPTION("checking camera connection", + return this->device->IsConnected();); + return false; +} + +void raspCam::captureEvery(const std::chrono::milliseconds& interval) { + loguru::set_thread_name("raspiberry pi camera"); + if (!this->isConnected()) { + LOG_F(ERROR, "LUCID Camera not connected. Cannot capture photos"); + return; + } + + this->arenaDeviceLock.lock(); + CATCH_ARENA_EXCEPTION("starting stream", + this->device->StartStream();); + this->arenaDeviceLock.unlock(); + + while (this->isTakingPictures) { + LOG_F(INFO, "Taking picture with RPI camera"); + std::optional newImage = + this->takePicture(this->takePictureTimeout); + + if (newImage.has_value()) { + WriteLock lock(this->imageQueueLock); + this->imageQueue.push_back(newImage.value()); + lock.unlock(); + LOG_F(INFO, "Took picture with RPI camera"); + } else { + LOG_F(ERROR, "Unable to take picture. Trying again in %ld ms", interval.count()); + } + + std::this_thread::sleep_for(interval); + } + + // TODO figure out nondeterministic seg fault /shrug + // this->arenaDeviceLock.lock(); + // CATCH_ARENA_EXCEPTION("stopping stream", + // this->device->StopStream();); + // this->arenaDeviceLock.unlock(); +} + +std::optional raspCam::takePicture(const std::chrono::milliseconds& timeout, + std::shared_ptr mavlinkClient) { + if (!this->isConnected()) { + LOG_F(ERROR, "LUCID Camera not connected. Cannot take picture"); + return {}; + } + + WriteLock lock(this->arenaDeviceLock); + + // need this SINGLE_ARG macro because otherwise the preprocessor gets confused + // by commas in the code and thinks that additional arguments are passed in + CATCH_ARENA_EXCEPTION("getting image", SINGLE_ARG( + Arena::IImage* pImage = this->device->GetImage(timeout.count()); + std::optional telemetry = queryMavlinkImageTelemetry(mavlinkClient); + uint64_t timestamp = getUnixTime_s().count(); + + static int imageCounter = 0; + LOG_F(INFO, "Taking image: %d", imageCounter++); + LOG_F(INFO, "Missed packet: %ld", + Arena::GetNodeValue(device->GetTLStreamNodeMap(), "StreamMissedPacketCount")); + + LOG_F(WARNING, "Image buffer size: %lu", pImage->GetSizeOfBuffer()); + if (pImage->IsIncomplete()) { + LOG_F(ERROR, "Image has incomplete data"); + // TODO: determine if we want to return images with incomplete data + // return {}; + } + + std::optional mat = imgConvert(pImage); + + this->device->RequeueBuffer(pImage); // frees the data of pImage + + if (!mat.has_value()) { + return {}; + } + + // TODO: replace with mavlink telemtry + return ImageData{ + .DATA = mat.value(), + .TIMESTAMP = timestamp, + .TELEMETRY = telemetry + };)); + + // return nullopt if an exception is thrown while getting image + return {}; +} + +void raspCam::startStreaming() { + if (!this->isConnected()) { + LOG_F(ERROR, "LUCID Camera not connected. Cannot start streaming"); + return; + } + + WriteLock lock(this->arenaDeviceLock); + CATCH_ARENA_EXCEPTION("starting stream", + this->device->StartStream();); +} + +std::optional raspCam::imgConvert(Arena::IImage* pImage) { + CATCH_ARENA_EXCEPTION("converting Arena Image to OpenCV", + // convert original image to BGR8 which is what opencv expects. + // note that this allocates a new image buffer that must be + // freed by Arena::ImageFactory::Destroy + Arena::IImage *pConverted = Arena::ImageFactory::Convert( + pImage, + BGR8); + + // create an image using the raw data from the converted BGR image. + // note that we need to clone this cv::Mat since otherwise the underlying + // data gets freed by Arena::ImageFactory::Destroy + cv::Mat mat = cv::Mat( + static_cast(pConverted->GetHeight()), + static_cast(pConverted->GetWidth()), + CV_8UC3, + // welcome to casting hell :) + const_cast(reinterpret_cast(pConverted->GetData()))) + .clone(); + + // freeing underlying lucid buffers + Arena::ImageFactory::Destroy(pConverted); + + return mat;); + + // return nullopt if an exception is thrown during conversion + return {}; +} + +#endif // ARENA_SDK_INSTALLED diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index e8675ea5..de73b73c 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -37,4 +37,4 @@ target_add_loguru(${LIB_NAME}) target_add_onnxruntime(${LIB_NAME}) target_include_directories(${LIB_NAME} PRIVATE ${ImageMagick_INCLUDE_DIRS}) -target_link_libraries(${LIB_NAME} PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) \ No newline at end of file +target_link_libraries(${LIB_NAME} PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) diff --git a/src/network/CMakeLists.txt b/src/network/CMakeLists.txt index 69ad91f0..3153c33a 100644 --- a/src/network/CMakeLists.txt +++ b/src/network/CMakeLists.txt @@ -5,6 +5,10 @@ set(COMMON_FILES gcs_routes.cpp gcs.cpp mavlink.cpp + tcp_client.cpp + tcp_server.cpp + udp_client.cpp + udp_server.cpp ) set(FILES @@ -44,6 +48,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_add_onnxruntime(${LIB_NAME}) target_include_directories(${LIB_NAME} PRIVATE ${ImageMagick_INCLUDE_DIRS}) @@ -70,6 +75,8 @@ 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}) \ No newline at end of file +target_link_libraries(${LIB_NAME} PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) diff --git a/src/network/tcp_client.cpp b/src/network/tcp_client.cpp new file mode 100644 index 00000000..7bd556ce --- /dev/null +++ b/src/network/tcp_client.cpp @@ -0,0 +1,81 @@ +#include +#include "network/tcp_client.hpp" + + +Client::Client(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_) { + + this->ip = ip; + this->port = port; + +} + +bool Client::connect() { + boost::system::error_code ec; + asio::ip::tcp::endpoint endpoint_(asio::ip::tcp::endpoint(asio::ip::make_address(this->ip), this->port)); + + // TODO: what to do if failed to connect? do we keep retrying? + this->socket_.connect(endpoint_, ec); + + if (ec) { + LOG_F(INFO, std::string("Failed to connect: " + ec.message()).c_str()); + return false; + } + + LOG_F(INFO, std::string("Connected to: " + this->ip + " on port: " + std::to_string(this->port)).c_str()); + + return true; +} + +bool Client::send(std::uint8_t request) { + boost::system::error_code ec; + + LOG_F(INFO, std::string("Sending request" + static_cast(request)).c_str()); + + int bytesSent = asio::write(this->socket_, asio::buffer(&request, sizeof(std::uint8_t)), ec); + + if (ec) { + // TODO: what do we do if we fail to send a request? keep retrying or drop that request? + LOG_F(INFO, std::string("Failed to send request: " + ec.message()).c_str()); + return false; + } + + LOG_F(INFO, std::string("Bytes sent: " + bytesSent).c_str()); + return true; +} + +Header Client::recvHeader() { + boost::system::error_code ec; + Header header; + + // TODO: might have to specify 12 bytes + int bytesRead = asio::read(this->socket_, asio::buffer(&header, sizeof(Header)), ec); + + if (ec) { + // TODO: what to do when read fails + LOG_F(INFO, std::string("Failed to read header: " + ec.message()).c_str()); + } + + LOG_F(INFO, std::string("Bytes read (header): " + bytesRead).c_str()); + + return header; +} + +std::vector Client::recvBody(const int bufSize) { + + boost::system::error_code ec; + + LOG_F(INFO, std::string("Reading in bufSize (body): " + std::to_string(bufSize)).c_str()); + + std::vector recvbuf(bufSize); + + int bytesRead = asio::read(this->socket_, asio::buffer(recvbuf), ec); + + if (ec) { + // TODO: what to do when read fails + LOG_F(INFO, std::string("Failed to read body: " + ec.message()).c_str()); + } + + LOG_F(INFO, std::string("Bytes read (body): " + bytesRead).c_str()); + + return recvbuf; +} diff --git a/src/network/tcp_server.cpp b/src/network/tcp_server.cpp new file mode 100644 index 00000000..daa8806c --- /dev/null +++ b/src/network/tcp_server.cpp @@ -0,0 +1,101 @@ +#include "network/tcp_server.hpp" + +Server::Server(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_), acceptor_(*io_context_, asio::ip::tcp::endpoint(asio::ip::make_address(ip), port)) { + this->ip = ip; + this->port = port; +} + +void Server::start() { + + boost::system::error_code ec; + + this->acceptor_.accept(this->socket_, ec); + + if (ec) { + std::cout << "Error code: " << ec.value() << '\n'; + } + + std::cout << "Listening on port: " << this->port << '\n'; + + recv(); +} + +cv::Mat Server::createBGR() { + cv::Mat image = cv::imread("blurb.jpeg"); + if (image.empty()) { + std::cerr << "Failed to load image. Check path: blurb.jpeg\n"; + exit(1); // or return a default image + } + std::cout << "Image is not empty" << '\n'; + return image; +} + +cv::Mat Server::createYUV() { + // read in an image + cv::Mat image = cv::imread("blurb.jpeg"); + + // convert into yuv420 + cv::Mat yuv_img(IMG_HEIGHT * 3 / 2, IMG_WIDTH, CV_8UC1); + cv::cvtColor(image, yuv_img, cv::COLOR_BGR2YUV_I420); + + return yuv_img; +} + +void Server::send() { + boost::system::error_code ec; + + std::cout << "creating the image" << '\n'; + + // cv::Mat img = createBGR(); + cv::Mat img = createYUV(); + + std:: cout << "putting image into buffer" << '\n'; + + // put the image into a buffer + cv::Mat flatten = img.reshape(1, img.total() * img.channels()); + std::vector imgBuffer = img.isContinuous() ? flatten : flatten.clone(); + + // check the size of the buffer + if (imgBuffer.size() != BUFFER_SIZE) { + std::cout << "size: " << imgBuffer.size() << " expected: " << BUFFER_SIZE << '\n'; + return; + } + + // write the buffer + asio::write(this->socket_, asio::buffer(imgBuffer), ec); + + if (ec) { + std::cout << "server send failed" << '\n'; + } +} + +void Server::recv() { + boost::system::error_code ec; + + // read in request, will be a char + + char request; + + asio::read(this->socket_, asio::buffer(&request, sizeof(char)), ec); + + if (ec) { + // TODO: what to do if can't read the request + } + + std::cout << "Request received: " << request << '\n'; + handleRequest(request); +} + +void Server::handleRequest(char request) { + if (request == 'p') { + // take a picture + this->send(); + } else if (request == 'e') { + // close the connection + this->shutdown(); + } +} + +void Server::shutdown() { + std::cout << "shutting down" << '\n'; +} diff --git a/src/network/udp_client.cpp b/src/network/udp_client.cpp new file mode 100644 index 00000000..8f5dd291 --- /dev/null +++ b/src/network/udp_client.cpp @@ -0,0 +1,111 @@ +#include "network/udp_client.hpp" + +UDPClient::UDPClient(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_) { + this->ip = ip; + this->port = port; +} + +bool UDPClient::connect() { + boost::system::error_code ec; + asio::ip::udp::endpoint endpoint_(asio::ip::udp::endpoint(asio::ip::make_address(this->ip), this->port)); + + // open the UDP socket using ip::udp::v4() + this->socket_.open(asio::ip::udp::v4(), ec); + + if (ec) { + LOG_F(ERROR, "Failed to connect"); + return false; + } + + LOG_F(INFO, "Connected to %s on port %d", this->ip.c_str(), this->port); + + return true; +} + +// send a request to the server +bool UDPClient::send(std::uint8_t request) { + boost::system::error_code ec; + asio::ip::udp::endpoint endpoint_(asio::ip::udp::endpoint(asio::ip::make_address(this->ip), this->port)); + + LOG_F(INFO, "Sending request %c", request); + + int bytesSent = this->socket_.send_to(asio::buffer(&request, sizeof(request)), endpoint_, 0, ec); + + if (ec) { + LOG_F(ERROR, "Failed to send request: %s", ec.message().c_str()); + return false; + } + + LOG_F(INFO, "Sent %d bytes", bytesSent); + + return true; +} + +// receive an image back from the server +Header UDPClient::recvHeader() { + boost::system::error_code ec; + // asio::ip::udp::endpoint endpoint_(asio::ip::udp::endpoint(asio::ip::make_address(this->ip), this->port)); + asio::ip::udp::endpoint sender_endpoint; + + Header header; + + int bytesRead = this->socket_.receive_from(asio::buffer(&header, sizeof(Header)), sender_endpoint, 0, ec); + + if (ec) { + LOG_F(ERROR, "Failed to read header: %s", ec.message().c_str()); + return {}; + } + + LOG_F(INFO, "Bytes read (header): %d", bytesRead); + + return header; +} + +std::vector UDPClient::recvBody(const int mem_size, const int total_chunks) { + boost::system::error_code ec; + // asio::ip::udp::endpoint endpoint_(asio::ip::udp::endpoint(asio::ip::make_address(this->ip), this->port)); + asio::ip::udp::endpoint sender_endpoint; + + const int bufSize = mem_size * total_chunks; + int totalBytesRead = 0; + + LOG_F(INFO, "Reading in bufSize (body): %d", bufSize); + + std::vector buf(bufSize); + + int totalChunks = 0; + + for (int i = 0; i < total_chunks; i++) { + char chunk_buf[CHUNK_SIZE + sizeof(uint32_t)]; + + int bytesRead = this->socket_.receive_from(asio::buffer(chunk_buf), sender_endpoint, 0, ec); + + int chunk_idx = ntohl(*reinterpret_cast(chunk_buf)); + int data_size = bytesRead - sizeof(uint32_t); + int offset = chunk_idx * CHUNK_SIZE; + + // copy into buffer + memcpy(buf.data() + offset, chunk_buf + sizeof(uint32_t), data_size); + + totalBytesRead += data_size; + totalChunks += 1; + + // LOG_F(INFO, "Chunk: %d, Total bytes read: %d, Total chunks: %d", chunk_idx, totalBytesRead, totalChunks); + } + + if (ec) { + LOG_F(ERROR, "Failed to send body: %s", ec.message().c_str()); + return {}; + } + + if (totalBytesRead != bufSize) { + LOG_F(ERROR, "Total bytes read: %d, Expected bytes: %d", totalBytesRead, bufSize); + return {}; // TODO: not sure what to return here, incomplete read + } + + LOG_F(INFO, "Bytes read: %d", totalBytesRead); + + LOG_F(INFO, "Buffer size: %lu", buf.size()); + + return buf; +} diff --git a/src/network/udp_server.cpp b/src/network/udp_server.cpp new file mode 100644 index 00000000..97a7943d --- /dev/null +++ b/src/network/udp_server.cpp @@ -0,0 +1,303 @@ +#include +#include "network/udp_server.hpp" + +cv::Mat UDPServer::createBGR() { + std::cout << std::filesystem::current_path() << '\n'; + cv::Mat image = cv::imread("/workspaces/obcpp/tests/integration/images/blurb.jpeg"); + if (image.empty()) { + std::cerr << "Failed to load image. Check path: blurb.jpeg\n"; + exit(1); // or return a default image + } + std::cout << "Image is not empty" << '\n'; + return image; +} + +cv::Mat UDPServer::createYUV() { + std::cout << std::filesystem::current_path() << '\n'; + // read in an image + cv::Mat image = cv::imread("/workspaces/obcpp/tests/integration/images/blurb.jpeg"); + + // convert into yuv420 + cv::Mat yuv_img(IMG_HEIGHT * 3 / 2, IMG_WIDTH, CV_8UC1); + cv::cvtColor(image, yuv_img, cv::COLOR_BGR2YUV_I420); + + return yuv_img; +} + +UDPServer::UDPServer(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_){ + this->ip = ip; + this->port = port; +} + +bool UDPServer::start() { + boost::system::error_code open_ec; + boost::system::error_code bind_ec; + asio::ip::udp::endpoint endpoint_(asio::ip::udp::endpoint(asio::ip::make_address(this->ip), this->port)); + + std::cout << "Attempting to connect to " << this->ip << " on port " << this->port << '\n'; + + // open the udp socket + this->socket_.open(asio::ip::udp::v4(), open_ec); + + if (open_ec) { + std::cout << "Failed to open socket: " << open_ec.message() << '\n'; + return false; + } + + // bind to this address and port + this->socket_.bind(endpoint_, bind_ec); + + std::cout << "Endpoint: " << endpoint_.address() << endpoint_.port() << + '\n'; + + if (bind_ec) { + std::cout << "Failed to bind socket: " << bind_ec.message() << '\n'; + return false; + } + + return true; +} + +void UDPServer::send(asio::ip::udp::endpoint & endpoint) { + // Note: use the endpoint from recv() to send back + + boost::system::error_code header_ec; + boost::system::error_code body_ec; + + std::cout << "Taking picture (reads in image)" << '\n'; + + // cv::Mat img = createBGR(); + cv::Mat img = createYUV(); + + std:: cout << "Putting image into buffer" << '\n'; + + cv::Mat flatten = img.reshape(1, img.total() * img.channels()); + std::vector imgBuffer = img.isContinuous() ? flatten : flatten.clone(); + + if (imgBuffer.size() != BUFFER_SIZE) { + std::cout << "size: " << imgBuffer.size() << " expected: " << BUFFER_SIZE << '\n'; + return; + } + + uint32_t total_chunks = imgBuffer.size() / CHUNK_SIZE; + + Header header; + + header.magic = htonl(EXPECTED_MAGIC); + header.mem_size = htonl(CHUNK_SIZE); + header.total_chunks = htonl(total_chunks); + + // send header + int bytesSentHeader = this->socket_.send_to(asio::buffer(&header, sizeof(header)), endpoint, 0, header_ec); + + if (header_ec) { + std::cout << "Sending header failed: " << header_ec.message() << '\n'; + return; + } + + std::cout << "Read bytes (header): " << bytesSentHeader << '\n'; + + // send body + + const int buf_size = imgBuffer.size(); + + std::cout << "Total chunks: " << imgBuffer.size() / CHUNK_SIZE << '\n'; + + int totalBytesSent = 0; + + for (uint32_t i = 0; i < total_chunks; i++) { + const size_t offset = i * CHUNK_SIZE; + const size_t remaining = buf_size - offset; + const size_t data_size = std::min(CHUNK_SIZE, remaining); + + std::vector packet(sizeof(uint32_t) + data_size); + + // puts chunk index as the header + uint32_t* index_ptr = reinterpret_cast(packet.data()); + *index_ptr = htonl(i); + + + memcpy(packet.data() + sizeof(uint32_t), imgBuffer.data() + offset, data_size); + + std::this_thread::sleep_for(std::chrono::microseconds(50)); + int bytesSentBody = this->socket_.send_to(asio::buffer(packet), endpoint, 0, body_ec); + + totalBytesSent += (bytesSentBody - sizeof(uint32_t)); + + if (body_ec) { + std::cout << "Sending body failed: " << body_ec.message() << '\n'; + return; + } + + } + + std::cout << "Finished sending " << totalBytesSent << " bytes" << '\n'; + + // int bytesSentBody = this->socket_.send_to(asio::buffer(imgBuffer), endpoint, 0, body_ec); + + // std::cout << "Read bytes (body): " << bytesSentBody << '\n'; + +} + +// expecting a request from the client +void UDPServer::recv() { + // + boost::system::error_code ec; + asio::ip::udp::endpoint client_endpoint; + + char request; + + int bytesRead = this->socket_.receive_from(asio::buffer(&request, sizeof(request)), client_endpoint, 0, ec); + + if (ec) { + std::cout << "Failed to read request: " << ec.message() << '\n'; + } + + std::cout << "Bytes read (request): " << bytesRead << '\n'; + + handleRequest(request, client_endpoint); +} + +void UDPServer::handleRequest(char request, asio::ip::udp::endpoint & endpoint) { + if (request == 'I') { + this->send(endpoint); + } else if (request == 'e') { + this->shutdown(); + } else { + std::cout << "Invalid request: " << request << '\n'; + } +} + +void UDPServer::shutdown() { + std::cout << "shutting down" << '\n'; +} + +// cv::Mat UDPServer::createBGR() { +// cv::Mat image = cv::imread("blurb.jpeg"); +// if (image.empty()) { +// std::cerr << "Failed to load image. Check path: blurb.jpeg\n"; +// exit(1); // or return a default image +// } +// std::cout << "Image is not empty" << '\n'; +// return image; +// } + +// cv::Mat UDPServer::createYUV() { +// // read in an image +// cv::Mat image = cv::imread("blurb.jpeg"); + +// // convert into yuv420 +// cv::Mat yuv_img(IMG_HEIGHT * 3 / 2, IMG_WIDTH, CV_8UC1); +// cv::cvtColor(image, yuv_img, cv::COLOR_BGR2YUV_I420); + +// return yuv_img; +// } + +// UDPServer::UDPServer(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_){ +// this->ip = ip; +// this->port = port; +// } + +// bool UDPServer::start() { +// boost::system::error_code open_ec; +// boost::system::error_code bind_ec; +// asio::ip::udp::endpoint endpoint_(asio::ip::udp::endpoint(asio::ip::make_address(this->ip), this->port)); + +// std::cout << ("Attempting to connect to %s on port %d", this->ip, this->port) << '\n'; + +// // open the udp socket +// this->socket_.open(asio::ip::udp::v4(), open_ec); + +// if (open_ec) { +// std::cout << ("Failed to open socket: %s", open_ec.message()) << '\n'; +// return false; +// } + +// // bind to this address and port +// this->socket_.bind(endpoint_, bind_ec); + +// if (bind_ec) { +// std::cout << ("Failed to bind socket: %s", bind_ec.message()) << '\n'; +// return false; +// } + +// return true; +// } + +// void UDPServer::send(asio::ip::udp::endpoint & endpoint) { +// // Note: use the endpoint from recv() to send back + +// boost::system::error_code header_ec; +// boost::system::error_code body_ec; + +// std::cout << "Taking picture (reads in image)" << '\n'; + +// // cv::Mat img = createBGR(); +// cv::Mat img = createYUV(); + +// std:: cout << "Putting image into buffer" << '\n'; + +// cv::Mat flatten = img.reshape(1, img.total() * img.channels()); +// std::vector imgBuffer = img.isContinuous() ? flatten : flatten.clone(); + +// if (imgBuffer.size() != BUFFER_SIZE) { +// std::cout << "size: " << imgBuffer.size() << " expected: " << BUFFER_SIZE << '\n'; +// return; +// } + +// Header header; + +// header.magic = htonl(EXPECTED_MAGIC); +// header.mem_size = htonl(CHUNK_SIZE); +// header.total_chunks = htonl(imgBuffer.size() / CHUNK_SIZE); + +// // send header +// int bytesReadHeader = this->socket_.send_to(asio::buffer(&header, sizeof(header)), endpoint, 0, header_ec); + +// if (header_ec) { +// std::cout << ("Sending header failed: %s", header_ec.message()) << '\n'; +// return; +// } + +// // send body +// int bytesReadBody = this->socket_.send_to(asio::buffer(imgBuffer), endpoint, 0, body_ec); + +// if (body_ec) { +// std::cout << ("Sending body failed: %s", body_ec.message()) << '\n'; +// return; +// } + +// } + +// // expecting a request from the client +// void UDPServer::recv() { +// // +// boost::system::error_code ec; +// asio::ip::udp::endpoint client_endpoint; + +// char request; + +// int bytesRead = this->socket_.receive_from(asio::buffer(&request, sizeof(request)), client_endpoint, 0, ec); + +// if (ec) { +// std::cout << ("Failed to read request: %s", ec.message()) << '\n'; +// } + +// std::cout << ("Bytes read (request): %d", bytesRead) << '\n'; + +// handleRequest(request, client_endpoint); +// } + +// void UDPServer::handleRequest(char request, asio::ip::udp::endpoint & endpoint) { +// if (request == 'p') { +// this->send(endpoint); +// } else if (request == 'e') { +// this->shutdown(); +// } else { +// std::cout << ("Invalid request: %c", request) << '\n'; +// } +// } + +// void UDPServer::shutdown() { +// std::cout << "shutting down" << '\n'; +// } diff --git a/tests/integration/CMakeLists.txt b/tests/integration/CMakeLists.txt index b8594b1c..208903c0 100644 --- a/tests/integration/CMakeLists.txt +++ b/tests/integration/CMakeLists.txt @@ -134,9 +134,85 @@ target_add_loguru(airdrop_packets) # add_executable(airdrop_sockets src/network/airdrop_sockets.c tests/integration/airdrop_sockets.c) # target_include_directories(airdrop_sockets PRIVATE ${INCLUDE_DIRECTORY}) -# cuda_check -add_executable(cuda_check ${SOURCES} cuda_check.cpp) -target_add_torch(cuda_check) +# add_executable(camera_playground camera_playground.cpp) +# target_include_directories(camera_playground PRIVATE ${INCLUDE_DIRECTORY}) +# add_custom_target( +# run_camera_playground +# ${CMAKE_COMMAND} +# -E env LD_LIBRARY_s/target_siamese_1.pt +# ) + +add_executable(tcp_client_mock "tcp_client_mock.cpp") +target_link_libraries(tcp_client_mock PRIVATE + obcpp_lib +) +target_include_directories(tcp_client_mock PRIVATE ${INCLUDE_DIRECTORY}) +target_add_boost(tcp_client_mock) +target_add_protobuf(tcp_client_mock) +target_add_torch(tcp_client_mock) +target_add_torchvision(tcp_client_mock) +target_add_json(tcp_client_mock) +target_add_opencv(tcp_client_mock) +target_add_httplib(tcp_client_mock) +target_add_mavsdk(tcp_client_mock) +target_add_matplot(tcp_client_mock) +target_add_loguru(tcp_client_mock) +target_include_directories(tcp_client_mock PRIVATE ${ImageMagick_INCLUDE_DIRS}) +target_link_libraries(tcp_client_mock PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) + +add_executable(udp_client_mock "udp_client_mock.cpp") +target_link_libraries(udp_client_mock PRIVATE + obcpp_lib +) +target_include_directories(udp_client_mock PRIVATE ${INCLUDE_DIRECTORY}) +target_add_boost(udp_client_mock) +target_add_protobuf(udp_client_mock) +target_add_torch(udp_client_mock) +target_add_torchvision(udp_client_mock) +target_add_json(udp_client_mock) +target_add_opencv(udp_client_mock) +target_add_httplib(udp_client_mock) +target_add_mavsdk(udp_client_mock) +target_add_matplot(udp_client_mock) +target_add_loguru(udp_client_mock) +target_include_directories(udp_client_mock PRIVATE ${ImageMagick_INCLUDE_DIRS}) +target_link_libraries(udp_client_mock PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) + +add_executable(tcp_server_mock "tcp_server_mock.cpp") +target_link_libraries(tcp_server_mock PRIVATE + obcpp_lib +) +target_include_directories(tcp_server_mock PRIVATE ${INCLUDE_DIRECTORY}) +target_add_boost(tcp_server_mock) +target_add_protobuf(tcp_server_mock) +target_add_torch(tcp_server_mock) +target_add_torchvision(tcp_server_mock) +target_add_json(tcp_server_mock) +target_add_opencv(tcp_server_mock) +target_add_httplib(tcp_server_mock) +target_add_mavsdk(tcp_server_mock) +target_add_matplot(tcp_server_mock) +target_add_loguru(tcp_server_mock) +target_include_directories(tcp_server_mock PRIVATE ${ImageMagick_INCLUDE_DIRS}) +target_link_libraries(tcp_server_mock PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) + +add_executable(udp_server_mock "udp_server_mock.cpp") +target_link_libraries(udp_server_mock PRIVATE + obcpp_lib +) +target_include_directories(udp_server_mock PRIVATE ${INCLUDE_DIRECTORY}) +target_add_boost(udp_server_mock) +target_add_protobuf(udp_server_mock) +target_add_torch(udp_server_mock) +target_add_torchvision(udp_server_mock) +target_add_json(udp_server_mock) +target_add_opencv(udp_server_mock) +target_add_httplib(udp_server_mock) +target_add_mavsdk(udp_server_mock) +target_add_matplot(udp_server_mock) +target_add_loguru(udp_server_mock) +target_include_directories(udp_server_mock PRIVATE ${ImageMagick_INCLUDE_DIRS}) +target_link_libraries(udp_server_mock PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) add_executable(drop_it_like_its_hot "drop_it_like_its_hot.cpp") target_link_libraries(drop_it_like_its_hot PRIVATE obcpp_lib) @@ -158,4 +234,4 @@ add_executable(clustering "clustering.cpp") target_link_libraries(clustering PRIVATE obcpp_lib obcpp_camera) target_add_matplot(clustering) target_add_json(clustering) -target_add_protobuf(clustering) \ No newline at end of file +target_add_protobuf(clustering) diff --git a/tests/integration/camera_lucid.cpp b/tests/integration/camera_lucid.cpp new file mode 100644 index 00000000..0ff71e85 --- /dev/null +++ b/tests/integration/camera_lucid.cpp @@ -0,0 +1,48 @@ +#include +#include + +#include +#include + +#include "camera/interface.hpp" +// #include "camera/lucid.hpp" +#include "core/mission_state.hpp" +#include "network/mavlink.hpp" +#include "utilities/common.hpp" + +using namespace std::chrono_literals; + +int main (int argc, char *argv[]) { + if (argc < 2) { + LOG_F(ERROR, "Usage: ./bin/camera_lucid [path_to_config] [optional: output_dir]"); + exit(1); + } + std::filesystem::path output_dir = std::filesystem::current_path(); + if (argc >= 3) { + output_dir = argv[2]; + } + OBCConfig config(argc, argv); + + auto mav = std::make_shared("serial:///dev/ttyACM0"); + + LucidCamera camera(config.camera); + + camera.connect(); + LOG_F(INFO, "Connected to LUCID camera!"); + + camera.startTakingPictures(1s, mav); + + // need to sleep to let camera background thread to run + std::this_thread::sleep_for(10s); + camera.stopTakingPictures(); + + std::deque images = camera.getAllImages(); + for (const ImageData& image : images) { + std::filesystem::path filepath = config.camera.save_dir / std::to_string(image.TIMESTAMP); + saveImageToFile(image.DATA, filepath); + if (image.TELEMETRY.has_value()) { + saveImageTelemetryToFile(image.TELEMETRY.value(), filepath); + } + LOG_F(INFO, "Saving LUCID image to %s", filepath.string().c_str()); + } +} diff --git a/tests/integration/tcp_client_mock.cpp b/tests/integration/tcp_client_mock.cpp new file mode 100644 index 00000000..a29fcaf5 --- /dev/null +++ b/tests/integration/tcp_client_mock.cpp @@ -0,0 +1,30 @@ +#include +#include +#include +#include +#include "network/tcp_client.hpp" +#include "camera/rpi.hpp" +#include "utilities/obc_config.hpp" +#include "core/mission_state.hpp" +#include "utilities/common.hpp" + +int main(int argc, char* argv[]) { + + CameraConfig config; + + asio::io_context io_context_; + + RPICamera camera(config, &io_context_); + + camera.connect(); + + std::optional img = camera.takePicture(std::chrono::milliseconds(1000), nullptr); + + if (img.has_value()) { + std::filesystem::path base_dir = "/workspaces/obcpp/images"; + std::filesystem::path filepath = base_dir / std::to_string(img.value().TIMESTAMP); + img.value().saveToFile(filepath); + } + + return 0; +} diff --git a/tests/integration/tcp_server_mock.cpp b/tests/integration/tcp_server_mock.cpp new file mode 100644 index 00000000..147df75e --- /dev/null +++ b/tests/integration/tcp_server_mock.cpp @@ -0,0 +1,22 @@ +#include +#include +#include +#include "network/tcp_server.hpp" + +namespace asio = boost::asio; + +const std::string SERVER_IP = "127.0.0.1"; +const int SERVER_PORT = 5000; + +int main() { + + asio::io_context io_context_; + Server server(&io_context_, SERVER_IP, SERVER_PORT); + + //for (;;) { + // start the server in a while loop + server.start(); + //} + + return 0; +} diff --git a/tests/integration/udp_client_mock.cpp b/tests/integration/udp_client_mock.cpp new file mode 100644 index 00000000..457e2841 --- /dev/null +++ b/tests/integration/udp_client_mock.cpp @@ -0,0 +1,35 @@ +#include +#include +#include +#include +#include +#include "network/udp_client.hpp" +#include "camera/rpi.hpp" +#include "utilities/obc_config.hpp" +#include "core/mission_state.hpp" +#include "utilities/common.hpp" + +int main(int argc, char* argv[]) { + + CameraConfig config; + + asio::io_context io_context_; + + RPICamera camera(config, &io_context_); + + camera.connect(); + + std::optional img = camera.takePicture(std::chrono::milliseconds(1000), nullptr); + + if (img.has_value()) { + LOG_F(INFO, "Image has value"); + // cv::imshow("img", img.value().DATA); + // cv::waitKey(0); + std::filesystem::path base_dir = "/workspaces/obcpp/images"; + std::filesystem::path filepath = base_dir / std::to_string(img.value().TIMESTAMP) / ".jpg"; + img.value().saveToFile(std::filesystem::current_path()); // TODO: able to save images but idk what the correct file path should be + std::cout << "Saving image to: " << filepath << std::endl; + } + + return 0; +} diff --git a/tests/integration/udp_server_mock.cpp b/tests/integration/udp_server_mock.cpp new file mode 100644 index 00000000..26227696 --- /dev/null +++ b/tests/integration/udp_server_mock.cpp @@ -0,0 +1,24 @@ +#include +#include +#include +#include "network/udp_server.hpp" + +namespace asio = boost::asio; + +const std::string IP = "0.0.0.0"; +const int PORT = 5000; + +int main() { + asio::io_context io_context_; + UDPServer server(&io_context_, SERVER_IP, SERVER_PORT); + + // for (;;) { + // start the server in a while loop + server.start(); + // } + + + for (;;) { + server.recv(); + } +}