From 77f70d0a608316542d5c0b6762487bcfb6bd130b Mon Sep 17 00:00:00 2001 From: tuas-travis-ci Date: Thu, 18 Apr 2024 18:15:09 -0700 Subject: [PATCH] WIP lucid on jetson --- .devcontainer/{ => jetson}/devcontainer.json | 4 +- .devcontainer/x86/devcontainer.json | 28 ++++++++ .gitmodules | 2 +- docker/Dockerfile.jetson | 19 ++++- include/camera/interface.hpp | 47 ++---------- include/camera/lucid.hpp | 39 +++++++++- include/camera/mock.hpp | 29 +++++--- include/pathing/cartesian.hpp | 17 ++--- src/camera/lucid.cpp | 30 ++++++++ src/camera/mock.cpp | 75 ++++++++++++++++---- tests/integration/CMakeLists.txt | 17 ++++- tests/integration/camera_lucid.cpp | 21 ++++++ tests/integration/camera_mock.cpp | 26 +++++++ tests/unit/camera/mock.cpp | 3 +- 14 files changed, 277 insertions(+), 80 deletions(-) rename .devcontainer/{ => jetson}/devcontainer.json (91%) create mode 100644 .devcontainer/x86/devcontainer.json create mode 100644 src/camera/lucid.cpp create mode 100644 tests/integration/camera_mock.cpp diff --git a/.devcontainer/devcontainer.json b/.devcontainer/jetson/devcontainer.json similarity index 91% rename from .devcontainer/devcontainer.json rename to .devcontainer/jetson/devcontainer.json index 2f83d5b7..48249209 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/jetson/devcontainer.json @@ -1,7 +1,7 @@ // See this page for reference of options: https://containers.dev/implementors/json_reference { - "name": "Existing Dockerfile", - "image": "ghcr.io/tritonuas/obcpp:x86", + "name": "Jetson", + "image": "tritonuas/obcpp:jetson", // 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/.devcontainer/x86/devcontainer.json b/.devcontainer/x86/devcontainer.json new file mode 100644 index 00000000..526a0701 --- /dev/null +++ b/.devcontainer/x86/devcontainer.json @@ -0,0 +1,28 @@ +// See this page for reference of options: https://containers.dev/implementors/json_reference +{ + "name": "x86", + "image": "ghcr.io/tritonuas/obcpp:main", + // 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` + // "runArgs": ["--device=/dev/ttyACM0"], + + "customizations": { + "vscode": { + "settings": { + // Use bash instead ofk sh + "terminal.integrated.defaultProfile.linux": "bash" + }, + "extensions": [ + "shd101wyy.markdown-preview-enhanced", + "ms-vscode.cpptools", + "nick-dimeglio.family-guy-funny-moments", + "twxs.cmake", + "me-dutour-mathieu.vscode-github-actions" + ] + } + } + + // Use 'postCreateCommand' to run commands after the container is created. + // "postCreateCommand": "" +} diff --git a/.gitmodules b/.gitmodules index faa17ada..6de8417a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ [submodule "protos"] path = protos - url = git@github.com:tritonuas/protos.git + url = https://github.com/tritonuas/protos.git \ No newline at end of file diff --git a/docker/Dockerfile.jetson b/docker/Dockerfile.jetson index 3ad234d9..fcd902dd 100644 --- a/docker/Dockerfile.jetson +++ b/docker/Dockerfile.jetson @@ -87,15 +87,30 @@ RUN update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-10 10 RUN update-alternatives --set gcc /usr/bin/gcc-10 RUN update-alternatives --set g++ /usr/bin/g++-10 +RUN pip3 install gdown +ENV PATH="${PATH}:${HOME}/.local/bin" + +ARG ARENA_INSTALL_DIR=/arena-tmp +ARG ARENA_TAR_PATH="${ARENA_INSTALL_DIR}ArenaSDK_Linux.tar.gz" +ARG ARENA_EXTRACTED_PATH="${ARENA_INSTALL_DIR}/ArenaSDK_Linux_ARM64" +WORKDIR ${ARENA_INSTALL_DIR} +RUN gdown 1VtBji-cWfetM5nXZwt55JuHPWPGahQOH -O ${ARENA_TAR_PATH} +RUN tar -xvzf ${ARENA_TAR_PATH} +WORKDIR ${ARENA_EXTRACTED_PATH} +RUN sh Arena_SDK_ARM64.conf + +RUN apt-get update && apt-get install ninja-build + WORKDIR /obcpp COPY . . +RUN git submodule update --init RUN rm -rf /obcpp/build WORKDIR /obcpp/build ENV CMAKE_PREFIX_PATH="/usr/local/lib/python3.8/dist-packages/torch/share/cmake/Torch;/usr/local/share/cmake/TorchVision" -RUN GITHUB_ACTIONS=true cmake -DCMAKE_PREFIX_PATH="/usr/local/lib/python3.8/dist-packages/torch/share/cmake/Torch;/usr/local/share/cmake/TorchVision" -DCMAKE_MODULE_PATH="/usr/local/share/cmake/TorchVision" -DCMAKE_BUILD_TYPE="Release" .. +RUN GITHUB_ACTIONS=true cmake -DCMAKE_PREFIX_PATH="/usr/local/lib/python3.8/dist-packages/torch/share/cmake/Torch;/usr/local/share/cmake/TorchVision" -DCMAKE_MODULE_PATH="/usr/local/share/cmake/TorchVision" -DCMAKE_BUILD_TYPE="Release" -DCMAKE_JOB_POOLS="j=2" .. -RUN make obcpp cuda_check load_torchvision_model VERBOSE=1 +RUN ninja obcpp cuda_check load_torchvision_model # login as non-root user # USER $USERNAME diff --git a/include/camera/interface.hpp b/include/camera/interface.hpp index 78428226..00e36128 100644 --- a/include/camera/interface.hpp +++ b/include/camera/interface.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -54,67 +55,33 @@ class ImageData { class CameraConfiguration { private: nlohmann::json configJson; - public: explicit CameraConfiguration(nlohmann::json config); - void updateConfig(nlohmann::json newSetting); - - // void updateConfigField(std::string key, T value); - nlohmann::json getConfig(); - - nlohmann::json getConfigField(); + // void updateConfigField(std::string key, T value); }; class CameraInterface { private: CameraConfiguration config; - std::unique_ptr recentPicture; // might need to move it to public - bool doneTakingPicture; // overengineering time - std::string uploadPath; - // Interpreter interp - // TODO: SERVER CONNECTION HERE ? - - void imgConvert(); public: explicit CameraInterface(CameraConfiguration config); - virtual ~CameraInterface() = default; virtual void connect() = 0; + virtual bool isConnected() = 0; - virtual bool verifyConnection() = 0; - - virtual void takePicture() = 0; - - virtual ImageData getLastPicture() = 0; - - virtual bool takePictureForSeconds(int sec) = 0; + virtual void startTakingPictures(std::chrono::seconds interval) = 0; + virtual void stopTakingPictures() = 0; - virtual void startTakingPictures(double intervalSec) = 0; - - virtual bool isDoneTakingPictures() = 0; + virtual std::optional getLatestImage() = 0; + virtual std::queue getAllImages() = 0; CameraConfiguration getConfig(); - void updateConfig(CameraConfiguration newConfig); - void updateConfig(nlohmann::json newJsonConfig); - - std::string getUploadPath(); - - void setUploadPath(std::string path); - - void uploadPicture(ImageData img); - - std::vector listPicturesFromUploadPath(); - - ImageData getImageByName(std::string name); - - // server connection methods here - // virtual methods for all possible camera actions }; #endif // INCLUDE_CAMERA_INTERFACE_HPP_ diff --git a/include/camera/lucid.hpp b/include/camera/lucid.hpp index 4caf0642..4faef10e 100644 --- a/include/camera/lucid.hpp +++ b/include/camera/lucid.hpp @@ -1,14 +1,47 @@ #ifndef INCLUDE_CAMERA_LUCID_HPP_ #define INCLUDE_CAMERA_LUCID_HPP_ -#ifdef ARENA_SDK_INSTALLED +// #ifdef ARENA_SDK_INSTALLED + +#include #include "camera/interface.hpp" class LucidCamera : public CameraInterface { - // override all the camera connection interface functions + public: + LucidCamera(); + ~LucidCamera(); + + void connect(); + bool isConnected(); + + void startTakingPictures(std::chrono::seconds interval) override; + void stopTakingPictures() override; + + std::optional getLatestImage() override; + std::queue getAllImages() override; + + private: + static std::shared_mutex arenaSystemLock; + static Arena::ISystem* pSystem; + + + std::atomic_bool isConnected; + + std::atomic_bool isTakingPictures; + + void captureEvery(std::chrono::seconds interval); + + std::queue imageQueue; + std::shared_mutex imageQueueLock; + + std::thread captureThread; + + ImageData takePicture(); + std::shared_mutex imageQueueMut; + }; -#endif // ARENA_SDK_INSTALLED +// #endif // ARENA_SDK_INSTALLED #endif // INCLUDE_CAMERA_LUCID_HPP_ diff --git a/include/camera/mock.hpp b/include/camera/mock.hpp index 0a66d76c..e653b600 100644 --- a/include/camera/mock.hpp +++ b/include/camera/mock.hpp @@ -1,24 +1,37 @@ #ifndef INCLUDE_CAMERA_MOCK_HPP_ #define INCLUDE_CAMERA_MOCK_HPP_ +#include #include +#include #include "camera/interface.hpp" class MockCamera : public CameraInterface { public: explicit MockCamera(CameraConfiguration config); - ~MockCamera() = default; + ~MockCamera(); + void connect() override; - bool verifyConnection() override; - void takePicture() override; - ImageData getLastPicture() override; - bool takePictureForSeconds(int sec) override; - void startTakingPictures(double intervalSec) override; - bool isDoneTakingPictures() override; + bool isConnected() override; + + void startTakingPictures(std::chrono::seconds interval) override; + void stopTakingPictures() override; + + std::optional getLatestImage() override; + std::queue getAllImages() override; private: - std::unique_ptr lastPicture; + std::atomic_bool isTakingPictures; + + void captureEvery(std::chrono::seconds interval); + + std::queue imageQueue; + std::shared_mutex imageQueueLock; + + std::thread captureThread; + + ImageData takePicture(); }; #endif // INCLUDE_CAMERA_MOCK_HPP_ diff --git a/include/pathing/cartesian.hpp b/include/pathing/cartesian.hpp index 86b3804f..2591d571 100644 --- a/include/pathing/cartesian.hpp +++ b/include/pathing/cartesian.hpp @@ -5,6 +5,7 @@ #include #include #include +#include "math.h" #include "protos/obc.pb.h" #include "utilities/datatypes.hpp" @@ -34,13 +35,13 @@ class CartesianConverter { this->center.set_longitude((min_lng + max_lng) / 2.0); this->latlng_0.set_altitude(this->center.altitude()); - this->latlng_0.set_latitude(this->center.latitude() * std::numbers::pi / 180.0); - this->latlng_0.set_longitude(this->center.longitude() * std::numbers::pi / 180.0); + this->latlng_0.set_latitude(this->center.latitude() * M_PI / 180.0); + this->latlng_0.set_longitude(this->center.longitude() * M_PI / 180.0); // Essentially what is localizing us to this part of the globe // "Standard Parallels" - this->phi_0 = (min_lat - 1) * std::numbers::pi / 180.0; - this->phi_1 = (max_lat + 1) * std::numbers::pi / 180.0; + this->phi_0 = (min_lat - 1) * M_PI / 180.0; + this->phi_1 = (max_lat + 1) * M_PI / 180.0; } GPSCoord toLatLng(XYZCoord coord) const { @@ -52,15 +53,15 @@ class CartesianConverter { std::pow(rho0 - coord.y, 2.0)) / EARTH_RADIUS_METERS; double theta = std::atan(coord.x / (rho0 - coord.y)); - double lat = std::asin((c - rho * rho * n * n) / 2.0 / n) * 180.0 / std::numbers::pi; - double lng = (this->latlng_0.longitude() + theta / n) * 180.0 / std::numbers::pi; + double lat = std::asin((c - rho * rho * n * n) / 2.0 / n) * 180.0 / M_PI; + double lng = (this->latlng_0.longitude() + theta / n) * 180.0 / M_PI; return makeGPSCoord(lat, lng, coord.z); } XYZCoord toXYZ(GPSCoord coord) const { - double lat = coord.latitude() * std::numbers::pi / 180.0; - double lng = coord.longitude() * std::numbers::pi / 180.0; + double lat = coord.latitude() * M_PI / 180.0; + double lng = coord.longitude() * M_PI / 180.0; double n = 1.0 / 2.0 * (std::sin(this->phi_0) + std::sin(this->phi_1)); double theta = n * (lng - this->latlng_0.longitude()); diff --git a/src/camera/lucid.cpp b/src/camera/lucid.cpp new file mode 100644 index 00000000..adff28bb --- /dev/null +++ b/src/camera/lucid.cpp @@ -0,0 +1,30 @@ +#include "camera/lucid.hpp" + +#include +#include +#include + +#include + +#include "utilities/locks.hpp" + +LucidCamera::LucidCamera() { + +} + +LucidCamera::~LucidCamera() { + +} + +void LucidCamera::connect() { + WriteLock lock(this->arenaSystemLock) + pSystem = Arena::OpenSystem(); +} + +bool LucidCamera::isConnected(); + +void LucidCamera::startTakingPictures(std::chrono::seconds interval) override; +void LucidCamera::stopTakingPictures() override; + +std::optional LucidCamera::getLatestImage() override; +std::queue LucidCamera::getAllImages() override; \ No newline at end of file diff --git a/src/camera/mock.cpp b/src/camera/mock.cpp index c1662a6b..770619eb 100644 --- a/src/camera/mock.cpp +++ b/src/camera/mock.cpp @@ -1,29 +1,76 @@ #include "camera/mock.hpp" +#include +#include +#include + +#include + +#include "utilities/locks.hpp" + + MockCamera::MockCamera(CameraConfiguration config) : CameraInterface(config) {} +MockCamera::~MockCamera() { + this->stopTakingPictures(); +} + void MockCamera::connect() { return; } -bool MockCamera::verifyConnection() { return true; } +bool MockCamera::isConnected() { return true; } -void MockCamera::takePicture() { - ImageData newImg("mock_image.jpg", "/real/path/mock_image.jpg", - cv::Mat(cv::Size(4000, 3000), CV_8UC3, cv::Scalar(255)), - ImageTelemetry(38.31568, 76.55006, 75, 20, 100, 5, 3)); +void MockCamera::startTakingPictures(std::chrono::seconds interval) { + this->isTakingPictures = true; + try { + this->captureThread = std::thread(&MockCamera::captureEvery, this, interval); + // this->captureThread.detach(); + // captureThread = std::thread([this, interval] { this->captureEvery(interval); }); + } catch (const std::exception& e) { + std::cerr << e.what() << std::endl; + } + return; +} - lastPicture = std::make_unique(newImg); +void MockCamera::stopTakingPictures() { + if (!this->isTakingPictures) { + return; + } + + this->captureThread.join(); + + return; } -ImageData MockCamera::getLastPicture() { return *lastPicture; } +std::optional MockCamera::getLatestImage() { + ReadLock lock(this->imageQueueLock); + ImageData lastImage = this->imageQueue.front(); + this->imageQueue.pop(); + return lastImage; +} -bool MockCamera::takePictureForSeconds(int sec) { - // TODO: - return true; +std::queue MockCamera::getAllImages() { + ReadLock lock(this->imageQueueLock); + std::queue outputQueue = this->imageQueue; + this->imageQueue = std::queue(); + return outputQueue; } -void MockCamera::startTakingPictures(double intervalSec) { - // TODO: - return; +void MockCamera::captureEvery(std::chrono::seconds interval) { + while (this->isTakingPictures) { + LOG_F(INFO, "Taking picture with mock camera\n"); + ImageData newImage = this->takePicture(); + + WriteLock lock(this->imageQueueLock); + this->imageQueue.push(newImage); + lock.unlock(); + + std::this_thread::sleep_for(interval); + } +} + +ImageData MockCamera::takePicture() { + return ImageData("mock_image.jpg", "/real/path/mock_image.jpg", + cv::Mat(cv::Size(4000, 3000), CV_8UC3, cv::Scalar(255)), + ImageTelemetry(38.31568, 76.55006, 75, 20, 100, 5, 3)); } -bool MockCamera::isDoneTakingPictures() { return false; } diff --git a/tests/integration/CMakeLists.txt b/tests/integration/CMakeLists.txt index 12b0cd67..e43faa06 100644 --- a/tests/integration/CMakeLists.txt +++ b/tests/integration/CMakeLists.txt @@ -204,4 +204,19 @@ target_add_mavsdk(camera_lucid) target_add_matplot(camera_lucid) target_add_loguru(camera_lucid) target_include_directories(camera_lucid PRIVATE ${ImageMagick_INCLUDE_DIRS}) -target_link_libraries(camera_lucid PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) \ No newline at end of file +target_link_libraries(camera_lucid PRIVATE -Wl,--copy-dt-needed-entries ${ImageMagick_LIBRARIES}) + +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}) diff --git a/tests/integration/camera_lucid.cpp b/tests/integration/camera_lucid.cpp index fd238750..2febd97a 100644 --- a/tests/integration/camera_lucid.cpp +++ b/tests/integration/camera_lucid.cpp @@ -1,4 +1,25 @@ +#include + +#include "camera/interface.hpp" +#include "camera/lucid.hpp" + + +using namespace std::chrono_literals; int main (int argc, char *argv[]) { + CameraConfiguration config({ + {"SampleConfigKey", 100}, + {"ExposureTime", 1000}, + }); + LucidCameracamera(config); + + camera.connect(); + camera.startTakingPictures(1s); + std::this_thread::sleep_for(2s); + std::optional image = camera.getLatestImage(); + if (image.has_value()) { + cv::imwrite("lucid_img.jpg", image.value().getData()); + } + camera.stopTakingPictures(); } \ No newline at end of file diff --git a/tests/integration/camera_mock.cpp b/tests/integration/camera_mock.cpp new file mode 100644 index 00000000..74140af5 --- /dev/null +++ b/tests/integration/camera_mock.cpp @@ -0,0 +1,26 @@ +#include + +#include "camera/interface.hpp" +#include "camera/mock.hpp" + +#include + +using namespace std::chrono_literals; + +int main (int argc, char *argv[]) { + CameraConfiguration config({ + {"SampleConfigKey", 100}, + {"ExposureTime", 1000}, + }); + MockCamera camera(config); + + camera.connect(); + + camera.startTakingPictures(1s); + std::this_thread::sleep_for(2s); + std::optional image = camera.getLatestImage(); + if (image.has_value()) { + cv::imwrite("mock_img.jpg", image.value().getData()); + } + camera.stopTakingPictures(); +} \ No newline at end of file diff --git a/tests/unit/camera/mock.cpp b/tests/unit/camera/mock.cpp index e76fbbba..919267c3 100644 --- a/tests/unit/camera/mock.cpp +++ b/tests/unit/camera/mock.cpp @@ -13,8 +13,9 @@ TEST(MockCamera, TakePicture) { camera.connect(); - camera.takePicture(); + camera.startTakingPictures(); ImageData image = camera.getLastPicture(); + camera.stopTakingPictures(); EXPECT_EQ(image.getData().size(), cv::Size(4000, 3000)); } \ No newline at end of file