From 864111e0a1660c29517a401ecc6671c4bdf2e247 Mon Sep 17 00:00:00 2001 From: mountainduu Date: Fri, 24 Jan 2025 18:10:44 -0800 Subject: [PATCH 01/43] started on the rpi interface --- include/camera/{lucid.hpp => rpi.hpp} | 5 +- include/camera/rpi.hpp~ | 149 +++++++++++ src/camera/rpi.cpp | 360 ++++++++++++++++++++++++++ src/camera/{lucid.cpp => rpi.cpp~} | 38 ++- 4 files changed, 528 insertions(+), 24 deletions(-) rename include/camera/{lucid.hpp => rpi.hpp} (97%) create mode 100644 include/camera/rpi.hpp~ create mode 100644 src/camera/rpi.cpp rename src/camera/{lucid.cpp => rpi.cpp~} (91%) diff --git a/include/camera/lucid.hpp b/include/camera/rpi.hpp similarity index 97% rename from include/camera/lucid.hpp rename to include/camera/rpi.hpp index ba2fb25d..e80f8c67 100644 --- a/include/camera/lucid.hpp +++ b/include/camera/rpi.hpp @@ -1,6 +1,3 @@ -#ifndef INCLUDE_CAMERA_LUCID_HPP_ -#define INCLUDE_CAMERA_LUCID_HPP_ - #ifdef ARENA_SDK_INSTALLED #include @@ -29,7 +26,7 @@ using namespace std::chrono_literals; // NOLINT * This class is thread safe, meaning that you can access camera resources * across different threads even though there's one physical camera. */ -class LucidCamera : public CameraInterface { +class RaspPi : public CameraInterface { public: explicit LucidCamera(CameraConfig config); ~LucidCamera(); diff --git a/include/camera/rpi.hpp~ b/include/camera/rpi.hpp~ new file mode 100644 index 00000000..e80f8c67 --- /dev/null +++ b/include/camera/rpi.hpp~ @@ -0,0 +1,149 @@ +#ifdef ARENA_SDK_INSTALLED + +#include + +#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 + +/** + * LucidCamera is the class to interface with cameras from + * LUCID Vision Labs (https://thinklucid.com/) that use their + * Arena SDK (https://thinklucid.com/arena-software-development-kit/). + * + * This class is thread safe, meaning that you can access camera resources + * across different threads even though there's one physical camera. +*/ +class RaspPi : public CameraInterface { + public: + explicit LucidCamera(CameraConfig config); + ~LucidCamera(); + + /** + * Connect to the LUCID camera. Note that this function will synchronously + * block indefintely until a connection to the camera can be established. + */ + void connect() override; + bool isConnected() override; + + /** + * Start taking photos at an interval in a background thread. + * Also requires a shared_ptr to a MavlinkClient to tag + * images with flight telemetry at capture time. + */ + void startTakingPictures(const std::chrono::milliseconds& interval, + std::shared_ptr mavlinkClient) override; + + /** + * Close background thread started by startTakingPictures + */ + void stopTakingPictures() override; + + /** + * Get the latest image that the camera took. This pops the latest + * image from a queue of images which means that the same image won't. + * be returned in two subsequent calls + */ + std::optional getLatestImage() override; + + /** + * getAllImages returns a queue of all the images that have been + * cached since the last call to getAllImages. Once this is called, + * it returns the cached images and clears the internal cache. + */ + std::deque getAllImages() override; + + /** + * Blocking call that takes an image. If it takes longer than the timeout + * to capture the image, no image is returned. + */ + std::optional takePicture(const std::chrono::milliseconds& timeout, + std::shared_ptr mavlinkClient) override; + void startStreaming() override; + + private: + /** + * Takes an image and sleeps for the specified interval before + * taking another image + */ + void captureEvery(const std::chrono::milliseconds& interval, + std::shared_ptr mavlinkClient); + + /** + * Converts between ArenaSDK Image type to an OpenCV cv::Mat + */ + std::optional imgConvert(Arena::IImage* pImage); + + void configureSettings(); + + // Lock around Arena system. + inline static std::shared_mutex arenaSystemLock; + // Arena system must be static since only a single system + // can be active on the same machine regardless of how + // many LucidCamera instances are created. + inline static Arena::ISystem* system; + + // Lock around Arena device. + inline static std::shared_mutex arenaDeviceLock; + // Arena device must be static since only a single device + // can be active on the same machine regardless of how + // many LucidCamera instances are created. + inline static Arena::IDevice* device; + + std::atomic_bool isTakingPictures; + + std::deque imageQueue; + std::shared_mutex imageQueueLock; + + // thread that will capture images on a set interval + std::thread captureThread; + + std::shared_mutex imageQueueMut; + + const std::chrono::milliseconds connectionTimeout = 1000ms; + const std::chrono::milliseconds connectionRetry = 500ms; + + // TODO: need to catch timeout exception + const std::chrono::milliseconds takePictureTimeout = 1000ms; +}; + +#define SINGLE_ARG(...) __VA_ARGS__ + +/** + * Macro to gracefully catch Arena SDK exceptions, print an error, + * and not propogate the exceptions and crash the OBC. + * + * context is a C string (char*) that explains what the code is doing + * + * code is the code itself +*/ +#define CATCH_ARENA_EXCEPTION(context, code) \ + try { \ + code \ + } \ + catch (GenICam::GenericException &ge) { \ + LOG_F(ERROR, "GenICam exception thrown when %s: %s", context, ge.what()); \ + } \ + catch (std::exception &ex) { \ + LOG_F(ERROR, "Standard exception thrown when %s: %s", context, ex.what()); \ + } \ + catch (...) { \ + LOG_F(ERROR, "Unexpected exception thrown when %s", context); \ + } \ + LOG_F(INFO, "%s succeeded", context) + +#endif // ARENA_SDK_INSTALLED + +#endif // INCLUDE_CAMERA_LUCID_HPP_ diff --git a/src/camera/rpi.cpp b/src/camera/rpi.cpp new file mode 100644 index 00000000..c264fb52 --- /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, "RPI 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/camera/lucid.cpp b/src/camera/rpi.cpp~ similarity index 91% rename from src/camera/lucid.cpp rename to src/camera/rpi.cpp~ index 2e6f2c05..c17943cc 100644 --- a/src/camera/lucid.cpp +++ b/src/camera/rpi.cpp~ @@ -13,7 +13,6 @@ #include #include -#include "camera/lucid.hpp" #include "camera/interface.hpp" #include "network/mavlink.hpp" #include "utilities/locks.hpp" @@ -22,13 +21,13 @@ using json = nlohmann::json; -LucidCamera::LucidCamera(CameraConfig config) : +raspCam::raspCam(CameraConfig config) : CameraInterface(config) { this->connect(); this->startStreaming(); } -void LucidCamera::connect() { +void raspCam::connect() { if (this->isConnected()) { return; } @@ -62,7 +61,7 @@ void LucidCamera::connect() { this->configureSettings(); } -LucidCamera::~LucidCamera() { +raspCam::~raspCam() { // aquire locks to Arena System and Device WriteLock systemLock(this->arenaSystemLock); WriteLock deviceLock(this->arenaDeviceLock); @@ -72,7 +71,7 @@ LucidCamera::~LucidCamera() { Arena::CloseSystem(this->system);); } -void LucidCamera::startTakingPictures(const std::chrono::milliseconds& interval, +void raspCam::startTakingPictures(const std::chrono::milliseconds& interval, std::shared_ptr mavlinkClient) { if (this->isTakingPictures) { return; @@ -80,10 +79,10 @@ void LucidCamera::startTakingPictures(const std::chrono::milliseconds& interval, this->isTakingPictures = true; - this->captureThread = std::thread(&LucidCamera::captureEvery, this, interval, mavlinkClient); + this->captureThread = std::thread(&raspCam::captureEvery, this, interval); } -void LucidCamera::stopTakingPictures() { +void raspCam::stopTakingPictures() { if (!this->isTakingPictures) { return; } @@ -93,7 +92,7 @@ void LucidCamera::stopTakingPictures() { this->captureThread.join(); } -void LucidCamera::configureSettings() { +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( @@ -210,21 +209,21 @@ void LucidCamera::configureSettings() { this->config.lucid.exposure_auto_lower_limit);); } -std::optional LucidCamera::getLatestImage() { +std::optional raspCam::getLatestImage() { ReadLock lock(this->imageQueueLock); ImageData lastImage = this->imageQueue.front(); this->imageQueue.pop_front(); return lastImage; } -std::deque LucidCamera::getAllImages() { +std::deque raspCam::getAllImages() { ReadLock lock(this->imageQueueLock); std::deque outputQueue = this->imageQueue; this->imageQueue = std::deque(); return outputQueue; } -bool LucidCamera::isConnected() { +bool raspCam::isConnected() { if (this->device == nullptr) { return false; } @@ -235,9 +234,8 @@ bool LucidCamera::isConnected() { return false; } -void LucidCamera::captureEvery(const std::chrono::milliseconds& interval, - std::shared_ptr mavlinkClient) { - loguru::set_thread_name("lucid camera"); +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; @@ -249,15 +247,15 @@ void LucidCamera::captureEvery(const std::chrono::milliseconds& interval, this->arenaDeviceLock.unlock(); while (this->isTakingPictures) { - LOG_F(INFO, "Taking picture with LUCID camera"); + LOG_F(INFO, "Taking picture with RPI camera"); std::optional newImage = - this->takePicture(this->takePictureTimeout, mavlinkClient); + 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 LUCID camera"); + LOG_F(INFO, "Took picture with RPI camera"); } else { LOG_F(ERROR, "Unable to take picture. Trying again in %ld ms", interval.count()); } @@ -272,7 +270,7 @@ void LucidCamera::captureEvery(const std::chrono::milliseconds& interval, // this->arenaDeviceLock.unlock(); } -std::optional LucidCamera::takePicture(const std::chrono::milliseconds& timeout, +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"); @@ -319,7 +317,7 @@ std::optional LucidCamera::takePicture(const std::chrono::millisecond return {}; } -void LucidCamera::startStreaming() { +void raspCam::startStreaming() { if (!this->isConnected()) { LOG_F(ERROR, "LUCID Camera not connected. Cannot start streaming"); return; @@ -330,7 +328,7 @@ void LucidCamera::startStreaming() { this->device->StartStream();); } -std::optional LucidCamera::imgConvert(Arena::IImage* pImage) { +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 From 078eae96a47328622bcf92647e73ebd0408b6c18 Mon Sep 17 00:00:00 2001 From: Boris Ryabov Date: Tue, 28 Jan 2025 23:59:44 -0800 Subject: [PATCH 02/43] [LIB] add boost --- .devcontainer/devcontainer.json | 4 ++-- CMakeLists.txt | 1 + deps/boost/boost.cmake | 14 ++++++++++++++ src/CMakeLists.txt | 1 + src/camera/CMakeLists.txt | 2 +- src/network/CMakeLists.txt | 1 + src/network/camera_socket.cpp | 0 7 files changed, 20 insertions(+), 3 deletions(-) create mode 100644 deps/boost/boost.cmake create mode 100644 src/network/camera_socket.cpp diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 0d4da1d4..e0efbdc2 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,7 +1,7 @@ // See this page for reference of options: https://containers.dev/implementors/json_reference { - "name": "x86", - "image": "ghcr.io/tritonuas/obcpp:x86", + "name": "arm", + "image": "ghcr.io/tritonuas/obcpp:arm", // enable when need to connect over USB to pixhawk // also: need to run obcpp with sudo or add tuas user to dialout group with // `sudo usermod -aG dialout tuas && newgrp && bash` diff --git a/CMakeLists.txt b/CMakeLists.txt index b935c2e3..892fed32 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..56d8a8d7 --- /dev/null +++ b/deps/boost/boost.cmake @@ -0,0 +1,14 @@ +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 + ) + +endfunction() \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 62eff399..f6d722cf 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -54,6 +54,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}) # for some reason calling target_add_imagemagick here conflicts with, so we are including/linking without the function call # target_add_imagemagick(${PROJECT_NAME}) target_include_directories(${PROJECT_NAME} PRIVATE ${ImageMagick_INCLUDE_DIRS}) diff --git a/src/camera/CMakeLists.txt b/src/camera/CMakeLists.txt index 2fbe068a..0e6b458d 100644 --- a/src/camera/CMakeLists.txt +++ b/src/camera/CMakeLists.txt @@ -3,7 +3,7 @@ set(LIB_NAME obcpp_camera) set(FILES interface.cpp mock.cpp - lucid.cpp + rpi.cpp ) SET(LIB_DEPS diff --git a/src/network/CMakeLists.txt b/src/network/CMakeLists.txt index d716267a..5c0fba0f 100644 --- a/src/network/CMakeLists.txt +++ b/src/network/CMakeLists.txt @@ -46,6 +46,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}) diff --git a/src/network/camera_socket.cpp b/src/network/camera_socket.cpp new file mode 100644 index 00000000..e69de29b From c73359260ed73eed75b18ab329c385533e6006e0 Mon Sep 17 00:00:00 2001 From: Boris Ryabov Date: Wed, 29 Jan 2025 01:22:36 -0800 Subject: [PATCH 03/43] [ADD] some basic socket design ideas --- include/network/camera_client.hpp | 45 ++++++++++++++++++++++++++ include/network/camera_server.hpp | 52 +++++++++++++++++++++++++++++++ 2 files changed, 97 insertions(+) create mode 100644 include/network/camera_client.hpp create mode 100644 include/network/camera_server.hpp diff --git a/include/network/camera_client.hpp b/include/network/camera_client.hpp new file mode 100644 index 00000000..c32a1a7e --- /dev/null +++ b/include/network/camera_client.hpp @@ -0,0 +1,45 @@ +#include +#include +#include + +// https://www.codeproject.com/Articles/1264257/Socket-Programming-in-Cplusplus-using-boost-asio-T +using namespace boost::asio; +using ip::tcp; + +class CameraClient { + public: + /* + * Create the socket + * Params: + * - port (int) - which port to occupy + */ + explicit CameraClient(int port); + ~CameraClient(); + + /* + * Connect to server (OBC) + * Params: + * - IP (string) - regular ip + * - port (int) - port + */ + bool connect(std::string ip, int port); + + /* + * Sends pics out + * Params: + * - header (string) - format should be "image_dimensions, buffer_size" + * - image (vector) - vector of bytes of image + * It should also probably have a terminator + */ + bool send(std::string header, std::vector image); + + // Recieves a command to take pics + // TODO: figure out what should be return type + bool read(); + + private: + // ig? + string ip; + int port; + tcp::socket socket; +}; \ No newline at end of file diff --git a/include/network/camera_server.hpp b/include/network/camera_server.hpp new file mode 100644 index 00000000..5869db64 --- /dev/null +++ b/include/network/camera_server.hpp @@ -0,0 +1,52 @@ +#include +#include +#include + +// https://www.codeproject.com/Articles/1264257/Socket-Programming-in-Cplusplus-using-boost-asio-T +using namespace boost::asio; +using ip::tcp; + + +struct image_data_t { + int width; + int height; + size_t image_size_bytes; + std::vector data; +}; + + +class CameraServer +{ +public: + /* + * Create the socket + * Params: + * - port (int) - which port to occupy + */ + explicit CameraServer(int port); + ~CameraServer(); + + /* + * Connect to server (OBC) + * Params: + * - IP (string) - regular ip + * - port (int) - port + */ + bool connect(std::string ip, int port); + + /* + * Sends command to take and return pic + * Params: + * TODO: IDK WHAT TO SEND + */ + bool send(std::string command); + + // Recieves a command to take pics + image_data read(); + +private: + // ig? + string ip; + int port; + tcp::socket socket; +}; From f94e54a9f959620ca627c01d2a71e1be845aaf17 Mon Sep 17 00:00:00 2001 From: Boris Ryabov Date: Thu, 6 Feb 2025 13:10:22 -0800 Subject: [PATCH 04/43] hold --- src/network/camera_socket.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/network/camera_socket.cpp b/src/network/camera_socket.cpp index e69de29b..a5563a11 100644 --- a/src/network/camera_socket.cpp +++ b/src/network/camera_socket.cpp @@ -0,0 +1,15 @@ +#include +#include +#include + +// Very large just in case strerror(errno) actually ends up being incredibly large +#define AD_ERR_LEN 999 + +using namespace boost::asio; +using ip::tcp; + +tcp::socket socket; + +CameraSocket::CameraSocket() { + +} From d55398d84302813defea2dc8b6a99f61a3499b4c Mon Sep 17 00:00:00 2001 From: Boris Ryabov Date: Tue, 25 Feb 2025 13:44:18 -0800 Subject: [PATCH 05/43] [ADD] TCP interfaces, data structs --- include/network/camera_client.hpp | 4 +-- include/network/camera_data.hpp | 42 +++++++++++++++++++++++++++++++ include/network/camera_server.hpp | 34 ++++++++++++------------- 3 files changed, 61 insertions(+), 19 deletions(-) create mode 100644 include/network/camera_data.hpp diff --git a/include/network/camera_client.hpp b/include/network/camera_client.hpp index c32a1a7e..7dbd5624 100644 --- a/include/network/camera_client.hpp +++ b/include/network/camera_client.hpp @@ -1,6 +1,7 @@ #include #include #include +#include "camera_data.hpp" // https://www.codeproject.com/Articles/1264257/Socket-Programming-in-Cplusplus-using-boost-asio-T using namespace boost::asio; @@ -34,8 +35,7 @@ class CameraClient { bool send(std::string header, std::vector image); // Recieves a command to take pics - // TODO: figure out what should be return type - bool read(); + CameraResponse_t read(); private: // ig? diff --git a/include/network/camera_data.hpp b/include/network/camera_data.hpp new file mode 100644 index 00000000..2f2fe6b6 --- /dev/null +++ b/include/network/camera_data.hpp @@ -0,0 +1,42 @@ +#include + +struct ImageData_t +{ + int width; + int height; + size_t imageSizeBytes; + std::vector data; +}; + +// What does OBC needs from camera +enum class RequestType_t +{ + SENDIMAGE +}; + +// What is the status of the request (debugging + error handling) +enum class ResponseType_t +{ + SUCC, + ERROR +}; + +// 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; +}; + +// Response back from camera to OBC +struct CameraResponse_t +{ + int pid; + ResponseType_t responseType; + ImageData_t data; + +}; + diff --git a/include/network/camera_server.hpp b/include/network/camera_server.hpp index 5869db64..2e68c041 100644 --- a/include/network/camera_server.hpp +++ b/include/network/camera_server.hpp @@ -1,21 +1,15 @@ #include #include #include +#include "camera_data.hpp" // https://www.codeproject.com/Articles/1264257/Socket-Programming-in-Cplusplus-using-boost-asio-T using namespace boost::asio; using ip::tcp; - -struct image_data_t { - int width; - int height; - size_t image_size_bytes; - std::vector data; -}; - - -class CameraServer +// OBC-side socket. +// Sends requests to take pics to camera, sends it back up to +class ObcCameraServer { public: /* @@ -23,11 +17,11 @@ class CameraServer * Params: * - port (int) - which port to occupy */ - explicit CameraServer(int port); - ~CameraServer(); + explicit ObcCameraServer(int port); + ~ObcCameraServer(); /* - * Connect to server (OBC) + * Connect to client (camera) * Params: * - IP (string) - regular ip * - port (int) - port @@ -37,12 +31,18 @@ class CameraServer /* * Sends command to take and return pic * Params: - * TODO: IDK WHAT TO SEND + * - type (RequestType_t) - what OBC wants camera to do + * Returns: + * - bool - if success or not */ - bool send(std::string command); + bool send(RequestType_t type); - // Recieves a command to take pics - image_data read(); + /* + * Reads the response + * Returns: + * - CameraResponse_t - response that camera passed in + */ + CameraResponse_t read(); private: // ig? From efe4aa100372f44513c51ca77db0cdebd7e8a96c Mon Sep 17 00:00:00 2001 From: Boris Ryabov Date: Tue, 25 Feb 2025 14:27:52 -0800 Subject: [PATCH 06/43] [add] implementation files --- src/network/camera_client.cpp | 8 ++++++++ src/network/camera_server.cpp | 8 ++++++++ 2 files changed, 16 insertions(+) create mode 100644 src/network/camera_client.cpp create mode 100644 src/network/camera_server.cpp diff --git a/src/network/camera_client.cpp b/src/network/camera_client.cpp new file mode 100644 index 00000000..11b2fc89 --- /dev/null +++ b/src/network/camera_client.cpp @@ -0,0 +1,8 @@ +#include +#include +#include +#include "camera_data.hpp" + +// https://www.codeproject.com/Articles/1264257/Socket-Programming-in-Cplusplus-using-boost-asio-T +using namespace boost::asio; +using ip::tcp; diff --git a/src/network/camera_server.cpp b/src/network/camera_server.cpp new file mode 100644 index 00000000..149b1c0b --- /dev/null +++ b/src/network/camera_server.cpp @@ -0,0 +1,8 @@ +#include +#include +#include +#include "camera_data.hpp" + +// https://www.codeproject.com/Articles/1264257/Socket-Programming-in-Cplusplus-using-boost-asio-T +using namespace boost::asio; +using ip::tcp; \ No newline at end of file From 9d082dfd7da27cae185bd20c90713f677a547e7b Mon Sep 17 00:00:00 2001 From: Boris Ryabov Date: Fri, 28 Feb 2025 12:27:23 -0800 Subject: [PATCH 07/43] [ADD] changed headers, constr and connect funcs in cpp --- include/network/camera_client.hpp | 14 +++++++------- src/network/camera_client.cpp | 23 +++++++++++++++++++---- 2 files changed, 26 insertions(+), 11 deletions(-) diff --git a/include/network/camera_client.hpp b/include/network/camera_client.hpp index 7dbd5624..97b49d2b 100644 --- a/include/network/camera_client.hpp +++ b/include/network/camera_client.hpp @@ -1,20 +1,19 @@ #include #include #include -#include "camera_data.hpp" - +#include "network/camera_data.hpp" // https://www.codeproject.com/Articles/1264257/Socket-Programming-in-Cplusplus-using-boost-asio-T -using namespace boost::asio; -using ip::tcp; +namespace asio = boost::asio; class CameraClient { public: /* * Create the socket * Params: + * - io_context (boost::asio::io_context) - what io context to use * - port (int) - which port to occupy */ - explicit CameraClient(int port); + CameraClient(int port); ~CameraClient(); /* @@ -23,7 +22,7 @@ class CameraClient { * - IP (string) - regular ip * - port (int) - port */ - bool connect(std::string ip, int port); + bool connect(asio::io_context io_context, std::string ip, int port); /* * Sends pics out @@ -38,8 +37,9 @@ class CameraClient { CameraResponse_t read(); private: + // ig? string ip; int port; - tcp::socket socket; + asio::tcp::socket socket; }; \ No newline at end of file diff --git a/src/network/camera_client.cpp b/src/network/camera_client.cpp index 11b2fc89..4fb23ef3 100644 --- a/src/network/camera_client.cpp +++ b/src/network/camera_client.cpp @@ -1,8 +1,23 @@ #include #include #include -#include "camera_data.hpp" +#include "network/camera_data.hpp" +#include "network/camera_client.hpp" +namespace asio = boost::asio; -// https://www.codeproject.com/Articles/1264257/Socket-Programming-in-Cplusplus-using-boost-asio-T -using namespace boost::asio; -using ip::tcp; +CameraClient::CameraClient(int port) +{ + this->port=port; +} + +CameraClient::connect(asio::io_context * io_context, std::string ip, int port) +{ + // create an asio endpoint + asio::ip::tcp::endpoint endpoint(asio::make_adress(ip), port); + + // create a socket + this->socket(&io_context); + + // connect socket + this->socket::connect(&endpoint); +} \ No newline at end of file From 8f7530f6ea28a4d204f2edb94812d5714bd53e22 Mon Sep 17 00:00:00 2001 From: Boris Ryabov Date: Thu, 6 Mar 2025 17:35:19 -0800 Subject: [PATCH 08/43] [ADD] basic client + send function --- include/network/camera_client.hpp | 2 +- include/network/camera_server.hpp | 3 ++- src/network/camera_client.cpp | 21 +++++++++++++++++++-- 3 files changed, 22 insertions(+), 4 deletions(-) diff --git a/include/network/camera_client.hpp b/include/network/camera_client.hpp index 97b49d2b..c474810e 100644 --- a/include/network/camera_client.hpp +++ b/include/network/camera_client.hpp @@ -31,7 +31,7 @@ class CameraClient { * - image (vector) - vector of bytes of image * It should also probably have a terminator */ - bool send(std::string header, std::vector image); + bool send(std::vector image); // Recieves a command to take pics CameraResponse_t read(); diff --git a/include/network/camera_server.hpp b/include/network/camera_server.hpp index 2e68c041..427b7279 100644 --- a/include/network/camera_server.hpp +++ b/include/network/camera_server.hpp @@ -48,5 +48,6 @@ class ObcCameraServer // ig? string ip; int port; - tcp::socket socket; + std::pair camera_res; // used for reconstruction + asio::tcp::socket socket; }; diff --git a/src/network/camera_client.cpp b/src/network/camera_client.cpp index 4fb23ef3..31594b2d 100644 --- a/src/network/camera_client.cpp +++ b/src/network/camera_client.cpp @@ -10,7 +10,7 @@ CameraClient::CameraClient(int port) this->port=port; } -CameraClient::connect(asio::io_context * io_context, std::string ip, int port) +bool CameraClient::connect(asio::io_context *io_context, std::string ip, int port) { // create an asio endpoint asio::ip::tcp::endpoint endpoint(asio::make_adress(ip), port); @@ -20,4 +20,21 @@ CameraClient::connect(asio::io_context * io_context, std::string ip, int port) // connect socket this->socket::connect(&endpoint); -} \ No newline at end of file + + // TODO: error handling + + return true; +} + +// thinking if we just gonna run the same buff size and img proportions? +bool CameraClient::send(std::vector image) +{ + // TODO: after finishing working version, try to serialize and send whole data type + asio::buffer::const_buffer image_buffer = asio::buffer(&image, sizeof(image)); + + //TODO: PUT ERROR DATA TYPE VAR HERE + + asio::socket::write(socket, image_buffrer); // TODO: PUT ERROR HANDLER HERE + + return true; +} From 7d65cf69b636b3a44e81227bfe1ae98c8ccedeea Mon Sep 17 00:00:00 2001 From: smhitle <91502030+smhitle@users.noreply.github.com> Date: Tue, 15 Apr 2025 07:18:50 -0700 Subject: [PATCH 09/43] TCP Client and Server added synchronous client and synchronous test server able to serialize data in a struct and send it between client and server can also successfully send an image, however not in the yuv420 as expected from the RPI side sync_client_mock.cpp and sync_server_mock.cpp need to be run separately and the server needs to be started first in order for the client to successfully connect to it currently working on converting everything into asynchronous Asynchronous TODOS - need to make the async client thread safe, need to learn how to use strand and such to protect shared resources and the execution context - need to ensure that requests to take images complete in the requested order, in other words no images out of order. this isn't a problem in the synchronous version but asynchronous needs to handle it - what do we do in the case any of the async functions fail? - once we receive the image in yuv420 format, does it need to be reassembled by imgConvert() or is that on the cv pipeline? also need to save to local storage instead of keeping it in memory what is the expected format to save as? i.e name or title - also need to handle errors --- .devcontainer/devcontainer.json | 4 +- include/network/async_client.hpp | 68 +++++++++ include/network/async_server.hpp | 0 include/network/camera_data.hpp | 44 +++++- include/network/camera_server.hpp | 12 +- include/network/serialize.hpp | 23 +++ include/network/sync_client.hpp | 108 ++++++++++++++ include/network/sync_server.hpp | 114 +++++++++++++++ src/network/async_client.cpp | 184 ++++++++++++++++++++++++ src/network/async_server.cpp | 0 src/network/camera_server.cpp | 76 +++++++++- src/network/sync_client.cpp | 151 ++++++++++++++++++++ src/network/sync_server.cpp | 188 +++++++++++++++++++++++++ tests/integration/sync_client_mock.cpp | 70 +++++++++ tests/integration/sync_server_mock.cpp | 43 ++++++ 15 files changed, 1073 insertions(+), 12 deletions(-) create mode 100644 include/network/async_client.hpp create mode 100644 include/network/async_server.hpp create mode 100644 include/network/serialize.hpp create mode 100644 include/network/sync_client.hpp create mode 100644 include/network/sync_server.hpp create mode 100644 src/network/async_client.cpp create mode 100644 src/network/async_server.cpp create mode 100644 src/network/sync_client.cpp create mode 100644 src/network/sync_server.cpp create mode 100644 tests/integration/sync_client_mock.cpp create mode 100644 tests/integration/sync_server_mock.cpp diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index e0efbdc2..0d4da1d4 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,7 +1,7 @@ // See this page for reference of options: https://containers.dev/implementors/json_reference { - "name": "arm", - "image": "ghcr.io/tritonuas/obcpp:arm", + "name": "x86", + "image": "ghcr.io/tritonuas/obcpp:x86", // enable when need to connect over USB to pixhawk // also: need to run obcpp with sudo or add tuas user to dialout group with // `sudo usermod -aG dialout tuas && newgrp && bash` diff --git a/include/network/async_client.hpp b/include/network/async_client.hpp new file mode 100644 index 00000000..c70c165b --- /dev/null +++ b/include/network/async_client.hpp @@ -0,0 +1,68 @@ +#include +#include +#include +#include "camera_data.hpp" + +#ifndef CLIENT_H +#define CLIENT_H + +namespace asio = boost::asio; + +class Client { + private: + std::string ip; + int port; + // asio::ip::tcp::resolver resolver_; + asio::ip::tcp::socket socket_; // TODO: does the socket need a lock? + + // TODO: likely need locks around these + // TODO: scratch that, don't make shared class bufs, have the ownership be part of the async functions + asio::streambuf sendbuf; + asio::streambuf recvbuf; + + // TODO: this vector should really be protected by locks, what is the difference between the Locks provided in utilities/locks.hpp, utilities/lockptr.hpp, shared_mutex and any other mutex like objects i should be aware of + std::queue images; + + // These functions will handle sending the request + + /** + * This will take in the following request, make a "packet", serialize the data and then put it into the send buffer to send to the camera + */ + void createRequestPacket(RequestType_t request); + + /** + * + */ + void send(RequestType_t requestType); + + // These functions will handle reading the response + + /** + * + */ + void deconstructPacket(); + + /** + * + */ + void recv(); + + cv::Mat imgConvert(); + + public: + + Client(asio::io_context* io_context_, std::string ip, int port); + + void connect(); + + // TODO: unsure what this should return + void ping(); + + void takePicture(); + + // TODO: return type should probably change + cv::Mat getLatestImage(); + +}; + +#endif diff --git a/include/network/async_server.hpp b/include/network/async_server.hpp new file mode 100644 index 00000000..e69de29b diff --git a/include/network/camera_data.hpp b/include/network/camera_data.hpp index 2f2fe6b6..3f125089 100644 --- a/include/network/camera_data.hpp +++ b/include/network/camera_data.hpp @@ -1,11 +1,26 @@ #include +#include +#include +#include +#include + +#ifndef CAMERA_DATA_H +#define CAMERA_DATA_H struct ImageData_t { int width; int height; size_t imageSizeBytes; - std::vector data; + 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 @@ -14,6 +29,12 @@ 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 { @@ -21,6 +42,12 @@ enum class ResponseType_t 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 { @@ -29,6 +56,12 @@ struct CameraRequest_t // 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 @@ -36,7 +69,14 @@ struct CameraResponse_t { int pid; ResponseType_t responseType; - ImageData_t data; + ImageData_t imageData; + template + void serialize(Archive & ar, const unsigned int version) { + ar & pid; + ar & responseType; + ar & imageData; + } }; +#endif diff --git a/include/network/camera_server.hpp b/include/network/camera_server.hpp index 427b7279..74c01f3e 100644 --- a/include/network/camera_server.hpp +++ b/include/network/camera_server.hpp @@ -4,8 +4,7 @@ #include "camera_data.hpp" // https://www.codeproject.com/Articles/1264257/Socket-Programming-in-Cplusplus-using-boost-asio-T -using namespace boost::asio; -using ip::tcp; +namespace asio = boost::asio; // OBC-side socket. // Sends requests to take pics to camera, sends it back up to @@ -17,7 +16,7 @@ class ObcCameraServer * Params: * - port (int) - which port to occupy */ - explicit ObcCameraServer(int port); + explicit ObcCameraServer(std::string ip, int port, asio::io_context& io_context_); ~ObcCameraServer(); /* @@ -26,7 +25,7 @@ class ObcCameraServer * - IP (string) - regular ip * - port (int) - port */ - bool connect(std::string ip, int port); + bool connect(); /* * Sends command to take and return pic @@ -46,8 +45,9 @@ class ObcCameraServer private: // ig? - string ip; + std::string ip; int port; std::pair camera_res; // used for reconstruction - asio::tcp::socket socket; + asio::ip::tcp::socket socket_; + asio::ip::tcp::acceptor acceptor_; }; diff --git a/include/network/serialize.hpp b/include/network/serialize.hpp new file mode 100644 index 00000000..b2ab93f2 --- /dev/null +++ b/include/network/serialize.hpp @@ -0,0 +1,23 @@ +#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; + } +} diff --git a/include/network/sync_client.hpp b/include/network/sync_client.hpp new file mode 100644 index 00000000..696b3962 --- /dev/null +++ b/include/network/sync_client.hpp @@ -0,0 +1,108 @@ +#include +#include +#include +#include "camera_data.hpp" +#include "serialize.hpp" + +#ifndef CLIENT_H +#define CLIENT_H + +namespace asio = boost::asio; + +class Client { + private: + asio::ip::tcp::socket socket_; + std::string ip; + int port; + + // underlying buffer for communication + asio::streambuf sendbuf; + asio::streambuf receivebuf; + + // TODO: these error codes are useless if we handle them on the spot of failure + + // free functions error codes + boost::system::error_code ec_write; + boost::system::error_code ec_read; + + // socket specific error codes + boost::system::error_code ec_shutdown; + boost::system::error_code ec_close; + + // image buffer + std::vector images; + + /** + * Creates the request from the request type + */ + CameraRequest_t createRequest(RequestType_t requestType); + + /** + * Creates a packet to send based off request type + */ + void createRequestPacket(RequestType_t requestType); + + /** + * Send the header of the packet + */ + void sendHeader(); + + /** + * Send the body of the packet + */ + void sendBody(); + + /** + * Read in the header of the packet + * + * @return std::uint32_t Size of the body to be received + */ + std::uint32_t receiveHeader(); + + /** + * Read in the body of the packet + * + * @param size Size of packet body + */ + void receiveBody(std::uint32_t size); + + /** + * Deconstructs the received packet + */ + CameraResponse_t deconstructPacket(); + + /** + * Handler for what is received + */ + void handlePacket(); + + public: + Client(asio::io_context* io_context_, std::string ip, int port); + + /** + * Connects to the server with the specified ip and port + */ + void connect(); + + /** + * Sends a packet to the server + */ + void send(RequestType_t requestType); + + /** + * Reads in packets send from the server + */ + void receive(); + + /** + * Closes the connection between client and server + */ + void disconnect(); + + /** + * This is a testing function to see what images are in the image buffer + */ + void showImages(); +}; + +#endif diff --git a/include/network/sync_server.hpp b/include/network/sync_server.hpp new file mode 100644 index 00000000..84d96077 --- /dev/null +++ b/include/network/sync_server.hpp @@ -0,0 +1,114 @@ +#include +#include +#include "camera_data.hpp" +#include "serialize.hpp" + +namespace asio = boost::asio; + +class Server { + private: + std::string ip; + int port; + asio::ip::tcp::socket socket_; + asio::ip::tcp::acceptor acceptor_; + + asio::streambuf sendbuf; + asio::streambuf receivebuf; + + // free functions + boost::system::error_code ec_write; + boost::system::error_code ec_read; + + // socket specific + boost::system::error_code ec_shutdown; + boost::system::error_code ec_close; + + // acceptor specific + boost::system::error_code ec_acceptor_init; + boost::system::error_code ec_accept; + + ImageData_t createImageData(); + /** + * Creates the response from the request + */ + CameraResponse_t createResponse(ResponseType_t response); + + /** + * Creates a packet to send based off the request + */ + void createResponsePacket(ResponseType_t response); + + /** + * Send the header of the packet + */ + void sendHeader(); + + /** + * Send the body of the packet + */ + void sendBody(); + + /** + * Read in the header of the packet + * + * @return std::uint32_t Size of the body to be received + */ + std::uint32_t receiveHeader(); + + /** + * Read in the body of the packet + * + * @param size Size of packet body + */ + void receiveBody(std::uint32_t size); + + /** + * Deconstructs the received packet + */ + CameraRequest_t deconstructPacket(); + + /** + * Handler for the packet based on what was received + */ + void handlePacket(); + + public: + + Server(asio::io_context* io_context_, std::string ip, int port); + + /** + * Accepts connections on the underlying socket + */ + void connect(); + + /** + * Sends a packet to the client + */ + void send(ResponseType_t responseType); + + /** + * Reads in packets send from the client + */ + void receive(); + + /** + * Closes the socket + */ + void close(); + + /** + * Shuts down the socket + */ + void shutdown(); + + /** + * Returns the error if an error occurred on a read operation + */ + boost::system::error_code getReadError(); + + /** + * Returns the error if an error occurred on a write operation + */ + boost::system::error_code getWriteError(); + +}; diff --git a/src/network/async_client.cpp b/src/network/async_client.cpp new file mode 100644 index 00000000..3b60fd4b --- /dev/null +++ b/src/network/async_client.cpp @@ -0,0 +1,184 @@ +#include +#include +#include "include/network/async_client.hpp" +#include "include/utilities/locks.hpp" +#include "include/network/serialize.hpp" + +namespace asio = boost::asio; + +void Client::createRequestPacket(RequestType_t requestType) { + + // form the data + CameraRequest_t request; + request.requestType = requestType; + request.pid = 1010; // TODO: change the PID or completely remove PID + + // serialize the data + serialh::serialize(&request, &this->sendbuf); + +} + +void Client::send(RequestType_t requestType) { + + // TODO: probably need to lock here + + this->createRequestPacket(requestType); + + // two sends here, first send the size of the serialized data, then send the serialized data itself + // TODO: is a chained callback structure the best way to do this? + + std::uint32_t headerLength = static_cast(this->sendbuf.size()); + + asio::async_write(this->socket_, asio::buffer(&headerLength, sizeof(headerLength)), + [this](boost::system::error_code ec, std::size_t bytes_written) { + + if (ec) { + // TODO: what do we do if sending the header fails? + } + + asio::async_write(this->socket_, this->sendbuf, + [this](boost::system::error_code ec, std::size_t bytes_written) { + + if (ec) { + // TODO: what do we do if sending the body fails? + } + + }); + }); + +} + +void Client::recv() { + + // TODO: maybe make this shared as well + std::uint32_t headerLength = {}; + + asio::async_read(this->socket_, asio::buffer(&headerLength, sizeof(headerLength)), + [this, headerLength](boost::system::error_code ec, std::size_t bytes_read) { + + if (ec) { + // TODO: what to do if sending header length fails, best guess is to not send the packet at all + } + + asio::streambuf buf; + + std::shared_ptr recvbuf {&buf}; + + asio::async_read(this->socket_, recvbuf->prepare(headerLength), + [this, headerLength, recvbuf](boost::system::error_code ec, std::size_t bytes_read) { + + if (ec || bytes_read != headerLength) { + + // we read some data but not all of it so its likely not usable + recvbuf->consume(bytes_read); + + // TODO: what do we do + } + + recvbuf->commit(bytes_read); + + cv::Mat m = this->imgConvert(); // TODO: have to pass the recvbuf through here + + this->images.push(m); + + }); + + }); + +} + +cv::Mat Client::imgConvert() { + + // TODO: apparently the expected format will be YUV420, look into what that is. can be done with ffmpeg per Daniel. the current code just assumes that what was sent is in jpeg format or whatever, not YUV420 + // using opencv cvtColor(src, dst, cv::COLOR_YUV2BGR_NV21)? + // will this function be reading in images and then converting them? or are we converting them as we receive the image buffers + + + // this logic is what deconstructPacket() was supposed to be + CameraResponse_t response; + + serialh::deserialize(&response, &this->recvbuf); + + std::vector image = response.imageData.imgBuffer; + + // "reassemble" the image + cv::Mat m = cv::Mat(response.imageData.height, response.imageData.width, CV_8UC3); + + if (image.size() == (response.imageData.imageSizeBytes)) { + std::memcpy(m.data, image.data(), sizeof(std::uint8_t) * response.imageData.imageSizeBytes); + } + + return m; + +} + +Client::Client(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_) { + + this->ip = ip; + this->port = port; + + // TODO: could use the socket keepalive option here to keep a persistent connection, unsure how it differs from the ping() function. idk what the ping() function even does. + +} + +void Client::connect() { + + // TODO: should we check if its already connected? maybe multiple threads could try calling connect() + + // make the endpoint + asio::ip::tcp::endpoint endpoint_(asio::ip::tcp::endpoint(asio::ip::make_address(this->ip), this->port)); + + // asynchronously connect to endpoint + // TODO: is a lambda appropriate here as the completion handler? + this->socket_.async_connect(endpoint_, + [this](boost::system::error_code ec) { + + // TODO: what do we do in case of an error? + if (ec) { + std::cout << "Failed to connect" << '\n'; + std::cout << "Reason: " << ec.message() << " code: " << ec.value() << '\n'; + } + + std::cout << "Connected to: " << this->ip << " on port: " << this->port << '\n'; + + }); + +} + +void Client::ping() { + + /* + This is what i assume the function does: + - basically just open up a new connection and send a packet with Ping + other stuff and wait for a response back + - if it fails to connect or a response is taking too long, then something has happened but what do we do? print out a request timed out message? + */ + +} + +void Client::takePicture() { + + this->send(RequestType_t::SENDIMAGE); + + this->recv(); + +} + +cv::Mat Client::getLatestImage() { + + // TODO: what if there are no images in the image buffer? + + cv::Mat latestImage {}; + + // TODO: acquire a lock, just regular not a read or write lock? + + latestImage = this->images.front(); + this->images.pop(); + + // TODO: release the lock + + return latestImage; + +} + + + diff --git a/src/network/async_server.cpp b/src/network/async_server.cpp new file mode 100644 index 00000000..e69de29b diff --git a/src/network/camera_server.cpp b/src/network/camera_server.cpp index 149b1c0b..94040f16 100644 --- a/src/network/camera_server.cpp +++ b/src/network/camera_server.cpp @@ -2,7 +2,79 @@ #include #include #include "camera_data.hpp" +#include "camera_server.hpp" // https://www.codeproject.com/Articles/1264257/Socket-Programming-in-Cplusplus-using-boost-asio-T -using namespace boost::asio; -using ip::tcp; \ No newline at end of file +namespace asio = boost::asio; + +// may throw an error due to acceptor construction +ObcCameraServer::ObcCameraServer(std::string ip, int port, asio::io_context& io_context_) : socket_(io_context_), acceptor_(asio::ip::tcp::endpoint(asio::ip::make_address(ip), port)) { + + this->ip = ip; + this->port = port; + +} + +bool ObcCameraServer::connect() { + + try { + + // may throw an error + this->acceptor_.accept(this->socket_); + return true; + + } catch(std::exception& e) { + + return false; + } + +} + +bool ObcCameraServer::send(RequestType_t type) { + + CameraRequest_t request; + request.pid = 0; // TODO: no idea what pid is + request.requestType = type; + + asio::streambuf buf; + boost::system::error_code ec; + + // serialize the request struct + // TODO: create helper functions (serialize.hpp) + std::ostream os(&buf); + boost::archive::binary_oarchive oa(os); + oa << request; + + // send the serialized data, may result in an error + asio::write(this->socket_, buf, ec); + + if (ec) { + return false; + } + + return true; +} + +CameraResponse_t ObcCameraServer::read() { + + CameraResponse_t response; + asio::streambuf buf; + boost::system::error_code ec; + + // read the data from the socket, may result in an error + asio::read(this->socket_, buf, ec); + + if (ec) { + // TODO: what do we return if no data read + } + + // deserialize the data + // TODO: create helper functions (serialize.hpp) + std::istream is(&buf); + boost::archive::binary_iarchive ia(is); + ia >> response; + + return response; +} + + diff --git a/src/network/sync_client.cpp b/src/network/sync_client.cpp new file mode 100644 index 00000000..1e97f361 --- /dev/null +++ b/src/network/sync_client.cpp @@ -0,0 +1,151 @@ +#include "include/network/sync_client.hpp" + +CameraRequest_t Client::createRequest(RequestType_t requestType) { + + CameraRequest_t request; + request.pid = 6969; + request.requestType = requestType; + + return request; + +} + +void Client::createRequestPacket(RequestType_t requestType) { + + CameraRequest_t request = createRequest(requestType); + + serialh::serialize(&request, &this->sendbuf); + +} + +void Client::sendHeader() { + + std::uint32_t headerLength = static_cast(this->sendbuf.size()); + + // TODO: error handler + asio::write(this->socket_, asio::buffer(&headerLength, sizeof(headerLength))); + +} + +void Client::sendBody() { + + // TODO: error handler + asio::write(this->socket_, this->sendbuf); + +} + +std::uint32_t Client::receiveHeader() { + + std::uint32_t size {}; + + // TODO: error handler + asio::read(this->socket_, asio::buffer(&size, sizeof(size))); + + return size; + +} + +void Client::receiveBody(std::uint32_t size) { + + // TODO: error handler + asio::read(this->socket_, this->receivebuf.prepare(size)); + this->receivebuf.commit(size); + +} + +CameraResponse_t Client::deconstructPacket() { + + CameraResponse_t response; + + serialh::deserialize(&response, &this->receivebuf); + + return response; + +} + +void Client::handlePacket() { + + // deconstruct packet + CameraResponse_t response = deconstructPacket(); + + std::vector image = response.imageData.imgBuffer; + + cv::Mat m = cv::Mat(response.imageData.height, response.imageData.width, CV_8UC3); + + if (image.size() == (response.imageData.imageSizeBytes)) { + std::memcpy(m.data, image.data(), sizeof(std::uint8_t) * response.imageData.imageSizeBytes); + } + + // Add image to image buffer + this->images.push_back(m); +} + +Client::Client(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_) { + + this->ip = ip; + this->port = port; + +} + +void Client::connect() { + + // TODO: difference between exception handling and error handling + try { + + asio::ip::tcp::endpoint endpoint_(asio::ip::tcp::endpoint(asio::ip::make_address(this->ip), this->port)); + + this->socket_.connect(endpoint_); + + std::cout << "Connected to: " << this->ip << " on port " << this->port << '\n'; + + } catch (std::exception& e) { + + std::cout << e.what() << '\n'; + + } + +} + +void Client::send(RequestType_t requestType) { + + createRequestPacket(requestType); + + // send a header containing the length/size + sendHeader(); + + // send the serialized data in the streambuf + sendBody(); + +} + +void Client::receive() { + + std::uint32_t bodySize = receiveHeader(); + receiveBody(bodySize); + + handlePacket(); + +} + +void Client::disconnect() { + + // TODO: error handling + this->socket_.shutdown(asio::ip::tcp::socket::shutdown_both); + this->socket_.close(); + +} + +void Client::showImages(){ + + for (cv::Mat img : this->images) { + + std::string windowName = "Window"; + cv::namedWindow(windowName); + cv::resizeWindow(windowName, 200, 200); + cv::imshow(windowName, img); + + cv::waitKey(0); + + cv::destroyWindow(windowName); + } +} diff --git a/src/network/sync_server.cpp b/src/network/sync_server.cpp new file mode 100644 index 00000000..8cc2773d --- /dev/null +++ b/src/network/sync_server.cpp @@ -0,0 +1,188 @@ +#include +#include "include/network/sync_server.hpp" + + +ImageData_t Server::createImageData() { + + ImageData_t i; + cv::Mat img = cv::imread("birb.jpg"); // should be whatever the image is + + // converts a Mat to vector (Mat is of type CV_8UC3) + cv::Mat flatten = img.reshape(1, img.total() * img.channels()); + std::vector imgBuffer = img.isContinuous() ? flatten : flatten.clone(); + + // fill struct with appropriate data + i.height = img.rows; + i.width = img.cols; + i.imageSizeBytes = img.total() * img.elemSize(); + i.imgBuffer = imgBuffer; + + return i; + +} + +CameraResponse_t Server::createResponse(ResponseType_t responseType) { + + ImageData_t imgData = createImageData(); + + // fill struct with appropriate data + CameraResponse_t response; + response.pid = 1010; + response.responseType = responseType; + response.imageData = imgData; + + return response; + +} + +void Server::createResponsePacket(ResponseType_t responseType) { + + CameraResponse_t response = createResponse(responseType); + + serialh::serialize(&response, &this->sendbuf); + +} + +void Server::sendHeader() { + + std::uint32_t headerLength = static_cast(this->sendbuf.size()); + + // TODO: error handler + asio::write(this->socket_, asio::buffer(&headerLength, sizeof(headerLength))); + +} + +void Server::sendBody() { + + // TODO: error handler + asio::write(this->socket_, this->sendbuf); + +} + +std::uint32_t Server::receiveHeader() { + + std::uint32_t size {}; + + // TODO: error handler + asio::read(this->socket_, asio::buffer(&size, sizeof(size)), ec_read); + + if (ec_read) { + std::cout << "Failed to receive header: " << ec_read.message() << '\n'; + } + + return size; + +} + +void Server::receiveBody(std::uint32_t size) { + + // TODO: error handler + asio::read(this->socket_, this->receivebuf.prepare(size)); + + if (ec_read) { + std::cout << "Failed to receive body: " << ec_read.message() << '\n'; + } + + this->receivebuf.commit(size); + +} + +CameraRequest_t Server::deconstructPacket() { + + CameraRequest_t request; + + serialh::deserialize(&request, &this->receivebuf); + + return request; + +} + +void Server::handlePacket() { + + // TODO: deconstruct packet + CameraRequest_t request = deconstructPacket(); + + std::cout << request.pid << '\n'; + std::cout << static_cast(request.requestType) << '\n'; + + // TODO: in this testing case prepare an image + +} + +// TODO: error handler (acceptor initialization) +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::connect() { + + try { + + // TODO: error handler (switch to not exception?) + this->acceptor_.accept(this->socket_); + std::cout << "Listening on port: " << this->port << '\n'; + + } catch (std::exception& e) { + + std::cout << e.what() << '\n'; + + } + +} + +void Server::send(ResponseType_t responseType) { + + createResponsePacket(responseType); + + sendHeader(); + sendBody(); + +} + +void Server::receive() { + + std::uint32_t bodySize = receiveHeader(); + receiveBody(bodySize); + + // TODO: if the client closes the connection, there won't be anything in the receivebuf and it throws an exception + + if (this->ec_read) { + return; + } + + handlePacket(); + +} + +void Server::close() { + + // TODO: error handler + this->socket_.shutdown(asio::ip::tcp::socket::shutdown_both); + + // TODO: error handler + this->socket_.close(); + +} + +void Server::shutdown() { + + this->acceptor_.close(); + close(); + this->socket_.release(); // TODO: unsure if this is correct + +} + +boost::system::error_code Server::getReadError() { + + return this->ec_read; + +} + +boost::system::error_code Server::getWriteError() { + + return this->ec_write; + +} diff --git a/tests/integration/sync_client_mock.cpp b/tests/integration/sync_client_mock.cpp new file mode 100644 index 00000000..a21644ee --- /dev/null +++ b/tests/integration/sync_client_mock.cpp @@ -0,0 +1,70 @@ +#include +#include +#include +#include "include/network/sync_client.hpp" + +namespace asio = boost::asio; + +const std::string ip {"127.0.0.1"}; +constexpr uint32_t port {5000}; + +/* +what do we do when we are unable to ()? +what do we do when we fail to accept incoming connections? +what do we do when our send() fails? +what do we do when our write() fails? +what do we do when our shutdown fails? +what do we do when our close fails? +*/ + +/** + * Method used for testing purposes, converts enum to string + */ +std::string convertEnumToString(RequestType_t request) { + if (request == RequestType_t::SENDIMAGE) { + return "SENDIMAGE"; + } else if (request == RequestType_t::PING) { + return "PING"; + } else { + return "SHUTDOWN"; + } +} + +int main() { + + std::vector requests; + requests.push_back(RequestType_t::SENDIMAGE); + requests.push_back(RequestType_t::SENDIMAGE); + requests.push_back(RequestType_t::SENDIMAGE); + requests.push_back(RequestType_t::PING); + requests.push_back(RequestType_t::SENDIMAGE); + requests.push_back(RequestType_t::SENDIMAGE); + requests.push_back(RequestType_t::SHUTDOWN); + + asio::io_context io_context_; + asio::streambuf buf; + + // Initialize the client + Client client(&io_context_, ip, port); + + // TODO: what happens when unable to connect? + client.connect(); + + // Start sending messages + for (RequestType_t request : requests) { + + std::cout << "Sending: " << convertEnumToString(request) << '\n'; + + // send the buffer + client.send(request); + + // TODO: should probably replace with handlePacket which will send() based on what was received? idk + client.receive(); + } + + client.showImages(); + + client.disconnect(); + + return EXIT_SUCCESS; +} diff --git a/tests/integration/sync_server_mock.cpp b/tests/integration/sync_server_mock.cpp new file mode 100644 index 00000000..15913155 --- /dev/null +++ b/tests/integration/sync_server_mock.cpp @@ -0,0 +1,43 @@ +#include +#include +#include +#include "include/network/sync_server.hpp" + +namespace asio = boost::asio; + +const std::string ip {"127.0.0.1"}; +constexpr uint32_t port {5000}; + +int main() { + + asio::io_context io_context_; + + Server server(&io_context_, ip, port); + + for (;;) { + + server.connect(); + + while (true) { + + server.receive(); + + if (server.getReadError() == asio::error::eof) { + // TODO: what else do we do? can we still use the current socket to accept connections? + break; + } + + // TODO: should probably replace with handlePacket which will send() based on what was received? idk + server.send(ResponseType_t::SUCC); + + } + + std::cout << "Client closed the connection" << '\n'; + + server.close(); + } + + server.shutdown(); + + return EXIT_SUCCESS; +} From 6e3b2569e4c3dd132edfa603c183adc412e8b71b Mon Sep 17 00:00:00 2001 From: Christopher Lee Date: Thu, 17 Apr 2025 18:01:13 -0700 Subject: [PATCH 10/43] change boost import --- deps/boost/boost.cmake | 8 +++++--- src/camera/CMakeLists.txt | 3 --- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/deps/boost/boost.cmake b/deps/boost/boost.cmake index 56d8a8d7..dc5c199e 100644 --- a/deps/boost/boost.cmake +++ b/deps/boost/boost.cmake @@ -7,8 +7,10 @@ function(target_add_boost target_name) DOWNLOAD_EXTRACT_TIMESTAMP true ) FetchContent_MakeAvailable(boost) - target_link_libraries(${target_name} PRIVATE - boost - ) + # target_link_libraries(${target_name} PRIVATE + # boost + # ) + target_include_directories(${target_name} PRIVATE ${boost_SOURCE_DIR}) + endfunction() \ No newline at end of file diff --git a/src/camera/CMakeLists.txt b/src/camera/CMakeLists.txt index e7764032..4be00e7d 100644 --- a/src/camera/CMakeLists.txt +++ b/src/camera/CMakeLists.txt @@ -3,10 +3,7 @@ set(LIB_NAME obcpp_camera) set(FILES interface.cpp mock.cpp -<<<<<<< HEAD rpi.cpp -======= ->>>>>>> main ) SET(LIB_DEPS From cb845f66c5fe6b44be2f23d17e3add853740abb0 Mon Sep 17 00:00:00 2001 From: Christopher Lee Date: Fri, 18 Apr 2025 12:41:03 -0700 Subject: [PATCH 11/43] added VERY INEFFICIENT way to make serialization to work --- deps/boost/boost.cmake | 60 +++++++++++++++++++++++++------- src/CMakeLists.txt | 5 ++- src/network/CMakeLists.txt | 4 +++ tests/integration/CMakeLists.txt | 37 ++++++++++++++++++++ 4 files changed, 93 insertions(+), 13 deletions(-) diff --git a/deps/boost/boost.cmake b/deps/boost/boost.cmake index dc5c199e..63ede52d 100644 --- a/deps/boost/boost.cmake +++ b/deps/boost/boost.cmake @@ -1,16 +1,52 @@ function(target_add_boost target_name) - include(FetchContent) + 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}) + 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() \ No newline at end of file +endfunction() + +# --- Just for Serialization ): --- +include(FetchContent) +message(STATUS "Setting up Boost 1.87.0 via FetchContent...") + +FetchContent_Declare( + Boost_Source + GIT_REPOSITORY https://github.com/boostorg/boost.git + GIT_TAG boost-1.87.0 +) + +set(Boost_USE_STATIC_LIBS ON CACHE BOOL "Force static Boost libraries" FORCE) + +# OPTION A (Example Variable - Verify Name): +# set(BOOST_LIBS_TO_BUILD serialization CACHE STRING "Specific Boost libraries to build" FORCE) +# OPTION B (Alternative Example - Verify Name): +# set(BOOST_INCLUDE_LIBRARIES serialization CACHE STRING "Specific Boost libraries to build" FORCE) + +message(WARNING "Boost build configuration needs the correct variable set to specify building 'serialization' for Boost 1.87.0 CMake superproject!") + + +message(STATUS "Boost_Source declared. Configuring and making available...") +FetchContent_MakeAvailable(Boost_Source) + +if(TARGET Boost::serialization) + get_target_property(BoostSerializationType Boost::serialization TYPE) + if(BoostSerializationType STREQUAL "INTERFACE_LIBRARY") + # This means CMake created a target but didn't link it to a compiled library + message(FATAL_ERROR "Boost::serialization target exists but is INTERFACE only. Boost build DID NOT compile serialization sources. Check Boost build configuration variables above (BOOST_LIBS_TO_BUILD or similar for 1.87.0)!") + else() + message(STATUS "Boost::serialization target type: ${BoostSerializationType}. Looks like a compiled library target exists.") + endif() + message(STATUS "Boost FetchContent setup complete.") +else() + message(FATAL_ERROR "Target Boost::serialization was NOT created by FetchContent_MakeAvailable(Boost_Source). Check Boost build configuration variables and logs in build/_deps/boost_source-subbuild/boost_source-populate-*.log") +endif() \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 0b8bb8d1..baa4561a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -43,7 +43,10 @@ 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 + Boost::serialization +) target_add_protobuf(${PROJECT_NAME}) target_add_torch(${PROJECT_NAME}) target_add_torchvision(${PROJECT_NAME}) diff --git a/src/network/CMakeLists.txt b/src/network/CMakeLists.txt index c1d679df..94513212 100644 --- a/src/network/CMakeLists.txt +++ b/src/network/CMakeLists.txt @@ -5,6 +5,8 @@ set(COMMON_FILES gcs_routes.cpp gcs.cpp mavlink.cpp + sync_client.cpp + sync_server.cpp ) set(FILES @@ -32,6 +34,7 @@ add_library(${LIB_NAME} STATIC target_link_libraries(${LIB_NAME} PRIVATE ${LIB_DEPS} + Boost::serialization ) set_unity_for_target(${LIB_NAME}) @@ -58,6 +61,7 @@ add_library(${LIB_NAME} STATIC ${PROJECT_SOURCE_DIR}/build/gen_protos/protos/obc target_link_libraries(${LIB_NAME} PRIVATE ${LIB_DEPS} + Boost::serialization ) set_unity_for_target(${LIB_NAME}) diff --git a/tests/integration/CMakeLists.txt b/tests/integration/CMakeLists.txt index b1f9e6d7..1f3b2592 100644 --- a/tests/integration/CMakeLists.txt +++ b/tests/integration/CMakeLists.txt @@ -293,6 +293,43 @@ target_add_loguru(camera_mock) target_include_directories(camera_mock PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(camera_mock PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) +add_executable(sync_client_mock "sync_client_mock.cpp") +target_link_libraries(sync_client_mock PRIVATE + obcpp_lib + Boost::serialization +) +target_include_directories(sync_client_mock PRIVATE ${INCLUDE_DIRECTORY}) +target_add_boost(sync_client_mock) +target_add_protobuf(sync_client_mock) +target_add_torch(sync_client_mock) +target_add_torchvision(sync_client_mock) +target_add_json(sync_client_mock) +target_add_opencv(sync_client_mock) +target_add_httplib(sync_client_mock) +target_add_mavsdk(sync_client_mock) +target_add_matplot(sync_client_mock) +target_add_loguru(sync_client_mock) +target_include_directories(sync_client_mock PRIVATE ${ImageMagick_INCLUDE_DIRS}) +target_link_libraries(sync_client_mock PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) + +add_executable(sync_server_mock "sync_server_mock.cpp") +target_link_libraries(sync_server_mock PRIVATE + obcpp_lib + Boost::serialization +) +target_include_directories(sync_server_mock PRIVATE ${INCLUDE_DIRECTORY}) +target_add_boost(sync_server_mock) +target_add_protobuf(sync_server_mock) +target_add_torch(sync_server_mock) +target_add_torchvision(sync_server_mock) +target_add_json(sync_server_mock) +target_add_opencv(sync_server_mock) +target_add_httplib(sync_server_mock) +target_add_mavsdk(sync_server_mock) +target_add_matplot(sync_server_mock) +target_add_loguru(sync_server_mock) +target_include_directories(sync_server_mock PRIVATE ${ImageMagick_INCLUDE_DIRS}) +target_link_libraries(sync_server_mock PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) # cuda_check add_executable(cuda_check ${SOURCES} cuda_check.cpp) target_add_torch(cuda_check) From a396fa6787fb25a864b0973fb39124b351dc12f7 Mon Sep 17 00:00:00 2001 From: smhitle <91502030+smhitle@users.noreply.github.com> Date: Mon, 5 May 2025 18:02:36 -0700 Subject: [PATCH 12/43] Changes to the client. Modified interface.hpp/.cpp to match the required functionality. Need to further change to support the RPI functionality. --- deps/boost/boost.cmake | 23 +- include/camera/interface.hpp | 58 +--- include/camera/rpi.hpp | 180 ++++-------- include/camera/rpi.hpp~ | 149 ---------- include/network/async_client.hpp | 6 +- include/network/camera_client.hpp | 13 +- include/network/camera_data.hpp | 6 +- include/network/camera_server.hpp | 5 + include/network/client.hpp | 51 ++++ include/network/serialize.hpp | 5 + include/network/sync_client.hpp | 108 -------- include/network/sync_server.hpp | 5 + src/camera/CMakeLists.txt | 2 +- src/camera/interface.cpp | 28 +- src/camera/rpi.cpp | 366 ++++--------------------- src/core/CMakeLists.txt | 2 +- src/network/CMakeLists.txt | 7 +- src/network/camera_client.cpp | 52 ++-- src/network/camera_server.cpp | 4 +- src/network/camera_socket.cpp | 4 +- src/network/client.cpp | 215 +++++++++++++++ src/network/sync_client.cpp | 151 ---------- tests/integration/CMakeLists.txt | 2 +- tests/integration/camera_lucid.cpp | 48 ++++ tests/integration/sync_client_mock.cpp | 71 +---- 25 files changed, 522 insertions(+), 1039 deletions(-) delete mode 100644 include/camera/rpi.hpp~ create mode 100644 include/network/client.hpp delete mode 100644 include/network/sync_client.hpp create mode 100644 src/network/client.cpp delete mode 100644 src/network/sync_client.cpp create mode 100644 tests/integration/camera_lucid.cpp diff --git a/deps/boost/boost.cmake b/deps/boost/boost.cmake index 63ede52d..bb63b89f 100644 --- a/deps/boost/boost.cmake +++ b/deps/boost/boost.cmake @@ -1,17 +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}) - + 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() @@ -49,4 +48,4 @@ if(TARGET Boost::serialization) message(STATUS "Boost FetchContent setup complete.") else() message(FATAL_ERROR "Target Boost::serialization was NOT created by FetchContent_MakeAvailable(Boost_Source). Check Boost build configuration variables and logs in build/_deps/boost_source-subbuild/boost_source-populate-*.log") -endif() \ No newline at end of file +endif() diff --git a/include/camera/interface.hpp b/include/camera/interface.hpp index ef7b3df5..90fd25bd 100644 --- a/include/camera/interface.hpp +++ b/include/camera/interface.hpp @@ -20,7 +20,6 @@ using json = nlohmann::json; using Mat = cv::Mat; -// struct to contain all telemetry that should be tagged with an image. struct ImageTelemetry { double latitude_deg; double longitude_deg; @@ -32,15 +31,6 @@ struct ImageTelemetry { double roll_deg; }; -/** - * Given a shared ptr to a mavlink client, query it for the telemetry - * information needed for ImageTelemetry and the CV pipeline. - * - * If nullptr is passed for mavlinkClient, nullopt is returned -*/ -std::optional queryMavlinkImageTelemetry( - std::shared_ptr mavlinkClient); - struct ImageData { cv::Mat DATA; uint64_t TIMESTAMP; @@ -62,41 +52,17 @@ void saveImageTelemetryToFile(const ImageTelemetry& telemetry, const std::filesystem::path& filepath); class CameraInterface { - protected: - CameraConfig config; - - public: - explicit CameraInterface(const CameraConfig& config); - virtual ~CameraInterface() = default; - - virtual void connect() = 0; - virtual bool isConnected() = 0; - - /** - * Start taking photos at an interval in a background thread. - * Also requires a shared_ptr to a MavlinkClient to tag - * images with flight telemetry at capture time. - */ - virtual void startTakingPictures(const std::chrono::milliseconds& interval, - std::shared_ptr mavlinkClient) = 0; - /** - * Close background thread started by startTakingPictures - */ - virtual void stopTakingPictures() = 0; - - // Get the latest buffered image - virtual std::optional getLatestImage() = 0; - // Get all the recently buffered images - virtual std::deque getAllImages() = 0; - - virtual void startStreaming() = 0; - - /** - * Blocking call that takes an image. If it takes longer than the timeout - * to capture the image, no image is returned. - */ - virtual std::optional takePicture(const std::chrono::milliseconds& timeout, - std::shared_ptr mavlinkClient) = 0; -}; + protected: + CameraConfig config; + + public: + explicit CameraInterface(const CameraConfig& config); + // explicit CameraInterface(); + virtual ~CameraInterface() = default; + + virtual void connect() = 0; + + virtual std::optional takePicture(const std::chrono::milliseconds& timeout) = 0; + }; #endif // INCLUDE_CAMERA_INTERFACE_HPP_ diff --git a/include/camera/rpi.hpp b/include/camera/rpi.hpp index e80f8c67..74e07cf3 100644 --- a/include/camera/rpi.hpp +++ b/include/camera/rpi.hpp @@ -1,6 +1,6 @@ -#ifdef ARENA_SDK_INSTALLED +#ifndef INCLUDE_CAMERA_RPI_HPP_ +#define INCLUDE_CAMERA_RPI_HPP_ -#include #include #include @@ -18,132 +18,54 @@ using json = nlohmann::json; using namespace std::chrono_literals; // NOLINT -/** - * LucidCamera is the class to interface with cameras from - * LUCID Vision Labs (https://thinklucid.com/) that use their - * Arena SDK (https://thinklucid.com/arena-software-development-kit/). - * - * This class is thread safe, meaning that you can access camera resources - * across different threads even though there's one physical camera. -*/ -class RaspPi : public CameraInterface { - public: - explicit LucidCamera(CameraConfig config); - ~LucidCamera(); - - /** - * Connect to the LUCID camera. Note that this function will synchronously - * block indefintely until a connection to the camera can be established. - */ - void connect() override; - bool isConnected() override; - - /** - * Start taking photos at an interval in a background thread. - * Also requires a shared_ptr to a MavlinkClient to tag - * images with flight telemetry at capture time. - */ - void startTakingPictures(const std::chrono::milliseconds& interval, - std::shared_ptr mavlinkClient) override; - - /** - * Close background thread started by startTakingPictures - */ - void stopTakingPictures() override; - - /** - * Get the latest image that the camera took. This pops the latest - * image from a queue of images which means that the same image won't. - * be returned in two subsequent calls - */ - std::optional getLatestImage() override; - - /** - * getAllImages returns a queue of all the images that have been - * cached since the last call to getAllImages. Once this is called, - * it returns the cached images and clears the internal cache. - */ - std::deque getAllImages() override; - - /** - * Blocking call that takes an image. If it takes longer than the timeout - * to capture the image, no image is returned. - */ - std::optional takePicture(const std::chrono::milliseconds& timeout, - std::shared_ptr mavlinkClient) override; - void startStreaming() override; - - private: - /** - * Takes an image and sleeps for the specified interval before - * taking another image - */ - void captureEvery(const std::chrono::milliseconds& interval, - std::shared_ptr mavlinkClient); - - /** - * Converts between ArenaSDK Image type to an OpenCV cv::Mat - */ - std::optional imgConvert(Arena::IImage* pImage); - - void configureSettings(); - - // Lock around Arena system. - inline static std::shared_mutex arenaSystemLock; - // Arena system must be static since only a single system - // can be active on the same machine regardless of how - // many LucidCamera instances are created. - inline static Arena::ISystem* system; - - // Lock around Arena device. - inline static std::shared_mutex arenaDeviceLock; - // Arena device must be static since only a single device - // can be active on the same machine regardless of how - // many LucidCamera instances are created. - inline static Arena::IDevice* device; - - std::atomic_bool isTakingPictures; - - std::deque imageQueue; - std::shared_mutex imageQueueLock; - - // thread that will capture images on a set interval - std::thread captureThread; - - std::shared_mutex imageQueueMut; - - const std::chrono::milliseconds connectionTimeout = 1000ms; - const std::chrono::milliseconds connectionRetry = 500ms; - - // TODO: need to catch timeout exception - const std::chrono::milliseconds takePictureTimeout = 1000ms; +#include "interface.hpp" +#include "network/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 = 'p'; +const std::uint8_t END_REQUEST = 'e'; +const std::uint8_t LOCK_REQUEST = 'l'; + +class RPICamera : public CameraInterface { + private: + Client client; + asio::io_context io_context_; + + 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? + + std::atomic_bool isConnected; + + // TODO: do we need these? + // const std::chrono::milliseconds connectionTimeout = 1000ms; + // const std::chrono::milliseconds connectionRetry = 500ms; + // const std::chrono::milliseconds takePictureTimeout = 1000ms; + + std::optional imgConvert(std::vector imgbuf); + + public: + + explicit RPICamera(CameraConfig config, asio::io_context* io_context_); + // explicit RPICamera(asio::io_context* io_context_); + + // TODO: destructor? + // ~RPICamera(); + + void connect() override; + // bool isConnected() override; + + std::optional takePicture(const std::chrono::milliseconds& timeout) override; // TODO: mavlink + + // TODO: unsure how to implement + // void ping(); }; -#define SINGLE_ARG(...) __VA_ARGS__ - -/** - * Macro to gracefully catch Arena SDK exceptions, print an error, - * and not propogate the exceptions and crash the OBC. - * - * context is a C string (char*) that explains what the code is doing - * - * code is the code itself -*/ -#define CATCH_ARENA_EXCEPTION(context, code) \ - try { \ - code \ - } \ - catch (GenICam::GenericException &ge) { \ - LOG_F(ERROR, "GenICam exception thrown when %s: %s", context, ge.what()); \ - } \ - catch (std::exception &ex) { \ - LOG_F(ERROR, "Standard exception thrown when %s: %s", context, ex.what()); \ - } \ - catch (...) { \ - LOG_F(ERROR, "Unexpected exception thrown when %s", context); \ - } \ - LOG_F(INFO, "%s succeeded", context) - -#endif // ARENA_SDK_INSTALLED - -#endif // INCLUDE_CAMERA_LUCID_HPP_ +#endif diff --git a/include/camera/rpi.hpp~ b/include/camera/rpi.hpp~ deleted file mode 100644 index e80f8c67..00000000 --- a/include/camera/rpi.hpp~ +++ /dev/null @@ -1,149 +0,0 @@ -#ifdef ARENA_SDK_INSTALLED - -#include - -#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 - -/** - * LucidCamera is the class to interface with cameras from - * LUCID Vision Labs (https://thinklucid.com/) that use their - * Arena SDK (https://thinklucid.com/arena-software-development-kit/). - * - * This class is thread safe, meaning that you can access camera resources - * across different threads even though there's one physical camera. -*/ -class RaspPi : public CameraInterface { - public: - explicit LucidCamera(CameraConfig config); - ~LucidCamera(); - - /** - * Connect to the LUCID camera. Note that this function will synchronously - * block indefintely until a connection to the camera can be established. - */ - void connect() override; - bool isConnected() override; - - /** - * Start taking photos at an interval in a background thread. - * Also requires a shared_ptr to a MavlinkClient to tag - * images with flight telemetry at capture time. - */ - void startTakingPictures(const std::chrono::milliseconds& interval, - std::shared_ptr mavlinkClient) override; - - /** - * Close background thread started by startTakingPictures - */ - void stopTakingPictures() override; - - /** - * Get the latest image that the camera took. This pops the latest - * image from a queue of images which means that the same image won't. - * be returned in two subsequent calls - */ - std::optional getLatestImage() override; - - /** - * getAllImages returns a queue of all the images that have been - * cached since the last call to getAllImages. Once this is called, - * it returns the cached images and clears the internal cache. - */ - std::deque getAllImages() override; - - /** - * Blocking call that takes an image. If it takes longer than the timeout - * to capture the image, no image is returned. - */ - std::optional takePicture(const std::chrono::milliseconds& timeout, - std::shared_ptr mavlinkClient) override; - void startStreaming() override; - - private: - /** - * Takes an image and sleeps for the specified interval before - * taking another image - */ - void captureEvery(const std::chrono::milliseconds& interval, - std::shared_ptr mavlinkClient); - - /** - * Converts between ArenaSDK Image type to an OpenCV cv::Mat - */ - std::optional imgConvert(Arena::IImage* pImage); - - void configureSettings(); - - // Lock around Arena system. - inline static std::shared_mutex arenaSystemLock; - // Arena system must be static since only a single system - // can be active on the same machine regardless of how - // many LucidCamera instances are created. - inline static Arena::ISystem* system; - - // Lock around Arena device. - inline static std::shared_mutex arenaDeviceLock; - // Arena device must be static since only a single device - // can be active on the same machine regardless of how - // many LucidCamera instances are created. - inline static Arena::IDevice* device; - - std::atomic_bool isTakingPictures; - - std::deque imageQueue; - std::shared_mutex imageQueueLock; - - // thread that will capture images on a set interval - std::thread captureThread; - - std::shared_mutex imageQueueMut; - - const std::chrono::milliseconds connectionTimeout = 1000ms; - const std::chrono::milliseconds connectionRetry = 500ms; - - // TODO: need to catch timeout exception - const std::chrono::milliseconds takePictureTimeout = 1000ms; -}; - -#define SINGLE_ARG(...) __VA_ARGS__ - -/** - * Macro to gracefully catch Arena SDK exceptions, print an error, - * and not propogate the exceptions and crash the OBC. - * - * context is a C string (char*) that explains what the code is doing - * - * code is the code itself -*/ -#define CATCH_ARENA_EXCEPTION(context, code) \ - try { \ - code \ - } \ - catch (GenICam::GenericException &ge) { \ - LOG_F(ERROR, "GenICam exception thrown when %s: %s", context, ge.what()); \ - } \ - catch (std::exception &ex) { \ - LOG_F(ERROR, "Standard exception thrown when %s: %s", context, ex.what()); \ - } \ - catch (...) { \ - LOG_F(ERROR, "Unexpected exception thrown when %s", context); \ - } \ - LOG_F(INFO, "%s succeeded", context) - -#endif // ARENA_SDK_INSTALLED - -#endif // INCLUDE_CAMERA_LUCID_HPP_ diff --git a/include/network/async_client.hpp b/include/network/async_client.hpp index c70c165b..c9b9677f 100644 --- a/include/network/async_client.hpp +++ b/include/network/async_client.hpp @@ -1,11 +1,11 @@ +#ifndef INCLUDE_NETWORK_ASYNC_CLIENT_HPP_ +#define INCLUDE_NETWORK_ASYNC_CLIENT_HPP_ + #include #include #include #include "camera_data.hpp" -#ifndef CLIENT_H -#define CLIENT_H - namespace asio = boost::asio; class Client { diff --git a/include/network/camera_client.hpp b/include/network/camera_client.hpp index c474810e..5343a3bb 100644 --- a/include/network/camera_client.hpp +++ b/include/network/camera_client.hpp @@ -1,3 +1,6 @@ +#ifndef INCLUDE_NETWORK_CAMERA_CLIENT_HPP_ +#define INCLUDE_NETWORK_CAMERA_CLIENT_HPP_ + #include #include #include @@ -39,7 +42,9 @@ class CameraClient { private: // ig? - string ip; - int port; - asio::tcp::socket socket; -}; \ No newline at end of file + // string ip; + // int port; + // asio::tcp::socket socket; +}; + +#endif diff --git a/include/network/camera_data.hpp b/include/network/camera_data.hpp index 3f125089..87e95150 100644 --- a/include/network/camera_data.hpp +++ b/include/network/camera_data.hpp @@ -1,12 +1,12 @@ +#ifndef INCLUDE_NETWORK_CAMERA_DATA_HPP_ +#define INCLUDE_NETWORK_CAMERA_DATA_HPP_ + #include #include #include #include #include -#ifndef CAMERA_DATA_H -#define CAMERA_DATA_H - struct ImageData_t { int width; diff --git a/include/network/camera_server.hpp b/include/network/camera_server.hpp index 74c01f3e..0de0f5fd 100644 --- a/include/network/camera_server.hpp +++ b/include/network/camera_server.hpp @@ -1,3 +1,6 @@ +#ifndef INCLUDE_NETWORK_CAMERA_SERVER_HPP_ +#define INCLUDE_NETWORK_CAMERA_SERVER_HPP_ + #include #include #include @@ -51,3 +54,5 @@ class ObcCameraServer asio::ip::tcp::socket socket_; asio::ip::tcp::acceptor acceptor_; }; + +#endif diff --git a/include/network/client.hpp b/include/network/client.hpp new file mode 100644 index 00000000..227c0e6f --- /dev/null +++ b/include/network/client.hpp @@ -0,0 +1,51 @@ +#ifndef INCLUDE_NETWORK_CLIENT_HPP_ +#define INCLUDE_NETWORK_CLIENT_HPP_ + +#include +#include +#include +#include "camera_data.hpp" + +// #include +// #include + +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. + void send(std::uint8_t request); + + /** + * + */ + // TODO: not sure what the return type should be, std::vector ? + std::vector recv(const int bufSize); + + std::string getIP(); + + int getPort(); + +}; + +#endif diff --git a/include/network/serialize.hpp b/include/network/serialize.hpp index b2ab93f2..e05621d2 100644 --- a/include/network/serialize.hpp +++ b/include/network/serialize.hpp @@ -1,3 +1,6 @@ +#ifndef INCLUDE_NETWORK_SERIALIZE_HPP_ +#define INCLUDE_NETWORK_SERIALIZE_HPP_ + #include /* @@ -21,3 +24,5 @@ namespace serialh { ia >> *response; } } + +#endif diff --git a/include/network/sync_client.hpp b/include/network/sync_client.hpp deleted file mode 100644 index 696b3962..00000000 --- a/include/network/sync_client.hpp +++ /dev/null @@ -1,108 +0,0 @@ -#include -#include -#include -#include "camera_data.hpp" -#include "serialize.hpp" - -#ifndef CLIENT_H -#define CLIENT_H - -namespace asio = boost::asio; - -class Client { - private: - asio::ip::tcp::socket socket_; - std::string ip; - int port; - - // underlying buffer for communication - asio::streambuf sendbuf; - asio::streambuf receivebuf; - - // TODO: these error codes are useless if we handle them on the spot of failure - - // free functions error codes - boost::system::error_code ec_write; - boost::system::error_code ec_read; - - // socket specific error codes - boost::system::error_code ec_shutdown; - boost::system::error_code ec_close; - - // image buffer - std::vector images; - - /** - * Creates the request from the request type - */ - CameraRequest_t createRequest(RequestType_t requestType); - - /** - * Creates a packet to send based off request type - */ - void createRequestPacket(RequestType_t requestType); - - /** - * Send the header of the packet - */ - void sendHeader(); - - /** - * Send the body of the packet - */ - void sendBody(); - - /** - * Read in the header of the packet - * - * @return std::uint32_t Size of the body to be received - */ - std::uint32_t receiveHeader(); - - /** - * Read in the body of the packet - * - * @param size Size of packet body - */ - void receiveBody(std::uint32_t size); - - /** - * Deconstructs the received packet - */ - CameraResponse_t deconstructPacket(); - - /** - * Handler for what is received - */ - void handlePacket(); - - public: - Client(asio::io_context* io_context_, std::string ip, int port); - - /** - * Connects to the server with the specified ip and port - */ - void connect(); - - /** - * Sends a packet to the server - */ - void send(RequestType_t requestType); - - /** - * Reads in packets send from the server - */ - void receive(); - - /** - * Closes the connection between client and server - */ - void disconnect(); - - /** - * This is a testing function to see what images are in the image buffer - */ - void showImages(); -}; - -#endif diff --git a/include/network/sync_server.hpp b/include/network/sync_server.hpp index 84d96077..b9e53e6f 100644 --- a/include/network/sync_server.hpp +++ b/include/network/sync_server.hpp @@ -1,3 +1,6 @@ +#ifndef INCLUDE_NETWORK_SYNC_SERVER_HPP__ +#define INCLUDE_NETWORK_SYNC_SERVER_HPP__ + #include #include #include "camera_data.hpp" @@ -112,3 +115,5 @@ class Server { boost::system::error_code getWriteError(); }; + +#endif diff --git a/src/camera/CMakeLists.txt b/src/camera/CMakeLists.txt index 4be00e7d..b1d880a0 100644 --- a/src/camera/CMakeLists.txt +++ b/src/camera/CMakeLists.txt @@ -32,4 +32,4 @@ target_add_matplot(${LIB_NAME}) target_add_loguru(${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 80c7fa94..ac913655 100644 --- a/src/camera/interface.cpp +++ b/src/camera/interface.cpp @@ -39,37 +39,12 @@ void saveImageTelemetryToFile(const ImageTelemetry& telemetry, 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(); - - 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 { try { std::filesystem::path save_dir = directory; @@ -83,6 +58,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 index c264fb52..9b6dfd41 100644 --- a/src/camera/rpi.cpp +++ b/src/camera/rpi.cpp @@ -1,5 +1,3 @@ -#ifdef ARENA_SDK_INSTALLED - #include #include #include @@ -19,342 +17,78 @@ #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();); +#include "camera/rpi.hpp" - 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()); +const uint32_t IMG_WIDTH = 2028; +const uint32_t IMG_HEIGHT = 1520; +const uint32_t BUFFER_SIZE = IMG_WIDTH * IMG_HEIGHT * 3 / 2; - 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); +RPICamera::RPICamera(asio::io_context* io_context_) : CameraInterface(config), client(io_context_, SERVER_IP, SERVER_PORT) { +// TODO: do we need this? not sure how we're configuring the camera if that's being done on Daniel's side - 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); -} +// RPICamera::RPICamera(asio::io_context* io_context_) : client(io_context_, SERVER_IP, SERVER_PORT) { +// // // TODO: do we need this? not sure how we're configuring the camera if that's being done on Daniel's side + +// } -void raspCam::stopTakingPictures() { - if (!this->isTakingPictures) { +// TODO +void RPICamera::connect() { + if (this->isConnected == true) { 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; + + while (!isConnected) { + // this->isConnected = this->client->connect(); + this->isConnected = client.connect(); + } + + // TODO: switch to LOG_F? + std::cout << "Connected to: " << client.getIP() << " on port: " << client.getPort() << '\n'; + + // tells the camera to start the camera thread + client.send(START_REQUEST); } -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; -} +std::optional RPICamera::takePicture(const std::chrono::milliseconds& timeout) { + + // client sends a request to take a pictures + client.send(PICTURE_REQUEST); -void raspCam::captureEvery(const std::chrono::milliseconds& interval) { - loguru::set_thread_name("raspiberry pi camera"); - if (!this->isConnected()) { - LOG_F(ERROR, "RPI Camera not connected. Cannot capture photos"); - return; - } + // client receives a response TODO: might have to adjust the datatype + std::vector imgbuf = client.recv(BUFFER_SIZE); - this->arenaDeviceLock.lock(); - CATCH_ARENA_EXCEPTION("starting stream", - this->device->StartStream();); - this->arenaDeviceLock.unlock(); + // TODO: have to parse the imgbuf possibly depending on how the response is structured? - while (this->isTakingPictures) { - LOG_F(INFO, "Taking picture with RPI camera"); - std::optional newImage = - this->takePicture(this->takePictureTimeout); + // give the image buffer to imgConvert + std::optional mat = imgConvert(imgbuf); - 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()); - } + if (!mat.has_value()) { + return {}; + } - std::this_thread::sleep_for(interval); - } + uint64_t timestamp = getUnixTime_s().count(); - // TODO figure out nondeterministic seg fault /shrug - // this->arenaDeviceLock.lock(); - // CATCH_ARENA_EXCEPTION("stopping stream", - // this->device->StopStream();); - // this->arenaDeviceLock.unlock(); + return ImageData { + .DATA = mat.value(), + .TIMESTAMP = timestamp, + .TELEMETRY = {} + }; } -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"); +std::optional RPICamera::imgConvert(std::vector buf) { + // TODO: if the sizes don't match return nullopt + if (buf.size() != BUFFER_SIZE) { 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(); + // put raw bytes in cv::Mat + cv::Mat yuv420_img(IMG_HEIGHT * 3 / 2, IMG_WIDTH, CV_8UC1, buf.data()); - // freeing underlying lucid buffers - Arena::ImageFactory::Destroy(pConverted); + cv::Mat bgr_img(IMG_HEIGHT, IMG_WIDTH, CV_8UC3); + cv::cvtColor(yuv420_img, bgr_img, cv::COLOR_YUV420p2BGR); // TODO: not sure if this is the right color space - return mat;); - - // return nullopt if an exception is thrown during conversion - return {}; + return bgr_img; } - -#endif // ARENA_SDK_INSTALLED diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index 9c4bac18..b2517eee 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -37,4 +37,4 @@ target_add_matplot(${LIB_NAME}) target_add_loguru(${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 94513212..7ff5eacf 100644 --- a/src/network/CMakeLists.txt +++ b/src/network/CMakeLists.txt @@ -5,8 +5,7 @@ set(COMMON_FILES gcs_routes.cpp gcs.cpp mavlink.cpp - sync_client.cpp - sync_server.cpp + client.cpp ) set(FILES @@ -75,6 +74,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/camera_client.cpp b/src/network/camera_client.cpp index 31594b2d..02b1ece3 100644 --- a/src/network/camera_client.cpp +++ b/src/network/camera_client.cpp @@ -1,40 +1,40 @@ #include #include #include -#include "network/camera_data.hpp" -#include "network/camera_client.hpp" +#include "include/network/camera_data.hpp" +#include "include/network/camera_client.hpp" namespace asio = boost::asio; -CameraClient::CameraClient(int port) -{ - this->port=port; -} +// CameraClient::CameraClient(int port) +// { +// this->port=port; +// } -bool CameraClient::connect(asio::io_context *io_context, std::string ip, int port) -{ - // create an asio endpoint - asio::ip::tcp::endpoint endpoint(asio::make_adress(ip), port); +// bool CameraClient::connect(asio::io_context *io_context, std::string ip, int port) +// { +// // create an asio endpoint +// asio::ip::tcp::endpoint endpoint(asio::make_adress(ip), port); - // create a socket - this->socket(&io_context); +// // create a socket +// this->socket(&io_context); - // connect socket - this->socket::connect(&endpoint); +// // connect socket +// this->socket::connect(&endpoint); - // TODO: error handling +// // TODO: error handling - return true; -} +// return true; +// } -// thinking if we just gonna run the same buff size and img proportions? -bool CameraClient::send(std::vector image) -{ - // TODO: after finishing working version, try to serialize and send whole data type - asio::buffer::const_buffer image_buffer = asio::buffer(&image, sizeof(image)); +// // thinking if we just gonna run the same buff size and img proportions? +// bool CameraClient::send(std::vector image) +// { +// // TODO: after finishing working version, try to serialize and send whole data type +// asio::buffer::const_buffer image_buffer = asio::buffer(&image, sizeof(image)); - //TODO: PUT ERROR DATA TYPE VAR HERE +// //TODO: PUT ERROR DATA TYPE VAR HERE - asio::socket::write(socket, image_buffrer); // TODO: PUT ERROR HANDLER HERE +// asio::socket::write(socket, image_buffrer); // TODO: PUT ERROR HANDLER HERE - return true; -} +// return true; +// } diff --git a/src/network/camera_server.cpp b/src/network/camera_server.cpp index 94040f16..5fe71a02 100644 --- a/src/network/camera_server.cpp +++ b/src/network/camera_server.cpp @@ -1,8 +1,8 @@ #include #include #include -#include "camera_data.hpp" -#include "camera_server.hpp" +#include "include/network/camera_data.hpp" +#include "include/network/camera_server.hpp" // https://www.codeproject.com/Articles/1264257/Socket-Programming-in-Cplusplus-using-boost-asio-T namespace asio = boost::asio; diff --git a/src/network/camera_socket.cpp b/src/network/camera_socket.cpp index a5563a11..3b0e9bbe 100644 --- a/src/network/camera_socket.cpp +++ b/src/network/camera_socket.cpp @@ -10,6 +10,6 @@ using ip::tcp; tcp::socket socket; -CameraSocket::CameraSocket() { +// CameraSocket::CameraSocket() { -} +// } diff --git a/src/network/client.cpp b/src/network/client.cpp new file mode 100644 index 00000000..9133ad85 --- /dev/null +++ b/src/network/client.cpp @@ -0,0 +1,215 @@ +#include "network/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) { + return false; + } + + return true; +} + +void Client::send(std::uint8_t request) { + boost::system::error_code ec; + + 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? + } + +} + +std::vector Client::recv(const int bufSize) { + + // TODO: not sure if this is very efficient or if there's a better way to do this + + // asio::streambuf recvbuf; + boost::system::error_code ec; + + std::vector recvbuf(bufSize); + + asio::read(this->socket_, asio::buffer(recvbuf), ec); + + if (ec) { + // TODO: what to do when read fails + } + + // recvbuf.commit(bufSize); + + return recvbuf; +} + +std::string Client::getIP() { + return this->ip; +} + +int Client::getPort() { + return this->port; +} + + + +// #include "include/network/sync_client.hpp" + +// CameraRequest_t Client::createRequest(RequestType_t requestType) { + +// CameraRequest_t request; +// request.pid = 6969; +// request.requestType = requestType; + +// return request; + +// } + +// void Client::createRequestPacket(RequestType_t requestType) { + +// CameraRequest_t request = createRequest(requestType); + +// serialh::serialize(&request, &this->sendbuf); + +// } + +// void Client::sendHeader() { + +// std::uint32_t headerLength = static_cast(this->sendbuf.size()); + +// // TODO: error handler +// asio::write(this->socket_, asio::buffer(&headerLength, sizeof(headerLength))); + +// } + +// void Client::sendBody() { + +// // TODO: error handler +// asio::write(this->socket_, this->sendbuf); + +// } + +// std::uint32_t Client::receiveHeader() { + +// std::uint32_t size {}; + +// // TODO: error handler +// asio::read(this->socket_, asio::buffer(&size, sizeof(size))); + +// return size; + +// } + +// void Client::receiveBody(std::uint32_t size) { + +// // TODO: error handler +// asio::read(this->socket_, this->receivebuf.prepare(size)); +// this->receivebuf.commit(size); + +// } + +// CameraResponse_t Client::deconstructPacket() { + +// CameraResponse_t response; + +// serialh::deserialize(&response, &this->receivebuf); + +// return response; + +// } + +// void Client::handlePacket() { + +// // deconstruct packet +// CameraResponse_t response = deconstructPacket(); + +// std::vector image = response.imageData.imgBuffer; + +// cv::Mat m = cv::Mat(response.imageData.height, response.imageData.width, CV_8UC3); + +// if (image.size() == (response.imageData.imageSizeBytes)) { +// std::memcpy(m.data, image.data(), sizeof(std::uint8_t) * response.imageData.imageSizeBytes); +// } + +// // Add image to image buffer +// this->images.push_back(m); +// } + +// Client::Client(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_) { + +// this->ip = ip; +// this->port = port; + +// } + +// void Client::connect() { + +// // TODO: difference between exception handling and error handling +// try { + +// asio::ip::tcp::endpoint endpoint_(asio::ip::tcp::endpoint(asio::ip::make_address(this->ip), this->port)); + +// this->socket_.connect(endpoint_); + +// std::cout << "Connected to: " << this->ip << " on port " << this->port << '\n'; + +// } catch (std::exception& e) { + +// std::cout << e.what() << '\n'; + +// } + +// } + +// void Client::send(RequestType_t requestType) { + +// createRequestPacket(requestType); + +// // send a header containing the length/size +// sendHeader(); + +// // send the serialized data in the streambuf +// sendBody(); + +// } + +// void Client::receive() { + +// std::uint32_t bodySize = receiveHeader(); +// receiveBody(bodySize); + +// handlePacket(); + +// } + +// void Client::disconnect() { + +// // TODO: error handling +// this->socket_.shutdown(asio::ip::tcp::socket::shutdown_both); +// this->socket_.close(); + +// } + +// void Client::showImages(){ + +// for (cv::Mat img : this->images) { + +// std::string windowName = "Window"; +// cv::namedWindow(windowName); +// cv::resizeWindow(windowName, 200, 200); +// cv::imshow(windowName, img); + +// cv::waitKey(0); + +// cv::destroyWindow(windowName); +// } +// } diff --git a/src/network/sync_client.cpp b/src/network/sync_client.cpp deleted file mode 100644 index 1e97f361..00000000 --- a/src/network/sync_client.cpp +++ /dev/null @@ -1,151 +0,0 @@ -#include "include/network/sync_client.hpp" - -CameraRequest_t Client::createRequest(RequestType_t requestType) { - - CameraRequest_t request; - request.pid = 6969; - request.requestType = requestType; - - return request; - -} - -void Client::createRequestPacket(RequestType_t requestType) { - - CameraRequest_t request = createRequest(requestType); - - serialh::serialize(&request, &this->sendbuf); - -} - -void Client::sendHeader() { - - std::uint32_t headerLength = static_cast(this->sendbuf.size()); - - // TODO: error handler - asio::write(this->socket_, asio::buffer(&headerLength, sizeof(headerLength))); - -} - -void Client::sendBody() { - - // TODO: error handler - asio::write(this->socket_, this->sendbuf); - -} - -std::uint32_t Client::receiveHeader() { - - std::uint32_t size {}; - - // TODO: error handler - asio::read(this->socket_, asio::buffer(&size, sizeof(size))); - - return size; - -} - -void Client::receiveBody(std::uint32_t size) { - - // TODO: error handler - asio::read(this->socket_, this->receivebuf.prepare(size)); - this->receivebuf.commit(size); - -} - -CameraResponse_t Client::deconstructPacket() { - - CameraResponse_t response; - - serialh::deserialize(&response, &this->receivebuf); - - return response; - -} - -void Client::handlePacket() { - - // deconstruct packet - CameraResponse_t response = deconstructPacket(); - - std::vector image = response.imageData.imgBuffer; - - cv::Mat m = cv::Mat(response.imageData.height, response.imageData.width, CV_8UC3); - - if (image.size() == (response.imageData.imageSizeBytes)) { - std::memcpy(m.data, image.data(), sizeof(std::uint8_t) * response.imageData.imageSizeBytes); - } - - // Add image to image buffer - this->images.push_back(m); -} - -Client::Client(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_) { - - this->ip = ip; - this->port = port; - -} - -void Client::connect() { - - // TODO: difference between exception handling and error handling - try { - - asio::ip::tcp::endpoint endpoint_(asio::ip::tcp::endpoint(asio::ip::make_address(this->ip), this->port)); - - this->socket_.connect(endpoint_); - - std::cout << "Connected to: " << this->ip << " on port " << this->port << '\n'; - - } catch (std::exception& e) { - - std::cout << e.what() << '\n'; - - } - -} - -void Client::send(RequestType_t requestType) { - - createRequestPacket(requestType); - - // send a header containing the length/size - sendHeader(); - - // send the serialized data in the streambuf - sendBody(); - -} - -void Client::receive() { - - std::uint32_t bodySize = receiveHeader(); - receiveBody(bodySize); - - handlePacket(); - -} - -void Client::disconnect() { - - // TODO: error handling - this->socket_.shutdown(asio::ip::tcp::socket::shutdown_both); - this->socket_.close(); - -} - -void Client::showImages(){ - - for (cv::Mat img : this->images) { - - std::string windowName = "Window"; - cv::namedWindow(windowName); - cv::resizeWindow(windowName, 200, 200); - cv::imshow(windowName, img); - - cv::waitKey(0); - - cv::destroyWindow(windowName); - } -} diff --git a/tests/integration/CMakeLists.txt b/tests/integration/CMakeLists.txt index 1f3b2592..d3b0bc6d 100644 --- a/tests/integration/CMakeLists.txt +++ b/tests/integration/CMakeLists.txt @@ -343,4 +343,4 @@ target_add_loguru(cuda_check) # for some reason calling target_add_imagemagick here conflicts with, so we are including/linking without the function call # target_add_imagemagick(cuda_check) target_include_directories(cuda_check PRIVATE ${ImageMagick_INCLUDE_DIRS}) -target_link_libraries(cuda_check PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) \ No newline at end of file +target_link_libraries(cuda_check PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) 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/sync_client_mock.cpp b/tests/integration/sync_client_mock.cpp index a21644ee..29363d66 100644 --- a/tests/integration/sync_client_mock.cpp +++ b/tests/integration/sync_client_mock.cpp @@ -1,70 +1,29 @@ #include #include #include -#include "include/network/sync_client.hpp" +#include "network/client.hpp" +#include "camera/rpi.hpp" +#include "utilities/obc_config.hpp" -namespace asio = boost::asio; +int main(int argc, char* argv[]) { -const std::string ip {"127.0.0.1"}; -constexpr uint32_t port {5000}; + OBCConfig config(argc, argv); -/* -what do we do when we are unable to ()? -what do we do when we fail to accept incoming connections? -what do we do when our send() fails? -what do we do when our write() fails? -what do we do when our shutdown fails? -what do we do when our close fails? -*/ + asio::io_context io_context_; -/** - * Method used for testing purposes, converts enum to string - */ -std::string convertEnumToString(RequestType_t request) { - if (request == RequestType_t::SENDIMAGE) { - return "SENDIMAGE"; - } else if (request == RequestType_t::PING) { - return "PING"; - } else { - return "SHUTDOWN"; - } -} - -int main() { - - std::vector requests; - requests.push_back(RequestType_t::SENDIMAGE); - requests.push_back(RequestType_t::SENDIMAGE); - requests.push_back(RequestType_t::SENDIMAGE); - requests.push_back(RequestType_t::PING); - requests.push_back(RequestType_t::SENDIMAGE); - requests.push_back(RequestType_t::SENDIMAGE); - requests.push_back(RequestType_t::SHUTDOWN); + RPICamera camera(config.camera, &io_context_); + // RPICamera camera(&io_context_); - asio::io_context io_context_; - asio::streambuf buf; + std::deque imageQueue; - // Initialize the client - Client client(&io_context_, ip, port); - - // TODO: what happens when unable to connect? - client.connect(); + camera.connect(); - // Start sending messages - for (RequestType_t request : requests) { + std::optional img = camera.takePicture(std::chrono::milliseconds(1000)); - std::cout << "Sending: " << convertEnumToString(request) << '\n'; - - // send the buffer - client.send(request); - - // TODO: should probably replace with handlePacket which will send() based on what was received? idk - client.receive(); + if (img.has_value()) { + std::filesystem::path filepath = std::to_string(img.value().TIMESTAMP); + img.value().saveToFile(filepath); } - client.showImages(); - - client.disconnect(); - - return EXIT_SUCCESS; + return 0; } From 83e9ef6194714eae45ae068e6e57ce9243878c51 Mon Sep 17 00:00:00 2001 From: Christopher Lee Date: Wed, 7 May 2025 15:34:31 -0700 Subject: [PATCH 13/43] chore: remove boost:serialization --- deps/boost/boost.cmake | 39 ++------------------------------ src/CMakeLists.txt | 1 - src/network/CMakeLists.txt | 2 -- tests/integration/CMakeLists.txt | 2 -- 4 files changed, 2 insertions(+), 42 deletions(-) diff --git a/deps/boost/boost.cmake b/deps/boost/boost.cmake index bb63b89f..dc5c199e 100644 --- a/deps/boost/boost.cmake +++ b/deps/boost/boost.cmake @@ -1,5 +1,5 @@ function(target_add_boost target_name) - include(FetchContent) + 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 @@ -12,40 +12,5 @@ function(target_add_boost target_name) # ) target_include_directories(${target_name} PRIVATE ${boost_SOURCE_DIR}) -endfunction() -# --- Just for Serialization ): --- -include(FetchContent) -message(STATUS "Setting up Boost 1.87.0 via FetchContent...") - -FetchContent_Declare( - Boost_Source - GIT_REPOSITORY https://github.com/boostorg/boost.git - GIT_TAG boost-1.87.0 -) - -set(Boost_USE_STATIC_LIBS ON CACHE BOOL "Force static Boost libraries" FORCE) - -# OPTION A (Example Variable - Verify Name): -# set(BOOST_LIBS_TO_BUILD serialization CACHE STRING "Specific Boost libraries to build" FORCE) -# OPTION B (Alternative Example - Verify Name): -# set(BOOST_INCLUDE_LIBRARIES serialization CACHE STRING "Specific Boost libraries to build" FORCE) - -message(WARNING "Boost build configuration needs the correct variable set to specify building 'serialization' for Boost 1.87.0 CMake superproject!") - - -message(STATUS "Boost_Source declared. Configuring and making available...") -FetchContent_MakeAvailable(Boost_Source) - -if(TARGET Boost::serialization) - get_target_property(BoostSerializationType Boost::serialization TYPE) - if(BoostSerializationType STREQUAL "INTERFACE_LIBRARY") - # This means CMake created a target but didn't link it to a compiled library - message(FATAL_ERROR "Boost::serialization target exists but is INTERFACE only. Boost build DID NOT compile serialization sources. Check Boost build configuration variables above (BOOST_LIBS_TO_BUILD or similar for 1.87.0)!") - else() - message(STATUS "Boost::serialization target type: ${BoostSerializationType}. Looks like a compiled library target exists.") - endif() - message(STATUS "Boost FetchContent setup complete.") -else() - message(FATAL_ERROR "Target Boost::serialization was NOT created by FetchContent_MakeAvailable(Boost_Source). Check Boost build configuration variables and logs in build/_deps/boost_source-subbuild/boost_source-populate-*.log") -endif() +endfunction() \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index baa4561a..81f2efeb 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -45,7 +45,6 @@ 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 - Boost::serialization ) target_add_protobuf(${PROJECT_NAME}) target_add_torch(${PROJECT_NAME}) diff --git a/src/network/CMakeLists.txt b/src/network/CMakeLists.txt index 7ff5eacf..266deccc 100644 --- a/src/network/CMakeLists.txt +++ b/src/network/CMakeLists.txt @@ -33,7 +33,6 @@ add_library(${LIB_NAME} STATIC target_link_libraries(${LIB_NAME} PRIVATE ${LIB_DEPS} - Boost::serialization ) set_unity_for_target(${LIB_NAME}) @@ -60,7 +59,6 @@ add_library(${LIB_NAME} STATIC ${PROJECT_SOURCE_DIR}/build/gen_protos/protos/obc target_link_libraries(${LIB_NAME} PRIVATE ${LIB_DEPS} - Boost::serialization ) set_unity_for_target(${LIB_NAME}) diff --git a/tests/integration/CMakeLists.txt b/tests/integration/CMakeLists.txt index d3b0bc6d..3d26c681 100644 --- a/tests/integration/CMakeLists.txt +++ b/tests/integration/CMakeLists.txt @@ -296,7 +296,6 @@ target_link_libraries(camera_mock PRIVATE -Wl,--copy-dt-needed-entries ${ImageMa add_executable(sync_client_mock "sync_client_mock.cpp") target_link_libraries(sync_client_mock PRIVATE obcpp_lib - Boost::serialization ) target_include_directories(sync_client_mock PRIVATE ${INCLUDE_DIRECTORY}) target_add_boost(sync_client_mock) @@ -315,7 +314,6 @@ target_link_libraries(sync_client_mock PRIVATE -Wl,--copy-dt-needed-entries ${Im add_executable(sync_server_mock "sync_server_mock.cpp") target_link_libraries(sync_server_mock PRIVATE obcpp_lib - Boost::serialization ) target_include_directories(sync_server_mock PRIVATE ${INCLUDE_DIRECTORY}) target_add_boost(sync_server_mock) From 00d6adf6be9fb06f9629816d2f42e060e8dfdc3c Mon Sep 17 00:00:00 2001 From: smhitle <91502030+smhitle@users.noreply.github.com> Date: Thu, 8 May 2025 13:13:07 -0700 Subject: [PATCH 14/43] Undid changes to interface.cpp. Provided skeleton code, now compiles --- deps/boost/boost.cmake | 2 +- include/camera/interface.hpp | 27 +++++++++++++++---- include/camera/mock.hpp | 4 +-- include/camera/rpi.hpp | 35 +++++++++++++++++-------- src/camera/CMakeLists.txt | 1 + src/camera/interface.cpp | 26 +++++++++++++++++++ src/camera/mock.cpp | 26 +++++++++---------- src/camera/rpi.cpp | 50 +++++++++++++++++++++++++++--------- 8 files changed, 127 insertions(+), 44 deletions(-) diff --git a/deps/boost/boost.cmake b/deps/boost/boost.cmake index dc5c199e..556ea5cb 100644 --- a/deps/boost/boost.cmake +++ b/deps/boost/boost.cmake @@ -13,4 +13,4 @@ function(target_add_boost target_name) target_include_directories(${target_name} PRIVATE ${boost_SOURCE_DIR}) -endfunction() \ No newline at end of file +endfunction() diff --git a/include/camera/interface.hpp b/include/camera/interface.hpp index 90fd25bd..de033288 100644 --- a/include/camera/interface.hpp +++ b/include/camera/interface.hpp @@ -31,6 +31,15 @@ struct ImageTelemetry { double roll_deg; }; +/** + * Given a shared ptr to a mavlink client, query it for the telemetry + * information needed for ImageTelemetry and the CV pipeline. + * + * If nullptr is passed for mavlinkClient, nullopt is returned +*/ +std::optional queryMavlinkImageTelemetry( + std::shared_ptr mavlinkClient); + struct ImageData { cv::Mat DATA; uint64_t TIMESTAMP; @@ -56,13 +65,21 @@ class CameraInterface { CameraConfig config; public: - explicit CameraInterface(const CameraConfig& config); - // explicit CameraInterface(); - virtual ~CameraInterface() = default; + explicit CameraInterface(const CameraConfig& config); + // explicit CameraInterface(); + virtual ~CameraInterface() = default; - virtual void connect() = 0; + virtual void connect() = 0; + virtual bool isConnected() = 0; + + virtual void startTakingPictures(const std::chrono::milliseconds& interval, + std::shared_ptr mavlinkClient) = 0; + + virtual void stopTakingPictures() = 0; + + virtual void startStreaming() = 0; - virtual std::optional takePicture(const std::chrono::milliseconds& timeout) = 0; + virtual std::optional takePicture(const std::chrono::milliseconds& timeout, std::shared_ptr mavlinkClient) = 0; }; #endif // INCLUDE_CAMERA_INTERFACE_HPP_ diff --git a/include/camera/mock.hpp b/include/camera/mock.hpp index ef97ed27..301c3781 100644 --- a/include/camera/mock.hpp +++ b/include/camera/mock.hpp @@ -32,14 +32,14 @@ class MockCamera : public CameraInterface { * image from a queue of images which means that the same image won't. * be returned in two subsequent calls */ - std::optional getLatestImage() override; + // std::optional getLatestImage() override; /** * getAllImages returns a queue of all the images that have been * cached since the last call to getAllImages. Once this is called, * it returns the cached images and clears the internal cache. */ - std::deque getAllImages() override; + // std::deque getAllImages() override; /** * Blocking call that takes an image. If it takes longer than the timeout diff --git a/include/camera/rpi.hpp b/include/camera/rpi.hpp index 74e07cf3..2c4014e0 100644 --- a/include/camera/rpi.hpp +++ b/include/camera/rpi.hpp @@ -37,35 +37,48 @@ class RPICamera : public CameraInterface { Client 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? - std::atomic_bool isConnected; + /** + * Converts the image taken from the camera to a suitable format for the CV pipeline + */ + std::optional imgConvert(std::vector imgbuf); - // TODO: do we need these? - // const std::chrono::milliseconds connectionTimeout = 1000ms; - // const std::chrono::milliseconds connectionRetry = 500ms; - // const std::chrono::milliseconds takePictureTimeout = 1000ms; + /** + * Reads in the image data when taking a picture + */ + void readImage(); - std::optional imgConvert(std::vector imgbuf); + /** + * Reads in the telemetry data when taking a picture + */ + void readTelemetry(); public: explicit RPICamera(CameraConfig config, asio::io_context* io_context_); - // explicit RPICamera(asio::io_context* io_context_); // TODO: destructor? - // ~RPICamera(); + ~RPICamera(); void connect() override; - // bool isConnected() override; + bool isConnected() override; + + 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; - std::optional takePicture(const std::chrono::milliseconds& timeout) override; // TODO: mavlink + void startStreaming() override; // TODO: unsure how to implement - // void ping(); + void ping(); }; #endif diff --git a/src/camera/CMakeLists.txt b/src/camera/CMakeLists.txt index b1d880a0..da60fa31 100644 --- a/src/camera/CMakeLists.txt +++ b/src/camera/CMakeLists.txt @@ -30,6 +30,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}) diff --git a/src/camera/interface.cpp b/src/camera/interface.cpp index ac913655..c39be1e3 100644 --- a/src/camera/interface.cpp +++ b/src/camera/interface.cpp @@ -45,6 +45,32 @@ void saveImageTelemetryToFile(const ImageTelemetry& telemetry, 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(); + + 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 { try { std::filesystem::path save_dir = directory; diff --git a/src/camera/mock.cpp b/src/camera/mock.cpp index a125377d..abba6e2f 100644 --- a/src/camera/mock.cpp +++ b/src/camera/mock.cpp @@ -62,19 +62,19 @@ void MockCamera::stopTakingPictures() { this->captureThread.join(); } -std::optional MockCamera::getLatestImage() { - ReadLock lock(this->imageQueueLock); - ImageData lastImage = this->imageQueue.front(); - this->imageQueue.pop_front(); - return lastImage; -} - -std::deque MockCamera::getAllImages() { - ReadLock lock(this->imageQueueLock); - std::deque outputQueue = this->imageQueue; - this->imageQueue = std::deque(); - return outputQueue; -} +// std::optional MockCamera::getLatestImage() { +// ReadLock lock(this->imageQueueLock); +// ImageData lastImage = this->imageQueue.front(); +// this->imageQueue.pop_front(); +// return lastImage; +// } + +// std::deque MockCamera::getAllImages() { +// ReadLock lock(this->imageQueueLock); +// std::deque outputQueue = this->imageQueue; +// this->imageQueue = std::deque(); +// return outputQueue; +// } void MockCamera::captureEvery(const std::chrono::milliseconds& interval, std::shared_ptr mavlinkClient) { diff --git a/src/camera/rpi.cpp b/src/camera/rpi.cpp index 9b6dfd41..79e92d38 100644 --- a/src/camera/rpi.cpp +++ b/src/camera/rpi.cpp @@ -24,25 +24,19 @@ const uint32_t IMG_HEIGHT = 1520; const uint32_t BUFFER_SIZE = IMG_WIDTH * IMG_HEIGHT * 3 / 2; -RPICamera::RPICamera(asio::io_context* io_context_) : CameraInterface(config), client(io_context_, SERVER_IP, SERVER_PORT) { +RPICamera::RPICamera(CameraConfig config, asio::io_context* io_context_) : CameraInterface(config), client(io_context_, SERVER_IP, SERVER_PORT) { // TODO: do we need this? not sure how we're configuring the camera if that's being done on Daniel's side } -// RPICamera::RPICamera(asio::io_context* io_context_) : client(io_context_, SERVER_IP, SERVER_PORT) { -// // // TODO: do we need this? not sure how we're configuring the camera if that's being done on Daniel's side - -// } - // TODO void RPICamera::connect() { - if (this->isConnected == true) { + if (this->connected == true) { return; } - while (!isConnected) { - // this->isConnected = this->client->connect(); - this->isConnected = client.connect(); + while (!this->connected) { + this->connected = client.connect(); } // TODO: switch to LOG_F? @@ -52,7 +46,11 @@ void RPICamera::connect() { client.send(START_REQUEST); } -std::optional RPICamera::takePicture(const std::chrono::milliseconds& timeout) { +RPICamera::~RPICamera() { + +} + +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); @@ -88,7 +86,35 @@ std::optional RPICamera::imgConvert(std::vector buf) { 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_YUV420p2BGR); // TODO: not sure if this is the right color space + 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() { + +} + +void RPICamera::readImage() { + +} + +void RPICamera::readTelemetry() { + +} + +void RPICamera::startStreaming() { + +} + +bool RPICamera::isConnected() { + return true; +} From 7548cfbf7f95b64da872b8f61f5acf34eab9f744 Mon Sep 17 00:00:00 2001 From: smhitle <91502030+smhitle@users.noreply.github.com> Date: Thu, 8 May 2025 17:04:31 -0700 Subject: [PATCH 15/43] Prototypal code for the entire process --- include/camera/rpi.hpp | 10 +- include/network/client.hpp | 14 ++- include/network/rpi_connection.hpp | 18 +++ src/camera/rpi.cpp | 67 +++++++++--- src/network/client.cpp | 169 ++--------------------------- 5 files changed, 94 insertions(+), 184 deletions(-) create mode 100644 include/network/rpi_connection.hpp diff --git a/include/camera/rpi.hpp b/include/camera/rpi.hpp index 2c4014e0..3a0efb36 100644 --- a/include/camera/rpi.hpp +++ b/include/camera/rpi.hpp @@ -23,8 +23,8 @@ using namespace std::chrono_literals; // NOLINT namespace asio = boost::asio; -const std::string SERVER_IP = "192.168.68.1"; -const int SERVER_PORT = 25565; +// 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'; @@ -52,12 +52,14 @@ class RPICamera : public CameraInterface { /** * Reads in the image data when taking a picture */ - void readImage(); + std::vector readImage(); /** * Reads in the telemetry data when taking a picture */ - void readTelemetry(); + std::optional readTelemetry(); + + std::vector readPacket(); public: diff --git a/include/network/client.hpp b/include/network/client.hpp index 227c0e6f..bbc30dae 100644 --- a/include/network/client.hpp +++ b/include/network/client.hpp @@ -5,9 +5,7 @@ #include #include #include "camera_data.hpp" - -// #include -// #include +#include "rpi_connection.hpp" namespace asio = boost::asio; @@ -37,10 +35,14 @@ class Client { void send(std::uint8_t request); /** - * + * Reads in the header and fills a Header struct + */ + Header recvHeader(); + + /** + * Reads the actual data specified by the header */ - // TODO: not sure what the return type should be, std::vector ? - std::vector recv(const int bufSize); + std::vector recvBody(const int bufSize); std::string getIP(); diff --git a/include/network/rpi_connection.hpp b/include/network/rpi_connection.hpp new file mode 100644 index 00000000..1985ea38 --- /dev/null +++ b/include/network/rpi_connection.hpp @@ -0,0 +1,18 @@ +#ifndef INCLUDE_NETWORK_RPI_CONNECTION_HPP_ +#define INCLUDE_NETWORK_RPI_CONNECTION_HPP_ + +#include + +const std::string SERVER_IP = "192.168.68.1"; +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 diff --git a/src/camera/rpi.cpp b/src/camera/rpi.cpp index 79e92d38..7bab2c33 100644 --- a/src/camera/rpi.cpp +++ b/src/camera/rpi.cpp @@ -21,15 +21,14 @@ const uint32_t IMG_WIDTH = 2028; const uint32_t IMG_HEIGHT = 1520; -const uint32_t BUFFER_SIZE = IMG_WIDTH * IMG_HEIGHT * 3 / 2; +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) { -// TODO: do we need this? not sure how we're configuring the camera if that's being done on Daniel's side } -// TODO void RPICamera::connect() { if (this->connected == true) { return; @@ -47,7 +46,7 @@ void RPICamera::connect() { } 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) { @@ -55,10 +54,8 @@ std::optional RPICamera::takePicture(const std::chrono::milliseconds& // client sends a request to take a pictures client.send(PICTURE_REQUEST); - // client receives a response TODO: might have to adjust the datatype - std::vector imgbuf = client.recv(BUFFER_SIZE); - - // TODO: have to parse the imgbuf possibly depending on how the response is structured? + // TODO: get the imgbuf + std::vector imgbuf = readImage(); // give the image buffer to imgConvert std::optional mat = imgConvert(imgbuf); @@ -72,15 +69,16 @@ std::optional RPICamera::takePicture(const std::chrono::milliseconds& return ImageData { .DATA = mat.value(), .TIMESTAMP = timestamp, - .TELEMETRY = {} + .TELEMETRY = {} // TODO: nullopt for now }; } std::optional RPICamera::imgConvert(std::vector buf) { - // TODO: if the sizes don't match return nullopt - if (buf.size() != BUFFER_SIZE) { - return {}; - } + + // 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()); @@ -103,12 +101,51 @@ void RPICamera::ping() { } -void RPICamera::readImage() { +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()); + } + + return imgbuf; +} + +std::optional RPICamera::readTelemetry() { + + ImageTelemetry telemetry; + // asio::read() + return telemetry; } -void RPICamera::readTelemetry() { +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); + + // 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); + // TODO: idek if we have to do ntoh for the buffer + + return packet; } void RPICamera::startStreaming() { diff --git a/src/network/client.cpp b/src/network/client.cpp index 9133ad85..c1a12dbb 100644 --- a/src/network/client.cpp +++ b/src/network/client.cpp @@ -32,11 +32,18 @@ void Client::send(std::uint8_t request) { } -std::vector Client::recv(const int bufSize) { +Header Client::recvHeader() { + boost::system::error_code ec; + Header header; + + // TODO: might have to specify 12 bytes + asio::read(this->socket_, asio::buffer(&header, sizeof(Header)), ec); - // TODO: not sure if this is very efficient or if there's a better way to do this + return header; +} + +std::vector Client::recvBody(const int bufSize) { - // asio::streambuf recvbuf; boost::system::error_code ec; std::vector recvbuf(bufSize); @@ -46,8 +53,6 @@ std::vector Client::recv(const int bufSize) { if (ec) { // TODO: what to do when read fails } - - // recvbuf.commit(bufSize); return recvbuf; } @@ -59,157 +64,3 @@ std::string Client::getIP() { int Client::getPort() { return this->port; } - - - -// #include "include/network/sync_client.hpp" - -// CameraRequest_t Client::createRequest(RequestType_t requestType) { - -// CameraRequest_t request; -// request.pid = 6969; -// request.requestType = requestType; - -// return request; - -// } - -// void Client::createRequestPacket(RequestType_t requestType) { - -// CameraRequest_t request = createRequest(requestType); - -// serialh::serialize(&request, &this->sendbuf); - -// } - -// void Client::sendHeader() { - -// std::uint32_t headerLength = static_cast(this->sendbuf.size()); - -// // TODO: error handler -// asio::write(this->socket_, asio::buffer(&headerLength, sizeof(headerLength))); - -// } - -// void Client::sendBody() { - -// // TODO: error handler -// asio::write(this->socket_, this->sendbuf); - -// } - -// std::uint32_t Client::receiveHeader() { - -// std::uint32_t size {}; - -// // TODO: error handler -// asio::read(this->socket_, asio::buffer(&size, sizeof(size))); - -// return size; - -// } - -// void Client::receiveBody(std::uint32_t size) { - -// // TODO: error handler -// asio::read(this->socket_, this->receivebuf.prepare(size)); -// this->receivebuf.commit(size); - -// } - -// CameraResponse_t Client::deconstructPacket() { - -// CameraResponse_t response; - -// serialh::deserialize(&response, &this->receivebuf); - -// return response; - -// } - -// void Client::handlePacket() { - -// // deconstruct packet -// CameraResponse_t response = deconstructPacket(); - -// std::vector image = response.imageData.imgBuffer; - -// cv::Mat m = cv::Mat(response.imageData.height, response.imageData.width, CV_8UC3); - -// if (image.size() == (response.imageData.imageSizeBytes)) { -// std::memcpy(m.data, image.data(), sizeof(std::uint8_t) * response.imageData.imageSizeBytes); -// } - -// // Add image to image buffer -// this->images.push_back(m); -// } - -// Client::Client(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_) { - -// this->ip = ip; -// this->port = port; - -// } - -// void Client::connect() { - -// // TODO: difference between exception handling and error handling -// try { - -// asio::ip::tcp::endpoint endpoint_(asio::ip::tcp::endpoint(asio::ip::make_address(this->ip), this->port)); - -// this->socket_.connect(endpoint_); - -// std::cout << "Connected to: " << this->ip << " on port " << this->port << '\n'; - -// } catch (std::exception& e) { - -// std::cout << e.what() << '\n'; - -// } - -// } - -// void Client::send(RequestType_t requestType) { - -// createRequestPacket(requestType); - -// // send a header containing the length/size -// sendHeader(); - -// // send the serialized data in the streambuf -// sendBody(); - -// } - -// void Client::receive() { - -// std::uint32_t bodySize = receiveHeader(); -// receiveBody(bodySize); - -// handlePacket(); - -// } - -// void Client::disconnect() { - -// // TODO: error handling -// this->socket_.shutdown(asio::ip::tcp::socket::shutdown_both); -// this->socket_.close(); - -// } - -// void Client::showImages(){ - -// for (cv::Mat img : this->images) { - -// std::string windowName = "Window"; -// cv::namedWindow(windowName); -// cv::resizeWindow(windowName, 200, 200); -// cv::imshow(windowName, img); - -// cv::waitKey(0); - -// cv::destroyWindow(windowName); -// } -// } From 2a3b16393c967736d087f01cb9f5a05d74e624f5 Mon Sep 17 00:00:00 2001 From: smhitle <91502030+smhitle@users.noreply.github.com> Date: Thu, 8 May 2025 18:03:15 -0700 Subject: [PATCH 16/43] Testing on jetson --- include/camera/interface.hpp | 2 +- include/camera/rpi.hpp | 2 +- src/camera/rpi.cpp | 5 ++++- tests/integration/sync_client_mock.cpp | 24 ++++++++++++++++++------ 4 files changed, 24 insertions(+), 9 deletions(-) diff --git a/include/camera/interface.hpp b/include/camera/interface.hpp index de033288..32075674 100644 --- a/include/camera/interface.hpp +++ b/include/camera/interface.hpp @@ -79,7 +79,7 @@ class CameraInterface { virtual void startStreaming() = 0; - virtual std::optional takePicture(const std::chrono::milliseconds& timeout, std::shared_ptr mavlinkClient) = 0; + virtual std::optional takePicture(const std::chrono::milliseconds& timeout, std::shared_ptr mavlinkClient) = 0; }; #endif // INCLUDE_CAMERA_INTERFACE_HPP_ diff --git a/include/camera/rpi.hpp b/include/camera/rpi.hpp index 3a0efb36..99465c31 100644 --- a/include/camera/rpi.hpp +++ b/include/camera/rpi.hpp @@ -28,7 +28,7 @@ namespace asio = boost::asio; // 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 = 'p'; +const std::uint8_t PICTURE_REQUEST = 'I'; const std::uint8_t END_REQUEST = 'e'; const std::uint8_t LOCK_REQUEST = 'l'; diff --git a/src/camera/rpi.cpp b/src/camera/rpi.cpp index 7bab2c33..3bf0e94d 100644 --- a/src/camera/rpi.cpp +++ b/src/camera/rpi.cpp @@ -38,8 +38,11 @@ void RPICamera::connect() { this->connected = client.connect(); } + std::string log = "Connected to: " + client.getIP() + " on port: " + std::to_string(client.getPort()); + LOG_F(INFO, log.c_str()); + // TODO: switch to LOG_F? - std::cout << "Connected to: " << client.getIP() << " on port: " << client.getPort() << '\n'; + // std::cout << "Connected to: " << client.getIP() << " on port: " << client.getPort() << '\n'; // tells the camera to start the camera thread client.send(START_REQUEST); diff --git a/tests/integration/sync_client_mock.cpp b/tests/integration/sync_client_mock.cpp index 29363d66..631e1508 100644 --- a/tests/integration/sync_client_mock.cpp +++ b/tests/integration/sync_client_mock.cpp @@ -1,27 +1,39 @@ #include #include #include +#include #include "network/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[]) { + // if (argc < 2) { + // LOG_F(ERROR, "Usage: ./bin/camera_mock [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); + CameraConfig config; asio::io_context io_context_; - RPICamera camera(config.camera, &io_context_); - // RPICamera camera(&io_context_); + RPICamera camera(config, &io_context_); - std::deque imageQueue; + // std::deque imageQueue; + // auto mav = std::make_shared("serial:///dev/ttyACM0"); camera.connect(); - std::optional img = camera.takePicture(std::chrono::milliseconds(1000)); + std::optional img = camera.takePicture(std::chrono::milliseconds(1000), nullptr); if (img.has_value()) { - std::filesystem::path filepath = std::to_string(img.value().TIMESTAMP); + std::filesystem::path base_dir = "/workspaces/obcpp/images"; + std::filesystem::path filepath = base_dir / std::to_string(img.value().TIMESTAMP); img.value().saveToFile(filepath); } From d5a7adf231a246a73e4f5b68bf5a6cec318bc045 Mon Sep 17 00:00:00 2001 From: smhitle <91502030+smhitle@users.noreply.github.com> Date: Fri, 9 May 2025 12:31:56 -0700 Subject: [PATCH 17/43] Added log statements for debugging --- src/camera/rpi.cpp | 8 +------- src/network/client.cpp | 30 ++++++++++++++++++++++++++---- 2 files changed, 27 insertions(+), 11 deletions(-) diff --git a/src/camera/rpi.cpp b/src/camera/rpi.cpp index 3bf0e94d..c4549e97 100644 --- a/src/camera/rpi.cpp +++ b/src/camera/rpi.cpp @@ -26,7 +26,7 @@ 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() { @@ -37,12 +37,6 @@ void RPICamera::connect() { while (!this->connected) { this->connected = client.connect(); } - - std::string log = "Connected to: " + client.getIP() + " on port: " + std::to_string(client.getPort()); - LOG_F(INFO, log.c_str()); - - // TODO: switch to LOG_F? - // std::cout << "Connected to: " << client.getIP() << " on port: " << client.getPort() << '\n'; // tells the camera to start the camera thread client.send(START_REQUEST); diff --git a/src/network/client.cpp b/src/network/client.cpp index c1a12dbb..c0fd2039 100644 --- a/src/network/client.cpp +++ b/src/network/client.cpp @@ -1,5 +1,7 @@ +#include #include "network/client.hpp" + Client::Client(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_) { this->ip = ip; @@ -15,21 +17,29 @@ bool Client::connect() { 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; } void Client::send(std::uint8_t request) { boost::system::error_code ec; - asio::write(this->socket_, asio::buffer(&request, sizeof(std::uint8_t)), 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()); } + LOG_F(INFO, std::string("Bytes sent: " + bytesSent).c_str()); + } Header Client::recvHeader() { @@ -37,7 +47,14 @@ Header Client::recvHeader() { Header header; // TODO: might have to specify 12 bytes - asio::read(this->socket_, asio::buffer(&header, sizeof(Header)), ec); + 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; } @@ -46,13 +63,18 @@ 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); - asio::read(this->socket_, asio::buffer(recvbuf), ec); + int bytesRead = asio::read(this->socket_, asio::buffer(recvbuf), ec); - if (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; } From d573496c945ea460efa698c9f20a649a9c60f05c Mon Sep 17 00:00:00 2001 From: smhitle <91502030+smhitle@users.noreply.github.com> Date: Fri, 9 May 2025 14:33:27 -0700 Subject: [PATCH 18/43] Added a mock rpi server --- include/network/mock_server.hpp | 37 +++++++++ include/network/server.hpp | 4 - src/network/CMakeLists.txt | 1 + src/network/mock_server.cpp | 105 +++++++++++++++++++++++++ tests/integration/CMakeLists.txt | 1 + tests/integration/sync_server_mock.cpp | 37 ++------- 6 files changed, 152 insertions(+), 33 deletions(-) create mode 100644 include/network/mock_server.hpp delete mode 100644 include/network/server.hpp create mode 100644 src/network/mock_server.cpp diff --git a/include/network/mock_server.hpp b/include/network/mock_server.hpp new file mode 100644 index 00000000..db2bf313 --- /dev/null +++ b/include/network/mock_server.hpp @@ -0,0 +1,37 @@ +#ifndef INCLUDE_NETWORK_SERVER_HPP_ +#define INCLUDE_NETWORK_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 // INCLUDE_NETWORK_SERVER_HPP_ 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/src/network/CMakeLists.txt b/src/network/CMakeLists.txt index 266deccc..d3f72383 100644 --- a/src/network/CMakeLists.txt +++ b/src/network/CMakeLists.txt @@ -6,6 +6,7 @@ set(COMMON_FILES gcs.cpp mavlink.cpp client.cpp + mock_server.cpp ) set(FILES diff --git a/src/network/mock_server.cpp b/src/network/mock_server.cpp new file mode 100644 index 00000000..49f71af2 --- /dev/null +++ b/src/network/mock_server.cpp @@ -0,0 +1,105 @@ +#include "network/mock_server.hpp" + +const uint32_t IMG_WIDTH = 2028; +const uint32_t IMG_HEIGHT = 1520; +const uint32_t BUFFER_SIZE = IMG_WIDTH * IMG_HEIGHT * 3 / 2; + +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/tests/integration/CMakeLists.txt b/tests/integration/CMakeLists.txt index 3d26c681..69a86c9f 100644 --- a/tests/integration/CMakeLists.txt +++ b/tests/integration/CMakeLists.txt @@ -328,6 +328,7 @@ target_add_matplot(sync_server_mock) target_add_loguru(sync_server_mock) target_include_directories(sync_server_mock PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(sync_server_mock PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) + # cuda_check add_executable(cuda_check ${SOURCES} cuda_check.cpp) target_add_torch(cuda_check) diff --git a/tests/integration/sync_server_mock.cpp b/tests/integration/sync_server_mock.cpp index 15913155..743fa2dd 100644 --- a/tests/integration/sync_server_mock.cpp +++ b/tests/integration/sync_server_mock.cpp @@ -1,43 +1,22 @@ #include #include #include -#include "include/network/sync_server.hpp" +#include "network/mock_server.hpp" namespace asio = boost::asio; -const std::string ip {"127.0.0.1"}; -constexpr uint32_t port {5000}; +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); - Server server(&io_context_, ip, port); - - for (;;) { - - server.connect(); - - while (true) { - - server.receive(); - - if (server.getReadError() == asio::error::eof) { - // TODO: what else do we do? can we still use the current socket to accept connections? - break; - } - - // TODO: should probably replace with handlePacket which will send() based on what was received? idk - server.send(ResponseType_t::SUCC); - - } - - std::cout << "Client closed the connection" << '\n'; - - server.close(); - } - - server.shutdown(); + //for (;;) { + // start the server in a while loop + server.start(); + //} return EXIT_SUCCESS; } From 0d12b7fbf27a600015ce05dd63b2e976c6d11457 Mon Sep 17 00:00:00 2001 From: smhitle <91502030+smhitle@users.noreply.github.com> Date: Thu, 15 May 2025 15:29:56 -0700 Subject: [PATCH 19/43] Deleted unused files, switched to UDP instead of TCP --- include/camera/rpi.hpp | 4 +- include/network/async_client.hpp | 68 ------- include/network/async_server.hpp | 0 include/network/camera_client.hpp | 50 ----- include/network/camera_server.hpp | 58 ------ include/network/sync_server.hpp | 119 ----------- .../network/{client.hpp => tcp_client.hpp} | 9 +- .../{mock_server.hpp => tcp_server.hpp} | 6 +- include/network/udp_client.hpp | 30 +++ include/network/udp_server.hpp | 34 ++++ src/network/CMakeLists.txt | 5 +- src/network/async_client.cpp | 184 ----------------- src/network/async_server.cpp | 0 src/network/camera_client.cpp | 40 ---- src/network/camera_server.cpp | 80 -------- src/network/camera_socket.cpp | 15 -- src/network/sync_server.cpp | 188 ------------------ src/network/{client.cpp => tcp_client.cpp} | 15 +- .../{mock_server.cpp => tcp_server.cpp} | 2 +- src/network/udp_client.cpp | 81 ++++++++ src/network/udp_server.cpp | 35 ++++ tests/integration/CMakeLists.txt | 60 +++--- ...nc_client_mock.cpp => tcp_client_mock.cpp} | 13 +- ...nc_server_mock.cpp => tcp_server_mock.cpp} | 4 +- tests/integration/udp_client_mock.cpp | 30 +++ tests/integration/udp_server_mock.cpp | 12 ++ 26 files changed, 271 insertions(+), 871 deletions(-) delete mode 100644 include/network/async_client.hpp delete mode 100644 include/network/async_server.hpp delete mode 100644 include/network/camera_client.hpp delete mode 100644 include/network/camera_server.hpp delete mode 100644 include/network/sync_server.hpp rename include/network/{client.hpp => tcp_client.hpp} (89%) rename include/network/{mock_server.hpp => tcp_server.hpp} (84%) create mode 100644 include/network/udp_client.hpp create mode 100644 include/network/udp_server.hpp delete mode 100644 src/network/async_client.cpp delete mode 100644 src/network/async_server.cpp delete mode 100644 src/network/camera_client.cpp delete mode 100644 src/network/camera_server.cpp delete mode 100644 src/network/camera_socket.cpp delete mode 100644 src/network/sync_server.cpp rename src/network/{client.cpp => tcp_client.cpp} (92%) rename src/network/{mock_server.cpp => tcp_server.cpp} (98%) create mode 100644 src/network/udp_client.cpp create mode 100644 src/network/udp_server.cpp rename tests/integration/{sync_client_mock.cpp => tcp_client_mock.cpp} (62%) rename tests/integration/{sync_server_mock.cpp => tcp_server_mock.cpp} (86%) create mode 100644 tests/integration/udp_client_mock.cpp create mode 100644 tests/integration/udp_server_mock.cpp diff --git a/include/camera/rpi.hpp b/include/camera/rpi.hpp index 99465c31..b8cf0e2a 100644 --- a/include/camera/rpi.hpp +++ b/include/camera/rpi.hpp @@ -19,7 +19,7 @@ using json = nlohmann::json; using namespace std::chrono_literals; // NOLINT #include "interface.hpp" -#include "network/client.hpp" +#include "network/udp_client.hpp" namespace asio = boost::asio; @@ -34,7 +34,7 @@ const std::uint8_t LOCK_REQUEST = 'l'; class RPICamera : public CameraInterface { private: - Client client; + UDPClient client; asio::io_context io_context_; std::atomic_bool connected; diff --git a/include/network/async_client.hpp b/include/network/async_client.hpp deleted file mode 100644 index c9b9677f..00000000 --- a/include/network/async_client.hpp +++ /dev/null @@ -1,68 +0,0 @@ -#ifndef INCLUDE_NETWORK_ASYNC_CLIENT_HPP_ -#define INCLUDE_NETWORK_ASYNC_CLIENT_HPP_ - -#include -#include -#include -#include "camera_data.hpp" - -namespace asio = boost::asio; - -class Client { - private: - std::string ip; - int port; - // asio::ip::tcp::resolver resolver_; - asio::ip::tcp::socket socket_; // TODO: does the socket need a lock? - - // TODO: likely need locks around these - // TODO: scratch that, don't make shared class bufs, have the ownership be part of the async functions - asio::streambuf sendbuf; - asio::streambuf recvbuf; - - // TODO: this vector should really be protected by locks, what is the difference between the Locks provided in utilities/locks.hpp, utilities/lockptr.hpp, shared_mutex and any other mutex like objects i should be aware of - std::queue images; - - // These functions will handle sending the request - - /** - * This will take in the following request, make a "packet", serialize the data and then put it into the send buffer to send to the camera - */ - void createRequestPacket(RequestType_t request); - - /** - * - */ - void send(RequestType_t requestType); - - // These functions will handle reading the response - - /** - * - */ - void deconstructPacket(); - - /** - * - */ - void recv(); - - cv::Mat imgConvert(); - - public: - - Client(asio::io_context* io_context_, std::string ip, int port); - - void connect(); - - // TODO: unsure what this should return - void ping(); - - void takePicture(); - - // TODO: return type should probably change - cv::Mat getLatestImage(); - -}; - -#endif diff --git a/include/network/async_server.hpp b/include/network/async_server.hpp deleted file mode 100644 index e69de29b..00000000 diff --git a/include/network/camera_client.hpp b/include/network/camera_client.hpp deleted file mode 100644 index 5343a3bb..00000000 --- a/include/network/camera_client.hpp +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef INCLUDE_NETWORK_CAMERA_CLIENT_HPP_ -#define INCLUDE_NETWORK_CAMERA_CLIENT_HPP_ - -#include -#include -#include -#include "network/camera_data.hpp" -// https://www.codeproject.com/Articles/1264257/Socket-Programming-in-Cplusplus-using-boost-asio-T -namespace asio = boost::asio; - -class CameraClient { - public: - /* - * Create the socket - * Params: - * - io_context (boost::asio::io_context) - what io context to use - * - port (int) - which port to occupy - */ - CameraClient(int port); - ~CameraClient(); - - /* - * Connect to server (OBC) - * Params: - * - IP (string) - regular ip - * - port (int) - port - */ - bool connect(asio::io_context io_context, std::string ip, int port); - - /* - * Sends pics out - * Params: - * - header (string) - format should be "image_dimensions, buffer_size" - * - image (vector) - vector of bytes of image - * It should also probably have a terminator - */ - bool send(std::vector image); - - // Recieves a command to take pics - CameraResponse_t read(); - - private: - - // ig? - // string ip; - // int port; - // asio::tcp::socket socket; -}; - -#endif diff --git a/include/network/camera_server.hpp b/include/network/camera_server.hpp deleted file mode 100644 index 0de0f5fd..00000000 --- a/include/network/camera_server.hpp +++ /dev/null @@ -1,58 +0,0 @@ -#ifndef INCLUDE_NETWORK_CAMERA_SERVER_HPP_ -#define INCLUDE_NETWORK_CAMERA_SERVER_HPP_ - -#include -#include -#include -#include "camera_data.hpp" - -// https://www.codeproject.com/Articles/1264257/Socket-Programming-in-Cplusplus-using-boost-asio-T -namespace asio = boost::asio; - -// OBC-side socket. -// Sends requests to take pics to camera, sends it back up to -class ObcCameraServer -{ -public: - /* - * Create the socket - * Params: - * - port (int) - which port to occupy - */ - explicit ObcCameraServer(std::string ip, int port, asio::io_context& io_context_); - ~ObcCameraServer(); - - /* - * Connect to client (camera) - * Params: - * - IP (string) - regular ip - * - port (int) - port - */ - bool connect(); - - /* - * Sends command to take and return pic - * Params: - * - type (RequestType_t) - what OBC wants camera to do - * Returns: - * - bool - if success or not - */ - bool send(RequestType_t type); - - /* - * Reads the response - * Returns: - * - CameraResponse_t - response that camera passed in - */ - CameraResponse_t read(); - -private: - // ig? - std::string ip; - int port; - std::pair camera_res; // used for reconstruction - asio::ip::tcp::socket socket_; - asio::ip::tcp::acceptor acceptor_; -}; - -#endif diff --git a/include/network/sync_server.hpp b/include/network/sync_server.hpp deleted file mode 100644 index b9e53e6f..00000000 --- a/include/network/sync_server.hpp +++ /dev/null @@ -1,119 +0,0 @@ -#ifndef INCLUDE_NETWORK_SYNC_SERVER_HPP__ -#define INCLUDE_NETWORK_SYNC_SERVER_HPP__ - -#include -#include -#include "camera_data.hpp" -#include "serialize.hpp" - -namespace asio = boost::asio; - -class Server { - private: - std::string ip; - int port; - asio::ip::tcp::socket socket_; - asio::ip::tcp::acceptor acceptor_; - - asio::streambuf sendbuf; - asio::streambuf receivebuf; - - // free functions - boost::system::error_code ec_write; - boost::system::error_code ec_read; - - // socket specific - boost::system::error_code ec_shutdown; - boost::system::error_code ec_close; - - // acceptor specific - boost::system::error_code ec_acceptor_init; - boost::system::error_code ec_accept; - - ImageData_t createImageData(); - /** - * Creates the response from the request - */ - CameraResponse_t createResponse(ResponseType_t response); - - /** - * Creates a packet to send based off the request - */ - void createResponsePacket(ResponseType_t response); - - /** - * Send the header of the packet - */ - void sendHeader(); - - /** - * Send the body of the packet - */ - void sendBody(); - - /** - * Read in the header of the packet - * - * @return std::uint32_t Size of the body to be received - */ - std::uint32_t receiveHeader(); - - /** - * Read in the body of the packet - * - * @param size Size of packet body - */ - void receiveBody(std::uint32_t size); - - /** - * Deconstructs the received packet - */ - CameraRequest_t deconstructPacket(); - - /** - * Handler for the packet based on what was received - */ - void handlePacket(); - - public: - - Server(asio::io_context* io_context_, std::string ip, int port); - - /** - * Accepts connections on the underlying socket - */ - void connect(); - - /** - * Sends a packet to the client - */ - void send(ResponseType_t responseType); - - /** - * Reads in packets send from the client - */ - void receive(); - - /** - * Closes the socket - */ - void close(); - - /** - * Shuts down the socket - */ - void shutdown(); - - /** - * Returns the error if an error occurred on a read operation - */ - boost::system::error_code getReadError(); - - /** - * Returns the error if an error occurred on a write operation - */ - boost::system::error_code getWriteError(); - -}; - -#endif diff --git a/include/network/client.hpp b/include/network/tcp_client.hpp similarity index 89% rename from include/network/client.hpp rename to include/network/tcp_client.hpp index bbc30dae..7962cb6c 100644 --- a/include/network/client.hpp +++ b/include/network/tcp_client.hpp @@ -4,7 +4,6 @@ #include #include #include -#include "camera_data.hpp" #include "rpi_connection.hpp" namespace asio = boost::asio; @@ -32,22 +31,20 @@ class Client { * */ // TODO: so send is supposed to send a char but im unsure what the type should be uchar, uint8_t, etc. - void send(std::uint8_t request); + 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); - std::string getIP(); - - int getPort(); - }; #endif diff --git a/include/network/mock_server.hpp b/include/network/tcp_server.hpp similarity index 84% rename from include/network/mock_server.hpp rename to include/network/tcp_server.hpp index db2bf313..61c90851 100644 --- a/include/network/mock_server.hpp +++ b/include/network/tcp_server.hpp @@ -1,5 +1,5 @@ -#ifndef INCLUDE_NETWORK_SERVER_HPP_ -#define INCLUDE_NETWORK_SERVER_HPP_ +#ifndef INCLUDE_NETWORK_TCP_SERVER_HPP_ +#define INCLUDE_NETWORK_TCP_SERVER_HPP_ #include #include @@ -34,4 +34,4 @@ class Server { void shutdown(); }; -#endif // INCLUDE_NETWORK_SERVER_HPP_ +#endif diff --git a/include/network/udp_client.hpp b/include/network/udp_client.hpp new file mode 100644 index 00000000..0f72dea8 --- /dev/null +++ b/include/network/udp_client.hpp @@ -0,0 +1,30 @@ +#ifndef INCLUDE_NETWORK_UDP_CLIENT_HPP_ +#define INCLUDE_NETWORK_UDP_CLIENT_HPP_ + +#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 bufSize); +}; + +#endif diff --git a/include/network/udp_server.hpp b/include/network/udp_server.hpp new file mode 100644 index 00000000..d0996e2c --- /dev/null +++ b/include/network/udp_server.hpp @@ -0,0 +1,34 @@ +#ifndef INCLUDE_NETWORK_UDP_SERVER_HPP_ +#define INCLUDE_NETWORK_UDP_SERVER_HPP_ + +#include +#include +#include + +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); + + void start(); + + void send(); + + void recv(); + + void handleRequest(char request); + + void shutdown(); +}; + +#endif diff --git a/src/network/CMakeLists.txt b/src/network/CMakeLists.txt index d3f72383..c907e864 100644 --- a/src/network/CMakeLists.txt +++ b/src/network/CMakeLists.txt @@ -5,8 +5,9 @@ set(COMMON_FILES gcs_routes.cpp gcs.cpp mavlink.cpp - client.cpp - mock_server.cpp + tcp_client.cpp + tcp_mock_server.cpp + udp_client.cpp ) set(FILES diff --git a/src/network/async_client.cpp b/src/network/async_client.cpp deleted file mode 100644 index 3b60fd4b..00000000 --- a/src/network/async_client.cpp +++ /dev/null @@ -1,184 +0,0 @@ -#include -#include -#include "include/network/async_client.hpp" -#include "include/utilities/locks.hpp" -#include "include/network/serialize.hpp" - -namespace asio = boost::asio; - -void Client::createRequestPacket(RequestType_t requestType) { - - // form the data - CameraRequest_t request; - request.requestType = requestType; - request.pid = 1010; // TODO: change the PID or completely remove PID - - // serialize the data - serialh::serialize(&request, &this->sendbuf); - -} - -void Client::send(RequestType_t requestType) { - - // TODO: probably need to lock here - - this->createRequestPacket(requestType); - - // two sends here, first send the size of the serialized data, then send the serialized data itself - // TODO: is a chained callback structure the best way to do this? - - std::uint32_t headerLength = static_cast(this->sendbuf.size()); - - asio::async_write(this->socket_, asio::buffer(&headerLength, sizeof(headerLength)), - [this](boost::system::error_code ec, std::size_t bytes_written) { - - if (ec) { - // TODO: what do we do if sending the header fails? - } - - asio::async_write(this->socket_, this->sendbuf, - [this](boost::system::error_code ec, std::size_t bytes_written) { - - if (ec) { - // TODO: what do we do if sending the body fails? - } - - }); - }); - -} - -void Client::recv() { - - // TODO: maybe make this shared as well - std::uint32_t headerLength = {}; - - asio::async_read(this->socket_, asio::buffer(&headerLength, sizeof(headerLength)), - [this, headerLength](boost::system::error_code ec, std::size_t bytes_read) { - - if (ec) { - // TODO: what to do if sending header length fails, best guess is to not send the packet at all - } - - asio::streambuf buf; - - std::shared_ptr recvbuf {&buf}; - - asio::async_read(this->socket_, recvbuf->prepare(headerLength), - [this, headerLength, recvbuf](boost::system::error_code ec, std::size_t bytes_read) { - - if (ec || bytes_read != headerLength) { - - // we read some data but not all of it so its likely not usable - recvbuf->consume(bytes_read); - - // TODO: what do we do - } - - recvbuf->commit(bytes_read); - - cv::Mat m = this->imgConvert(); // TODO: have to pass the recvbuf through here - - this->images.push(m); - - }); - - }); - -} - -cv::Mat Client::imgConvert() { - - // TODO: apparently the expected format will be YUV420, look into what that is. can be done with ffmpeg per Daniel. the current code just assumes that what was sent is in jpeg format or whatever, not YUV420 - // using opencv cvtColor(src, dst, cv::COLOR_YUV2BGR_NV21)? - // will this function be reading in images and then converting them? or are we converting them as we receive the image buffers - - - // this logic is what deconstructPacket() was supposed to be - CameraResponse_t response; - - serialh::deserialize(&response, &this->recvbuf); - - std::vector image = response.imageData.imgBuffer; - - // "reassemble" the image - cv::Mat m = cv::Mat(response.imageData.height, response.imageData.width, CV_8UC3); - - if (image.size() == (response.imageData.imageSizeBytes)) { - std::memcpy(m.data, image.data(), sizeof(std::uint8_t) * response.imageData.imageSizeBytes); - } - - return m; - -} - -Client::Client(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_) { - - this->ip = ip; - this->port = port; - - // TODO: could use the socket keepalive option here to keep a persistent connection, unsure how it differs from the ping() function. idk what the ping() function even does. - -} - -void Client::connect() { - - // TODO: should we check if its already connected? maybe multiple threads could try calling connect() - - // make the endpoint - asio::ip::tcp::endpoint endpoint_(asio::ip::tcp::endpoint(asio::ip::make_address(this->ip), this->port)); - - // asynchronously connect to endpoint - // TODO: is a lambda appropriate here as the completion handler? - this->socket_.async_connect(endpoint_, - [this](boost::system::error_code ec) { - - // TODO: what do we do in case of an error? - if (ec) { - std::cout << "Failed to connect" << '\n'; - std::cout << "Reason: " << ec.message() << " code: " << ec.value() << '\n'; - } - - std::cout << "Connected to: " << this->ip << " on port: " << this->port << '\n'; - - }); - -} - -void Client::ping() { - - /* - This is what i assume the function does: - - basically just open up a new connection and send a packet with Ping + other stuff and wait for a response back - - if it fails to connect or a response is taking too long, then something has happened but what do we do? print out a request timed out message? - */ - -} - -void Client::takePicture() { - - this->send(RequestType_t::SENDIMAGE); - - this->recv(); - -} - -cv::Mat Client::getLatestImage() { - - // TODO: what if there are no images in the image buffer? - - cv::Mat latestImage {}; - - // TODO: acquire a lock, just regular not a read or write lock? - - latestImage = this->images.front(); - this->images.pop(); - - // TODO: release the lock - - return latestImage; - -} - - - diff --git a/src/network/async_server.cpp b/src/network/async_server.cpp deleted file mode 100644 index e69de29b..00000000 diff --git a/src/network/camera_client.cpp b/src/network/camera_client.cpp deleted file mode 100644 index 02b1ece3..00000000 --- a/src/network/camera_client.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include -#include -#include -#include "include/network/camera_data.hpp" -#include "include/network/camera_client.hpp" -namespace asio = boost::asio; - -// CameraClient::CameraClient(int port) -// { -// this->port=port; -// } - -// bool CameraClient::connect(asio::io_context *io_context, std::string ip, int port) -// { -// // create an asio endpoint -// asio::ip::tcp::endpoint endpoint(asio::make_adress(ip), port); - -// // create a socket -// this->socket(&io_context); - -// // connect socket -// this->socket::connect(&endpoint); - -// // TODO: error handling - -// return true; -// } - -// // thinking if we just gonna run the same buff size and img proportions? -// bool CameraClient::send(std::vector image) -// { -// // TODO: after finishing working version, try to serialize and send whole data type -// asio::buffer::const_buffer image_buffer = asio::buffer(&image, sizeof(image)); - -// //TODO: PUT ERROR DATA TYPE VAR HERE - -// asio::socket::write(socket, image_buffrer); // TODO: PUT ERROR HANDLER HERE - -// return true; -// } diff --git a/src/network/camera_server.cpp b/src/network/camera_server.cpp deleted file mode 100644 index 5fe71a02..00000000 --- a/src/network/camera_server.cpp +++ /dev/null @@ -1,80 +0,0 @@ -#include -#include -#include -#include "include/network/camera_data.hpp" -#include "include/network/camera_server.hpp" - -// https://www.codeproject.com/Articles/1264257/Socket-Programming-in-Cplusplus-using-boost-asio-T -namespace asio = boost::asio; - -// may throw an error due to acceptor construction -ObcCameraServer::ObcCameraServer(std::string ip, int port, asio::io_context& io_context_) : socket_(io_context_), acceptor_(asio::ip::tcp::endpoint(asio::ip::make_address(ip), port)) { - - this->ip = ip; - this->port = port; - -} - -bool ObcCameraServer::connect() { - - try { - - // may throw an error - this->acceptor_.accept(this->socket_); - return true; - - } catch(std::exception& e) { - - return false; - } - -} - -bool ObcCameraServer::send(RequestType_t type) { - - CameraRequest_t request; - request.pid = 0; // TODO: no idea what pid is - request.requestType = type; - - asio::streambuf buf; - boost::system::error_code ec; - - // serialize the request struct - // TODO: create helper functions (serialize.hpp) - std::ostream os(&buf); - boost::archive::binary_oarchive oa(os); - oa << request; - - // send the serialized data, may result in an error - asio::write(this->socket_, buf, ec); - - if (ec) { - return false; - } - - return true; -} - -CameraResponse_t ObcCameraServer::read() { - - CameraResponse_t response; - asio::streambuf buf; - boost::system::error_code ec; - - // read the data from the socket, may result in an error - asio::read(this->socket_, buf, ec); - - if (ec) { - // TODO: what do we return if no data read - } - - // deserialize the data - // TODO: create helper functions (serialize.hpp) - std::istream is(&buf); - boost::archive::binary_iarchive ia(is); - ia >> response; - - return response; -} - - diff --git a/src/network/camera_socket.cpp b/src/network/camera_socket.cpp deleted file mode 100644 index 3b0e9bbe..00000000 --- a/src/network/camera_socket.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include -#include -#include - -// Very large just in case strerror(errno) actually ends up being incredibly large -#define AD_ERR_LEN 999 - -using namespace boost::asio; -using ip::tcp; - -tcp::socket socket; - -// CameraSocket::CameraSocket() { - -// } diff --git a/src/network/sync_server.cpp b/src/network/sync_server.cpp deleted file mode 100644 index 8cc2773d..00000000 --- a/src/network/sync_server.cpp +++ /dev/null @@ -1,188 +0,0 @@ -#include -#include "include/network/sync_server.hpp" - - -ImageData_t Server::createImageData() { - - ImageData_t i; - cv::Mat img = cv::imread("birb.jpg"); // should be whatever the image is - - // converts a Mat to vector (Mat is of type CV_8UC3) - cv::Mat flatten = img.reshape(1, img.total() * img.channels()); - std::vector imgBuffer = img.isContinuous() ? flatten : flatten.clone(); - - // fill struct with appropriate data - i.height = img.rows; - i.width = img.cols; - i.imageSizeBytes = img.total() * img.elemSize(); - i.imgBuffer = imgBuffer; - - return i; - -} - -CameraResponse_t Server::createResponse(ResponseType_t responseType) { - - ImageData_t imgData = createImageData(); - - // fill struct with appropriate data - CameraResponse_t response; - response.pid = 1010; - response.responseType = responseType; - response.imageData = imgData; - - return response; - -} - -void Server::createResponsePacket(ResponseType_t responseType) { - - CameraResponse_t response = createResponse(responseType); - - serialh::serialize(&response, &this->sendbuf); - -} - -void Server::sendHeader() { - - std::uint32_t headerLength = static_cast(this->sendbuf.size()); - - // TODO: error handler - asio::write(this->socket_, asio::buffer(&headerLength, sizeof(headerLength))); - -} - -void Server::sendBody() { - - // TODO: error handler - asio::write(this->socket_, this->sendbuf); - -} - -std::uint32_t Server::receiveHeader() { - - std::uint32_t size {}; - - // TODO: error handler - asio::read(this->socket_, asio::buffer(&size, sizeof(size)), ec_read); - - if (ec_read) { - std::cout << "Failed to receive header: " << ec_read.message() << '\n'; - } - - return size; - -} - -void Server::receiveBody(std::uint32_t size) { - - // TODO: error handler - asio::read(this->socket_, this->receivebuf.prepare(size)); - - if (ec_read) { - std::cout << "Failed to receive body: " << ec_read.message() << '\n'; - } - - this->receivebuf.commit(size); - -} - -CameraRequest_t Server::deconstructPacket() { - - CameraRequest_t request; - - serialh::deserialize(&request, &this->receivebuf); - - return request; - -} - -void Server::handlePacket() { - - // TODO: deconstruct packet - CameraRequest_t request = deconstructPacket(); - - std::cout << request.pid << '\n'; - std::cout << static_cast(request.requestType) << '\n'; - - // TODO: in this testing case prepare an image - -} - -// TODO: error handler (acceptor initialization) -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::connect() { - - try { - - // TODO: error handler (switch to not exception?) - this->acceptor_.accept(this->socket_); - std::cout << "Listening on port: " << this->port << '\n'; - - } catch (std::exception& e) { - - std::cout << e.what() << '\n'; - - } - -} - -void Server::send(ResponseType_t responseType) { - - createResponsePacket(responseType); - - sendHeader(); - sendBody(); - -} - -void Server::receive() { - - std::uint32_t bodySize = receiveHeader(); - receiveBody(bodySize); - - // TODO: if the client closes the connection, there won't be anything in the receivebuf and it throws an exception - - if (this->ec_read) { - return; - } - - handlePacket(); - -} - -void Server::close() { - - // TODO: error handler - this->socket_.shutdown(asio::ip::tcp::socket::shutdown_both); - - // TODO: error handler - this->socket_.close(); - -} - -void Server::shutdown() { - - this->acceptor_.close(); - close(); - this->socket_.release(); // TODO: unsure if this is correct - -} - -boost::system::error_code Server::getReadError() { - - return this->ec_read; - -} - -boost::system::error_code Server::getWriteError() { - - return this->ec_write; - -} diff --git a/src/network/client.cpp b/src/network/tcp_client.cpp similarity index 92% rename from src/network/client.cpp rename to src/network/tcp_client.cpp index c0fd2039..7bd556ce 100644 --- a/src/network/client.cpp +++ b/src/network/tcp_client.cpp @@ -1,5 +1,5 @@ #include -#include "network/client.hpp" +#include "network/tcp_client.hpp" Client::Client(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_) { @@ -26,7 +26,7 @@ bool Client::connect() { return true; } -void Client::send(std::uint8_t request) { +bool Client::send(std::uint8_t request) { boost::system::error_code ec; LOG_F(INFO, std::string("Sending request" + static_cast(request)).c_str()); @@ -36,10 +36,11 @@ void Client::send(std::uint8_t request) { 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() { @@ -78,11 +79,3 @@ std::vector Client::recvBody(const int bufSize) { return recvbuf; } - -std::string Client::getIP() { - return this->ip; -} - -int Client::getPort() { - return this->port; -} diff --git a/src/network/mock_server.cpp b/src/network/tcp_server.cpp similarity index 98% rename from src/network/mock_server.cpp rename to src/network/tcp_server.cpp index 49f71af2..d3f135b5 100644 --- a/src/network/mock_server.cpp +++ b/src/network/tcp_server.cpp @@ -1,4 +1,4 @@ -#include "network/mock_server.hpp" +#include "network/tcp_server.hpp" const uint32_t IMG_WIDTH = 2028; const uint32_t IMG_HEIGHT = 1520; diff --git a/src/network/udp_client.cpp b/src/network/udp_client.cpp new file mode 100644 index 00000000..b2b8114f --- /dev/null +++ b/src/network/udp_client.cpp @@ -0,0 +1,81 @@ +#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) { + std::cout << "Failed to connect" << '\n'; + return false; + } + + std::cout << ("Connected to %s on port %d", this->ip, this->port) << '\n'; + + 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)); + + std::cout << ("Sending request %c", request) << '\n'; + + int bytesSent = this->socket_.send_to(asio::buffer(&request, sizeof(request)), endpoint_, 0, ec); + + if (ec) { + std::cout << ("Failed to send request: %s", ec.message()) << '\n'; + return false; + } + + std::cout << ("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)); + + Header header; + + int bytesRead = this->socket_.receive_from(asio::buffer(&header, sizeof(Header)), endpoint_, 0, ec); + + if (ec) { + std::cout << ("Failed to read header: %s", ec.message()) << '\n'; + return {}; + } + + std::cout << ("Bytes read (header): %d", bytesRead) << '\n'; + + return header; +} + +std::vector UDPClient::recvBody(const int bufSize) { + boost::system::error_code ec; + asio::ip::udp::endpoint endpoint_(asio::ip::udp::endpoint(asio::ip::make_address(this->ip), this->port)); + + std::cout << ("Reading in bufSize (body): %d", bufSize) << '\n'; + + std::vector buf(bufSize); + + int bytesRead = this->socket_.receive_from(asio::buffer(buf), endpoint_, 0, ec); + + if (ec) { + std::cout << ("Failed to send body: %s", ec.message()) << '\n'; + return {}; + } + + std::cout << ("Bytes read: %d", bytesRead) << '\n'; + + return buf; +} diff --git a/src/network/udp_server.cpp b/src/network/udp_server.cpp new file mode 100644 index 00000000..57cd6d22 --- /dev/null +++ b/src/network/udp_server.cpp @@ -0,0 +1,35 @@ +#include "network/udp_server.hpp" + +const uint32_t IMG_WIDTH = 2028; +const uint32_t IMG_HEIGHT = 1520; +const uint32_t BUFFER_SIZE = IMG_WIDTH * IMG_HEIGHT * 3 / 2; + +void UDPServer::takePicture() { + +} + + + +UDPServer::UDPServer(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_){ + +} + +void UDPServer::start() { + +} + +void UDPServer::send() { + +} + +void UDPServer::recv() { + +} + +void UDPServer::handleRequest(char request) { + +} + +void UDPServer::shutdown() { + +} diff --git a/tests/integration/CMakeLists.txt b/tests/integration/CMakeLists.txt index 69a86c9f..695de75f 100644 --- a/tests/integration/CMakeLists.txt +++ b/tests/integration/CMakeLists.txt @@ -293,41 +293,41 @@ target_add_loguru(camera_mock) target_include_directories(camera_mock PRIVATE ${ImageMagick_INCLUDE_DIRS}) target_link_libraries(camera_mock PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) -add_executable(sync_client_mock "sync_client_mock.cpp") -target_link_libraries(sync_client_mock PRIVATE +add_executable(tcp_client_mock "tcp_client_mock.cpp") +target_link_libraries(tcp_client_mock PRIVATE obcpp_lib ) -target_include_directories(sync_client_mock PRIVATE ${INCLUDE_DIRECTORY}) -target_add_boost(sync_client_mock) -target_add_protobuf(sync_client_mock) -target_add_torch(sync_client_mock) -target_add_torchvision(sync_client_mock) -target_add_json(sync_client_mock) -target_add_opencv(sync_client_mock) -target_add_httplib(sync_client_mock) -target_add_mavsdk(sync_client_mock) -target_add_matplot(sync_client_mock) -target_add_loguru(sync_client_mock) -target_include_directories(sync_client_mock PRIVATE ${ImageMagick_INCLUDE_DIRS}) -target_link_libraries(sync_client_mock PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) +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(sync_server_mock "sync_server_mock.cpp") -target_link_libraries(sync_server_mock PRIVATE +add_executable(tcp_server_mock "tcp_server_mock.cpp") +target_link_libraries(tcp_server_mock PRIVATE obcpp_lib ) -target_include_directories(sync_server_mock PRIVATE ${INCLUDE_DIRECTORY}) -target_add_boost(sync_server_mock) -target_add_protobuf(sync_server_mock) -target_add_torch(sync_server_mock) -target_add_torchvision(sync_server_mock) -target_add_json(sync_server_mock) -target_add_opencv(sync_server_mock) -target_add_httplib(sync_server_mock) -target_add_mavsdk(sync_server_mock) -target_add_matplot(sync_server_mock) -target_add_loguru(sync_server_mock) -target_include_directories(sync_server_mock PRIVATE ${ImageMagick_INCLUDE_DIRS}) -target_link_libraries(sync_server_mock PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) +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}) # cuda_check add_executable(cuda_check ${SOURCES} cuda_check.cpp) diff --git a/tests/integration/sync_client_mock.cpp b/tests/integration/tcp_client_mock.cpp similarity index 62% rename from tests/integration/sync_client_mock.cpp rename to tests/integration/tcp_client_mock.cpp index 631e1508..a29fcaf5 100644 --- a/tests/integration/sync_client_mock.cpp +++ b/tests/integration/tcp_client_mock.cpp @@ -2,21 +2,13 @@ #include #include #include -#include "network/client.hpp" +#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[]) { - // if (argc < 2) { - // LOG_F(ERROR, "Usage: ./bin/camera_mock [path_to_config] [optional: output_dir]"); - // exit(1); - // } - // std::filesystem::path output_dir = std::filesystem::current_path(); - // if (argc >= 3) { - // output_dir = argv[2]; - // } CameraConfig config; @@ -24,9 +16,6 @@ int main(int argc, char* argv[]) { RPICamera camera(config, &io_context_); - // std::deque imageQueue; - // auto mav = std::make_shared("serial:///dev/ttyACM0"); - camera.connect(); std::optional img = camera.takePicture(std::chrono::milliseconds(1000), nullptr); diff --git a/tests/integration/sync_server_mock.cpp b/tests/integration/tcp_server_mock.cpp similarity index 86% rename from tests/integration/sync_server_mock.cpp rename to tests/integration/tcp_server_mock.cpp index 743fa2dd..147df75e 100644 --- a/tests/integration/sync_server_mock.cpp +++ b/tests/integration/tcp_server_mock.cpp @@ -1,7 +1,7 @@ #include #include #include -#include "network/mock_server.hpp" +#include "network/tcp_server.hpp" namespace asio = boost::asio; @@ -18,5 +18,5 @@ int main() { server.start(); //} - return EXIT_SUCCESS; + return 0; } diff --git a/tests/integration/udp_client_mock.cpp b/tests/integration/udp_client_mock.cpp new file mode 100644 index 00000000..20e4cd99 --- /dev/null +++ b/tests/integration/udp_client_mock.cpp @@ -0,0 +1,30 @@ +#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()) { + 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/udp_server_mock.cpp b/tests/integration/udp_server_mock.cpp new file mode 100644 index 00000000..0c68cd04 --- /dev/null +++ b/tests/integration/udp_server_mock.cpp @@ -0,0 +1,12 @@ +#include +#include +#include + +namespace asio = boost::asio; + +const std::string SERVER_IP = "127.0.0.1"; +const int SERVER_PORT = 5000; + +int main() { + return 0; +} From 6c68c3f5b5aaf51f3c03b566f0b33ebc13572ed5 Mon Sep 17 00:00:00 2001 From: smhitle <91502030+smhitle@users.noreply.github.com> Date: Thu, 15 May 2025 17:51:59 -0700 Subject: [PATCH 20/43] Added server mock stuff --- include/network/rpi_connection.hpp | 4 + include/network/udp_server.hpp | 7 +- src/camera/rpi.cpp | 6 +- src/network/CMakeLists.txt | 3 +- src/network/tcp_server.cpp | 4 - src/network/udp_client.cpp | 10 ++- src/network/udp_server.cpp | 118 ++++++++++++++++++++++++++--- 7 files changed, 126 insertions(+), 26 deletions(-) diff --git a/include/network/rpi_connection.hpp b/include/network/rpi_connection.hpp index 1985ea38..6db0fee0 100644 --- a/include/network/rpi_connection.hpp +++ b/include/network/rpi_connection.hpp @@ -3,6 +3,10 @@ #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; const int headerSize = 12; diff --git a/include/network/udp_server.hpp b/include/network/udp_server.hpp index d0996e2c..4e53f9f1 100644 --- a/include/network/udp_server.hpp +++ b/include/network/udp_server.hpp @@ -4,6 +4,7 @@ #include #include #include +#include "network/rpi_connection.hpp" namespace asio = boost::asio; @@ -20,13 +21,13 @@ class UDPServer { public: UDPServer(asio::io_context* io_context_, std::string ip, int port); - void start(); + bool start(); - void send(); + void send(asio::ip::udp::endpoint & endpoint); void recv(); - void handleRequest(char request); + void handleRequest(char request, asio::ip::udp::endpoint & endpoint); void shutdown(); }; diff --git a/src/camera/rpi.cpp b/src/camera/rpi.cpp index c4549e97..eb579c73 100644 --- a/src/camera/rpi.cpp +++ b/src/camera/rpi.cpp @@ -19,9 +19,9 @@ #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_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; diff --git a/src/network/CMakeLists.txt b/src/network/CMakeLists.txt index c907e864..537d4bcd 100644 --- a/src/network/CMakeLists.txt +++ b/src/network/CMakeLists.txt @@ -6,8 +6,9 @@ set(COMMON_FILES gcs.cpp mavlink.cpp tcp_client.cpp - tcp_mock_server.cpp + tcp_server.cpp udp_client.cpp + udp_server.cpp ) set(FILES diff --git a/src/network/tcp_server.cpp b/src/network/tcp_server.cpp index d3f135b5..daa8806c 100644 --- a/src/network/tcp_server.cpp +++ b/src/network/tcp_server.cpp @@ -1,9 +1,5 @@ #include "network/tcp_server.hpp" -const uint32_t IMG_WIDTH = 2028; -const uint32_t IMG_HEIGHT = 1520; -const uint32_t BUFFER_SIZE = IMG_WIDTH * IMG_HEIGHT * 3 / 2; - 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; diff --git a/src/network/udp_client.cpp b/src/network/udp_client.cpp index b2b8114f..15eaf0d6 100644 --- a/src/network/udp_client.cpp +++ b/src/network/udp_client.cpp @@ -44,11 +44,12 @@ bool UDPClient::send(std::uint8_t request) { // 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 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)), endpoint_, 0, ec); + int bytesRead = this->socket_.receive_from(asio::buffer(&header, sizeof(Header)), sender_endpoint, 0, ec); if (ec) { std::cout << ("Failed to read header: %s", ec.message()) << '\n'; @@ -62,13 +63,14 @@ Header UDPClient::recvHeader() { std::vector UDPClient::recvBody(const int bufSize) { 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 endpoint_(asio::ip::udp::endpoint(asio::ip::make_address(this->ip), this->port)); + asio::ip::udp::endpoint sender_endpoint; std::cout << ("Reading in bufSize (body): %d", bufSize) << '\n'; std::vector buf(bufSize); - int bytesRead = this->socket_.receive_from(asio::buffer(buf), endpoint_, 0, ec); + int bytesRead = this->socket_.receive_from(asio::buffer(buf), sender_endpoint, 0, ec); if (ec) { std::cout << ("Failed to send body: %s", ec.message()) << '\n'; diff --git a/src/network/udp_server.cpp b/src/network/udp_server.cpp index 57cd6d22..befffb0a 100644 --- a/src/network/udp_server.cpp +++ b/src/network/udp_server.cpp @@ -1,35 +1,131 @@ #include "network/udp_server.hpp" -const uint32_t IMG_WIDTH = 2028; -const uint32_t IMG_HEIGHT = 1520; -const uint32_t BUFFER_SIZE = IMG_WIDTH * IMG_HEIGHT * 3 / 2; +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"); -void UDPServer::takePicture() { + // 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)); -UDPServer::UDPServer(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_){ + 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); -void UDPServer::start() { + if (bind_ec) { + std::cout << ("Failed to bind socket: %s", bind_ec.message()) << '\n'; + return false; + } + return true; } -void UDPServer::send() { +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'; + } -void UDPServer::handleRequest(char request) { + std::cout << ("Bytes read (request): %d", bytesRead) << '\n'; + handleRequest(request, client_endpoint); } -void UDPServer::shutdown() { +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'; } From 4121c05e08cfa1e5259d3b4821d0b268d989f085 Mon Sep 17 00:00:00 2001 From: smhitle <91502030+smhitle@users.noreply.github.com> Date: Sun, 18 May 2025 12:46:03 -0700 Subject: [PATCH 21/43] Changed cout's to LOG_F's --- include/network/rpi_connection.hpp | 5 +++++ include/network/udp_client.hpp | 1 + src/network/udp_client.cpp | 22 +++++++++++----------- tests/integration/CMakeLists.txt | 18 ++++++++++++++++++ 4 files changed, 35 insertions(+), 11 deletions(-) diff --git a/include/network/rpi_connection.hpp b/include/network/rpi_connection.hpp index 6db0fee0..418887ac 100644 --- a/include/network/rpi_connection.hpp +++ b/include/network/rpi_connection.hpp @@ -9,6 +9,11 @@ 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 = "172.28.114.172"; +// const int SERVER_PORT = 5000; + const int headerSize = 12; const uint32_t EXPECTED_MAGIC = 0x12345678; const size_t CHUNK_SIZE = 1024; diff --git a/include/network/udp_client.hpp b/include/network/udp_client.hpp index 0f72dea8..dba31dad 100644 --- a/include/network/udp_client.hpp +++ b/include/network/udp_client.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include "rpi_connection.hpp" namespace asio = boost::asio; diff --git a/src/network/udp_client.cpp b/src/network/udp_client.cpp index 15eaf0d6..a54d1978 100644 --- a/src/network/udp_client.cpp +++ b/src/network/udp_client.cpp @@ -13,31 +13,31 @@ bool UDPClient::connect() { this->socket_.open(asio::ip::udp::v4(), ec); if (ec) { - std::cout << "Failed to connect" << '\n'; + LOG_F(ERROR, "Failed to connect"); return false; } - std::cout << ("Connected to %s on port %d", this->ip, this->port) << '\n'; + 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)); - std::cout << ("Sending request %c", request) << '\n'; + LOG_F(INFO, "Sending request %c", request); int bytesSent = this->socket_.send_to(asio::buffer(&request, sizeof(request)), endpoint_, 0, ec); if (ec) { - std::cout << ("Failed to send request: %s", ec.message()) << '\n'; + LOG_F(ERROR, "Failed to send request: %s", ec.message().c_str()); return false; } - std::cout << ("Sent %d bytes", bytesSent); + LOG_F(INFO, "Sent %d bytes", bytesSent); + return true; } @@ -52,11 +52,11 @@ Header UDPClient::recvHeader() { int bytesRead = this->socket_.receive_from(asio::buffer(&header, sizeof(Header)), sender_endpoint, 0, ec); if (ec) { - std::cout << ("Failed to read header: %s", ec.message()) << '\n'; + LOG_F(ERROR, "Failed to read header: %s", ec.message().c_str()); return {}; } - std::cout << ("Bytes read (header): %d", bytesRead) << '\n'; + LOG_F(INFO, "Bytes read (header): %d", bytesRead); return header; } @@ -66,18 +66,18 @@ std::vector UDPClient::recvBody(const int bufSize) { // asio::ip::udp::endpoint endpoint_(asio::ip::udp::endpoint(asio::ip::make_address(this->ip), this->port)); asio::ip::udp::endpoint sender_endpoint; - std::cout << ("Reading in bufSize (body): %d", bufSize) << '\n'; + LOG_F(INFO, "Reading in bufSize (body): %d", bufSize); std::vector buf(bufSize); int bytesRead = this->socket_.receive_from(asio::buffer(buf), sender_endpoint, 0, ec); if (ec) { - std::cout << ("Failed to send body: %s", ec.message()) << '\n'; + LOG_F(ERROR, "Failed to send body: %s", ec.message().c_str()); return {}; } - std::cout << ("Bytes read: %d", bytesRead) << '\n'; + LOG_F(INFO, "Bytes read: %d", bytesRead); return buf; } diff --git a/tests/integration/CMakeLists.txt b/tests/integration/CMakeLists.txt index 695de75f..00f132ac 100644 --- a/tests/integration/CMakeLists.txt +++ b/tests/integration/CMakeLists.txt @@ -311,6 +311,24 @@ 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 From b8eb719ce990b1df71ac0f91cf0393daa8bfcee6 Mon Sep 17 00:00:00 2001 From: smhitle <91502030+smhitle@users.noreply.github.com> Date: Thu, 22 May 2025 14:28:45 -0700 Subject: [PATCH 22/43] Can read in udp packets correctly (hopefully) --- include/network/rpi_connection.hpp | 8 +++---- include/network/udp_client.hpp | 2 +- src/camera/rpi.cpp | 27 ++++++++++++--------- src/network/udp_client.cpp | 34 ++++++++++++++++++++++++--- tests/integration/udp_client_mock.cpp | 9 +++++-- 5 files changed, 59 insertions(+), 21 deletions(-) diff --git a/include/network/rpi_connection.hpp b/include/network/rpi_connection.hpp index 418887ac..53fc9652 100644 --- a/include/network/rpi_connection.hpp +++ b/include/network/rpi_connection.hpp @@ -7,12 +7,12 @@ 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; +// const std::string SERVER_IP = "192.168.68.1"; +// const int SERVER_PORT = 25565; // local testing only -// const std::string SERVER_IP = "172.28.114.172"; -// const int SERVER_PORT = 5000; +const std::string SERVER_IP = "172.28.114.172"; +const int SERVER_PORT = 5000; const int headerSize = 12; const uint32_t EXPECTED_MAGIC = 0x12345678; diff --git a/include/network/udp_client.hpp b/include/network/udp_client.hpp index dba31dad..246d218c 100644 --- a/include/network/udp_client.hpp +++ b/include/network/udp_client.hpp @@ -25,7 +25,7 @@ class UDPClient { Header recvHeader(); - std::vector recvBody(const int bufSize); + std::vector recvBody(const int mem_size, const int total_chunks); }; #endif diff --git a/src/camera/rpi.cpp b/src/camera/rpi.cpp index eb579c73..bcb00066 100644 --- a/src/camera/rpi.cpp +++ b/src/camera/rpi.cpp @@ -101,17 +101,20 @@ void RPICamera::ping() { std::vector RPICamera::readImage() { // 3 separate reads for 3 separate files - std::vector imgbuf; + // std::vector imgbuf; - for (int i = 0; i < 3; i++) { - std::vector packet = readPacket(); + // 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()); - } + // // 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()); + // } - return imgbuf; + std::vector packet = readPacket(); + + return packet; + // return imgbuf; } std::optional RPICamera::readTelemetry() { @@ -133,15 +136,17 @@ std::vector RPICamera::readPacket() { 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); - - // TODO: idek if we have to do ntoh for the buffer + packet = client.recvBody(header.mem_size, header.total_chunks); + io_context_.run(); + return packet; } diff --git a/src/network/udp_client.cpp b/src/network/udp_client.cpp index a54d1978..8f5dd291 100644 --- a/src/network/udp_client.cpp +++ b/src/network/udp_client.cpp @@ -61,23 +61,51 @@ Header UDPClient::recvHeader() { return header; } -std::vector UDPClient::recvBody(const int bufSize) { +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 bytesRead = this->socket_.receive_from(asio::buffer(buf), sender_endpoint, 0, ec); + 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 {}; } - LOG_F(INFO, "Bytes read: %d", bytesRead); + 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/tests/integration/udp_client_mock.cpp b/tests/integration/udp_client_mock.cpp index 20e4cd99..457e2841 100644 --- a/tests/integration/udp_client_mock.cpp +++ b/tests/integration/udp_client_mock.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include "network/udp_client.hpp" #include "camera/rpi.hpp" #include "utilities/obc_config.hpp" @@ -21,9 +22,13 @@ int main(int argc, char* argv[]) { 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); - img.value().saveToFile(filepath); + 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; From 4d3f71763b322a08dd4e8529cc81600052108e08 Mon Sep 17 00:00:00 2001 From: smhitle <91502030+smhitle@users.noreply.github.com> Date: Sun, 25 May 2025 12:53:30 -0700 Subject: [PATCH 23/43] Updated mock udp server --- src/network/udp_server.cpp | 197 ++++++++++++++++++++++++-- tests/integration/udp_server_mock.cpp | 14 +- 2 files changed, 196 insertions(+), 15 deletions(-) diff --git a/src/network/udp_server.cpp b/src/network/udp_server.cpp index befffb0a..ffae3f67 100644 --- a/src/network/udp_server.cpp +++ b/src/network/udp_server.cpp @@ -31,21 +31,24 @@ bool UDPServer::start() { 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'; + 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: %s", open_ec.message()) << '\n'; + 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: %s", bind_ec.message()) << '\n'; + std::cout << "Failed to bind socket: " << bind_ec.message() << '\n'; return false; } @@ -73,28 +76,64 @@ void UDPServer::send(asio::ip::udp::endpoint & endpoint) { 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(imgBuffer.size() / CHUNK_SIZE); + header.total_chunks = htonl(total_chunks); // send header - int bytesReadHeader = this->socket_.send_to(asio::buffer(&header, sizeof(header)), endpoint, 0, header_ec); + int bytesSentHeader = 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'; + std::cout << "Sending header failed: " << header_ec.message() << '\n'; return; } + std::cout << "Read bytes (header): " << bytesSentHeader << '\n'; + // 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; + 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 @@ -108,24 +147,154 @@ void UDPServer::recv() { 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 << "Failed to read request: " << ec.message() << '\n'; } - std::cout << ("Bytes read (request): %d", bytesRead) << '\n'; + std::cout << "Bytes read (request): " << bytesRead << '\n'; handleRequest(request, client_endpoint); } void UDPServer::handleRequest(char request, asio::ip::udp::endpoint & endpoint) { - if (request == 'p') { + if (request == 'I') { this->send(endpoint); } else if (request == 'e') { this->shutdown(); } else { - std::cout << ("Invalid request: %c", request) << '\n'; + 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/udp_server_mock.cpp b/tests/integration/udp_server_mock.cpp index 0c68cd04..2df34627 100644 --- a/tests/integration/udp_server_mock.cpp +++ b/tests/integration/udp_server_mock.cpp @@ -1,6 +1,7 @@ #include #include #include +#include "network/udp_server.hpp" namespace asio = boost::asio; @@ -8,5 +9,16 @@ const std::string SERVER_IP = "127.0.0.1"; const int SERVER_PORT = 5000; int main() { - return 0; + 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(); + } } From 6f78c76faa3fbc3ca13fe14bbf57d43a316f94ad Mon Sep 17 00:00:00 2001 From: smhitle <91502030+smhitle@users.noreply.github.com> Date: Sun, 25 May 2025 12:56:50 -0700 Subject: [PATCH 24/43] Updated cmake for server mock --- tests/integration/CMakeLists.txt | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/tests/integration/CMakeLists.txt b/tests/integration/CMakeLists.txt index 00f132ac..a6ce17b6 100644 --- a/tests/integration/CMakeLists.txt +++ b/tests/integration/CMakeLists.txt @@ -347,6 +347,24 @@ 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}) + # cuda_check add_executable(cuda_check ${SOURCES} cuda_check.cpp) target_add_torch(cuda_check) From 19cacc75c3eb3395b5b98e5874a81c4fd1c7d11f Mon Sep 17 00:00:00 2001 From: smhitle <91502030+smhitle@users.noreply.github.com> Date: Sun, 25 May 2025 13:44:47 -0700 Subject: [PATCH 25/43] Mock server works for dev container purposes --- include/network/rpi_connection.hpp | 2 +- src/network/udp_server.cpp | 7 +++++-- tests/integration/udp_server_mock.cpp | 4 ++-- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/include/network/rpi_connection.hpp b/include/network/rpi_connection.hpp index 53fc9652..f1440807 100644 --- a/include/network/rpi_connection.hpp +++ b/include/network/rpi_connection.hpp @@ -11,7 +11,7 @@ const uint32_t BUFFER_SIZE = IMG_WIDTH * IMG_HEIGHT * 3 / 2; // const int SERVER_PORT = 25565; // local testing only -const std::string SERVER_IP = "172.28.114.172"; +const std::string SERVER_IP = "0.0.0.0"; const int SERVER_PORT = 5000; const int headerSize = 12; diff --git a/src/network/udp_server.cpp b/src/network/udp_server.cpp index ffae3f67..97a7943d 100644 --- a/src/network/udp_server.cpp +++ b/src/network/udp_server.cpp @@ -1,7 +1,9 @@ +#include #include "network/udp_server.hpp" cv::Mat UDPServer::createBGR() { - cv::Mat image = cv::imread("blurb.jpeg"); + 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 @@ -11,8 +13,9 @@ cv::Mat UDPServer::createBGR() { } cv::Mat UDPServer::createYUV() { + std::cout << std::filesystem::current_path() << '\n'; // read in an image - cv::Mat image = cv::imread("blurb.jpeg"); + 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); diff --git a/tests/integration/udp_server_mock.cpp b/tests/integration/udp_server_mock.cpp index 2df34627..26227696 100644 --- a/tests/integration/udp_server_mock.cpp +++ b/tests/integration/udp_server_mock.cpp @@ -5,8 +5,8 @@ namespace asio = boost::asio; -const std::string SERVER_IP = "127.0.0.1"; -const int SERVER_PORT = 5000; +const std::string IP = "0.0.0.0"; +const int PORT = 5000; int main() { asio::io_context io_context_; From a9f90d7a65f0461abd4300f3f6272b14402d1921 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Sun, 9 Nov 2025 15:35:30 -0800 Subject: [PATCH 26/43] chore: update integration/cmake --- tests/integration/CMakeLists.txt | 33 -------------------------------- 1 file changed, 33 deletions(-) diff --git a/tests/integration/CMakeLists.txt b/tests/integration/CMakeLists.txt index cdfc7382..208903c0 100644 --- a/tests/integration/CMakeLists.txt +++ b/tests/integration/CMakeLists.txt @@ -142,21 +142,6 @@ target_add_loguru(airdrop_packets) # -E env LD_LIBRARY_s/target_siamese_1.pt # ) -add_executable(camera_mock "camera_mock.cpp") -target_link_libraries(camera_mock PRIVATE obcpp_lib) -target_include_directories(camera_mock PRIVATE ${INCLUDE_DIRECTORY}) -target_add_protobuf(camera_mock) -target_add_torch(camera_mock) -target_add_torchvision(camera_mock) -target_add_json(camera_mock) -target_add_opencv(camera_mock) -target_add_httplib(camera_mock) -target_add_mavsdk(camera_mock) -target_add_matplot(camera_mock) -target_add_loguru(camera_mock) -target_include_directories(camera_mock PRIVATE ${ImageMagick_INCLUDE_DIRS}) -target_link_libraries(camera_mock PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) - add_executable(tcp_client_mock "tcp_client_mock.cpp") target_link_libraries(tcp_client_mock PRIVATE obcpp_lib @@ -229,24 +214,6 @@ 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}) -# cuda_check -add_executable(cuda_check ${SOURCES} cuda_check.cpp) -target_add_torch(cuda_check) -target_add_json(cuda_check) -target_add_httplib(cuda_check) -target_add_mavsdk(cuda_check) -target_add_matplot(cuda_check) -target_add_protobuf(cuda_check) -target_add_opencv(cuda_check) -target_add_loguru(cuda_check) -# for some reason calling target_add_imagemagick here conflicts with, so we are including/linking without the function call -# target_add_imagemagick(cuda_check) -target_include_directories(cuda_check PRIVATE ${ImageMagick_INCLUDE_DIRS}) -target_link_libraries(cuda_check PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) -# cuda_check -add_executable(cuda_check ${SOURCES} cuda_check.cpp) -target_add_torch(cuda_check) - add_executable(drop_it_like_its_hot "drop_it_like_its_hot.cpp") target_link_libraries(drop_it_like_its_hot PRIVATE obcpp_lib) target_include_directories(drop_it_like_its_hot PRIVATE ${INCLUDE_DIRECTORY}) From d0d76d4250d0a118a7706db074041d1e997f6ad2 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Sun, 9 Nov 2025 15:46:37 -0800 Subject: [PATCH 27/43] fix: mock camera (merge incorrect) --- include/camera/interface.hpp | 53 ++++++++++++++-------- include/camera/mock.hpp | 4 +- src/camera/interface.cpp | 86 +++++++++++------------------------- src/camera/mock.cpp | 26 +++++------ 4 files changed, 77 insertions(+), 92 deletions(-) diff --git a/include/camera/interface.hpp b/include/camera/interface.hpp index 5f6c4dff..bffbf204 100644 --- a/include/camera/interface.hpp +++ b/include/camera/interface.hpp @@ -22,6 +22,7 @@ class MavlinkClient; using json = nlohmann::json; using Mat = cv::Mat; +// struct to contain all telemetry that should be tagged with an image. struct ImageTelemetry { double latitude_deg; double longitude_deg; @@ -63,25 +64,41 @@ void saveImageTelemetryToFile(const ImageTelemetry& telemetry, const std::filesystem::path& filepath); class CameraInterface { - protected: - CameraConfig config; - - public: - explicit CameraInterface(const CameraConfig& config); - // explicit CameraInterface(); - virtual ~CameraInterface() = default; - - virtual void connect() = 0; - virtual bool isConnected() = 0; - - virtual void startTakingPictures(const std::chrono::milliseconds& interval, - std::shared_ptr mavlinkClient) = 0; + protected: + CameraConfig config; + + public: + explicit CameraInterface(const CameraConfig& config); + virtual ~CameraInterface() = default; + + virtual void connect() = 0; + virtual bool isConnected() = 0; - virtual void stopTakingPictures() = 0; + /** + * Start taking photos at an interval in a background thread. + * Also requires a shared_ptr to a MavlinkClient to tag + * images with flight telemetry at capture time. + */ + virtual void startTakingPictures(const std::chrono::milliseconds& interval, + std::shared_ptr mavlinkClient) = 0; + /** + * Close background thread started by startTakingPictures + */ + virtual void stopTakingPictures() = 0; - virtual void startStreaming() = 0; + // Get the latest buffered image + virtual std::optional getLatestImage() = 0; + // Get all the recently buffered images + virtual std::deque getAllImages() = 0; - virtual std::optional takePicture(const std::chrono::milliseconds& timeout, std::shared_ptr mavlinkClient) = 0; - }; + virtual void startStreaming() = 0; + + /** + * Blocking call that takes an image. If it takes longer than the timeout + * to capture the image, no image is returned. + */ + virtual std::optional takePicture(const std::chrono::milliseconds& timeout, + 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/mock.hpp b/include/camera/mock.hpp index 906e9965..58b9b516 100644 --- a/include/camera/mock.hpp +++ b/include/camera/mock.hpp @@ -35,14 +35,14 @@ class MockCamera : public CameraInterface { * image from a queue of images which means that the same image won't. * be returned in two subsequent calls */ - // std::optional getLatestImage() override; + std::optional getLatestImage() override; /** * getAllImages returns a queue of all the images that have been * cached since the last call to getAllImages. Once this is called, * it returns the cached images and clears the internal cache. */ - // std::deque getAllImages() override; + std::deque getAllImages() override; /** * Blocking call that takes an image. If it takes longer than the timeout diff --git a/src/camera/interface.cpp b/src/camera/interface.cpp index fe504902..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,41 +16,36 @@ 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()); - // std::cout << "Failed to save telemetry json to " << filepath.string().c_str() << '\n'; - 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 {}; + 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(); @@ -59,41 +53,15 @@ std::optional queryMavlinkImageTelemetry( 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 - }; - } - - 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::cout << "lat_deg" << lat_deg << std::endl; - std::cout << "long deg" << lon_deg << std::endl; - std::cout << "altitude" << altitude_agl_m << std::endl; - 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 { diff --git a/src/camera/mock.cpp b/src/camera/mock.cpp index bb55a435..730c3a61 100644 --- a/src/camera/mock.cpp +++ b/src/camera/mock.cpp @@ -66,19 +66,19 @@ void MockCamera::stopTakingPictures() { this->captureThread.join(); } -// std::optional MockCamera::getLatestImage() { -// ReadLock lock(this->imageQueueLock); -// ImageData lastImage = this->imageQueue.front(); -// this->imageQueue.pop_front(); -// return lastImage; -// } - -// std::deque MockCamera::getAllImages() { -// ReadLock lock(this->imageQueueLock); -// std::deque outputQueue = this->imageQueue; -// this->imageQueue = std::deque(); -// return outputQueue; -// } +std::optional MockCamera::getLatestImage() { + ReadLock lock(this->imageQueueLock); + ImageData lastImage = this->imageQueue.front(); + this->imageQueue.pop_front(); + return lastImage; +} + +std::deque MockCamera::getAllImages() { + ReadLock lock(this->imageQueueLock); + std::deque outputQueue = this->imageQueue; + this->imageQueue = std::deque(); + return outputQueue; +} void MockCamera::captureEvery(const std::chrono::milliseconds& interval, std::shared_ptr mavlinkClient) { From 98d302a900c8c4d8d5172ee57453ce6d1df865b0 Mon Sep 17 00:00:00 2001 From: AskewParity Date: Fri, 14 Nov 2025 13:45:54 -0800 Subject: [PATCH 28/43] fix: linking issue for camera and mavlink --- include/camera/rpi.hpp | 2 ++ src/camera/CMakeLists.txt | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/include/camera/rpi.hpp b/include/camera/rpi.hpp index b8cf0e2a..18022651 100644 --- a/include/camera/rpi.hpp +++ b/include/camera/rpi.hpp @@ -71,6 +71,8 @@ class RPICamera : public CameraInterface { 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, diff --git a/src/camera/CMakeLists.txt b/src/camera/CMakeLists.txt index dec3739c..943efaf4 100644 --- a/src/camera/CMakeLists.txt +++ b/src/camera/CMakeLists.txt @@ -9,7 +9,7 @@ set(FILES SET(LIB_DEPS obcpp_protos - # obcpp_network # circular dependency + obcpp_network # circular dependency - resolved at executable level ) add_library(${LIB_NAME} STATIC From 2e1baf6c9583000d50e76043ce6f1571f97cb38c Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Sun, 25 Jan 2026 01:51:48 -0800 Subject: [PATCH 29/43] chore: removed deprecated tcp files --- include/network/tcp_client.hpp | 50 ------------- include/network/tcp_server.hpp | 37 ---------- src/network/tcp_client.cpp | 81 --------------------- src/network/tcp_server.cpp | 101 -------------------------- tests/integration/tcp_client_mock.cpp | 30 -------- tests/integration/tcp_server_mock.cpp | 22 ------ 6 files changed, 321 deletions(-) delete mode 100644 include/network/tcp_client.hpp delete mode 100644 include/network/tcp_server.hpp delete mode 100644 src/network/tcp_client.cpp delete mode 100644 src/network/tcp_server.cpp delete mode 100644 tests/integration/tcp_client_mock.cpp delete mode 100644 tests/integration/tcp_server_mock.cpp diff --git a/include/network/tcp_client.hpp b/include/network/tcp_client.hpp deleted file mode 100644 index 7962cb6c..00000000 --- a/include/network/tcp_client.hpp +++ /dev/null @@ -1,50 +0,0 @@ -#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 deleted file mode 100644 index 61c90851..00000000 --- a/include/network/tcp_server.hpp +++ /dev/null @@ -1,37 +0,0 @@ -#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/src/network/tcp_client.cpp b/src/network/tcp_client.cpp deleted file mode 100644 index 7bd556ce..00000000 --- a/src/network/tcp_client.cpp +++ /dev/null @@ -1,81 +0,0 @@ -#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 deleted file mode 100644 index daa8806c..00000000 --- a/src/network/tcp_server.cpp +++ /dev/null @@ -1,101 +0,0 @@ -#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/tests/integration/tcp_client_mock.cpp b/tests/integration/tcp_client_mock.cpp deleted file mode 100644 index a29fcaf5..00000000 --- a/tests/integration/tcp_client_mock.cpp +++ /dev/null @@ -1,30 +0,0 @@ -#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 deleted file mode 100644 index 147df75e..00000000 --- a/tests/integration/tcp_server_mock.cpp +++ /dev/null @@ -1,22 +0,0 @@ -#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; -} From 43bd3ca8d3e11fba25d29d617a1d1598b6a4b0fa Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Sun, 25 Jan 2026 01:52:55 -0800 Subject: [PATCH 30/43] feat: update rpi config + plane request logic --- include/camera/rpi.hpp | 61 ++--- include/network/rpi_connection.hpp | 20 +- src/camera/rpi.cpp | 224 +++++++++--------- src/camera/rpi.cpp~ | 360 ----------------------------- 4 files changed, 140 insertions(+), 525 deletions(-) delete mode 100644 src/camera/rpi.cpp~ diff --git a/include/camera/rpi.hpp b/include/camera/rpi.hpp index 18022651..2d75897c 100644 --- a/include/camera/rpi.hpp +++ b/include/camera/rpi.hpp @@ -1,88 +1,61 @@ #ifndef INCLUDE_CAMERA_RPI_HPP_ #define INCLUDE_CAMERA_RPI_HPP_ - #include #include #include #include #include #include +#include #include - #include "camera/interface.hpp" #include "network/mavlink.hpp" +#include "network/udp_client.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; + 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 + * Converts the 3-plane raw data to BGR cv::Mat, handling stride/padding */ - std::optional imgConvert(std::vector imgbuf); + std::optional imgConvert(const std::vector>& planes); /** - * Reads in the image data when taking a picture + * Reads the 3 planes (Y, U, V) from the camera */ - std::vector readImage(); + std::vector> readImage(); - /** - * Reads in the telemetry data when taking a picture - */ - std::optional readTelemetry(); - - std::vector readPacket(); - - public: + public: + explicit RPICamera(CameraConfig config, asio::io_context* io_context_); + ~RPICamera(); - explicit RPICamera(CameraConfig config, asio::io_context* io_context_); - - // TODO: destructor? - ~RPICamera(); - - void connect() override; - bool isConnected() override; + 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; + + 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 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 +#endif \ No newline at end of file diff --git a/include/network/rpi_connection.hpp b/include/network/rpi_connection.hpp index f1440807..0c113d89 100644 --- a/include/network/rpi_connection.hpp +++ b/include/network/rpi_connection.hpp @@ -3,16 +3,18 @@ #include -const uint32_t IMG_WIDTH = 2028; -const uint32_t IMG_HEIGHT = 1520; -const uint32_t BUFFER_SIZE = IMG_WIDTH * IMG_HEIGHT * 3 / 2; +// Image Config +const uint32_t IMG_WIDTH = 1456; +const uint32_t IMG_HEIGHT = 1088; +const uint32_t IMG_BUFFER = IMG_WIDTH * IMG_HEIGHT * 3 / 2; -// const std::string SERVER_IP = "192.168.68.1"; -// const int SERVER_PORT = 25565; +// Libcamera Strides/Padding +const uint32_t STRIDE_Y = 1472; +const uint32_t STRIDE_UV = 736; -// local testing only -const std::string SERVER_IP = "0.0.0.0"; -const int SERVER_PORT = 5000; +// 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; @@ -24,4 +26,4 @@ struct Header { uint32_t mem_size; }; -#endif +#endif \ No newline at end of file diff --git a/src/camera/rpi.cpp b/src/camera/rpi.cpp index bcb00066..05b3c37f 100644 --- a/src/camera/rpi.cpp +++ b/src/camera/rpi.cpp @@ -2,158 +2,158 @@ #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" +#include "network/rpi_connection.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; - +// Setup Logging +// ... -RPICamera::RPICamera(CameraConfig config, asio::io_context* io_context_) : CameraInterface(config), client(io_context_, SERVER_IP, SERVER_PORT) { +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; + if (this->connected) return; + + // Keep trying to connect/bind logic + // For UDP, "connect" just means opening the socket which is fast + if (client.connect()) { + this->connected = true; + // Optionally send START_REQUEST if needed, but 'I' usually works standalone (i think) + // client.send(START_REQUEST); } - - 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 -} +RPICamera::~RPICamera() {} 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; -} + + // 1. Send Request + if (!client.send(PICTURE_REQUEST)) { + LOG_F(ERROR, "Failed to send picture request"); + return {}; + } -void RPICamera::startTakingPictures(const std::chrono::milliseconds& timeout, std::shared_ptr mavlinkClient) { + // 2. Read 3 Planes + std::vector> planes = readImage(); -} + if (planes.size() != 3) { + LOG_F(ERROR, "Failed to read all 3 planes"); + return {}; + } -void RPICamera::stopTakingPictures() { + // 3. Convert to cv::Mat + std::optional mat = imgConvert(planes); -} + if (!mat.has_value()) { + return {}; + } -void RPICamera::ping() { + uint64_t timestamp = getUnixTime_s().count(); + return ImageData { + .DATA = mat.value(), + .TIMESTAMP = timestamp, + .TELEMETRY = {} + }; } -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(); +std::vector> RPICamera::readImage() { + std::vector> planes; + + // We expect exactly 3 planes: Y, U, V + for (int i = 0; i < 3; i++) { + Header header = client.recvHeader(); + + // Convert endianness + header.magic = ntohl(header.magic); + header.mem_size = ntohl(header.mem_size); + header.total_chunks = ntohl(header.total_chunks); + + if (header.magic != EXPECTED_MAGIC) { + LOG_F(ERROR, "Invalid Magic on plane %d: %x", i, header.magic); + return {}; + } + + std::vector planeData = client.recvBody(header.mem_size, header.total_chunks); + if (planeData.empty()) { + LOG_F(ERROR, "Failed to receive body for plane %d", i); + return {}; + } + + planes.push_back(std::move(planeData)); + } - return packet; - // return imgbuf; + return planes; } -std::optional RPICamera::readTelemetry() { +std::optional RPICamera::imgConvert(const std::vector>& planes) { + // We rely on constants from rpi_connection.hpp: + // IMG_WIDTH, IMG_HEIGHT, STRIDE_Y, STRIDE_UV - ImageTelemetry telemetry; - // asio::read() + if (planes.size() != 3) return {}; - return telemetry; -} + const std::vector& p0 = planes[0]; // Y + const std::vector& p1 = planes[1]; // U + const std::vector& p2 = planes[2]; // V -std::vector RPICamera::readPacket() { + // Create wrappers around the raw data with specific stride (Step) + // Note: cv::Mat constructor with data pointer does not copy data, so we must be careful with lifetime + // But we copy immediately below. + cv::Mat y_src(IMG_HEIGHT, IMG_WIDTH, CV_8UC1, (void*)p0.data(), STRIDE_Y); + cv::Mat u_src(IMG_HEIGHT/2, IMG_WIDTH/2, CV_8UC1, (void*)p1.data(), STRIDE_UV); + cv::Mat v_src(IMG_HEIGHT/2, IMG_WIDTH/2, CV_8UC1, (void*)p2.data(), STRIDE_UV); - std::vector packet; + // Allocate continuous buffer for standard I420 + cv::Mat yuv_continuous(IMG_HEIGHT + IMG_HEIGHT/2, IMG_WIDTH, CV_8UC1); - Header header = client.recvHeader(); + // Copy Y (Removes padding) + y_src.copyTo(yuv_continuous(cv::Rect(0, 0, IMG_WIDTH, IMG_HEIGHT))); - // TODO: ntohl or ntohs? - header.magic = ntohl(header.magic); - header.mem_size = ntohl(header.mem_size); - header.total_chunks = ntohl(header.total_chunks); + // Copy U and V (Removes padding) + // We need to flatten them carefully into the buffer after Y + size_t y_size = IMG_WIDTH * IMG_HEIGHT; + size_t uv_size = (IMG_WIDTH / 2) * (IMG_HEIGHT / 2); - LOG_F(INFO, "mem_size: %d, total_chunks: %d", header.mem_size, header.total_chunks); + uint8_t* u_dst = yuv_continuous.data + y_size; + uint8_t* v_dst = u_dst + uv_size; - // 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 + // Copy U + if (u_src.isContinuous()) { + memcpy(u_dst, u_src.data, uv_size); + } else { + for (int row = 0; row < u_src.rows; ++row) { + memcpy(u_dst + row * (IMG_WIDTH/2), u_src.ptr(row), IMG_WIDTH/2); + } } - packet = client.recvBody(header.mem_size, header.total_chunks); - - io_context_.run(); - - return packet; -} + // Copy V + if (v_src.isContinuous()) { + memcpy(v_dst, v_src.data, uv_size); + } else { + for (int row = 0; row < v_src.rows; ++row) { + memcpy(v_dst + row * (IMG_WIDTH/2), v_src.ptr(row), IMG_WIDTH/2); + } + } -void RPICamera::startStreaming() { + // Convert I420 to BGR + cv::Mat bgr_img; + cv::cvtColor(yuv_continuous, bgr_img, cv::COLOR_YUV2BGR_I420); + return bgr_img; } -bool RPICamera::isConnected() { - return true; -} +// Unused methods stubbed out +void RPICamera::startTakingPictures(const std::chrono::milliseconds& timeout, std::shared_ptr mavlinkClient) {} +void RPICamera::stopTakingPictures() {} +void RPICamera::startStreaming() {} +void RPICamera::ping() {} +bool RPICamera::isConnected() { return connected; } \ No newline at end of file diff --git a/src/camera/rpi.cpp~ b/src/camera/rpi.cpp~ deleted file mode 100644 index c17943cc..00000000 --- a/src/camera/rpi.cpp~ +++ /dev/null @@ -1,360 +0,0 @@ -#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 From 4140fd74c593aeffa44127483df5c6ecaed22602 Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Sun, 25 Jan 2026 01:56:09 -0800 Subject: [PATCH 31/43] bug: fixed cmake --- src/network/CMakeLists.txt | 2 -- tests/integration/CMakeLists.txt | 48 ++++++++++---------------------- 2 files changed, 14 insertions(+), 36 deletions(-) diff --git a/src/network/CMakeLists.txt b/src/network/CMakeLists.txt index 3153c33a..8e444eb8 100644 --- a/src/network/CMakeLists.txt +++ b/src/network/CMakeLists.txt @@ -5,8 +5,6 @@ set(COMMON_FILES gcs_routes.cpp gcs.cpp mavlink.cpp - tcp_client.cpp - tcp_server.cpp udp_client.cpp udp_server.cpp ) diff --git a/tests/integration/CMakeLists.txt b/tests/integration/CMakeLists.txt index 208903c0..b2bcab33 100644 --- a/tests/integration/CMakeLists.txt +++ b/tests/integration/CMakeLists.txt @@ -142,23 +142,7 @@ target_add_loguru(airdrop_packets) # -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 @@ -178,23 +162,7 @@ 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 @@ -235,3 +203,15 @@ target_link_libraries(clustering PRIVATE obcpp_lib obcpp_camera) target_add_matplot(clustering) target_add_json(clustering) target_add_protobuf(clustering) + +add_executable(camera_integration_test "camera_integration_test.cpp") +target_link_libraries(camera_integration_test PRIVATE obcpp_lib) +target_include_directories(camera_integration_test PRIVATE ${INCLUDE_DIRECTORY}) +target_add_loguru(camera_integration_test) +target_add_opencv(camera_integration_test) +target_add_boost(camera_integration_test) +target_add_mavsdk(camera_integration_test) +target_add_matplot(camera_integration_test) +target_add_json(camera_integration_test) +target_include_directories(camera_integration_test PRIVATE ${ImageMagick_INCLUDE_DIRS}) +target_link_libraries(camera_integration_test PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) From ee20f3ddbd36578428912d25818beba72757b81f Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Sun, 25 Jan 2026 01:56:40 -0800 Subject: [PATCH 32/43] feat: matched client to specifications. Socket still times out sometimes --- src/network/udp_client.cpp | 90 ++++++++++--------- tests/integration/camera_integration_test.cpp | 54 +++++++++++ 2 files changed, 100 insertions(+), 44 deletions(-) create mode 100644 tests/integration/camera_integration_test.cpp diff --git a/src/network/udp_client.cpp b/src/network/udp_client.cpp index 8f5dd291..9056adcd 100644 --- a/src/network/udp_client.cpp +++ b/src/network/udp_client.cpp @@ -1,4 +1,5 @@ #include "network/udp_client.hpp" +#include UDPClient::UDPClient(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_) { this->ip = ip; @@ -7,9 +8,8 @@ UDPClient::UDPClient(asio::io_context* io_context_, std::string ip, int 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() + + // Open the UDP socket this->socket_.open(asio::ip::udp::v4(), ec); if (ec) { @@ -17,8 +17,13 @@ bool UDPClient::connect() { return false; } - LOG_F(INFO, "Connected to %s on port %d", this->ip.c_str(), this->port); + // Set Receive Timeout + struct timeval read_timeout; + read_timeout.tv_sec = 2; + read_timeout.tv_usec = 0; + setsockopt(this->socket_.native_handle(), SOL_SOCKET, SO_RCVTIMEO, &read_timeout, sizeof(read_timeout)); + LOG_F(INFO, "Connected to %s on port %d", this->ip.c_str(), this->port); return true; } @@ -36,17 +41,13 @@ bool UDPClient::send(std::uint8_t request) { 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); @@ -55,57 +56,58 @@ Header UDPClient::recvHeader() { LOG_F(ERROR, "Failed to read header: %s", ec.message().c_str()); return {}; } - - LOG_F(INFO, "Bytes read (header): %d", bytesRead); - + + // Network to Host Byte Order conversion handled by caller or here? + // rpi.cpp expects to handle it, but todo 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); - + const int bufSize = mem_size; std::vector buf(bufSize); + std::vector received_chunks(total_chunks, false); + + int chunks_received_count = 0; + + // Use raw buffer? hopefully no mem leaks + char chunk_buf[CHUNK_SIZE + sizeof(uint32_t)]; + + // Loop until all chunks received + // Note: we prolly want to add a max retry count or timeout check inside the loop + // But the socket timeout will break the loop eventually if data stops coming + while (chunks_received_count < total_chunks) { + + size_t bytesRead = this->socket_.receive_from(asio::buffer(chunk_buf), sender_endpoint, 0, ec); - int totalChunks = 0; - - for (int i = 0; i < total_chunks; i++) { - char chunk_buf[CHUNK_SIZE + sizeof(uint32_t)]; + if (ec) { + LOG_F(ERROR, "Receive chunk failed: %s", ec.message().c_str()); + // Return what we have or empty? Returning empty signals failure. + return {}; + } - int bytesRead = this->socket_.receive_from(asio::buffer(chunk_buf), sender_endpoint, 0, ec); + if (bytesRead < sizeof(uint32_t)) continue; - int chunk_idx = ntohl(*reinterpret_cast(chunk_buf)); - int data_size = bytesRead - sizeof(uint32_t); - int offset = chunk_idx * CHUNK_SIZE; + uint32_t chunk_idx = ntohl(*reinterpret_cast(chunk_buf)); + size_t data_size = bytesRead - sizeof(uint32_t); + size_t offset = chunk_idx * CHUNK_SIZE; + + if (offset + data_size > (size_t)bufSize) { + LOG_F(ERROR, "Chunk exceeds buffer bounds"); + continue; + } // 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 (!received_chunks[chunk_idx]) { + received_chunks[chunk_idx] = true; + chunks_received_count++; + } } - 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()); - + LOG_F(INFO, "Successfully reconstructed plane: %lu bytes", buf.size()); return buf; -} +} \ No newline at end of file diff --git a/tests/integration/camera_integration_test.cpp b/tests/integration/camera_integration_test.cpp new file mode 100644 index 00000000..071ebbcc --- /dev/null +++ b/tests/integration/camera_integration_test.cpp @@ -0,0 +1,54 @@ +#include +#include +#include +#include +#include +#include "camera/rpi.hpp" +#include "utilities/obc_config.hpp" +#include "core/mission_state.hpp" +#include "utilities/common.hpp" + +namespace fs = std::filesystem; + +int main(int argc, char* argv[]) { + loguru::init(argc, argv); + + CameraConfig config; + asio::io_context io_context_; + + std::cout << "[Test] Initializing RPICamera..." << std::endl; + RPICamera camera(config, &io_context_); + + std::cout << "[Test] Connecting..." << std::endl; + camera.connect(); + + if (!camera.isConnected()) { + std::cerr << "[Test] Failed to connect/bind socket" << std::endl; + return -1; + } + + std::cout << "[Test] Requesting Picture..." << std::endl; + std::optional img = camera.takePicture(std::chrono::milliseconds(3000), nullptr); + + if (img.has_value()) { + std::cout << "[Test] Image Received! Size: " << img.value().DATA.total() << " bytes." << std::endl; + + fs::path base_dir = "images"; + if (!fs::exists(base_dir)) { + fs::create_directory(base_dir); + } + + fs::path filepath = base_dir / (std::to_string(img.value().TIMESTAMP) + ".png"); + + if (img.value().saveToFile(base_dir.string())) { + std::cout << "[Test] Saved successfully to " << base_dir << std::endl; + } else { + std::cerr << "[Test] Failed to save image file." << std::endl; + } + } else { + std::cerr << "[Test] FAILED: No image received (Timeout or Error)." << std::endl; + return 1; + } + + return 0; +} \ No newline at end of file From f4cdabbdcd2d6d62a89b024adbd01744085d62c0 Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Sun, 25 Jan 2026 01:56:50 -0800 Subject: [PATCH 33/43] bug: compilation fixes --- src/network/gcs_routes.cpp | 6 +++--- src/network/udp_server.cpp | 6 ++++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index e5de8d40..d1fd7059 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -266,7 +266,7 @@ DEF_GCS_HANDLE(Get, camera, capture) { if (!compressed_image.empty()) { LOG_F(INFO, - "Compressed manual capture image from %d bytes to %d bytes (%.1f%% compression)", + "Compressed manual capture image from %zu bytes to %zu bytes (%.1f%% compression)", image->DATA.total() * image->DATA.elemSize(), compressed_data.size(), (1.0 - static_cast(compressed_data.size()) / (image->DATA.total() * image->DATA.elemSize())) * @@ -393,7 +393,7 @@ DEF_GCS_HANDLE(Get, targets, all) { compressed_image = cv::imdecode(compressed_data, cv::IMREAD_COLOR); if (!compressed_image.empty()) { - LOG_F(INFO, "Compressed image from %d bytes to %d bytes (%.1f%% compression)", + LOG_F(INFO, "Compressed image from %zu bytes to %zu bytes (%.1f%% compression)", run.annotatedImage.total() * run.annotatedImage.elemSize(), compressed_data.size(), (1.0 - static_cast(compressed_data.size()) / @@ -417,7 +417,7 @@ DEF_GCS_HANDLE(Get, targets, all) { // logic) if (run.coords.size() != run.bboxes.size()) { LOG_F(ERROR, - "Mismatch between coordinates (%ld) and bboxes (%ld) count in run_id %d. " + "Mismatch between coordinates (%zu) and bboxes (%zu) count in run_id %d. " "Skipping this run.", run.coords.size(), run.bboxes.size(), run.run_id); continue; // Skip this problematic run diff --git a/src/network/udp_server.cpp b/src/network/udp_server.cpp index 97a7943d..8017e10a 100644 --- a/src/network/udp_server.cpp +++ b/src/network/udp_server.cpp @@ -1,6 +1,8 @@ #include #include "network/udp_server.hpp" +// TODO: Didn't touch this for the most part since its for mocking. + cv::Mat UDPServer::createBGR() { std::cout << std::filesystem::current_path() << '\n'; cv::Mat image = cv::imread("/workspaces/obcpp/tests/integration/images/blurb.jpeg"); @@ -74,8 +76,8 @@ void UDPServer::send(asio::ip::udp::endpoint & endpoint) { 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'; + if (imgBuffer.size() != IMG_BUFFER) { + std::cout << "size: " << imgBuffer.size() << " expected: " << IMG_BUFFER << '\n'; return; } From a11407a7e93ca8fed59230c6565ab7ff66e495d1 Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Sun, 25 Jan 2026 12:41:16 -0800 Subject: [PATCH 34/43] feat: added camera count and interval --- tests/integration/camera_integration_test.cpp | 45 ++++++++++--------- 1 file changed, 25 insertions(+), 20 deletions(-) diff --git a/tests/integration/camera_integration_test.cpp b/tests/integration/camera_integration_test.cpp index 071ebbcc..4916d7d7 100644 --- a/tests/integration/camera_integration_test.cpp +++ b/tests/integration/camera_integration_test.cpp @@ -15,6 +15,8 @@ int main(int argc, char* argv[]) { CameraConfig config; asio::io_context io_context_; + const int NUM_IMAGES = 5; + const std::chrono::milliseconds IMAGE_INTERVAL = std::chrono::milliseconds(250); std::cout << "[Test] Initializing RPICamera..." << std::endl; RPICamera camera(config, &io_context_); @@ -27,28 +29,31 @@ int main(int argc, char* argv[]) { return -1; } - std::cout << "[Test] Requesting Picture..." << std::endl; - std::optional img = camera.takePicture(std::chrono::milliseconds(3000), nullptr); - - if (img.has_value()) { - std::cout << "[Test] Image Received! Size: " << img.value().DATA.total() << " bytes." << std::endl; - - fs::path base_dir = "images"; - if (!fs::exists(base_dir)) { - fs::create_directory(base_dir); - } - - fs::path filepath = base_dir / (std::to_string(img.value().TIMESTAMP) + ".png"); - - if (img.value().saveToFile(base_dir.string())) { - std::cout << "[Test] Saved successfully to " << base_dir << std::endl; + for (int i = 0; i < NUM_IMAGES; i++) { + std::cout << "[Test] Requesting Picture " << i + 1 << "/" << NUM_IMAGES << std::endl; + std::this_thread::sleep_for(IMAGE_INTERVAL); + std::optional img = camera.takePicture(std::chrono::milliseconds(3000), nullptr); + + if (img.has_value()) { + std::cout << "[Test] Image Received! Size: " << img.value().DATA.total() << " bytes." << std::endl; + + fs::path base_dir = "images"; + if (!fs::exists(base_dir)) { + fs::create_directory(base_dir); + } + + fs::path filepath = base_dir / (std::to_string(img.value().TIMESTAMP) + ".png"); + + if (img.value().saveToFile(base_dir.string())) { + std::cout << "[Test] Saved successfully to " << base_dir << std::endl; + } else { + std::cerr << "[Test] Failed to save image file." << std::endl; + } } else { - std::cerr << "[Test] Failed to save image file." << std::endl; + std::cerr << "[Test] FAILED: No image received (Timeout or Error)." << std::endl; + return 1; } - } else { - std::cerr << "[Test] FAILED: No image received (Timeout or Error)." << std::endl; - return 1; } return 0; -} \ No newline at end of file +} From abbb74a2d296e1e4f2c22d378253a3a68ea981e0 Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Wed, 4 Feb 2026 19:09:29 -0800 Subject: [PATCH 35/43] fix: filename collision --- src/camera/interface.cpp | 4 +++- src/camera/rpi.cpp | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/camera/interface.cpp b/src/camera/interface.cpp index 6fff55cf..85fdefa2 100644 --- a/src/camera/interface.cpp +++ b/src/camera/interface.cpp @@ -72,7 +72,9 @@ bool ImageData::saveToFile(std::string directory) const { std::filesystem::path json_filepath = save_dir / (std::to_string(this->TIMESTAMP) + std::string(".json")); - LOG_F(INFO, "Saving %s to %s", img_filepath, json_filepath); + LOG_F(INFO, "Saving image to %s", img_filepath.string().c_str()); + LOG_F(INFO, "Saving telemetry to %s", json_filepath.string().c_str()); + saveImageToFile(this->DATA, img_filepath); if (this->TELEMETRY.has_value()) { saveImageTelemetryToFile(this->TELEMETRY.value(), json_filepath); diff --git a/src/camera/rpi.cpp b/src/camera/rpi.cpp index 05b3c37f..7310f3fd 100644 --- a/src/camera/rpi.cpp +++ b/src/camera/rpi.cpp @@ -57,7 +57,9 @@ std::optional RPICamera::takePicture(const std::chrono::milliseconds& return {}; } - uint64_t timestamp = getUnixTime_s().count(); + uint64_t timestamp = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch() + ).count(); return ImageData { .DATA = mat.value(), From cf16f78a6464bb9f2f68c5386dba3277922a8dcb Mon Sep 17 00:00:00 2001 From: David Soto Date: Fri, 20 Feb 2026 01:25:12 +0000 Subject: [PATCH 36/43] kaz codes looky looky here --- include/core/mission_state.hpp | 4 ++++ include/network/rpi_connection.hpp | 8 +++++--- protos | 2 +- src/camera/rpi.cpp | 3 ++- src/core/CMakeLists.txt | 2 ++ src/core/mission_parameters.cpp | 8 ++++---- src/core/obc.cpp | 6 ++++++ 7 files changed, 24 insertions(+), 9 deletions(-) diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index f97de9b8..a3a0f5a1 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -114,6 +114,8 @@ class MissionState { std::optional next_airdrop_to_drop; + // asio::io_context io_testies; + private: std::mutex converter_mut; std::optional> converter; @@ -143,6 +145,8 @@ class MissionState { // with the detected_target specified by the index std::array cv_matches; + // boost::asio::io_context io_testies; + bool mappingIsDone; void _setTick(Tick* newTick); // does not acquire the tick_mut diff --git a/include/network/rpi_connection.hpp b/include/network/rpi_connection.hpp index 0c113d89..0603ba72 100644 --- a/include/network/rpi_connection.hpp +++ b/include/network/rpi_connection.hpp @@ -2,11 +2,13 @@ #define INCLUDE_NETWORK_RPI_CONNECTION_HPP_ #include +#pragma once +#include // Image Config -const uint32_t IMG_WIDTH = 1456; -const uint32_t IMG_HEIGHT = 1088; -const uint32_t IMG_BUFFER = IMG_WIDTH * IMG_HEIGHT * 3 / 2; +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; diff --git a/protos b/protos index 145b7ff1..0789219c 160000 --- a/protos +++ b/protos @@ -1 +1 @@ -Subproject commit 145b7ff1162351c09ca49b3fb4844023bbe3ffbb +Subproject commit 0789219cac47a9396b045d24f7152eabdd741dff diff --git a/src/camera/rpi.cpp b/src/camera/rpi.cpp index 7310f3fd..8b8e64a6 100644 --- a/src/camera/rpi.cpp +++ b/src/camera/rpi.cpp @@ -17,7 +17,8 @@ RPICamera::RPICamera(CameraConfig config, asio::io_context* io_context_) : CameraInterface(config), client(io_context_, SERVER_IP, SERVER_PORT) { - this->connected = false; + //this->connected = false; + LOG_F(INFO, "RPICamera exists"); } void RPICamera::connect() { diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index de73b73c..7b6a93e1 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -35,6 +35,8 @@ target_add_mavsdk(${LIB_NAME}) target_add_matplot(${LIB_NAME}) target_add_loguru(${LIB_NAME}) target_add_onnxruntime(${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}) diff --git a/src/core/mission_parameters.cpp b/src/core/mission_parameters.cpp index 2ed3f6f6..b0623303 100644 --- a/src/core/mission_parameters.cpp +++ b/src/core/mission_parameters.cpp @@ -95,9 +95,9 @@ std::optional MissionParameters::setMission( err += "Airdrop boundary must have at least 3 coordinates."; } - if (mission.mappingboundary().size() < 3) { - err += "Mapping boundary must have at least 3 coordinates."; - } + // if (mission.mappingboundary().size() < 3) { + // err += "Mapping boundary must have at least 3 coordinates."; + // } if (!err.empty()) { return err; @@ -106,7 +106,7 @@ std::optional MissionParameters::setMission( this->cached_mission = mission; this->flightBoundary = cconverter.toXYZ(mission.flightboundary()); this->airdropBoundary = cconverter.toXYZ(mission.airdropboundary()); - this->mappingBoundary = cconverter.toXYZ(mission.mappingboundary()); + // this->mappingBoundary = cconverter.toXYZ(mission.mappingboundary()); this->waypoints = cconverter.toXYZ(mission.waypoints()); for (const auto& airdrop : mission.airdropassignments()) { // Use const& for efficiency this->_setAirdrop(airdrop); diff --git a/src/core/obc.cpp b/src/core/obc.cpp index ada75b00..9fce2ed2 100644 --- a/src/core/obc.cpp +++ b/src/core/obc.cpp @@ -4,6 +4,7 @@ #include #include +#include "camera/rpi.hpp" #include "camera/mock.hpp" #include "camera/picamera.hpp" #include "core/obc.hpp" @@ -15,6 +16,8 @@ #include "network/airdrop_client.hpp" #include "utilities/logging.hpp" #include "utilities/obc_config.hpp" +#include +#include "network/rpi_connection.hpp" extern "C" { #include "network/airdrop_sockets.h" } @@ -47,6 +50,9 @@ OBC::OBC(OBCConfig config) { this->state->config.camera, 4956, 3040, 30)); } else if (this->state->config.camera.type == "lucid") { LOG_F(FATAL, "ATTEMPTING TO CONNECT TO LUCID CAMERA: LUCID CAMERA NO LONGER EXISTS"); + } else if(this->state->config.camera.type == "PiCamera"){ + asio::io_context io_testies; + this->state->setCamera(std::make_shared( config.camera, &io_testies)); } else { // default to mock if it's neither "mock" or "lucid" this->state->setCamera(std::make_shared(this->state->config.camera)); From 0feb640d88b37bef83ee498140ce5dbffb34ee3d Mon Sep 17 00:00:00 2001 From: David Soto Date: Fri, 20 Feb 2026 01:29:32 +0000 Subject: [PATCH 37/43] kaz looky looky part 2 --- configs/dev.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configs/dev.json b/configs/dev.json index c0b59d1f..ce50f431 100644 --- a/configs/dev.json +++ b/configs/dev.json @@ -57,7 +57,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": { From 5e69301af4d38c81a471f1495ed20e0c5d779818 Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Sat, 21 Feb 2026 23:37:22 -0800 Subject: [PATCH 38/43] feat: added mav --- src/camera/rpi.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/camera/rpi.cpp b/src/camera/rpi.cpp index 8b8e64a6..28dd55f3 100644 --- a/src/camera/rpi.cpp +++ b/src/camera/rpi.cpp @@ -37,6 +37,15 @@ RPICamera::~RPICamera() {} std::optional RPICamera::takePicture(const std::chrono::milliseconds& timeout, std::shared_ptr mavlinkClient) { + std::optional telemetryOpt = queryMavlinkImageTelemetry(mavlinkClient); + + if (!telemetryOpt.has_value()) { + LOG_F(ERROR, "Could not grab telemetry data from mavlink"); + return std::nullopt; + } + + ImageTelemetry telemetry = telemetryOpt.value(); + // 1. Send Request if (!client.send(PICTURE_REQUEST)) { LOG_F(ERROR, "Failed to send picture request"); @@ -65,7 +74,7 @@ std::optional RPICamera::takePicture(const std::chrono::milliseconds& return ImageData { .DATA = mat.value(), .TIMESTAMP = timestamp, - .TELEMETRY = {} + .TELEMETRY = telemetry }; } From 38491b322103eb6e1ca608370d706a6c8b0a6441 Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Mon, 23 Feb 2026 22:18:58 -0800 Subject: [PATCH 39/43] feat: added cmake changes --- src/network/gcs_routes.cpp | 2 +- src/network/udp_client.cpp | 2 +- src/pathing/CMakeLists.txt | 1 + src/ticks/CMakeLists.txt | 1 + 4 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index 2c59d847..2bfbc63b 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -273,7 +273,7 @@ DEF_GCS_HANDLE(Get, camera, capture) { std::string output; google::protobuf::util::MessageToJsonString(manual_image, &output); - LOG_RESPONSE(INFO, "Successfully captured image", OK, output.c_str(), mime::json); + LOG_RESPONSE_HD(INFO, "Successfully captured image", OK, output.c_str(), mime::json); } DEF_GCS_HANDLE(Post, dodropnow) { diff --git a/src/network/udp_client.cpp b/src/network/udp_client.cpp index 9056adcd..dee0c0fb 100644 --- a/src/network/udp_client.cpp +++ b/src/network/udp_client.cpp @@ -37,7 +37,7 @@ bool UDPClient::send(std::uint8_t 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()); + LOG_F(WARNING, "Failed to send request: %s", ec.message().c_str()); return false; } diff --git a/src/pathing/CMakeLists.txt b/src/pathing/CMakeLists.txt index 97bf7d8e..a2d80be2 100644 --- a/src/pathing/CMakeLists.txt +++ b/src/pathing/CMakeLists.txt @@ -24,6 +24,7 @@ target_link_libraries(${LIB_NAME} PRIVATE ${LIB_DEPS}) set_unity_for_target(${LIB_NAME}) +target_add_boost(${LIB_NAME}) target_add_protobuf(${LIB_NAME}) target_add_torch(${LIB_NAME}) target_add_torchvision(${LIB_NAME}) diff --git a/src/ticks/CMakeLists.txt b/src/ticks/CMakeLists.txt index 616c5804..23bc2605 100644 --- a/src/ticks/CMakeLists.txt +++ b/src/ticks/CMakeLists.txt @@ -37,6 +37,7 @@ target_link_libraries(${LIB_NAME} PRIVATE set_unity_for_target(${LIB_NAME}) +target_add_boost(${LIB_NAME}) target_add_protobuf(${LIB_NAME}) target_add_torch(${LIB_NAME}) target_add_torchvision(${LIB_NAME}) From 080d5abc1dc9fe7bf8b023b5d9fa3e1555891d0b Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Mon, 23 Feb 2026 22:19:14 -0800 Subject: [PATCH 40/43] fix: add io_context to state --- include/core/mission_state.hpp | 4 ++-- src/camera/interface.cpp | 5 +++++ src/core/obc.cpp | 6 +++--- 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index a3a0f5a1..d728e557 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include "camera/interface.hpp" #include "core/mission_parameters.hpp" @@ -113,8 +114,7 @@ class MissionState { OBCConfig config; std::optional next_airdrop_to_drop; - - // asio::io_context io_testies; + boost::asio::io_context raspy_io; private: std::mutex converter_mut; diff --git a/src/camera/interface.cpp b/src/camera/interface.cpp index 85fdefa2..caad3f65 100644 --- a/src/camera/interface.cpp +++ b/src/camera/interface.cpp @@ -65,6 +65,11 @@ std::optional queryMavlinkImageTelemetry( } bool ImageData::saveToFile(std::string directory) const { + if (this->TIMESTAMP == 0) { + LOG_F(ERROR, "Tried to save empty image"); + return false; + } + try { std::filesystem::path save_dir = directory; std::filesystem::path img_filepath = diff --git a/src/core/obc.cpp b/src/core/obc.cpp index 9fce2ed2..88b75158 100644 --- a/src/core/obc.cpp +++ b/src/core/obc.cpp @@ -16,7 +16,6 @@ #include "network/airdrop_client.hpp" #include "utilities/logging.hpp" #include "utilities/obc_config.hpp" -#include #include "network/rpi_connection.hpp" extern "C" { #include "network/airdrop_sockets.h" @@ -51,8 +50,9 @@ OBC::OBC(OBCConfig config) { } else if (this->state->config.camera.type == "lucid") { LOG_F(FATAL, "ATTEMPTING TO CONNECT TO LUCID CAMERA: LUCID CAMERA NO LONGER EXISTS"); } else if(this->state->config.camera.type == "PiCamera"){ - asio::io_context io_testies; - this->state->setCamera(std::make_shared( config.camera, &io_testies)); + // asio::io_context io_testies; + this->state->setCamera(std::make_shared(config.camera, &(this->state->raspy_io))); + LOG_F(INFO, "Starting Pi Camera Client"); } else { // default to mock if it's neither "mock" or "lucid" this->state->setCamera(std::make_shared(this->state->config.camera)); From b7a3a1bffefbe526cd1f388203edefa692bf138d Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Thu, 26 Feb 2026 15:26:19 -0800 Subject: [PATCH 41/43] fix: cmake linking --- deps/boost/boost.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/deps/boost/boost.cmake b/deps/boost/boost.cmake index 556ea5cb..ef2a4089 100644 --- a/deps/boost/boost.cmake +++ b/deps/boost/boost.cmake @@ -10,7 +10,7 @@ function(target_add_boost target_name) # target_link_libraries(${target_name} PRIVATE # boost # ) - target_include_directories(${target_name} PRIVATE ${boost_SOURCE_DIR}) + target_include_directories(${target_name} PUBLIC ${boost_SOURCE_DIR}) endfunction() From 3d7042c0cbe6e1835fbdcfbdcd5cd8da67b733e9 Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Thu, 26 Feb 2026 16:24:05 -0800 Subject: [PATCH 42/43] feat: implemented timeout --- include/network/udp_client.hpp | 6 +++++ src/camera/rpi.cpp | 19 +++++++++++----- src/network/udp_client.cpp | 40 ++++++++++++++++++++++++++++++++++ 3 files changed, 60 insertions(+), 5 deletions(-) diff --git a/include/network/udp_client.hpp b/include/network/udp_client.hpp index 246d218c..37543c0d 100644 --- a/include/network/udp_client.hpp +++ b/include/network/udp_client.hpp @@ -14,12 +14,18 @@ class UDPClient { asio::ip::udp::socket socket_; std::string ip; int port; + int current_timeout_ms_ = 2000; + + bool waitForData(); 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(); + + // Set Timeout for Socket + void setReceiveTimeout(int timeout_ms); bool send(std::uint8_t request); diff --git a/src/camera/rpi.cpp b/src/camera/rpi.cpp index 28dd55f3..6b7245b6 100644 --- a/src/camera/rpi.cpp +++ b/src/camera/rpi.cpp @@ -40,11 +40,13 @@ std::optional RPICamera::takePicture(const std::chrono::milliseconds& std::optional telemetryOpt = queryMavlinkImageTelemetry(mavlinkClient); if (!telemetryOpt.has_value()) { - LOG_F(ERROR, "Could not grab telemetry data from mavlink"); - return std::nullopt; + LOG_F(WARNING, "Could not grab telemetry data from mavlink"); } - ImageTelemetry telemetry = telemetryOpt.value(); + // Set timeout dynamically + client.setReceiveTimeout(timeout.count()); + + auto start_time = std::chrono::steady_clock::now(); // 1. Send Request if (!client.send(PICTURE_REQUEST)) { @@ -55,8 +57,15 @@ std::optional RPICamera::takePicture(const std::chrono::milliseconds& // 2. Read 3 Planes std::vector> planes = readImage(); + auto end_time = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(end_time - start_time); + if (planes.size() != 3) { - LOG_F(ERROR, "Failed to read all 3 planes"); + if (elapsed >= timeout - std::chrono::milliseconds(50)) { + LOG_F(ERROR, "Camera request timed out after %ld ms", timeout.count()); + } else { + LOG_F(ERROR, "Failed to read all 3 planes"); + } return {}; } @@ -74,7 +83,7 @@ std::optional RPICamera::takePicture(const std::chrono::milliseconds& return ImageData { .DATA = mat.value(), .TIMESTAMP = timestamp, - .TELEMETRY = telemetry + .TELEMETRY = telemetryOpt }; } diff --git a/src/network/udp_client.cpp b/src/network/udp_client.cpp index dee0c0fb..be1bafec 100644 --- a/src/network/udp_client.cpp +++ b/src/network/udp_client.cpp @@ -1,5 +1,6 @@ #include "network/udp_client.hpp" #include +#include UDPClient::UDPClient(asio::io_context* io_context_, std::string ip, int port) : socket_(*io_context_) { this->ip = ip; @@ -27,6 +28,35 @@ bool UDPClient::connect() { return true; } +void UDPClient::setReceiveTimeout(int timeout_ms) { + this->current_timeout_ms_ = timeout_ms; + struct timeval read_timeout; + read_timeout.tv_sec = timeout_ms / 1000; + read_timeout.tv_usec = (timeout_ms % 1000) * 1000; + setsockopt(this->socket_.native_handle(), SOL_SOCKET, SO_RCVTIMEO, &read_timeout, sizeof(read_timeout)); +} + +bool UDPClient::waitForData() { + fd_set fds; + FD_ZERO(&fds); + FD_SET(this->socket_.native_handle(), &fds); + + struct timeval tv; + tv.tv_sec = this->current_timeout_ms_ / 1000; + tv.tv_usec = (this->current_timeout_ms_ % 1000) * 1000; + + int rv = select(this->socket_.native_handle() + 1, &fds, NULL, NULL, &tv); + + if (rv == -1) { + LOG_F(ERROR, "Select failed in waitForData."); + return false; + } else if (rv == 0) { + // Timeout + return false; + } + return true; +} + // send a request to the server bool UDPClient::send(std::uint8_t request) { boost::system::error_code ec; @@ -50,6 +80,11 @@ Header UDPClient::recvHeader() { asio::ip::udp::endpoint sender_endpoint; Header header; + if (!this->waitForData()) { + LOG_F(WARNING, "Timeout waiting for header data."); + return {}; + } + int bytesRead = this->socket_.receive_from(asio::buffer(&header, sizeof(Header)), sender_endpoint, 0, ec); if (ec) { @@ -80,6 +115,11 @@ std::vector UDPClient::recvBody(const int mem_size, const int tota // But the socket timeout will break the loop eventually if data stops coming while (chunks_received_count < total_chunks) { + if (!this->waitForData()) { + LOG_F(WARNING, "Timeout waiting for body chunks."); + return {}; + } + size_t bytesRead = this->socket_.receive_from(asio::buffer(chunk_buf), sender_endpoint, 0, ec); if (ec) { From 88e2bf50d5359c2ecfc94bb919a7ff3d0304b48a Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Thu, 26 Feb 2026 16:26:36 -0800 Subject: [PATCH 43/43] chore: changed logging --- tests/integration/camera_integration_test.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/tests/integration/camera_integration_test.cpp b/tests/integration/camera_integration_test.cpp index 4916d7d7..78ca5d5e 100644 --- a/tests/integration/camera_integration_test.cpp +++ b/tests/integration/camera_integration_test.cpp @@ -18,24 +18,24 @@ int main(int argc, char* argv[]) { const int NUM_IMAGES = 5; const std::chrono::milliseconds IMAGE_INTERVAL = std::chrono::milliseconds(250); - std::cout << "[Test] Initializing RPICamera..." << std::endl; + LOG_F(INFO, "[Test] Initializing RPICamera..."); RPICamera camera(config, &io_context_); - std::cout << "[Test] Connecting..." << std::endl; + LOG_F(INFO, "[Test] Connecting..."); camera.connect(); if (!camera.isConnected()) { - std::cerr << "[Test] Failed to connect/bind socket" << std::endl; + LOG_F(ERROR, "[Test] Failed to connect/bind socket"); return -1; } for (int i = 0; i < NUM_IMAGES; i++) { - std::cout << "[Test] Requesting Picture " << i + 1 << "/" << NUM_IMAGES << std::endl; + LOG_F(INFO, "[Test] Requesting Picture %d/%d", i + 1, NUM_IMAGES); std::this_thread::sleep_for(IMAGE_INTERVAL); std::optional img = camera.takePicture(std::chrono::milliseconds(3000), nullptr); if (img.has_value()) { - std::cout << "[Test] Image Received! Size: " << img.value().DATA.total() << " bytes." << std::endl; + LOG_F(INFO, "[Test] Image Received! Size: %ld bytes.", img.value().DATA.total()); fs::path base_dir = "images"; if (!fs::exists(base_dir)) { @@ -45,12 +45,12 @@ int main(int argc, char* argv[]) { fs::path filepath = base_dir / (std::to_string(img.value().TIMESTAMP) + ".png"); if (img.value().saveToFile(base_dir.string())) { - std::cout << "[Test] Saved successfully to " << base_dir << std::endl; + LOG_F(INFO, "[Test] Saved successfully to %s", base_dir.c_str()); } else { - std::cerr << "[Test] Failed to save image file." << std::endl; + LOG_F(ERROR, "[Test] Failed to save image file."); } } else { - std::cerr << "[Test] FAILED: No image received (Timeout or Error)." << std::endl; + LOG_F(ERROR, "[Test] FAILED: No image received (Timeout or Error)."); return 1; } }