diff --git a/CMakeLists.txt b/CMakeLists.txt index c7ff85681..81182e42f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,7 +26,7 @@ if(WIN32) endif() # Create depthai project -project(depthai VERSION "2.9.0" LANGUAGES CXX C) +project(depthai VERSION "2.10.0" LANGUAGES CXX C) get_directory_property(has_parent PARENT_DIRECTORY) if(has_parent) set(DEPTHAI_VERSION ${PROJECT_VERSION} PARENT_SCOPE) @@ -135,6 +135,7 @@ add_library(${TARGET_CORE_NAME} "${DEPTHAI_BOOTLOADER_SHARED_SOURCES}" # sources src/device/Device.cpp + src/device/DeviceBase.cpp src/device/DeviceBootloader.cpp src/device/DataQueue.cpp src/device/CallbackHandler.cpp @@ -160,6 +161,7 @@ add_library(${TARGET_CORE_NAME} src/pipeline/node/IMU.cpp src/pipeline/node/EdgeDetector.cpp src/pipeline/node/SPIIn.cpp + src/pipeline/node/FeatureTracker.cpp src/pipeline/datatype/Buffer.cpp src/pipeline/datatype/ImgFrame.cpp src/pipeline/datatype/ImageManipConfig.cpp @@ -168,13 +170,15 @@ add_library(${TARGET_CORE_NAME} src/pipeline/datatype/ImgDetections.cpp src/pipeline/datatype/SpatialImgDetections.cpp src/pipeline/datatype/SystemInformation.cpp - src/pipeline/datatype/StreamPacketParser.cpp + src/pipeline/datatype/StreamMessageParser.cpp src/pipeline/datatype/SpatialLocationCalculatorData.cpp src/pipeline/datatype/SpatialLocationCalculatorConfig.cpp src/pipeline/datatype/Tracklets.cpp src/pipeline/datatype/IMUData.cpp src/pipeline/datatype/StereoDepthConfig.cpp src/pipeline/datatype/EdgeDetectorConfig.cpp + src/pipeline/datatype/TrackedFeatures.cpp + src/pipeline/datatype/FeatureTrackerConfig.cpp src/utility/Initialization.cpp src/utility/Resources.cpp src/xlink/XLinkConnection.cpp diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index dd5413424..47bae7a11 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "7039346f5bdc77912a9241d3cf5d11b8779ba62e") +set(DEPTHAI_DEVICE_SIDE_COMMIT "1928f3407397272cc9736aedcca667c6c7419057") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 8f6664417..c977e61e9 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -271,6 +271,10 @@ dai_add_example(bootloader_version src/bootloader_version.cpp ON) #dai_add_example(flash_bootloader src/flash_bootloader.cpp OFF) dai_add_example(edge_detector src/edge_detector.cpp ON) +dai_add_example(feature_tracker src/feature_tracker.cpp ON) + +dai_add_example(corner_detector src/corner_detector.cpp ON) + # Calibration Read and write samples dai_add_example(calibration_flash src/calibration_flash.cpp OFF) dai_add_example(calibration_flash_version5 src/calibration_flash_v5.cpp OFF) diff --git a/examples/src/corner_detector.cpp b/examples/src/corner_detector.cpp new file mode 100644 index 000000000..44921f7de --- /dev/null +++ b/examples/src/corner_detector.cpp @@ -0,0 +1,118 @@ +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" +#include "deque" +#include "unordered_map" +#include "unordered_set" + +static void drawFeatures(cv::Mat& frame, std::vector& features) { + static const auto pointColor = cv::Scalar(0, 0, 255); + static const int circleRadius = 2; + for(auto& feature : features) { + cv::circle(frame, cv::Point(feature.position.x, feature.position.y), circleRadius, pointColor, -1, cv::LINE_AA, 0); + } +} + +int main() { + using namespace std; + + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto featureTrackerLeft = pipeline.create(); + auto featureTrackerRight = pipeline.create(); + + auto xoutPassthroughFrameLeft = pipeline.create(); + auto xoutTrackedFeaturesLeft = pipeline.create(); + auto xoutPassthroughFrameRight = pipeline.create(); + auto xoutTrackedFeaturesRight = pipeline.create(); + auto xinTrackedFeaturesConfig = pipeline.create(); + + xoutPassthroughFrameLeft->setStreamName("passthroughFrameLeft"); + xoutTrackedFeaturesLeft->setStreamName("trackedFeaturesLeft"); + xoutPassthroughFrameRight->setStreamName("passthroughFrameRight"); + xoutTrackedFeaturesRight->setStreamName("trackedFeaturesRight"); + xinTrackedFeaturesConfig->setStreamName("trackedFeaturesConfig"); + + // Properties + monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + + // Disable optical flow + featureTrackerLeft->initialConfig.setMotionEstimator(false); + featureTrackerRight->initialConfig.setMotionEstimator(false); + + // Linking + monoLeft->out.link(featureTrackerLeft->inputImage); + featureTrackerLeft->passthroughInputImage.link(xoutPassthroughFrameLeft->input); + featureTrackerLeft->outputFeatures.link(xoutTrackedFeaturesLeft->input); + xinTrackedFeaturesConfig->out.link(featureTrackerLeft->inputConfig); + + monoRight->out.link(featureTrackerRight->inputImage); + featureTrackerRight->passthroughInputImage.link(xoutPassthroughFrameRight->input); + featureTrackerRight->outputFeatures.link(xoutTrackedFeaturesRight->input); + xinTrackedFeaturesConfig->out.link(featureTrackerRight->inputConfig); + + auto featureTrackerConfig = featureTrackerRight->initialConfig.get(); + + printf("Press 's' to switch between Harris and Shi-Thomasi corner detector! \n"); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Output queues used to receive the results + auto passthroughImageLeftQueue = device.getOutputQueue("passthroughFrameLeft", 8, false); + auto outputFeaturesLeftQueue = device.getOutputQueue("trackedFeaturesLeft", 8, false); + auto passthroughImageRightQueue = device.getOutputQueue("passthroughFrameRight", 8, false); + auto outputFeaturesRightQueue = device.getOutputQueue("trackedFeaturesRight", 8, false); + + auto inputFeatureTrackerConfigQueue = device.getInputQueue("trackedFeaturesConfig"); + + const auto leftWindowName = "left"; + const auto rightWindowName = "right"; + + while(true) { + auto inPassthroughFrameLeft = passthroughImageLeftQueue->get(); + cv::Mat passthroughFrameLeft = inPassthroughFrameLeft->getFrame(); + cv::Mat leftFrame; + cv::cvtColor(passthroughFrameLeft, leftFrame, cv::COLOR_GRAY2BGR); + + auto inPassthroughFrameRight = passthroughImageRightQueue->get(); + cv::Mat passthroughFrameRight = inPassthroughFrameRight->getFrame(); + cv::Mat rightFrame; + cv::cvtColor(passthroughFrameRight, rightFrame, cv::COLOR_GRAY2BGR); + + auto trackedFeaturesLeft = outputFeaturesLeftQueue->get()->trackedFeatures; + drawFeatures(leftFrame, trackedFeaturesLeft); + + auto trackedFeaturesRight = outputFeaturesRightQueue->get()->trackedFeatures; + drawFeatures(rightFrame, trackedFeaturesRight); + + // Show the frame + cv::imshow(leftWindowName, leftFrame); + cv::imshow(rightWindowName, rightFrame); + + int key = cv::waitKey(1); + if(key == 'q') { + break; + } else if(key == 's') { + if(featureTrackerConfig.cornerDetector.type == dai::FeatureTrackerConfig::CornerDetector::Type::HARRIS) { + featureTrackerConfig.cornerDetector.type = dai::FeatureTrackerConfig::CornerDetector::Type::SHI_THOMASI; + printf("Switching to Shi-Thomasi \n"); + } else { + featureTrackerConfig.cornerDetector.type = dai::FeatureTrackerConfig::CornerDetector::Type::HARRIS; + printf("Switching to Harris \n"); + } + auto cfg = dai::FeatureTrackerConfig(); + cfg.set(featureTrackerConfig); + inputFeatureTrackerConfigQueue->send(cfg); + } + } + return 0; +} diff --git a/examples/src/feature_tracker.cpp b/examples/src/feature_tracker.cpp new file mode 100644 index 000000000..c12e5ce3a --- /dev/null +++ b/examples/src/feature_tracker.cpp @@ -0,0 +1,194 @@ +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" +#include "deque" +#include "unordered_map" +#include "unordered_set" + +static const auto lineColor = cv::Scalar(200, 0, 200); +static const auto pointColor = cv::Scalar(0, 0, 255); + +class FeatureTrackerDrawer { + private: + static const int circleRadius = 2; + static const int maxTrackedFeaturesPathLength = 30; + // for how many frames the feature is tracked + static int trackedFeaturesPathLength; + + using featureIdType = decltype(dai::Point2f::x); + + std::unordered_set trackedIDs; + std::unordered_map> trackedFeaturesPath; + + std::string trackbarName; + std::string windowName; + + public: + void trackFeaturePath(std::vector& features) { + std::unordered_set newTrackedIDs; + for(auto& currentFeature : features) { + auto currentID = currentFeature.id; + newTrackedIDs.insert(currentID); + + if(!trackedFeaturesPath.count(currentID)) { + trackedFeaturesPath.insert({currentID, std::deque()}); + } + std::deque& path = trackedFeaturesPath.at(currentID); + + path.push_back(currentFeature.position); + while(path.size() > std::max(1, trackedFeaturesPathLength)) { + path.pop_front(); + } + } + + std::unordered_set featuresToRemove; + for(auto& oldId : trackedIDs) { + if(!newTrackedIDs.count(oldId)) { + featuresToRemove.insert(oldId); + } + } + + for(auto& id : featuresToRemove) { + trackedFeaturesPath.erase(id); + } + + trackedIDs = newTrackedIDs; + } + + void drawFeatures(cv::Mat& img) { + cv::setTrackbarPos(trackbarName.c_str(), windowName.c_str(), trackedFeaturesPathLength); + + for(auto& featurePath : trackedFeaturesPath) { + std::deque& path = featurePath.second; + int j = 0; + for(j = 0; j < path.size() - 1; j++) { + auto src = cv::Point(path[j].x, path[j].y); + auto dst = cv::Point(path[j + 1].x, path[j + 1].y); + cv::line(img, src, dst, lineColor, 1, cv::LINE_AA, 0); + } + + cv::circle(img, cv::Point(path[j].x, path[j].y), circleRadius, pointColor, -1, cv::LINE_AA, 0); + } + } + + FeatureTrackerDrawer(std::string trackbarName, std::string windowName) { + this->trackbarName = trackbarName; + this->windowName = windowName; + cv::namedWindow(windowName.c_str()); + cv::createTrackbar(trackbarName.c_str(), windowName.c_str(), &trackedFeaturesPathLength, maxTrackedFeaturesPathLength, nullptr); + } +}; + +int FeatureTrackerDrawer::trackedFeaturesPathLength = 10; + +int main() { + using namespace std; + + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto featureTrackerLeft = pipeline.create(); + auto featureTrackerRight = pipeline.create(); + + auto xoutPassthroughFrameLeft = pipeline.create(); + auto xoutTrackedFeaturesLeft = pipeline.create(); + auto xoutPassthroughFrameRight = pipeline.create(); + auto xoutTrackedFeaturesRight = pipeline.create(); + auto xinTrackedFeaturesConfig = pipeline.create(); + + xoutPassthroughFrameLeft->setStreamName("passthroughFrameLeft"); + xoutTrackedFeaturesLeft->setStreamName("trackedFeaturesLeft"); + xoutPassthroughFrameRight->setStreamName("passthroughFrameRight"); + xoutTrackedFeaturesRight->setStreamName("trackedFeaturesRight"); + xinTrackedFeaturesConfig->setStreamName("trackedFeaturesConfig"); + + // Properties + monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + + // Linking + monoLeft->out.link(featureTrackerLeft->inputImage); + featureTrackerLeft->passthroughInputImage.link(xoutPassthroughFrameLeft->input); + featureTrackerLeft->outputFeatures.link(xoutTrackedFeaturesLeft->input); + xinTrackedFeaturesConfig->out.link(featureTrackerLeft->inputConfig); + + monoRight->out.link(featureTrackerRight->inputImage); + featureTrackerRight->passthroughInputImage.link(xoutPassthroughFrameRight->input); + featureTrackerRight->outputFeatures.link(xoutTrackedFeaturesRight->input); + xinTrackedFeaturesConfig->out.link(featureTrackerRight->inputConfig); + + // By default the least mount of resources are allocated + // increasing it improves performance when optical flow is enabled + auto numShaves = 2; + auto numMemorySlices = 2; + featureTrackerLeft->setHardwareResources(numShaves, numMemorySlices); + featureTrackerRight->setHardwareResources(numShaves, numMemorySlices); + + auto featureTrackerConfig = featureTrackerRight->initialConfig.get(); + + printf("Press 's' to switch between Lucas-Kanade optical flow and hardware accelerated motion estimation! \n"); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Output queues used to receive the results + auto passthroughImageLeftQueue = device.getOutputQueue("passthroughFrameLeft", 8, false); + auto outputFeaturesLeftQueue = device.getOutputQueue("trackedFeaturesLeft", 8, false); + auto passthroughImageRightQueue = device.getOutputQueue("passthroughFrameRight", 8, false); + auto outputFeaturesRightQueue = device.getOutputQueue("trackedFeaturesRight", 8, false); + + auto inputFeatureTrackerConfigQueue = device.getInputQueue("trackedFeaturesConfig"); + + const auto leftWindowName = "left"; + auto leftFeatureDrawer = FeatureTrackerDrawer("Feature tracking duration (frames)", leftWindowName); + + const auto rightWindowName = "right"; + auto rightFeatureDrawer = FeatureTrackerDrawer("Feature tracking duration (frames)", rightWindowName); + + while(true) { + auto inPassthroughFrameLeft = passthroughImageLeftQueue->get(); + cv::Mat passthroughFrameLeft = inPassthroughFrameLeft->getFrame(); + cv::Mat leftFrame; + cv::cvtColor(passthroughFrameLeft, leftFrame, cv::COLOR_GRAY2BGR); + + auto inPassthroughFrameRight = passthroughImageRightQueue->get(); + cv::Mat passthroughFrameRight = inPassthroughFrameRight->getFrame(); + cv::Mat rightFrame; + cv::cvtColor(passthroughFrameRight, rightFrame, cv::COLOR_GRAY2BGR); + + auto trackedFeaturesLeft = outputFeaturesLeftQueue->get()->trackedFeatures; + leftFeatureDrawer.trackFeaturePath(trackedFeaturesLeft); + leftFeatureDrawer.drawFeatures(leftFrame); + + auto trackedFeaturesRight = outputFeaturesRightQueue->get()->trackedFeatures; + rightFeatureDrawer.trackFeaturePath(trackedFeaturesRight); + rightFeatureDrawer.drawFeatures(rightFrame); + + // Show the frame + cv::imshow(leftWindowName, leftFrame); + cv::imshow(rightWindowName, rightFrame); + + int key = cv::waitKey(1); + if(key == 'q') { + break; + } else if(key == 's') { + if(featureTrackerConfig.motionEstimator.type == dai::FeatureTrackerConfig::MotionEstimator::Type::LUCAS_KANADE_OPTICAL_FLOW) { + featureTrackerConfig.motionEstimator.type = dai::FeatureTrackerConfig::MotionEstimator::Type::HW_MOTION_ESTIMATION; + printf("Switching to hardware accelerated motion estimation \n"); + } else { + featureTrackerConfig.motionEstimator.type = dai::FeatureTrackerConfig::MotionEstimator::Type::LUCAS_KANADE_OPTICAL_FLOW; + printf("Switching to Lucas-Kanade optical flow \n"); + } + auto cfg = dai::FeatureTrackerConfig(); + cfg.set(featureTrackerConfig); + inputFeatureTrackerConfigQueue->send(cfg); + } + } + return 0; +} diff --git a/include/depthai/device/Device.hpp b/include/depthai/device/Device.hpp index 240f71466..94c47495a 100644 --- a/include/depthai/device/Device.hpp +++ b/include/depthai/device/Device.hpp @@ -1,92 +1,27 @@ #pragma once // std +#include #include +#include +#include #include #include -#include -#include +#include // project -#include "CallbackHandler.hpp" #include "DataQueue.hpp" -#include "depthai/common/CameraBoardSocket.hpp" -#include "depthai/common/UsbSpeed.hpp" -#include "depthai/device/CalibrationHandler.hpp" +#include "depthai/device/DeviceBase.hpp" #include "depthai/pipeline/Pipeline.hpp" -#include "depthai/utility/Pimpl.hpp" -#include "depthai/xlink/XLinkConnection.hpp" -#include "depthai/xlink/XLinkStream.hpp" - -// shared -#include "depthai-shared/common/ChipTemperature.hpp" -#include "depthai-shared/common/CpuUsage.hpp" -#include "depthai-shared/common/MemoryInfo.hpp" -#include "depthai-shared/log/LogLevel.hpp" -#include "depthai-shared/log/LogMessage.hpp" namespace dai { - -// Device (RAII), connects to device and maintains watchdog, timesync, ... - /** * Represents the DepthAI device with the methods to interact with it. + * Implements the host-side queues to connect with XLinkIn and XLinkOut nodes */ -class Device { +class Device : public DeviceBase { public: - // constants - - /// Default search time for constructors which discover devices - static constexpr std::chrono::seconds DEFAULT_SEARCH_TIME{3}; - /// Maximum number of elements in event queue - static constexpr std::size_t EVENT_QUEUE_MAXIMUM_SIZE{2048}; - /// Default rate at which system information is logged - static constexpr float DEFAULT_SYSTEM_INFORMATION_LOGGING_RATE_HZ{1.0f}; - - // static API - - /** - * Waits for any available device with a timeout - * - * @param timeout duration of time to wait for the any device - * @returns Tuple of bool and DeviceInfo. Bool specifies if device was found. DeviceInfo specifies the found device - */ - template - static std::tuple getAnyAvailableDevice(std::chrono::duration timeout); - - /** - * Gets any available device - * - * @returns Tuple of bool and DeviceInfo. Bool specifies if device was found. DeviceInfo specifies the found device - */ - static std::tuple getAnyAvailableDevice(); - - /** - * Gets first available device. Device can be either in XLINK_UNBOOTED or XLINK_BOOTLOADER state - * @returns Tuple of bool and DeviceInfo. Bool specifies if device was found. DeviceInfo specifies the found device - */ - static std::tuple getFirstAvailableDevice(); - - /** - * Finds a device by MX ID. Example: 14442C10D13EABCE00 - * @param mxId MyraidX ID which uniquely specifies a device - * @returns Tuple of bool and DeviceInfo. Bool specifies if device was found. DeviceInfo specifies the found device - */ - static std::tuple getDeviceByMxId(std::string mxId); - - /** - * Returns all connected devices - * @returns Vector of connected devices - */ - static std::vector getAllAvailableDevices(); - - /** - * Gets device firmware binary for a specific OpenVINO version - * @param usb2Mode USB2 mode firmware - * @param version Version of OpenVINO which firmware will support - * @returns Firmware binary - */ - static std::vector getEmbeddedDeviceBinary(bool usb2Mode, OpenVINO::Version version = Pipeline::DEFAULT_OPENVINO_VERSION); + using DeviceBase::DeviceBase; // inherit the ctors /** * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. @@ -119,170 +54,46 @@ class Device { * Connects to device specified by devInfo. * @param pipeline Pipeline to be executed on the device * @param devInfo DeviceInfo which specifies which device to connect to - * @param usb2Mode Boot device using USB2 mode firmware - */ - Device(const Pipeline& pipeline, const DeviceInfo& devInfo, bool usb2Mode = false); - - /** - * Connects to device specified by devInfo. - * @param pipeline Pipeline to be executed on the device - * @param devInfo DeviceInfo which specifies which device to connect to - * @param pathToCmd Path to custom device firmware */ - Device(const Pipeline& pipeline, const DeviceInfo& devInfo, const char* pathToCmd); + Device(const Pipeline& pipeline, const DeviceInfo& devInfo); /** * Connects to device specified by devInfo. * @param pipeline Pipeline to be executed on the device * @param devInfo DeviceInfo which specifies which device to connect to - * @param usb2Mode Path to custom device firmware - */ - Device(const Pipeline& pipeline, const DeviceInfo& devInfo, const std::string& pathToCmd); - - /** - * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. - * @param version OpenVINO version which the device will be booted with. Default is Pipeline::DEFAULT_OPENVINO_VERSION - */ - explicit Device(OpenVINO::Version version = Pipeline::DEFAULT_OPENVINO_VERSION); - - /** - * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. - * @param version OpenVINO version which the device will be booted with - * @param usb2Mode Boot device using USB2 mode firmware - */ - Device(OpenVINO::Version version, bool usb2Mode); - - /** - * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. - * @param version OpenVINO version which the device will be booted with - * @param pathToCmd Path to custom device firmware - */ - Device(OpenVINO::Version version, const char* pathToCmd); - - /** - * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. - * @param version OpenVINO version which the device will be booted with - * @param pathToCmd Path to custom device firmware - */ - Device(OpenVINO::Version version, const std::string& pathToCmd); - - /** - * Connects to device specified by devInfo. - * @param version OpenVINO version which the device will be booted with - * @param devInfo DeviceInfo which specifies which device to connect to * @param usb2Mode Boot device using USB2 mode firmware */ - Device(OpenVINO::Version version, const DeviceInfo& devInfo, bool usb2Mode = false); + Device(const Pipeline& pipeline, const DeviceInfo& devInfo, bool usb2Mode); /** * Connects to device specified by devInfo. - * @param version OpenVINO version which the device will be booted with + * @param pipeline Pipeline to be executed on the device * @param devInfo DeviceInfo which specifies which device to connect to * @param pathToCmd Path to custom device firmware */ - Device(OpenVINO::Version version, const DeviceInfo& devInfo, const char* pathToCmd); + Device(const Pipeline& pipeline, const DeviceInfo& devInfo, const char* pathToCmd); /** * Connects to device specified by devInfo. - * @param version OpenVINO version which the device will be booted with + * @param pipeline Pipeline to be executed on the device * @param devInfo DeviceInfo which specifies which device to connect to * @param usb2Mode Path to custom device firmware */ - Device(OpenVINO::Version version, const DeviceInfo& devInfo, const std::string& pathToCmd); - - /** - * Device destructor. Closes the connection and data queues. - */ - ~Device(); - - /** - * Checks if devices pipeline is already running - * - * @returns True if running, false otherwise - */ - bool isPipelineRunning(); - - /** - * Starts the execution of the devices pipeline - * - * @returns True if pipeline started, false otherwise - */ - [[deprecated("Device(pipeline) starts the pipeline automatically. See Device() and startPipeline(pipeline) otherwise")]] bool startPipeline(); - - /** - * Starts the execution of a given pipeline - * @param pipeline OpenVINO version of the pipeline must match the one which the device was booted with. - * - * @returns True if pipeline started, false otherwise - */ - bool startPipeline(const Pipeline& pipeline); - - /** - * Sets the devices logging severity level. This level affects which logs are transfered from device to host. - * - * @param level Logging severity - */ - void setLogLevel(LogLevel level); - - /** - * Gets current logging severity level of the device. - * - * @returns Logging severity level - */ - LogLevel getLogLevel(); - - /** - * Get the Device Info object o the device which is currently running - * - * @return DeviceInfo of the current device in execution - */ - DeviceInfo getDeviceInfo(); - - /** - * Sets logging level which decides printing level to standard output. - * If lower than setLogLevel, no messages will be printed - * - * @param level Standard output printing severity - */ - void setLogOutputLevel(LogLevel level); - - /** - * Gets logging level which decides printing level to standard output. - * - * @returns Standard output printing severity - */ - LogLevel getLogOutputLevel(); - - /** - * Add a callback for device logging. The callback will be called from a separate thread with the LogMessage being passed. - * - * @param callback Callback to call whenever a log message arrives - * @returns Id which can be used to later remove the callback - */ - int addLogCallback(std::function callback); + Device(const Pipeline& pipeline, const DeviceInfo& devInfo, const std::string& pathToCmd); /** - * Removes a callback - * - * @param callbackId Id of callback to be removed - * @returns True if callback was removed, false otherwise + * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. + * Uses OpenVINO version Pipeline::DEFAULT_OPENVINO_VERSION */ - bool removeLogCallback(int callbackId); + Device(); /** - * Sets rate of system information logging ("info" severity). Default 1Hz - * If parameter is less or equal to zero, then system information logging will be disabled - * - * @param rateHz Logging rate in Hz + * @brief dtor to close the device */ - void setSystemInformationLoggingRate(float rateHz); + ~Device() override; - /** - * Gets current rate of system information logging ("info" severity) in Hz. - * - * @returns Logging rate in Hz - */ - float getSystemInformationLoggingRate(); + /// Maximum number of elements in event queue + static constexpr std::size_t EVENT_QUEUE_MAXIMUM_SIZE{2048}; /** * Gets an output queue corresponding to stream name. If it doesn't exist it throws @@ -400,152 +211,18 @@ class Device { */ std::string getQueueEvent(std::chrono::microseconds timeout = std::chrono::microseconds(-1)); - /** - * Get MxId of device - * - * @returns MxId of connected device - */ - std::string getMxId(); - - /** - * Get cameras that are connected to the device - * - * @returns Vector of connected cameras - */ - std::vector getConnectedCameras(); - - /** - * Retrieves current DDR memory information from device - * - * @returns Used, remaining and total ddr memory - */ - MemoryInfo getDdrMemoryUsage(); - - /** - * Retrieves current CMX memory information from device - * - * @returns Used, remaining and total cmx memory - */ - MemoryInfo getCmxMemoryUsage(); - - /** - * Retrieves current CSS Leon CPU heap information from device - * - * @returns Used, remaining and total heap memory - */ - MemoryInfo getLeonCssHeapUsage(); - - /** - * Retrieves current MSS Leon CPU heap information from device - * - * @returns Used, remaining and total heap memory - */ - MemoryInfo getLeonMssHeapUsage(); - - /** - * Retrieves current chip temperature as measured by device - * - * @returns Temperature of various onboard sensors - */ - ChipTemperature getChipTemperature(); - - /** - * Retrieves average CSS Leon CPU usage - * - * @returns Average CPU usage and sampling duration - */ - CpuUsage getLeonCssCpuUsage(); - - /** - * Retrieves average MSS Leon CPU usage - * - * @returns Average CPU usage and sampling duration - */ - CpuUsage getLeonMssCpuUsage(); - - /** - * Stores the Calibration and Device information to the Device EEPROM - * - * @param calibrationObj CalibrationHandler object which is loaded with calibration information. - * - * @return true on successful flash, false on failure - */ - bool flashCalibration(CalibrationHandler calibrationDataHandler); - - /** - * Fetches the EEPROM data from the device and loads it into CalibrationHandler object - * - * @return The CalibrationHandler object containing the calibration currently flashed on device EEPROM - */ - CalibrationHandler readCalibration(); - - /** - * Retrieves USB connection speed - * - * @returns USB connection speed of connected device if applicable. Unknown otherwise. - */ - UsbSpeed getUsbSpeed(); - - /** - * Explicitly closes connection to device. - * @note This function does not need to be explicitly called - * as destructor closes the device automatically - */ - void close(); - - /** - * Is the device already closed (or disconnected) - */ - bool isClosed() const; - private: - // private static - void init(OpenVINO::Version version, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd); - void init(const Pipeline& pipeline, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd); - void init2(bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd, tl::optional pipeline); - void checkClosed() const; - - std::shared_ptr connection; - std::vector patchedCmd; - - DeviceInfo deviceInfo = {}; - std::unordered_map> outputQueueMap; std::unordered_map> inputQueueMap; std::unordered_map callbackIdMap; - // std::unordered_map callbackMap; - - // Log callback - int uniqueCallbackId = 0; - std::mutex logCallbackMapMtx; - std::unordered_map> logCallbackMap; // Event queue std::mutex eventMtx; std::condition_variable eventCv; std::deque eventQueue; - // Watchdog thread - std::thread watchdogThread; - std::atomic watchdogRunning{true}; - - // Timesync thread - std::thread timesyncThread; - std::atomic timesyncRunning{true}; - - // Logging thread - std::thread loggingThread; - std::atomic loggingRunning{true}; - - // closed - std::atomic closed{false}; - - // pimpl - class Impl; - Pimpl pimpl; - - // OpenVINO version device was booted with - OpenVINO::Version openvinoVersion; + bool startPipelineImpl(const Pipeline& pipeline) override; + void closeImpl() override; }; } // namespace dai diff --git a/include/depthai/device/DeviceBase.hpp b/include/depthai/device/DeviceBase.hpp new file mode 100644 index 000000000..b2bee8edf --- /dev/null +++ b/include/depthai/device/DeviceBase.hpp @@ -0,0 +1,499 @@ +#pragma once + +// std +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// project +#include "depthai/common/CameraBoardSocket.hpp" +#include "depthai/common/UsbSpeed.hpp" +#include "depthai/device/CalibrationHandler.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/utility/Pimpl.hpp" +#include "depthai/xlink/XLinkConnection.hpp" +#include "depthai/xlink/XLinkStream.hpp" + +// shared +#include "depthai-shared/common/ChipTemperature.hpp" +#include "depthai-shared/common/CpuUsage.hpp" +#include "depthai-shared/common/MemoryInfo.hpp" +#include "depthai-shared/log/LogLevel.hpp" +#include "depthai-shared/log/LogMessage.hpp" + +namespace dai { + +/** + * The core of depthai device for RAII, connects to device and maintains watchdog, timesync, ... + */ +class DeviceBase { + public: + // constants + + /// Default search time for constructors which discover devices + static constexpr std::chrono::seconds DEFAULT_SEARCH_TIME{3}; + /// Default rate at which system information is logged + static constexpr float DEFAULT_SYSTEM_INFORMATION_LOGGING_RATE_HZ{1.0f}; + + // static API + + /** + * Waits for any available device with a timeout + * + * @param timeout duration of time to wait for the any device + * @returns Tuple of bool and DeviceInfo. Bool specifies if device was found. DeviceInfo specifies the found device + */ + template + static std::tuple getAnyAvailableDevice(std::chrono::duration timeout); + + /** + * Gets any available device + * + * @returns Tuple of bool and DeviceInfo. Bool specifies if device was found. DeviceInfo specifies the found device + */ + static std::tuple getAnyAvailableDevice(); + + /** + * Gets first available device. Device can be either in XLINK_UNBOOTED or XLINK_BOOTLOADER state + * @returns Tuple of bool and DeviceInfo. Bool specifies if device was found. DeviceInfo specifies the found device + */ + static std::tuple getFirstAvailableDevice(); + + /** + * Finds a device by MX ID. Example: 14442C10D13EABCE00 + * @param mxId MyraidX ID which uniquely specifies a device + * @returns Tuple of bool and DeviceInfo. Bool specifies if device was found. DeviceInfo specifies the found device + */ + static std::tuple getDeviceByMxId(std::string mxId); + + /** + * Returns all connected devices + * @returns Vector of connected devices + */ + static std::vector getAllAvailableDevices(); + + /** + * Gets device firmware binary for a specific OpenVINO version + * @param usb2Mode USB2 mode firmware + * @param version Version of OpenVINO which firmware will support + * @returns Firmware binary + */ + static std::vector getEmbeddedDeviceBinary(bool usb2Mode, OpenVINO::Version version = Pipeline::DEFAULT_OPENVINO_VERSION); + + /** + * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. + * @param pipeline Pipeline to be executed on the device + */ + explicit DeviceBase(const Pipeline& pipeline); + + /** + * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. + * @param pipeline Pipeline to be executed on the device + * @param usb2Mode Boot device using USB2 mode firmware + */ + DeviceBase(const Pipeline& pipeline, bool usb2Mode); + + /** + * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. + * @param pipeline Pipeline to be executed on the device + * @param pathToCmd Path to custom device firmware + */ + DeviceBase(const Pipeline& pipeline, const char* pathToCmd); + + /** + * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. + * @param pipeline Pipeline to be executed on the device + * @param pathToCmd Path to custom device firmware + */ + DeviceBase(const Pipeline& pipeline, const std::string& pathToCmd); + + /** + * Connects to device specified by devInfo. + * @param pipeline Pipeline to be executed on the device + * @param devInfo DeviceInfo which specifies which device to connect to + */ + DeviceBase(const Pipeline& pipeline, const DeviceInfo& devInfo); + + /** + * Connects to device specified by devInfo. + * @param pipeline Pipeline to be executed on the device + * @param devInfo DeviceInfo which specifies which device to connect to + * @param usb2Mode Boot device using USB2 mode firmware + */ + DeviceBase(const Pipeline& pipeline, const DeviceInfo& devInfo, bool usb2Mode); + + /** + * Connects to device specified by devInfo. + * @param pipeline Pipeline to be executed on the device + * @param devInfo DeviceInfo which specifies which device to connect to + * @param pathToCmd Path to custom device firmware + */ + DeviceBase(const Pipeline& pipeline, const DeviceInfo& devInfo, const char* pathToCmd); + + /** + * Connects to device specified by devInfo. + * @param pipeline Pipeline to be executed on the device + * @param devInfo DeviceInfo which specifies which device to connect to + * @param pathToCmd Path to custom device firmware + */ + DeviceBase(const Pipeline& pipeline, const DeviceInfo& devInfo, const std::string& pathToCmd); + + /** + * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. + * Uses OpenVINO version Pipeline::DEFAULT_OPENVINO_VERSION + */ + DeviceBase(); + + /** + * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. + * @param version OpenVINO version which the device will be booted with. + */ + explicit DeviceBase(OpenVINO::Version version); + + /** + * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. + * @param version OpenVINO version which the device will be booted with + * @param usb2Mode Boot device using USB2 mode firmware + */ + DeviceBase(OpenVINO::Version version, bool usb2Mode); + + /** + * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. + * @param version OpenVINO version which the device will be booted with + * @param pathToCmd Path to custom device firmware + */ + DeviceBase(OpenVINO::Version version, const char* pathToCmd); + + /** + * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. + * @param version OpenVINO version which the device will be booted with + * @param pathToCmd Path to custom device firmware + */ + DeviceBase(OpenVINO::Version version, const std::string& pathToCmd); + + /** + * Connects to device specified by devInfo. + * @param version OpenVINO version which the device will be booted with + * @param devInfo DeviceInfo which specifies which device to connect to + */ + DeviceBase(OpenVINO::Version version, const DeviceInfo& devInfo); + + /** + * Connects to device specified by devInfo. + * @param version OpenVINO version which the device will be booted with + * @param devInfo DeviceInfo which specifies which device to connect to + * @param usb2Mode Boot device using USB2 mode firmware + */ + DeviceBase(OpenVINO::Version version, const DeviceInfo& devInfo, bool usb2Mode); + + /** + * Connects to device specified by devInfo. + * @param version OpenVINO version which the device will be booted with + * @param devInfo DeviceInfo which specifies which device to connect to + * @param pathToCmd Path to custom device firmware + */ + DeviceBase(OpenVINO::Version version, const DeviceInfo& devInfo, const char* pathToCmd); + + /** + * Connects to device specified by devInfo. + * @param version OpenVINO version which the device will be booted with + * @param devInfo DeviceInfo which specifies which device to connect to + * @param usb2Mode Path to custom device firmware + */ + DeviceBase(OpenVINO::Version version, const DeviceInfo& devInfo, const std::string& pathToCmd); + + /** + * Device destructor + * @note In the destructor of the derived class, remember to call close() + */ + virtual ~DeviceBase(); + + /** + * Checks if devices pipeline is already running + * + * @returns True if running, false otherwise + */ + bool isPipelineRunning(); + + /** + * Starts the execution of the devices pipeline + * + * @returns True if pipeline started, false otherwise + */ + [[deprecated("Device(pipeline) starts the pipeline automatically. See Device() and startPipeline(pipeline) otherwise")]] bool startPipeline(); + + /** + * Starts the execution of a given pipeline + * @param pipeline OpenVINO version of the pipeline must match the one which the device was booted with. + * + * @returns True if pipeline started, false otherwise + */ + bool startPipeline(const Pipeline& pipeline); + + /** + * Sets the devices logging severity level. This level affects which logs are transfered from device to host. + * + * @param level Logging severity + */ + void setLogLevel(LogLevel level); + + /** + * Gets current logging severity level of the device. + * + * @returns Logging severity level + */ + LogLevel getLogLevel(); + + /** + * Sets the chunk size for splitting device-sent XLink packets. A larger value could + * increase performance, and 0 disables chunking. A negative value is ignored. + * Device defaults are configured per protocol, currently 64*1024 for both USB and Ethernet. + * + * @param sizeBytes XLink chunk size in bytes + */ + void setXLinkChunkSize(int sizeBytes); + + /** + * Gets current XLink chunk size. + * + * @returns XLink chunk size in bytes + */ + int getXLinkChunkSize(); + + /** + * Get the Device Info object o the device which is currently running + * + * @return DeviceInfo of the current device in execution + */ + DeviceInfo getDeviceInfo(); + + /** + * Get MxId of device + * + * @returns MxId of connected device + */ + std::string getMxId(); + + /** + * Sets logging level which decides printing level to standard output. + * If lower than setLogLevel, no messages will be printed + * + * @param level Standard output printing severity + */ + void setLogOutputLevel(LogLevel level); + + /** + * Gets logging level which decides printing level to standard output. + * + * @returns Standard output printing severity + */ + LogLevel getLogOutputLevel(); + + /** + * Add a callback for device logging. The callback will be called from a separate thread with the LogMessage being passed. + * + * @param callback Callback to call whenever a log message arrives + * @returns Id which can be used to later remove the callback + */ + int addLogCallback(std::function callback); + + /** + * Removes a callback + * + * @param callbackId Id of callback to be removed + * @returns True if callback was removed, false otherwise + */ + bool removeLogCallback(int callbackId); + + /** + * Sets rate of system information logging ("info" severity). Default 1Hz + * If parameter is less or equal to zero, then system information logging will be disabled + * + * @param rateHz Logging rate in Hz + */ + void setSystemInformationLoggingRate(float rateHz); + + /** + * Gets current rate of system information logging ("info" severity) in Hz. + * + * @returns Logging rate in Hz + */ + float getSystemInformationLoggingRate(); + + /** + * Get cameras that are connected to the device + * + * @returns Vector of connected cameras + */ + std::vector getConnectedCameras(); + + /** + * Retrieves current DDR memory information from device + * + * @returns Used, remaining and total ddr memory + */ + MemoryInfo getDdrMemoryUsage(); + + /** + * Retrieves current CMX memory information from device + * + * @returns Used, remaining and total cmx memory + */ + MemoryInfo getCmxMemoryUsage(); + + /** + * Retrieves current CSS Leon CPU heap information from device + * + * @returns Used, remaining and total heap memory + */ + MemoryInfo getLeonCssHeapUsage(); + + /** + * Retrieves current MSS Leon CPU heap information from device + * + * @returns Used, remaining and total heap memory + */ + MemoryInfo getLeonMssHeapUsage(); + + /** + * Retrieves current chip temperature as measured by device + * + * @returns Temperature of various onboard sensors + */ + ChipTemperature getChipTemperature(); + + /** + * Retrieves average CSS Leon CPU usage + * + * @returns Average CPU usage and sampling duration + */ + CpuUsage getLeonCssCpuUsage(); + + /** + * Retrieves average MSS Leon CPU usage + * + * @returns Average CPU usage and sampling duration + */ + CpuUsage getLeonMssCpuUsage(); + + /** + * Stores the Calibration and Device information to the Device EEPROM + * + * @param calibrationObj CalibrationHandler object which is loaded with calibration information. + * + * @return true on successful flash, false on failure + */ + bool flashCalibration(CalibrationHandler calibrationDataHandler); + + /** + * Fetches the EEPROM data from the device and loads it into CalibrationHandler object + * + * @return The CalibrationHandler object containing the calibration currently flashed on device EEPROM + */ + CalibrationHandler readCalibration(); + + /** + * Retrieves USB connection speed + * + * @returns USB connection speed of connected device if applicable. Unknown otherwise. + */ + UsbSpeed getUsbSpeed(); + + /** + * Explicitly closes connection to device. + * @note This function does not need to be explicitly called + * as destructor closes the device automatically + */ + void close(); + + /** + * Is the device already closed (or disconnected) + */ + bool isClosed() const; + + std::shared_ptr getConnection() { + return connection; + } + + std::shared_ptr getConnection() const { + return connection; + } + + protected: + std::shared_ptr connection; + + /** + * @brief a safe way to start a pipeline, which is closed if any exception occurs + */ + void tryStartPipeline(const Pipeline& pipeline); + + /** + * throws an error if the device has been closed or the watchdog has died + */ + void checkClosed() const; + + /** + * Allows the derived classes to handle custom setup for starting the pipeline + * + * @param pipeline OpenVINO version of the pipeline must match the one which the device was booted with + * @sa startPipeline + * @note Remember to call this function in the overload to setup the comunication properly + * + * @returns True if pipeline started, false otherwise + */ + virtual bool startPipelineImpl(const Pipeline& pipeline); + + /** + * Allows the derived classes to handle custom setup for gracefully stopping the pipeline + * + * @note Remember to call this function in the overload to setup the comunication properly + */ + virtual void closeImpl(); + + private: + // private static + void init(OpenVINO::Version version, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd); + void init(const Pipeline& pipeline, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd); + void init2(bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd); + + DeviceInfo deviceInfo = {}; + + // Log callback + int uniqueCallbackId = 0; + std::mutex logCallbackMapMtx; + std::unordered_map> logCallbackMap; + + // Watchdog thread + std::thread watchdogThread; + std::atomic watchdogRunning{true}; + + // Timesync thread + std::thread timesyncThread; + std::atomic timesyncRunning{true}; + + // Logging thread + std::thread loggingThread; + std::atomic loggingRunning{true}; + + // RPC stream + std::unique_ptr rpcStream; + + // closed + std::atomic closed{false}; + + // pimpl + class Impl; + Pimpl pimpl; + + // OpenVINO version device was booted with + OpenVINO::Version openvinoVersion; +}; +} // namespace dai diff --git a/include/depthai/pipeline/Pipeline.hpp b/include/depthai/pipeline/Pipeline.hpp index 11d5f0d54..dbedfadcb 100644 --- a/include/depthai/pipeline/Pipeline.hpp +++ b/include/depthai/pipeline/Pipeline.hpp @@ -36,6 +36,7 @@ class PipelineImpl { PipelineSchema getPipelineSchema() const; OpenVINO::Version getPipelineOpenVINOVersion() const; void setCameraTuningBlobPath(const std::string& path); + void setXLinkChunkSize(int sizeBytes); // Access to nodes std::vector> getAllNodes() const; @@ -240,6 +241,15 @@ class Pipeline { void setCameraTuningBlobPath(const std::string& path) { impl()->setCameraTuningBlobPath(path); } + + /** + * Set chunk size for splitting device-sent XLink packets, in bytes. A larger value could + * increase performance, with 0 disabling chunking. A negative value won't modify the + * device defaults - configured per protocol, currently 64*1024 for both USB and Ethernet. + */ + void setXLinkChunkSize(int sizeBytes) { + impl()->setXLinkChunkSize(sizeBytes); + } }; } // namespace dai diff --git a/include/depthai/pipeline/datatype/ADatatype.hpp b/include/depthai/pipeline/datatype/ADatatype.hpp index d2194a5ff..87a0537e1 100644 --- a/include/depthai/pipeline/datatype/ADatatype.hpp +++ b/include/depthai/pipeline/datatype/ADatatype.hpp @@ -11,12 +11,13 @@ namespace dai { class ADatatype { protected: friend class DataInputQueue; - virtual std::shared_ptr serialize() const = 0; + friend class StreamMessageParser; std::shared_ptr raw; public: explicit ADatatype(std::shared_ptr r) : raw(std::move(r)) {} virtual ~ADatatype() = default; + virtual std::shared_ptr serialize() const = 0; std::shared_ptr getRaw() { return raw; } diff --git a/include/depthai/pipeline/datatype/Buffer.hpp b/include/depthai/pipeline/datatype/Buffer.hpp index 5d9520df0..1efd17b5a 100644 --- a/include/depthai/pipeline/datatype/Buffer.hpp +++ b/include/depthai/pipeline/datatype/Buffer.hpp @@ -10,7 +10,7 @@ namespace dai { /// Base message - buffer of binary data class Buffer : public ADatatype { - virtual std::shared_ptr serialize() const; + std::shared_ptr serialize() const override; public: /// Creates Buffer message diff --git a/include/depthai/pipeline/datatype/FeatureTrackerConfig.hpp b/include/depthai/pipeline/datatype/FeatureTrackerConfig.hpp new file mode 100644 index 000000000..eb35ed0a5 --- /dev/null +++ b/include/depthai/pipeline/datatype/FeatureTrackerConfig.hpp @@ -0,0 +1,103 @@ +#pragma once + +#include +#include + +#include "depthai-shared/datatype/RawFeatureTrackerConfig.hpp" +#include "depthai/pipeline/datatype/Buffer.hpp" + +namespace dai { + +/** + * FeatureTrackerConfig message. Carries config for feature tracking algorithm + */ +class FeatureTrackerConfig : public Buffer { + std::shared_ptr serialize() const override; + RawFeatureTrackerConfig& cfg; + + public: + // Raw* mirror + using CornerDetector = RawFeatureTrackerConfig::CornerDetector; + using MotionEstimator = RawFeatureTrackerConfig::MotionEstimator; + using FeatureMaintainer = RawFeatureTrackerConfig::FeatureMaintainer; + + /** + * Construct FeatureTrackerConfig message. + */ + FeatureTrackerConfig(); + explicit FeatureTrackerConfig(std::shared_ptr ptr); + virtual ~FeatureTrackerConfig() = default; + + /** + * Set corner detector algorithm type. + * @param cornerDetector Corner detector type, HARRIS or SHI_THOMASI + */ + void setCornerDetector(dai::FeatureTrackerConfig::CornerDetector::Type cornerDetector); + + /** + * Set corner detector full configuration. + * @param config Corner detector configuration + */ + void setCornerDetector(dai::FeatureTrackerConfig::CornerDetector config); + + /** + * Set optical flow as motion estimation algorithm type. + */ + void setOpticalFlow(); + + /** + * Set optical flow full configuration. + * @param config Optical flow configuration + */ + void setOpticalFlow(dai::FeatureTrackerConfig::MotionEstimator::OpticalFlow config); + + /** + * Set hardware accelerated motion estimation using block matching. + * Faster than optical flow (software implementation) but might not be as accurate. + */ + void setHwMotionEstimation(); + + /** + * Set number of target features to detect. + * @param numTargetFeatures Number of features + */ + void setNumTargetFeatures(std::int32_t numTargetFeatures); + + /** + * Enable or disable motion estimator. + * @param enable + */ + void setMotionEstimator(bool enable); + + /** + * Set motion estimator full configuration. + * @param config Motion estimator configuration + */ + void setMotionEstimator(dai::FeatureTrackerConfig::MotionEstimator config); + + /** + * Enable or disable feature maintainer. + * @param enable + */ + void setFeatureMaintainer(bool enable); + + /** + * Set feature maintainer full configuration. + * @param config feature maintainer configuration + */ + void setFeatureMaintainer(dai::FeatureTrackerConfig::FeatureMaintainer config); + + /** + * Set explicit configuration. + * @param config Explicit configuration + */ + void set(dai::RawFeatureTrackerConfig config); + + /** + * Retrieve configuration data for FeatureTracker. + * @returns config for feature tracking algorithm + */ + dai::RawFeatureTrackerConfig get() const; +}; + +} // namespace dai diff --git a/include/depthai/pipeline/datatype/NNData.hpp b/include/depthai/pipeline/datatype/NNData.hpp index fc0f2ec0d..e431048b9 100644 --- a/include/depthai/pipeline/datatype/NNData.hpp +++ b/include/depthai/pipeline/datatype/NNData.hpp @@ -14,7 +14,7 @@ namespace dai { */ class NNData : public Buffer { static constexpr int DATA_ALIGNMENT = 64; - std::shared_ptr serialize() const; + std::shared_ptr serialize() const override; RawNNData& rawNn; // store the data diff --git a/include/depthai/pipeline/datatype/StreamMessageParser.hpp b/include/depthai/pipeline/datatype/StreamMessageParser.hpp new file mode 100644 index 000000000..7c5b9c5b3 --- /dev/null +++ b/include/depthai/pipeline/datatype/StreamMessageParser.hpp @@ -0,0 +1,28 @@ +#pragma once + +// standard +#include + +// libraries +#include + +// project +#include "depthai/pipeline/datatype/ADatatype.hpp" + +// shared +#include "depthai-shared/datatype/RawBuffer.hpp" + +// StreamPacket structure -> || imgframepixels... , serialized_object, object_type, serialized_object_size || +// object_type -> DataType(int), serialized_object_size -> int + +namespace dai { +class StreamMessageParser { + public: + static std::shared_ptr parseMessage(streamPacketDesc_t* packet); + static std::shared_ptr parseMessageToADatatype(streamPacketDesc_t* packet); + static std::vector serializeMessage(const std::shared_ptr& data); + static std::vector serializeMessage(const RawBuffer& data); + static std::vector serializeMessage(const std::shared_ptr& data); + static std::vector serializeMessage(const ADatatype& data); +}; +} // namespace dai diff --git a/include/depthai/pipeline/datatype/TrackedFeatures.hpp b/include/depthai/pipeline/datatype/TrackedFeatures.hpp new file mode 100644 index 000000000..a034ed275 --- /dev/null +++ b/include/depthai/pipeline/datatype/TrackedFeatures.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include +#include + +#include "depthai-shared/datatype/RawTrackedFeatures.hpp" +#include "depthai/pipeline/datatype/Buffer.hpp" + +namespace dai { + +/** + * TrackedFeatures message. Carries position (X, Y) of tracked features and their ID. + */ +class TrackedFeatures : public Buffer { + std::shared_ptr serialize() const override; + RawTrackedFeatures& rawdata; + + public: + /** + * Construct TrackedFeatures message. + */ + TrackedFeatures(); + explicit TrackedFeatures(std::shared_ptr ptr); + virtual ~TrackedFeatures() = default; + + std::vector& trackedFeatures; +}; + +} // namespace dai diff --git a/include/depthai/pipeline/datatypes.hpp b/include/depthai/pipeline/datatypes.hpp index 7ffb0ce0e..c8a0561b6 100644 --- a/include/depthai/pipeline/datatypes.hpp +++ b/include/depthai/pipeline/datatypes.hpp @@ -4,6 +4,7 @@ #include "datatype/ADatatype.hpp" #include "datatype/Buffer.hpp" #include "datatype/CameraControl.hpp" +#include "datatype/FeatureTrackerConfig.hpp" #include "datatype/IMUData.hpp" #include "datatype/ImageManipConfig.hpp" #include "datatype/ImgDetections.hpp" @@ -13,4 +14,5 @@ #include "datatype/SpatialLocationCalculatorConfig.hpp" #include "datatype/SpatialLocationCalculatorData.hpp" #include "datatype/SystemInformation.hpp" -#include "datatype/Tracklets.hpp" +#include "datatype/TrackedFeatures.hpp" +#include "datatype/Tracklets.hpp" \ No newline at end of file diff --git a/include/depthai/pipeline/node/EdgeDetector.hpp b/include/depthai/pipeline/node/EdgeDetector.hpp index b3aec25cc..3e2ea959a 100644 --- a/include/depthai/pipeline/node/EdgeDetector.hpp +++ b/include/depthai/pipeline/node/EdgeDetector.hpp @@ -52,6 +52,11 @@ class EdgeDetector : public Node { */ Output outputImage{*this, "outputImage", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + /** + * Passthrough message on which the calculation was performed. + */ + Output passthroughInputImage{*this, "passthroughInputImage", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + // Functions to set properties /** * Specify whether or not wait until configuration message arrives to inputConfig Input. diff --git a/include/depthai/pipeline/node/FeatureTracker.hpp b/include/depthai/pipeline/node/FeatureTracker.hpp new file mode 100644 index 000000000..9da286b5d --- /dev/null +++ b/include/depthai/pipeline/node/FeatureTracker.hpp @@ -0,0 +1,79 @@ +#pragma once + +#include + +// standard +#include + +// shared +#include + +#include "depthai/pipeline/datatype/FeatureTrackerConfig.hpp" + +namespace dai { +namespace node { + +/** + * @brief FeatureTracker node. + * Performs feature tracking and reidentification using motion estimation between 2 consecutive frames. + */ +class FeatureTracker : public Node { + public: + using Properties = dai::FeatureTrackerProperties; + + private: + std::string getName() const override; + nlohmann::json getProperties() override; + std::shared_ptr clone() override; + + std::shared_ptr rawConfig; + Properties properties; + + public: + FeatureTracker(const std::shared_ptr& par, int64_t nodeId); + + /** + * Initial config to use for feature tracking. + */ + FeatureTrackerConfig initialConfig; + + /** + * Input FeatureTrackerConfig message with ability to modify parameters in runtime. + * Default queue is non-blocking with size 4. + */ + Input inputConfig{*this, "inputConfig", Input::Type::SReceiver, false, 4, {{DatatypeEnum::FeatureTrackerConfig, false}}}; + /** + * Input message with frame data on which feature tracking is performed. + * Default queue is non-blocking with size 4. + */ + Input inputImage{*this, "inputImage", Input::Type::SReceiver, false, 4, {{DatatypeEnum::ImgFrame, false}}}; + + /** + * Outputs TrackedFeatures message that carries tracked features results. + */ + Output outputFeatures{*this, "outputFeatures", Output::Type::MSender, {{DatatypeEnum::TrackedFeatures, false}}}; + + /** + * Passthrough message on which the calculation was performed. + * Suitable for when input queue is set to non-blocking behavior. + */ + Output passthroughInputImage{*this, "passthroughInputImage", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + + // Functions to set properties + /** + * Specify whether or not wait until configuration message arrives to inputConfig Input. + * @param wait True to wait for configuration message, false otherwise. + */ + void setWaitForConfigInput(bool wait); + + /** + * Specify allocated hardware resources for feature tracking. + * 2 shaves/memory slices are required for optical flow, 1 for corner detection only. + * @param numShaves Number of shaves. Maximum 2. + * @param numMemorySlices Number of memory slices. Maximum 2. + */ + void setHardwareResources(int numShaves, int numMemorySlices); +}; + +} // namespace node +} // namespace dai diff --git a/include/depthai/pipeline/nodes.hpp b/include/depthai/pipeline/nodes.hpp index 8c4f3b013..10b96e1a1 100644 --- a/include/depthai/pipeline/nodes.hpp +++ b/include/depthai/pipeline/nodes.hpp @@ -4,6 +4,7 @@ #include "node/ColorCamera.hpp" #include "node/DetectionNetwork.hpp" #include "node/EdgeDetector.hpp" +#include "node/FeatureTracker.hpp" #include "node/IMU.hpp" #include "node/ImageManip.hpp" #include "node/MonoCamera.hpp" diff --git a/shared/depthai-shared b/shared/depthai-shared index 5fb8913d8..a33b3e3f6 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 5fb8913d89055a74f162582ff09ff41009f6a76c +Subproject commit a33b3e3f62af68303ba909968d5bd9806e492262 diff --git a/src/device/CallbackHandler.cpp b/src/device/CallbackHandler.cpp index cf2176929..f31d81db8 100644 --- a/src/device/CallbackHandler.cpp +++ b/src/device/CallbackHandler.cpp @@ -2,7 +2,7 @@ // project #include "depthai/xlink/XLinkStream.hpp" -#include "pipeline/datatype/StreamPacketParser.hpp" +#include "pipeline/datatype/StreamMessageParser.hpp" namespace dai { @@ -24,14 +24,14 @@ CallbackHandler::CallbackHandler(std::shared_ptr conn, // read packet auto* packet = stream.readRaw(); // parse packet - auto data = parsePacket(packet); + auto data = StreamMessageParser::parseMessage(packet); // release packet stream.readRawRelease(); // CALLBACK auto toSend = callback(data); - auto serialized = serializeData(toSend); + auto serialized = StreamMessageParser::serializeMessage(toSend); // Write packet back stream.write(serialized); diff --git a/src/device/DataQueue.cpp b/src/device/DataQueue.cpp index 9ee802213..971da9a4f 100644 --- a/src/device/DataQueue.cpp +++ b/src/device/DataQueue.cpp @@ -6,7 +6,7 @@ // project #include "depthai/pipeline/datatype/ADatatype.hpp" #include "depthai/xlink/XLinkStream.hpp" -#include "pipeline/datatype/StreamPacketParser.hpp" +#include "pipeline/datatype/StreamMessageParser.hpp" // shared #include "depthai-shared/xlink/XLinkConstants.hpp" @@ -35,7 +35,7 @@ DataOutputQueue::DataOutputQueue(const std::shared_ptr& conn, c packet = stream.readRaw(); // parse packet - auto data = parsePacketToADatatype(packet); + auto data = StreamMessageParser::parseMessageToADatatype(packet); // Trace level debugging if(spdlog::get_level() == spdlog::level::trace) { @@ -194,7 +194,7 @@ DataInputQueue::DataInputQueue(const std::shared_ptr& conn, con } // serialize - auto serialized = serializeData(data); + auto serialized = StreamMessageParser::serializeMessage(data); // Blocking stream.write(serialized); diff --git a/src/device/Device.cpp b/src/device/Device.cpp index 4ac263728..8810a0fdc 100644 --- a/src/device/Device.cpp +++ b/src/device/Device.cpp @@ -7,10 +7,6 @@ #include "depthai-bootloader-shared/Bootloader.hpp" #include "depthai-bootloader-shared/XLinkConstants.hpp" #include "depthai-shared/datatype/RawImgFrame.hpp" -#include "depthai-shared/log/LogConstants.hpp" -#include "depthai-shared/log/LogLevel.hpp" -#include "depthai-shared/log/LogMessage.hpp" -#include "depthai-shared/pipeline/Assets.hpp" #include "depthai-shared/xlink/XLinkConstants.hpp" // project @@ -21,320 +17,53 @@ #include "pipeline/Pipeline.hpp" #include "utility/BootloaderHelper.hpp" #include "utility/Initialization.hpp" -#include "utility/PimplImpl.hpp" #include "utility/Resources.hpp" -// libraries -#include "nanorpc/core/client.h" -#include "nanorpc/packer/nlohmann_msgpack.h" -#include "spdlog/fmt/chrono.h" -#include "spdlog/sinks/stdout_color_sinks.h" -#include "spdlog/spdlog.h" - namespace dai { -// local static function -static LogLevel spdlogLevelToLogLevel(spdlog::level::level_enum level, LogLevel defaultValue = LogLevel::OFF) { - switch(level) { - case spdlog::level::trace: - return LogLevel::TRACE; - case spdlog::level::debug: - return LogLevel::DEBUG; - case spdlog::level::info: - return LogLevel::INFO; - case spdlog::level::warn: - return LogLevel::WARN; - case spdlog::level::err: - return LogLevel::ERR; - case spdlog::level::critical: - return LogLevel::CRITICAL; - case spdlog::level::off: - return LogLevel::OFF; - // Default - case spdlog::level::n_levels: - default: - return defaultValue; - break; - } - // Default - return defaultValue; -} -static spdlog::level::level_enum logLevelToSpdlogLevel(LogLevel level, spdlog::level::level_enum defaultValue = spdlog::level::off) { - switch(level) { - case LogLevel::TRACE: - return spdlog::level::trace; - case LogLevel::DEBUG: - return spdlog::level::debug; - case LogLevel::INFO: - return spdlog::level::info; - case LogLevel::WARN: - return spdlog::level::warn; - case LogLevel::ERR: - return spdlog::level::err; - case LogLevel::CRITICAL: - return spdlog::level::critical; - case LogLevel::OFF: - return spdlog::level::off; - } - // Default - return defaultValue; -} - // Common explicit instantiation, to remove the need to define in header -template std::tuple Device::getAnyAvailableDevice(std::chrono::nanoseconds); -template std::tuple Device::getAnyAvailableDevice(std::chrono::microseconds); -template std::tuple Device::getAnyAvailableDevice(std::chrono::milliseconds); -template std::tuple Device::getAnyAvailableDevice(std::chrono::seconds); -constexpr std::chrono::seconds Device::DEFAULT_SEARCH_TIME; constexpr std::size_t Device::EVENT_QUEUE_MAXIMUM_SIZE; -constexpr float Device::DEFAULT_SYSTEM_INFORMATION_LOGGING_RATE_HZ; - -template -std::tuple Device::getAnyAvailableDevice(std::chrono::duration timeout) { - using namespace std::chrono; - constexpr auto POOL_SLEEP_TIME = milliseconds(100); - - // First looks for UNBOOTED, then BOOTLOADER, for 'timeout' time - auto searchStartTime = steady_clock::now(); - bool found = false; - DeviceInfo deviceInfo; - do { - for(auto searchState : {X_LINK_UNBOOTED, X_LINK_BOOTLOADER}) { - std::tie(found, deviceInfo) = XLinkConnection::getFirstDevice(searchState); - if(found) break; - } - if(found) break; - // If 'timeout' < 'POOL_SLEEP_TIME', use 'timeout' as sleep time and then break - if(timeout < POOL_SLEEP_TIME) { - // sleep for 'timeout' - std::this_thread::sleep_for(timeout); - break; - } else { - std::this_thread::sleep_for(POOL_SLEEP_TIME); // default pool rate - } - } while(steady_clock::now() - searchStartTime < timeout); - - // If none were found, try BOOTED - if(!found) std::tie(found, deviceInfo) = XLinkConnection::getFirstDevice(X_LINK_BOOTED); - - return {found, deviceInfo}; +Device::Device(const Pipeline& pipeline) : DeviceBase(pipeline.getOpenVINOVersion()) { + tryStartPipeline(pipeline); } -// Default overload ('DEFAULT_SEARCH_TIME' timeout) -std::tuple Device::getAnyAvailableDevice() { - return getAnyAvailableDevice(DEFAULT_SEARCH_TIME); +Device::Device(const Pipeline& pipeline, bool usb2Mode) : DeviceBase(pipeline.getOpenVINOVersion(), usb2Mode) { + tryStartPipeline(pipeline); } -// static api - -// First tries to find UNBOOTED device, then BOOTLOADER device -std::tuple Device::getFirstAvailableDevice() { - bool found; - DeviceInfo dev; - std::tie(found, dev) = XLinkConnection::getFirstDevice(X_LINK_UNBOOTED); - if(!found) { - std::tie(found, dev) = XLinkConnection::getFirstDevice(X_LINK_BOOTLOADER); - } - return {found, dev}; +Device::Device(const Pipeline& pipeline, const char* pathToCmd) : DeviceBase(pipeline.getOpenVINOVersion(), pathToCmd) { + tryStartPipeline(pipeline); } -// Returns all devices which aren't already booted -std::vector Device::getAllAvailableDevices() { - std::vector availableDevices; - auto connectedDevices = XLinkConnection::getAllConnectedDevices(); - for(const auto& d : connectedDevices) { - if(d.state != X_LINK_BOOTED) availableDevices.push_back(d); - } - return availableDevices; +Device::Device(const Pipeline& pipeline, const std::string& pathToCmd) : DeviceBase(pipeline.getOpenVINOVersion(), pathToCmd) { + tryStartPipeline(pipeline); } -// First tries to find UNBOOTED device with mxId, then BOOTLOADER device with mxId -std::tuple Device::getDeviceByMxId(std::string mxId) { - std::vector availableDevices; - auto states = {X_LINK_UNBOOTED, X_LINK_BOOTLOADER}; - bool found; - DeviceInfo dev; - for(const auto& state : states) { - std::tie(found, dev) = XLinkConnection::getDeviceByMxId(mxId, state); - if(found) return {true, dev}; - } - return {false, DeviceInfo()}; +Device::Device(const Pipeline& pipeline, const DeviceInfo& devInfo) : DeviceBase(pipeline.getOpenVINOVersion(), devInfo, false) { + tryStartPipeline(pipeline); } -std::vector Device::getEmbeddedDeviceBinary(bool usb2Mode, OpenVINO::Version version) { - return Resources::getInstance().getDeviceFirmware(usb2Mode, version); +Device::Device(const Pipeline& pipeline, const DeviceInfo& devInfo, bool usb2Mode) : DeviceBase(pipeline.getOpenVINOVersion(), devInfo, usb2Mode) { + tryStartPipeline(pipeline); } -/* -std::vector Device::getAllConnectedDevices(){ - return XLinkConnection::getAllConnectedDevices(); +Device::Device(const Pipeline& pipeline, const DeviceInfo& devInfo, const char* pathToCmd) : DeviceBase(pipeline.getOpenVINOVersion(), devInfo, pathToCmd) { + tryStartPipeline(pipeline); } - -std::tuple Device::getFirstDevice(){ - return XLinkConnection::getFirstAvailableDevice(); +Device::Device(const Pipeline& pipeline, const DeviceInfo& devInfo, const std::string& pathToCmd) + : DeviceBase(pipeline.getOpenVINOVersion(), devInfo, pathToCmd) { + tryStartPipeline(pipeline); } -*/ - -/////////////////////////////////////////////// -// Impl section - use this to hide dependencies -/////////////////////////////////////////////// -class Device::Impl { - public: - Impl() = default; - - // Default sink - std::shared_ptr stdoutColorSink = std::make_shared(); - // Device Logger - DeviceLogger logger{"", stdoutColorSink}; - - // RPC - std::mutex rpcMutex; - std::unique_ptr rpcStream; - std::unique_ptr> rpcClient; - - void setLogLevel(LogLevel level); - LogLevel getLogLevel(); - void setPattern(const std::string& pattern); -}; - -void Device::Impl::setPattern(const std::string& pattern) { - logger.set_pattern(pattern); -} - -void Device::Impl::setLogLevel(LogLevel level) { - // Converts LogLevel to spdlog and reconfigures logger level - auto spdlogLevel = logLevelToSpdlogLevel(level, spdlog::level::warn); - // Set level for all configured sinks - logger.set_level(spdlogLevel); -} - -LogLevel Device::Impl::getLogLevel() { - // Converts spdlog to LogLevel - return spdlogLevelToLogLevel(logger.level(), LogLevel::WARN); -} - -/////////////////////////////////////////////// -// END OF Impl section -/////////////////////////////////////////////// - -Device::Device(const Pipeline& pipeline, const DeviceInfo& devInfo, bool usb2Mode) : deviceInfo(devInfo) { - init(pipeline, true, usb2Mode, ""); -} - -Device::Device(const Pipeline& pipeline, const DeviceInfo& devInfo, const char* pathToCmd) : deviceInfo(devInfo) { - init(pipeline, false, false, std::string(pathToCmd)); -} - -Device::Device(const Pipeline& pipeline, const DeviceInfo& devInfo, const std::string& pathToCmd) : deviceInfo(devInfo) { - init(pipeline, false, false, pathToCmd); -} - -Device::Device(const Pipeline& pipeline) { - // Searches for any available device for 'default' timeout - bool found = false; - std::tie(found, deviceInfo) = getAnyAvailableDevice(); +Device::Device() : DeviceBase() {} - // If no device found, throw - if(!found) throw std::runtime_error("No available devices"); - init(pipeline, true, false, ""); -} - -Device::Device(const Pipeline& pipeline, const char* pathToCmd) { - // Searches for any available device for 'default' timeout - - bool found = false; - std::tie(found, deviceInfo) = getAnyAvailableDevice(); - - // If no device found, throw - if(!found) throw std::runtime_error("No available devices"); - init(pipeline, false, false, std::string(pathToCmd)); -} - -Device::Device(const Pipeline& pipeline, const std::string& pathToCmd) { - // Searches for any available device for 'default' timeout - bool found = false; - std::tie(found, deviceInfo) = getAnyAvailableDevice(); - - // If no device found, throw - if(!found) throw std::runtime_error("No available devices"); - init(pipeline, false, false, pathToCmd); -} - -Device::Device(const Pipeline& pipeline, bool usb2Mode) { - // Searches for any available device for 'default' timeout - bool found = false; - std::tie(found, deviceInfo) = getAnyAvailableDevice(); - - // If no device found, throw - if(!found) throw std::runtime_error("No available devices"); - init(pipeline, true, usb2Mode, ""); -} - -Device::Device(OpenVINO::Version version, const DeviceInfo& devInfo, bool usb2Mode) : deviceInfo(devInfo) { - init(version, true, usb2Mode, ""); -} - -Device::Device(OpenVINO::Version version, const DeviceInfo& devInfo, const char* pathToCmd) : deviceInfo(devInfo) { - init(version, false, false, std::string(pathToCmd)); -} - -Device::Device(OpenVINO::Version version, const DeviceInfo& devInfo, const std::string& pathToCmd) : deviceInfo(devInfo) { - init(version, false, false, pathToCmd); -} - -Device::Device(OpenVINO::Version version) { - // Searches for any available device for 'default' timeout - - bool found = false; - std::tie(found, deviceInfo) = getAnyAvailableDevice(); - - // If no device found, throw - if(!found) throw std::runtime_error("No available devices"); - init(version, true, false, ""); -} - -Device::Device(OpenVINO::Version version, const char* pathToCmd) { - // Searches for any available device for 'default' timeout - - bool found = false; - std::tie(found, deviceInfo) = getAnyAvailableDevice(); - - // If no device found, throw - if(!found) throw std::runtime_error("No available devices"); - init(version, false, false, std::string(pathToCmd)); -} - -Device::Device(OpenVINO::Version version, const std::string& pathToCmd) { - // Searches for any available device for 'default' timeout - bool found = false; - std::tie(found, deviceInfo) = getAnyAvailableDevice(); - - // If no device found, throw - if(!found) throw std::runtime_error("No available devices"); - init(version, false, false, pathToCmd); -} - -Device::Device(OpenVINO::Version version, bool usb2Mode) { - // Searches for any available device for 'default' timeout - bool found = false; - std::tie(found, deviceInfo) = getAnyAvailableDevice(); - - // If no device found, throw - if(!found) throw std::runtime_error("No available devices"); - init(version, true, usb2Mode, ""); +Device::~Device() { + DeviceBase::close(); } -void Device::close() { - // Only allow to close once - if(closed.exchange(true)) return; - - using namespace std::chrono; - auto t1 = steady_clock::now(); - spdlog::debug("Device about to be closed..."); - +void Device::closeImpl() { // Remove callbacks to this from queues for(const auto& kv : callbackIdMap) { outputQueueMap[kv.first]->removeCallback(kv.second); @@ -342,329 +71,14 @@ void Device::close() { // Clear map callbackIdMap.clear(); - // Close connection first (so queues unblock) - connection->close(); - connection = nullptr; + // Close the device before clearing the queues + DeviceBase::closeImpl(); // Close and clear queues for(auto& kv : outputQueueMap) kv.second->close(); for(auto& kv : inputQueueMap) kv.second->close(); outputQueueMap.clear(); inputQueueMap.clear(); - - // Stop watchdog - watchdogRunning = false; - timesyncRunning = false; - loggingRunning = false; - - // Stop watchdog first (this resets and waits for link to fall down) - if(watchdogThread.joinable()) watchdogThread.join(); - // Then stop timesync - if(timesyncThread.joinable()) timesyncThread.join(); - // And at the end stop logging thread - if(loggingThread.joinable()) loggingThread.join(); - - // Close rpcStream - pimpl->rpcStream = nullptr; - - spdlog::debug("Device closed, {}", duration_cast(steady_clock::now() - t1).count()); -} - -bool Device::isClosed() const { - return closed || !watchdogRunning; -} - -void Device::checkClosed() const { - if(isClosed()) throw std::invalid_argument("Device already closed or disconnected"); -} - -Device::~Device() { - close(); -} - -void Device::init(OpenVINO::Version version, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd) { - // Initalize depthai library if not already - initialize(); - - // Specify the OpenVINO version - openvinoVersion = version; - - spdlog::debug("Device - OpenVINO version: {}", OpenVINO::getVersionName(openvinoVersion)); - - init2(embeddedMvcmd, usb2Mode, pathToMvcmd, tl::nullopt); -} - -void Device::init(const Pipeline& pipeline, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd) { - // Initalize depthai library if not already - initialize(); - - // Mark the OpenVINO version - openvinoVersion = pipeline.getOpenVINOVersion(); - - spdlog::debug("Device - pipeline serialized, OpenVINO version: {}", OpenVINO::getVersionName(openvinoVersion)); - - init2(embeddedMvcmd, usb2Mode, pathToMvcmd, pipeline); -} - -void Device::init2(bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd, tl::optional pipeline) { - // Set logging pattern of device (device id + shared pattern) - pimpl->setPattern(fmt::format("[{}] {}", deviceInfo.getMxId(), LOG_DEFAULT_PATTERN)); - - // Get embedded mvcmd - std::vector embeddedFw = Resources::getInstance().getDeviceFirmware(usb2Mode, openvinoVersion); - - // Init device (if bootloader, handle correctly - issue USB boot command) - if(deviceInfo.state == X_LINK_UNBOOTED) { - // Unbooted device found, boot and connect with XLinkConnection constructor - if(embeddedMvcmd) { - connection = std::make_shared(deviceInfo, embeddedFw); - } else { - connection = std::make_shared(deviceInfo, pathToMvcmd); - } - - } else if(deviceInfo.state == X_LINK_BOOTLOADER) { - // Scope so bootloaderConnection is desctructed and XLink cleans its state - { - // TODO(themarpe) - move this logic to DeviceBootloader - - // Bootloader state, proceed by issuing a command to bootloader - XLinkConnection bootloaderConnection(deviceInfo, X_LINK_BOOTLOADER); - - // Open stream - XLinkStream stream(bootloaderConnection, bootloader::XLINK_CHANNEL_BOOTLOADER, bootloader::XLINK_STREAM_MAX_SIZE); - - // prepare watchdog thread, which will keep device alive - std::atomic watchdogRunning{true}; - watchdogThread = std::thread([&]() { - // prepare watchdog thread - XLinkStream stream(bootloaderConnection, bootloader::XLINK_CHANNEL_WATCHDOG, 64); - std::vector watchdogKeepalive = {0, 0, 0, 0}; - while(watchdogRunning) { - try { - stream.write(watchdogKeepalive); - } catch(const std::exception&) { - break; - } - // Ping with a period half of that of the watchdog timeout - std::this_thread::sleep_for(bootloader::XLINK_WATCHDOG_TIMEOUT / 2); - } - }); - - // Send request for bootloader version - if(!sendBootloaderRequest(stream.getStreamId(), bootloader::request::GetBootloaderVersion{})) { - throw std::runtime_error("Error trying to connect to device"); - } - // Receive response - bootloader::response::BootloaderVersion ver; - if(!receiveBootloaderResponse(stream.getStreamId(), ver)) throw std::runtime_error("Error trying to connect to device"); - DeviceBootloader::Version version(ver.major, ver.minor, ver.patch); - - // If version is >= 0.0.12 then boot directly, otherwise jump to USB ROM bootloader - // Check if version is recent enough for this operation - if(version >= DeviceBootloader::Version(0, 0, 12)) { - // Send request to boot firmware directly from bootloader - dai::bootloader::request::BootMemory bootMemory; - bootMemory.totalSize = static_cast(embeddedFw.size()); - bootMemory.numPackets = ((static_cast(embeddedFw.size()) - 1) / bootloader::XLINK_STREAM_MAX_SIZE) + 1; - if(!sendBootloaderRequest(stream.getStreamId(), bootMemory)) { - throw std::runtime_error("Error trying to connect to device"); - } - - using namespace std::chrono; - auto t1 = steady_clock::now(); - // After that send numPackets of data - stream.writeSplit(embeddedFw.data(), embeddedFw.size(), bootloader::XLINK_STREAM_MAX_SIZE); - spdlog::debug( - "Booting FW with Bootloader. Version {}, Time taken: {}", version.toString(), duration_cast(steady_clock::now() - t1)); - - // After that the state will be BOOTED - deviceInfo.state = X_LINK_BOOTED; - - // TODO(themarpe) - Possibility of switching to another port for cleaner transitions - // strcat(deviceInfo.desc.name, ":11492"); - - } else { - spdlog::debug("Booting FW by jumping to USB ROM Bootloader first. Bootloader Version {}", version.toString()); - - // Send request to jump to USB bootloader - // Boot into USB ROM BOOTLOADER NOW - if(!sendBootloaderRequest(stream.getStreamId(), dai::bootloader::request::UsbRomBoot{})) { - throw std::runtime_error("Error trying to connect to device"); - } - - // After that the state will be UNBOOTED - deviceInfo.state = X_LINK_UNBOOTED; - } - - watchdogRunning = false; - watchdogThread.join(); - - // Dummy read, until link falls down and it returns an error code - streamPacketDesc_t* pPacket; - XLinkReadData(stream.getStreamId(), &pPacket); - } - - // Boot and connect with XLinkConnection constructor - if(embeddedMvcmd) { - connection = std::make_shared(deviceInfo, embeddedFw); - } else { - connection = std::make_shared(deviceInfo, pathToMvcmd); - } - - } else if(deviceInfo.state == X_LINK_BOOTED) { - // Connect without booting - if(embeddedMvcmd) { - connection = std::make_shared(deviceInfo, embeddedFw); - } else { - connection = std::make_shared(deviceInfo, pathToMvcmd); - } - } else { - throw std::runtime_error("Cannot find any device with given deviceInfo"); - } - - deviceInfo.state = X_LINK_BOOTED; - - // prepare rpc for both attached and host controlled mode - pimpl->rpcStream = std::make_unique(*connection, dai::XLINK_CHANNEL_MAIN_RPC, dai::XLINK_USB_BUFFER_MAX_SIZE); - - pimpl->rpcClient = std::make_unique>([this](nanorpc::core::type::buffer request) { - // Lock for time of the RPC call, to not mix the responses between calling threads. - // Note: might cause issues on Windows on incorrect shutdown. To be investigated - std::unique_lock lock(pimpl->rpcMutex); - - // Log the request data - if(spdlog::get_level() == spdlog::level::trace) { - spdlog::trace("RPC: {}", nlohmann::json::from_msgpack(request).dump()); - } - - // Send request to device - pimpl->rpcStream->write(std::move(request)); - - // Receive response back - // Send to nanorpc to parse - return pimpl->rpcStream->read(); - }); - - // prepare watchdog thread, which will keep device alive - watchdogThread = std::thread([this]() { - std::shared_ptr conn = this->connection; - while(watchdogRunning) { - try { - pimpl->rpcClient->call("watchdogKeepalive"); - } catch(const std::exception&) { - break; - } - // Ping with a period half of that of the watchdog timeout - std::this_thread::sleep_for(XLINK_WATCHDOG_TIMEOUT / 2); - } - - // Watchdog ended. Useful for checking disconnects - watchdogRunning = false; - }); - - // prepare timesync thread, which will keep device synchronized - timesyncThread = std::thread([this]() { - using namespace std::chrono; - - try { - XLinkStream stream(*this->connection, XLINK_CHANNEL_TIMESYNC, 128); - Timestamp timestamp = {}; - while(timesyncRunning) { - // Block - stream.read(); - - // Timestamp - auto d = std::chrono::steady_clock::now().time_since_epoch(); - timestamp.sec = duration_cast(d).count(); - timestamp.nsec = duration_cast(d).count() % 1000000000; - - // Write timestamp back - stream.write(×tamp, sizeof(timestamp)); - } - } catch(const std::exception& ex) { - // ignore - spdlog::debug("Timesync thread exception caught: {}", ex.what()); - } - - timesyncRunning = false; - }); - - // prepare logging thread, which will log device messages - loggingThread = std::thread([this]() { - using namespace std::chrono; - std::vector messages; - try { - XLinkStream stream(*this->connection, XLINK_CHANNEL_LOG, 128); - while(loggingRunning) { - // Block - auto log = stream.read(); - - // parse packet as msgpack - try { - auto j = nlohmann::json::from_msgpack(log); - // create pipeline schema from retrieved data - nlohmann::from_json(j, messages); - - spdlog::trace("Log vector decoded, size: {}", messages.size()); - - // log the messages in incremental order (0 -> size-1) - for(const auto& msg : messages) { - pimpl->logger.logMessage(msg); - } - - // Log to callbacks - { - // lock mtx to callback map (shared) - std::unique_lock l(logCallbackMapMtx); - for(const auto& msg : messages) { - for(const auto& kv : logCallbackMap) { - const auto& cb = kv.second; - // If available, callback with msg - if(cb) cb(msg); - } - } - } - - } catch(const nlohmann::json::exception& ex) { - spdlog::error("Exception while parsing or calling callbacks for log message from device: {}", ex.what()); - } - } - } catch(const std::exception& ex) { - // ignore exception from logging - spdlog::debug("Log thread exception caught: {}", ex.what()); - } - - loggingRunning = false; - }); - - // Below can throw - make sure to gracefully exit threads - try { - // Set logging level (if DEPTHAI_LEVEL lower than warning, then device is configured accordingly as well) - if(spdlog::get_level() < spdlog::level::warn) { - auto level = spdlogLevelToLogLevel(spdlog::get_level()); - setLogLevel(level); - setLogOutputLevel(level); - } else { - setLogLevel(LogLevel::WARN); - setLogOutputLevel(LogLevel::WARN); - } - - // Sets system inforation logging rate. By default 1s - setSystemInformationLoggingRate(DEFAULT_SYSTEM_INFORMATION_LOGGING_RATE_HZ); - - // Starts pipeline if given - if(pipeline) { - if(!startPipeline(*pipeline)) { - throw std::runtime_error("Couldn't start the pipeline"); - } - } - - } catch(const std::exception&) { - // close device (cleanup) - close(); - // Rethrow original exception - throw; - } } std::shared_ptr Device::getOutputQueue(const std::string& name) { @@ -850,179 +264,7 @@ std::string Device::getQueueEvent(std::chrono::microseconds timeout) { return getQueueEvent(getOutputQueueNames(), timeout); } -std::string Device::getMxId() { - checkClosed(); - - return pimpl->rpcClient->call("getMxId").as(); -} - -std::vector Device::getConnectedCameras() { - checkClosed(); - - return pimpl->rpcClient->call("getConnectedCameras").as>(); -} - -// Convinience functions for querying current system information -MemoryInfo Device::getDdrMemoryUsage() { - checkClosed(); - - return pimpl->rpcClient->call("getDdrUsage").as(); -} - -MemoryInfo Device::getCmxMemoryUsage() { - checkClosed(); - - return pimpl->rpcClient->call("getCmxUsage").as(); -} - -MemoryInfo Device::getLeonCssHeapUsage() { - checkClosed(); - - return pimpl->rpcClient->call("getLeonCssHeapUsage").as(); -} - -MemoryInfo Device::getLeonMssHeapUsage() { - checkClosed(); - - return pimpl->rpcClient->call("getLeonMssHeapUsage").as(); -} - -ChipTemperature Device::getChipTemperature() { - checkClosed(); - - return pimpl->rpcClient->call("getChipTemperature").as(); -} - -CpuUsage Device::getLeonCssCpuUsage() { - checkClosed(); - - return pimpl->rpcClient->call("getLeonCssCpuUsage").as(); -} - -CpuUsage Device::getLeonMssCpuUsage() { - checkClosed(); - - return pimpl->rpcClient->call("getLeonMssCpuUsage").as(); -} - -UsbSpeed Device::getUsbSpeed() { - checkClosed(); - - return pimpl->rpcClient->call("getUsbSpeed").as(); -} - -bool Device::isPipelineRunning() { - checkClosed(); - - return pimpl->rpcClient->call("isPipelineRunning").as(); -} - -void Device::setLogLevel(LogLevel level) { - checkClosed(); - - pimpl->rpcClient->call("setLogLevel", level); -} - -LogLevel Device::getLogLevel() { - checkClosed(); - - return pimpl->rpcClient->call("getLogLevel").as(); -} - -DeviceInfo Device::getDeviceInfo() { - return deviceInfo; -} - -void Device::setLogOutputLevel(LogLevel level) { - checkClosed(); - - pimpl->setLogLevel(level); -} - -LogLevel Device::getLogOutputLevel() { - checkClosed(); - - return pimpl->getLogLevel(); -} - -int Device::addLogCallback(std::function callback) { - checkClosed(); - - // Lock first - std::unique_lock l(logCallbackMapMtx); - - // Get unique id - int id = uniqueCallbackId++; - - // assign callback - logCallbackMap[id] = callback; - - // return id assigned to the callback - return id; -} - -bool Device::removeLogCallback(int callbackId) { - checkClosed(); - - // Lock first - std::unique_lock l(logCallbackMapMtx); - - // If callback with id 'callbackId' doesn't exists, return false - if(logCallbackMap.count(callbackId) == 0) return false; - - // Otherwise erase and return true - logCallbackMap.erase(callbackId); - return true; -} - -void Device::setSystemInformationLoggingRate(float rateHz) { - checkClosed(); - - pimpl->rpcClient->call("setSystemInformationLoggingRate", rateHz); -} - -float Device::getSystemInformationLoggingRate() { - checkClosed(); - - return pimpl->rpcClient->call("getSystemInformationLoggingRate").as(); -} - -bool Device::flashCalibration(CalibrationHandler calibrationDataHandler) { - if(!calibrationDataHandler.validateCameraArray()) { - throw std::runtime_error("Failed to validate the extrinsics connection. Enable debug mode for more information."); - } - return pimpl->rpcClient->call("storeToEeprom", calibrationDataHandler.getEepromData()).as(); -} - -CalibrationHandler Device::readCalibration() { - dai::EepromData eepromData = pimpl->rpcClient->call("readFromEeprom"); - return CalibrationHandler(eepromData); -} - -bool Device::startPipeline() { - // Deprecated - return true; -} - -bool Device::startPipeline(const Pipeline& pipeline) { - checkClosed(); - - // first check if pipeline is not already running - if(isPipelineRunning()) { - throw std::runtime_error("Pipeline is already running"); - } - - PipelineSchema schema; - Assets assets; - std::vector assetStorage; - - // Mark the OpenVINO version and serialize the pipeline - OpenVINO::Version pipelineOpenvinoVersion; - pipeline.serialize(schema, assets, assetStorage, pipelineOpenvinoVersion); - if(openvinoVersion != pipelineOpenvinoVersion) { - throw std::runtime_error("Device booted with different OpenVINO version that pipeline requires"); - } - +bool Device::startPipelineImpl(const Pipeline& pipeline) { // Open queues upfront, let queues know about data sizes (input queues) // Go through Pipeline and check for 'XLinkIn' and 'XLinkOut' nodes // and create corresponding default queues for them @@ -1032,6 +274,7 @@ bool Device::startPipeline(const Pipeline& pipeline) { if(xlinkIn == nullptr) { continue; } + // Create DataInputQueue's inputQueueMap[xlinkIn->getStreamName()] = std::make_shared(connection, xlinkIn->getStreamName()); // set max data size, for more verbosity @@ -1066,60 +309,7 @@ bool Device::startPipeline(const Pipeline& pipeline) { eventCv.notify_all(); }); } - - // if debug - if(spdlog::get_level() == spdlog::level::debug) { - nlohmann::json jSchema = schema; - spdlog::debug("Schema dump: {}", jSchema.dump()); - nlohmann::json jAssets = assets; - spdlog::debug("Asset map dump: {}", jAssets.dump()); - } - - // Load pipelineDesc, assets, and asset storage - pimpl->rpcClient->call("setPipelineSchema", schema); - - // Transfer storage != empty - if(!assetStorage.empty()) { - pimpl->rpcClient->call("setAssets", assets); - - // allocate, returns a pointer to memory on device side - auto memHandle = pimpl->rpcClient->call("memAlloc", static_cast(assetStorage.size())).as(); - - // Transfer the whole assetStorage in a separate thread - const std::string streamAssetStorage = "__stream_asset_storage"; - std::thread t1([this, &streamAssetStorage, &assetStorage]() { - XLinkStream stream(*connection, streamAssetStorage, XLINK_USB_BUFFER_MAX_SIZE); - int64_t offset = 0; - do { - int64_t toTransfer = std::min(static_cast(XLINK_USB_BUFFER_MAX_SIZE), static_cast(assetStorage.size() - offset)); - stream.write(&assetStorage[offset], toTransfer); - offset += toTransfer; - } while(offset < static_cast(assetStorage.size())); - }); - - // Open a channel to transfer AssetStorage - pimpl->rpcClient->call("readFromXLink", streamAssetStorage, memHandle, assetStorage.size()); - t1.join(); - - // After asset storage is transfers, set the asset storage - pimpl->rpcClient->call("setAssetStorage", memHandle, assetStorage.size()); - } - - // print assets on device side for test - pimpl->rpcClient->call("printAssets"); - - // Build and start the pipeline - bool success = false; - std::string errorMsg; - std::tie(success, errorMsg) = pimpl->rpcClient->call("buildPipeline").as>(); - if(success) { - pimpl->rpcClient->call("startPipeline"); - } else { - throw std::runtime_error(errorMsg); - return false; - } - - return true; + return DeviceBase::startPipelineImpl(pipeline); } } // namespace dai diff --git a/src/device/DeviceBase.cpp b/src/device/DeviceBase.cpp new file mode 100644 index 000000000..695ac40e3 --- /dev/null +++ b/src/device/DeviceBase.cpp @@ -0,0 +1,887 @@ +#include "depthai/device/DeviceBase.hpp" + +// std +#include + +// shared +#include "depthai-bootloader-shared/Bootloader.hpp" +#include "depthai-bootloader-shared/XLinkConstants.hpp" +#include "depthai-shared/datatype/RawImgFrame.hpp" +#include "depthai-shared/log/LogConstants.hpp" +#include "depthai-shared/log/LogLevel.hpp" +#include "depthai-shared/log/LogMessage.hpp" +#include "depthai-shared/pipeline/Assets.hpp" +#include "depthai-shared/xlink/XLinkConstants.hpp" + +// project +#include "DeviceLogger.hpp" +#include "depthai/pipeline/node/XLinkIn.hpp" +#include "depthai/pipeline/node/XLinkOut.hpp" +#include "pipeline/Pipeline.hpp" +#include "utility/BootloaderHelper.hpp" +#include "utility/Initialization.hpp" +#include "utility/PimplImpl.hpp" +#include "utility/Resources.hpp" + +// libraries +#include "nanorpc/core/client.h" +#include "nanorpc/packer/nlohmann_msgpack.h" +#include "spdlog/fmt/chrono.h" +#include "spdlog/sinks/stdout_color_sinks.h" +#include "spdlog/spdlog.h" + +namespace dai { + +// local static function +static LogLevel spdlogLevelToLogLevel(spdlog::level::level_enum level, LogLevel defaultValue = LogLevel::OFF) { + switch(level) { + case spdlog::level::trace: + return LogLevel::TRACE; + case spdlog::level::debug: + return LogLevel::DEBUG; + case spdlog::level::info: + return LogLevel::INFO; + case spdlog::level::warn: + return LogLevel::WARN; + case spdlog::level::err: + return LogLevel::ERR; + case spdlog::level::critical: + return LogLevel::CRITICAL; + case spdlog::level::off: + return LogLevel::OFF; + // Default + case spdlog::level::n_levels: + default: + return defaultValue; + break; + } + // Default + return defaultValue; +} +static spdlog::level::level_enum logLevelToSpdlogLevel(LogLevel level, spdlog::level::level_enum defaultValue = spdlog::level::off) { + switch(level) { + case LogLevel::TRACE: + return spdlog::level::trace; + case LogLevel::DEBUG: + return spdlog::level::debug; + case LogLevel::INFO: + return spdlog::level::info; + case LogLevel::WARN: + return spdlog::level::warn; + case LogLevel::ERR: + return spdlog::level::err; + case LogLevel::CRITICAL: + return spdlog::level::critical; + case LogLevel::OFF: + return spdlog::level::off; + } + // Default + return defaultValue; +} + +// Common explicit instantiation, to remove the need to define in header +template std::tuple DeviceBase::getAnyAvailableDevice(std::chrono::nanoseconds); +template std::tuple DeviceBase::getAnyAvailableDevice(std::chrono::microseconds); +template std::tuple DeviceBase::getAnyAvailableDevice(std::chrono::milliseconds); +template std::tuple DeviceBase::getAnyAvailableDevice(std::chrono::seconds); +constexpr std::chrono::seconds DeviceBase::DEFAULT_SEARCH_TIME; +constexpr float DeviceBase::DEFAULT_SYSTEM_INFORMATION_LOGGING_RATE_HZ; + +template +std::tuple DeviceBase::getAnyAvailableDevice(std::chrono::duration timeout) { + using namespace std::chrono; + constexpr auto POOL_SLEEP_TIME = milliseconds(100); + + // First looks for UNBOOTED, then BOOTLOADER, for 'timeout' time + auto searchStartTime = steady_clock::now(); + bool found = false; + DeviceInfo deviceInfo; + do { + for(auto searchState : {X_LINK_UNBOOTED, X_LINK_BOOTLOADER}) { + std::tie(found, deviceInfo) = XLinkConnection::getFirstDevice(searchState); + if(found) break; + } + if(found) break; + + // If 'timeout' < 'POOL_SLEEP_TIME', use 'timeout' as sleep time and then break + if(timeout < POOL_SLEEP_TIME) { + // sleep for 'timeout' + std::this_thread::sleep_for(timeout); + break; + } else { + std::this_thread::sleep_for(POOL_SLEEP_TIME); // default pool rate + } + } while(steady_clock::now() - searchStartTime < timeout); + + // If none were found, try BOOTED + if(!found) std::tie(found, deviceInfo) = XLinkConnection::getFirstDevice(X_LINK_BOOTED); + + return {found, deviceInfo}; +} + +// Default overload ('DEFAULT_SEARCH_TIME' timeout) +std::tuple DeviceBase::getAnyAvailableDevice() { + return getAnyAvailableDevice(DEFAULT_SEARCH_TIME); +} + +// static api + +// First tries to find UNBOOTED device, then BOOTLOADER device +std::tuple DeviceBase::getFirstAvailableDevice() { + bool found; + DeviceInfo dev; + std::tie(found, dev) = XLinkConnection::getFirstDevice(X_LINK_UNBOOTED); + if(!found) { + std::tie(found, dev) = XLinkConnection::getFirstDevice(X_LINK_BOOTLOADER); + } + return {found, dev}; +} + +// Returns all devices which aren't already booted +std::vector DeviceBase::getAllAvailableDevices() { + std::vector availableDevices; + auto connectedDevices = XLinkConnection::getAllConnectedDevices(); + for(const auto& d : connectedDevices) { + if(d.state != X_LINK_BOOTED) availableDevices.push_back(d); + } + return availableDevices; +} + +// First tries to find UNBOOTED device with mxId, then BOOTLOADER device with mxId +std::tuple DeviceBase::getDeviceByMxId(std::string mxId) { + std::vector availableDevices; + auto states = {X_LINK_UNBOOTED, X_LINK_BOOTLOADER}; + bool found; + DeviceInfo dev; + for(const auto& state : states) { + std::tie(found, dev) = XLinkConnection::getDeviceByMxId(mxId, state); + if(found) return {true, dev}; + } + return {false, DeviceInfo()}; +} + +std::vector DeviceBase::getEmbeddedDeviceBinary(bool usb2Mode, OpenVINO::Version version) { + return Resources::getInstance().getDeviceFirmware(usb2Mode, version); +} + +/* +std::vector DeviceBase::getAllConnectedDevices(){ + return XLinkConnection::getAllConnectedDevices(); +} + + +std::tuple DeviceBase::getFirstDevice(){ + return XLinkConnection::getFirstAvailableDevice(); +} +*/ + +/////////////////////////////////////////////// +// Impl section - use this to hide dependencies +/////////////////////////////////////////////// +class DeviceBase::Impl { + public: + Impl() = default; + + // Default sink + std::shared_ptr stdoutColorSink = std::make_shared(); + // Device Logger + DeviceLogger logger{"", stdoutColorSink}; + + // RPC + std::mutex rpcMutex; + std::unique_ptr rpcStream; + std::unique_ptr> rpcClient; + + void setLogLevel(LogLevel level); + LogLevel getLogLevel(); + void setPattern(const std::string& pattern); +}; + +void DeviceBase::Impl::setPattern(const std::string& pattern) { + logger.set_pattern(pattern); +} + +void DeviceBase::Impl::setLogLevel(LogLevel level) { + // Converts LogLevel to spdlog and reconfigures logger level + auto spdlogLevel = logLevelToSpdlogLevel(level, spdlog::level::warn); + // Set level for all configured sinks + logger.set_level(spdlogLevel); +} + +LogLevel DeviceBase::Impl::getLogLevel() { + // Converts spdlog to LogLevel + return spdlogLevelToLogLevel(logger.level(), LogLevel::WARN); +} + +/////////////////////////////////////////////// +// END OF Impl section +/////////////////////////////////////////////// + +DeviceBase::DeviceBase(OpenVINO::Version version, const DeviceInfo& devInfo) : DeviceBase(version, devInfo, false) {} + +DeviceBase::DeviceBase(OpenVINO::Version version, const DeviceInfo& devInfo, bool usb2Mode) : deviceInfo(devInfo) { + init(version, true, usb2Mode, ""); +} + +DeviceBase::DeviceBase(OpenVINO::Version version, const DeviceInfo& devInfo, const char* pathToCmd) : deviceInfo(devInfo) { + init(version, false, false, std::string(pathToCmd)); +} + +DeviceBase::DeviceBase(OpenVINO::Version version, const DeviceInfo& devInfo, const std::string& pathToCmd) : deviceInfo(devInfo) { + init(version, false, false, pathToCmd); +} + +DeviceBase::DeviceBase() : DeviceBase(Pipeline::DEFAULT_OPENVINO_VERSION) {} + +DeviceBase::DeviceBase(OpenVINO::Version version) { + // Searches for any available device for 'default' timeout + bool found = false; + std::tie(found, deviceInfo) = getAnyAvailableDevice(); + + // If no device found, throw + if(!found) throw std::runtime_error("No available devices"); + init(version, true, false, ""); +} + +DeviceBase::DeviceBase(OpenVINO::Version version, const char* pathToCmd) { + // Searches for any available device for 'default' timeout + + bool found = false; + std::tie(found, deviceInfo) = getAnyAvailableDevice(); + + // If no device found, throw + if(!found) throw std::runtime_error("No available devices"); + init(version, false, false, std::string(pathToCmd)); +} + +DeviceBase::DeviceBase(OpenVINO::Version version, const std::string& pathToCmd) { + // Searches for any available device for 'default' timeout + bool found = false; + std::tie(found, deviceInfo) = getAnyAvailableDevice(); + + // If no device found, throw + if(!found) throw std::runtime_error("No available devices"); + init(version, false, false, pathToCmd); +} + +DeviceBase::DeviceBase(OpenVINO::Version version, bool usb2Mode) { + // Searches for any available device for 'default' timeout + bool found = false; + std::tie(found, deviceInfo) = getAnyAvailableDevice(); + + // If no device found, throw + if(!found) throw std::runtime_error("No available devices"); + init(version, true, usb2Mode, ""); +} + +DeviceBase::DeviceBase(const Pipeline& pipeline) : DeviceBase(pipeline.getOpenVINOVersion()) { + tryStartPipeline(pipeline); +} + +DeviceBase::DeviceBase(const Pipeline& pipeline, bool usb2Mode) : DeviceBase(pipeline.getOpenVINOVersion(), usb2Mode) { + tryStartPipeline(pipeline); +} + +DeviceBase::DeviceBase(const Pipeline& pipeline, const char* pathToCmd) : DeviceBase(pipeline.getOpenVINOVersion(), pathToCmd) { + tryStartPipeline(pipeline); +} + +DeviceBase::DeviceBase(const Pipeline& pipeline, const std::string& pathToCmd) : DeviceBase(pipeline.getOpenVINOVersion(), pathToCmd) { + tryStartPipeline(pipeline); +} + +DeviceBase::DeviceBase(const Pipeline& pipeline, const DeviceInfo& devInfo) : DeviceBase(pipeline.getOpenVINOVersion(), devInfo, false) {} + +DeviceBase::DeviceBase(const Pipeline& pipeline, const DeviceInfo& devInfo, bool usb2Mode) : DeviceBase(pipeline.getOpenVINOVersion(), devInfo, usb2Mode) { + tryStartPipeline(pipeline); +} + +DeviceBase::DeviceBase(const Pipeline& pipeline, const DeviceInfo& devInfo, const char* pathToCmd) + : DeviceBase(pipeline.getOpenVINOVersion(), devInfo, pathToCmd) { + tryStartPipeline(pipeline); +} + +DeviceBase::DeviceBase(const Pipeline& pipeline, const DeviceInfo& devInfo, const std::string& pathToCmd) + : DeviceBase(pipeline.getOpenVINOVersion(), devInfo, pathToCmd) { + tryStartPipeline(pipeline); +} + +void DeviceBase::close() { + // Only allow to close once + if(closed.exchange(true)) return; + + closeImpl(); +} + +void DeviceBase::closeImpl() { + using namespace std::chrono; + auto t1 = steady_clock::now(); + spdlog::debug("Device about to be closed..."); + + // Close connection first (so queues unblock) + connection->close(); + connection = nullptr; + + // Stop watchdog + watchdogRunning = false; + timesyncRunning = false; + loggingRunning = false; + + // Stop watchdog first (this resets and waits for link to fall down) + if(watchdogThread.joinable()) watchdogThread.join(); + // Then stop timesync + if(timesyncThread.joinable()) timesyncThread.join(); + // And at the end stop logging thread + if(loggingThread.joinable()) loggingThread.join(); + + // Close rpcStream + pimpl->rpcStream = nullptr; + + spdlog::debug("Device closed, {}", duration_cast(steady_clock::now() - t1).count()); +} + +bool DeviceBase::isClosed() const { + return closed || !watchdogRunning; +} + +void DeviceBase::checkClosed() const { + if(isClosed()) throw std::invalid_argument("Device already closed or disconnected"); +} + +DeviceBase::~DeviceBase() { + // Only allow to close once + if(closed.exchange(true)) return; + + DeviceBase::closeImpl(); +} + +void DeviceBase::tryStartPipeline(const Pipeline& pipeline) { + try { + if(!startPipeline(pipeline)) { + throw std::runtime_error("Couldn't start the pipeline"); + } + } catch(const std::exception& e) { + close(); + throw e; + } +} + +void DeviceBase::init(OpenVINO::Version version, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd) { + // Initalize depthai library if not already + initialize(); + + // Specify the OpenVINO version + openvinoVersion = version; + + spdlog::debug("Device - OpenVINO version: {}", OpenVINO::getVersionName(openvinoVersion)); + + init2(embeddedMvcmd, usb2Mode, pathToMvcmd); +} + +void DeviceBase::init(const Pipeline& pipeline, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd) { + // Initalize depthai library if not already + initialize(); + + // Mark the OpenVINO version + openvinoVersion = pipeline.getOpenVINOVersion(); + + spdlog::debug("Device - pipeline serialized, OpenVINO version: {}", OpenVINO::getVersionName(openvinoVersion)); + + init2(embeddedMvcmd, usb2Mode, pathToMvcmd); +} + +void DeviceBase::init2(bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd) { + // Set logging pattern of device (device id + shared pattern) + pimpl->setPattern(fmt::format("[{}] {}", deviceInfo.getMxId(), LOG_DEFAULT_PATTERN)); + + // Get embedded mvcmd + std::vector embeddedFw = Resources::getInstance().getDeviceFirmware(usb2Mode, openvinoVersion); + + // Init device (if bootloader, handle correctly - issue USB boot command) + if(deviceInfo.state == X_LINK_UNBOOTED) { + // Unbooted device found, boot and connect with XLinkConnection constructor + if(embeddedMvcmd) { + connection = std::make_shared(deviceInfo, embeddedFw); + } else { + connection = std::make_shared(deviceInfo, pathToMvcmd); + } + + } else if(deviceInfo.state == X_LINK_BOOTLOADER) { + // Scope so bootloaderConnection is desctructed and XLink cleans its state + { + // TODO(themarpe) - move this logic to DeviceBootloader + + // Bootloader state, proceed by issuing a command to bootloader + XLinkConnection bootloaderConnection(deviceInfo, X_LINK_BOOTLOADER); + + // Open stream + XLinkStream stream(bootloaderConnection, bootloader::XLINK_CHANNEL_BOOTLOADER, bootloader::XLINK_STREAM_MAX_SIZE); + + // prepare watchdog thread, which will keep device alive + std::atomic watchdogRunning{true}; + watchdogThread = std::thread([&]() { + // prepare watchdog thread + XLinkStream stream(bootloaderConnection, bootloader::XLINK_CHANNEL_WATCHDOG, 64); + std::vector watchdogKeepalive = {0, 0, 0, 0}; + while(watchdogRunning) { + try { + stream.write(watchdogKeepalive); + } catch(const std::exception&) { + break; + } + // Ping with a period half of that of the watchdog timeout + std::this_thread::sleep_for(bootloader::XLINK_WATCHDOG_TIMEOUT / 2); + } + }); + + // Send request for bootloader version + if(!sendBootloaderRequest(stream.getStreamId(), bootloader::request::GetBootloaderVersion{})) { + throw std::runtime_error("Error trying to connect to device"); + } + // Receive response + bootloader::response::BootloaderVersion ver; + if(!receiveBootloaderResponse(stream.getStreamId(), ver)) throw std::runtime_error("Error trying to connect to device"); + DeviceBootloader::Version version(ver.major, ver.minor, ver.patch); + + // If version is >= 0.0.12 then boot directly, otherwise jump to USB ROM bootloader + // Check if version is recent enough for this operation + if(version >= DeviceBootloader::Version(0, 0, 12)) { + // Send request to boot firmware directly from bootloader + dai::bootloader::request::BootMemory bootMemory; + bootMemory.totalSize = static_cast(embeddedFw.size()); + bootMemory.numPackets = ((static_cast(embeddedFw.size()) - 1) / bootloader::XLINK_STREAM_MAX_SIZE) + 1; + if(!sendBootloaderRequest(stream.getStreamId(), bootMemory)) { + throw std::runtime_error("Error trying to connect to device"); + } + + using namespace std::chrono; + auto t1 = steady_clock::now(); + // After that send numPackets of data + stream.writeSplit(embeddedFw.data(), embeddedFw.size(), bootloader::XLINK_STREAM_MAX_SIZE); + spdlog::debug( + "Booting FW with Bootloader. Version {}, Time taken: {}", version.toString(), duration_cast(steady_clock::now() - t1)); + + // After that the state will be BOOTED + deviceInfo.state = X_LINK_BOOTED; + + // TODO(themarpe) - Possibility of switching to another port for cleaner transitions + // strcat(deviceInfo.desc.name, ":11492"); + + } else { + spdlog::debug("Booting FW by jumping to USB ROM Bootloader first. Bootloader Version {}", version.toString()); + + // Send request to jump to USB bootloader + // Boot into USB ROM BOOTLOADER NOW + if(!sendBootloaderRequest(stream.getStreamId(), dai::bootloader::request::UsbRomBoot{})) { + throw std::runtime_error("Error trying to connect to device"); + } + + // After that the state will be UNBOOTED + deviceInfo.state = X_LINK_UNBOOTED; + } + + watchdogRunning = false; + watchdogThread.join(); + + // Dummy read, until link falls down and it returns an error code + streamPacketDesc_t* pPacket; + XLinkReadData(stream.getStreamId(), &pPacket); + } + + // Boot and connect with XLinkConnection constructor + if(embeddedMvcmd) { + connection = std::make_shared(deviceInfo, embeddedFw); + } else { + connection = std::make_shared(deviceInfo, pathToMvcmd); + } + + } else if(deviceInfo.state == X_LINK_BOOTED) { + // Connect without booting + if(embeddedMvcmd) { + connection = std::make_shared(deviceInfo, embeddedFw); + } else { + connection = std::make_shared(deviceInfo, pathToMvcmd); + } + } else { + throw std::runtime_error("Cannot find any device with given deviceInfo"); + } + + deviceInfo.state = X_LINK_BOOTED; + + // prepare rpc for both attached and host controlled mode + pimpl->rpcStream = std::make_unique(*connection, dai::XLINK_CHANNEL_MAIN_RPC, dai::XLINK_USB_BUFFER_MAX_SIZE); + + pimpl->rpcClient = std::make_unique>([this](nanorpc::core::type::buffer request) { + // Lock for time of the RPC call, to not mix the responses between calling threads. + // Note: might cause issues on Windows on incorrect shutdown. To be investigated + std::unique_lock lock(pimpl->rpcMutex); + + // Log the request data + if(spdlog::get_level() == spdlog::level::trace) { + spdlog::trace("RPC: {}", nlohmann::json::from_msgpack(request).dump()); + } + + // Send request to device + pimpl->rpcStream->write(std::move(request)); + + // Receive response back + // Send to nanorpc to parse + return pimpl->rpcStream->read(); + }); + + // prepare watchdog thread, which will keep device alive + watchdogThread = std::thread([this]() { + std::shared_ptr conn = this->connection; + while(watchdogRunning) { + try { + pimpl->rpcClient->call("watchdogKeepalive"); + } catch(const std::exception&) { + break; + } + // Ping with a period half of that of the watchdog timeout + std::this_thread::sleep_for(XLINK_WATCHDOG_TIMEOUT / 2); + } + + // Watchdog ended. Useful for checking disconnects + watchdogRunning = false; + }); + + // prepare timesync thread, which will keep device synchronized + timesyncThread = std::thread([this]() { + using namespace std::chrono; + + try { + XLinkStream stream(*this->connection, XLINK_CHANNEL_TIMESYNC, 128); + Timestamp timestamp = {}; + while(timesyncRunning) { + // Block + stream.read(); + + // Timestamp + auto d = std::chrono::steady_clock::now().time_since_epoch(); + timestamp.sec = duration_cast(d).count(); + timestamp.nsec = duration_cast(d).count() % 1000000000; + + // Write timestamp back + stream.write(×tamp, sizeof(timestamp)); + } + } catch(const std::exception& ex) { + // ignore + spdlog::debug("Timesync thread exception caught: {}", ex.what()); + } + + timesyncRunning = false; + }); + + // prepare logging thread, which will log device messages + loggingThread = std::thread([this]() { + using namespace std::chrono; + std::vector messages; + try { + XLinkStream stream(*this->connection, XLINK_CHANNEL_LOG, 128); + while(loggingRunning) { + // Block + auto log = stream.read(); + + // parse packet as msgpack + try { + auto j = nlohmann::json::from_msgpack(log); + // create pipeline schema from retrieved data + nlohmann::from_json(j, messages); + + spdlog::trace("Log vector decoded, size: {}", messages.size()); + + // log the messages in incremental order (0 -> size-1) + for(const auto& msg : messages) { + pimpl->logger.logMessage(msg); + } + + // Log to callbacks + { + // lock mtx to callback map (shared) + std::unique_lock l(logCallbackMapMtx); + for(const auto& msg : messages) { + for(const auto& kv : logCallbackMap) { + const auto& cb = kv.second; + // If available, callback with msg + if(cb) cb(msg); + } + } + } + + } catch(const nlohmann::json::exception& ex) { + spdlog::error("Exception while parsing or calling callbacks for log message from device: {}", ex.what()); + } + } + } catch(const std::exception& ex) { + // ignore exception from logging + spdlog::debug("Log thread exception caught: {}", ex.what()); + } + + loggingRunning = false; + }); + + // Below can throw - make sure to gracefully exit threads + try { + // Set logging level (if DEPTHAI_LEVEL lower than warning, then device is configured accordingly as well) + if(spdlog::get_level() < spdlog::level::warn) { + auto level = spdlogLevelToLogLevel(spdlog::get_level()); + setLogLevel(level); + setLogOutputLevel(level); + } else { + setLogLevel(LogLevel::WARN); + setLogOutputLevel(LogLevel::WARN); + } + + // Sets system inforation logging rate. By default 1s + setSystemInformationLoggingRate(DEFAULT_SYSTEM_INFORMATION_LOGGING_RATE_HZ); + } catch(const std::exception&) { + // close device (cleanup) + close(); + // Rethrow original exception + throw; + } +} + +std::string DeviceBase::getMxId() { + checkClosed(); + + return pimpl->rpcClient->call("getMxId").as(); +} + +std::vector DeviceBase::getConnectedCameras() { + checkClosed(); + + return pimpl->rpcClient->call("getConnectedCameras").as>(); +} + +// Convinience functions for querying current system information +MemoryInfo DeviceBase::getDdrMemoryUsage() { + checkClosed(); + + return pimpl->rpcClient->call("getDdrUsage").as(); +} + +MemoryInfo DeviceBase::getCmxMemoryUsage() { + checkClosed(); + + return pimpl->rpcClient->call("getCmxUsage").as(); +} + +MemoryInfo DeviceBase::getLeonCssHeapUsage() { + checkClosed(); + + return pimpl->rpcClient->call("getLeonCssHeapUsage").as(); +} + +MemoryInfo DeviceBase::getLeonMssHeapUsage() { + checkClosed(); + + return pimpl->rpcClient->call("getLeonMssHeapUsage").as(); +} + +ChipTemperature DeviceBase::getChipTemperature() { + checkClosed(); + + return pimpl->rpcClient->call("getChipTemperature").as(); +} + +CpuUsage DeviceBase::getLeonCssCpuUsage() { + checkClosed(); + + return pimpl->rpcClient->call("getLeonCssCpuUsage").as(); +} + +CpuUsage DeviceBase::getLeonMssCpuUsage() { + checkClosed(); + + return pimpl->rpcClient->call("getLeonMssCpuUsage").as(); +} + +UsbSpeed DeviceBase::getUsbSpeed() { + checkClosed(); + + return pimpl->rpcClient->call("getUsbSpeed").as(); +} + +bool DeviceBase::isPipelineRunning() { + checkClosed(); + + return pimpl->rpcClient->call("isPipelineRunning").as(); +} + +void DeviceBase::setLogLevel(LogLevel level) { + checkClosed(); + + pimpl->rpcClient->call("setLogLevel", level); +} + +LogLevel DeviceBase::getLogLevel() { + checkClosed(); + + return pimpl->rpcClient->call("getLogLevel").as(); +} + +void DeviceBase::setXLinkChunkSize(int sizeBytes) { + checkClosed(); + + pimpl->rpcClient->call("setXLinkChunkSize", sizeBytes); +} + +int DeviceBase::getXLinkChunkSize() { + checkClosed(); + + return pimpl->rpcClient->call("getXLinkChunkSize").as(); +} + +DeviceInfo DeviceBase::getDeviceInfo() { + return deviceInfo; +} + +void DeviceBase::setLogOutputLevel(LogLevel level) { + checkClosed(); + + pimpl->setLogLevel(level); +} + +LogLevel DeviceBase::getLogOutputLevel() { + checkClosed(); + + return pimpl->getLogLevel(); +} + +int DeviceBase::addLogCallback(std::function callback) { + checkClosed(); + + // Lock first + std::unique_lock l(logCallbackMapMtx); + + // Get unique id + int id = uniqueCallbackId++; + + // assign callback + logCallbackMap[id] = callback; + + // return id assigned to the callback + return id; +} + +bool DeviceBase::removeLogCallback(int callbackId) { + checkClosed(); + + // Lock first + std::unique_lock l(logCallbackMapMtx); + + // If callback with id 'callbackId' doesn't exists, return false + if(logCallbackMap.count(callbackId) == 0) return false; + + // Otherwise erase and return true + logCallbackMap.erase(callbackId); + return true; +} + +void DeviceBase::setSystemInformationLoggingRate(float rateHz) { + checkClosed(); + + pimpl->rpcClient->call("setSystemInformationLoggingRate", rateHz); +} + +float DeviceBase::getSystemInformationLoggingRate() { + checkClosed(); + + return pimpl->rpcClient->call("getSystemInformationLoggingrate").as(); +} + +bool DeviceBase::flashCalibration(CalibrationHandler calibrationDataHandler) { + if(!calibrationDataHandler.validateCameraArray()) { + throw std::runtime_error("Failed to validate the extrinsics connection. Enable debug mode for more information."); + } + return pimpl->rpcClient->call("storeToEeprom", calibrationDataHandler.getEepromData()).as(); +} + +CalibrationHandler DeviceBase::readCalibration() { + dai::EepromData eepromData = pimpl->rpcClient->call("readFromEeprom"); + return CalibrationHandler(eepromData); +} + +bool DeviceBase::startPipeline() { + // Deprecated + return true; +} + +bool DeviceBase::startPipeline(const Pipeline& pipeline) { + // first check if pipeline is not already running + if(isPipelineRunning()) { + throw std::runtime_error("Pipeline is already running"); + } + + return startPipelineImpl(pipeline); +} + +bool DeviceBase::startPipelineImpl(const Pipeline& pipeline) { + PipelineSchema schema; + Assets assets; + std::vector assetStorage; + + // Mark the OpenVINO version and serialize the pipeline + OpenVINO::Version pipelineOpenvinoVersion; + pipeline.serialize(schema, assets, assetStorage, pipelineOpenvinoVersion); + if(openvinoVersion != pipelineOpenvinoVersion) { + throw std::runtime_error("Device booted with different OpenVINO version that pipeline requires"); + } + + // if debug + if(spdlog::get_level() == spdlog::level::debug) { + nlohmann::json jSchema = schema; + spdlog::debug("Schema dump: {}", jSchema.dump()); + nlohmann::json jAssets = assets; + spdlog::debug("Asset map dump: {}", jAssets.dump()); + } + + // Load pipelineDesc, assets, and asset storage + pimpl->rpcClient->call("setPipelineSchema", schema); + + // Transfer storage != empty + if(!assetStorage.empty()) { + pimpl->rpcClient->call("setAssets", assets); + + // allocate, returns a pointer to memory on device side + auto memHandle = pimpl->rpcClient->call("memAlloc", static_cast(assetStorage.size())).as(); + + // Transfer the whole assetStorage in a separate thread + const std::string streamAssetStorage = "__stream_asset_storage"; + std::thread t1([this, &streamAssetStorage, &assetStorage]() { + XLinkStream stream(*connection, streamAssetStorage, XLINK_USB_BUFFER_MAX_SIZE); + int64_t offset = 0; + do { + int64_t toTransfer = std::min(static_cast(XLINK_USB_BUFFER_MAX_SIZE), static_cast(assetStorage.size() - offset)); + stream.write(&assetStorage[offset], toTransfer); + offset += toTransfer; + } while(offset < static_cast(assetStorage.size())); + }); + + // Open a channel to transfer AssetStorage + pimpl->rpcClient->call("readFromXLink", streamAssetStorage, memHandle, assetStorage.size()); + t1.join(); + + // After asset storage is transfers, set the asset storage + pimpl->rpcClient->call("setAssetStorage", memHandle, assetStorage.size()); + } + + // print assets on device side for test + pimpl->rpcClient->call("printAssets"); + + // Build and start the pipeline + bool success = false; + std::string errorMsg; + std::tie(success, errorMsg) = pimpl->rpcClient->call("buildPipeline").as>(); + if(success) { + pimpl->rpcClient->call("startPipeline"); + } else { + throw std::runtime_error(errorMsg); + return false; + } + + return true; +} +} // namespace dai diff --git a/src/pipeline/Pipeline.cpp b/src/pipeline/Pipeline.cpp index 68f949153..62daf1cfe 100644 --- a/src/pipeline/Pipeline.cpp +++ b/src/pipeline/Pipeline.cpp @@ -245,6 +245,10 @@ void PipelineImpl::setCameraTuningBlobPath(const std::string& path) { globalProperties.cameraTuningBlobSize = asset->data.size(); } +void PipelineImpl::setXLinkChunkSize(int sizeBytes) { + globalProperties.xlinkChunkSize = sizeBytes; +} + // Remove node capability void PipelineImpl::remove(std::shared_ptr toRemove) { // Search for this node in 'nodes' vector. diff --git a/src/pipeline/datatype/FeatureTrackerConfig.cpp b/src/pipeline/datatype/FeatureTrackerConfig.cpp new file mode 100644 index 000000000..ab2a76af3 --- /dev/null +++ b/src/pipeline/datatype/FeatureTrackerConfig.cpp @@ -0,0 +1,65 @@ +#include "depthai/pipeline/datatype/FeatureTrackerConfig.hpp" + +namespace dai { + +std::shared_ptr FeatureTrackerConfig::serialize() const { + return raw; +} + +FeatureTrackerConfig::FeatureTrackerConfig() : Buffer(std::make_shared()), cfg(*dynamic_cast(raw.get())) {} +FeatureTrackerConfig::FeatureTrackerConfig(std::shared_ptr ptr) + : Buffer(std::move(ptr)), cfg(*dynamic_cast(raw.get())) {} + +dai::RawFeatureTrackerConfig FeatureTrackerConfig::get() const { + return cfg; +} + +void FeatureTrackerConfig::setCornerDetector(dai::FeatureTrackerConfig::CornerDetector::Type cornerDetector) { + cfg.cornerDetector.type = cornerDetector; +} + +void FeatureTrackerConfig::setCornerDetector(dai::FeatureTrackerConfig::CornerDetector config) { + cfg.cornerDetector = config; +} + +void FeatureTrackerConfig::setMotionEstimator(bool enable) { + cfg.motionEstimator.enable = enable; +} + +void FeatureTrackerConfig::setMotionEstimator(dai::FeatureTrackerConfig::MotionEstimator config) { + cfg.motionEstimator = config; +} + +void FeatureTrackerConfig::setOpticalFlow() { + cfg.motionEstimator.type = dai::FeatureTrackerConfig::MotionEstimator::Type::LUCAS_KANADE_OPTICAL_FLOW; + setMotionEstimator(true); +} + +void FeatureTrackerConfig::setOpticalFlow(dai::FeatureTrackerConfig::MotionEstimator::OpticalFlow config) { + cfg.motionEstimator.type = dai::FeatureTrackerConfig::MotionEstimator::Type::LUCAS_KANADE_OPTICAL_FLOW; + cfg.motionEstimator.opticalFlow = config; + setMotionEstimator(true); +} + +void FeatureTrackerConfig::setHwMotionEstimation() { + cfg.motionEstimator.type = dai::FeatureTrackerConfig::MotionEstimator::Type::HW_MOTION_ESTIMATION; + setMotionEstimator(true); +} + +void FeatureTrackerConfig::setFeatureMaintainer(bool enable) { + cfg.featureMaintainer.enable = enable; +} + +void FeatureTrackerConfig::setFeatureMaintainer(dai::FeatureTrackerConfig::FeatureMaintainer config) { + cfg.featureMaintainer = config; +} + +void FeatureTrackerConfig::set(dai::RawFeatureTrackerConfig config) { + cfg = config; +} + +void FeatureTrackerConfig::setNumTargetFeatures(std::int32_t numTargetFeatures) { + cfg.cornerDetector.numTargetFeatures = numTargetFeatures; +} + +} // namespace dai diff --git a/src/pipeline/datatype/StreamPacketParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp similarity index 81% rename from src/pipeline/datatype/StreamPacketParser.cpp rename to src/pipeline/datatype/StreamMessageParser.cpp index a4ba4f45c..81f8ddc12 100644 --- a/src/pipeline/datatype/StreamPacketParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -1,4 +1,4 @@ -#include "StreamPacketParser.hpp" +#include "depthai/pipeline/datatype/StreamMessageParser.hpp" // standard #include @@ -15,6 +15,7 @@ #include "depthai/pipeline/datatype/Buffer.hpp" #include "depthai/pipeline/datatype/CameraControl.hpp" #include "depthai/pipeline/datatype/EdgeDetectorConfig.hpp" +#include "depthai/pipeline/datatype/FeatureTrackerConfig.hpp" #include "depthai/pipeline/datatype/IMUData.hpp" #include "depthai/pipeline/datatype/ImageManipConfig.hpp" #include "depthai/pipeline/datatype/ImgDetections.hpp" @@ -25,6 +26,7 @@ #include "depthai/pipeline/datatype/SpatialLocationCalculatorData.hpp" #include "depthai/pipeline/datatype/StereoDepthConfig.hpp" #include "depthai/pipeline/datatype/SystemInformation.hpp" +#include "depthai/pipeline/datatype/TrackedFeatures.hpp" #include "depthai/pipeline/datatype/Tracklets.hpp" // shared @@ -32,6 +34,7 @@ #include "depthai-shared/datatype/RawBuffer.hpp" #include "depthai-shared/datatype/RawCameraControl.hpp" #include "depthai-shared/datatype/RawEdgeDetectorConfig.hpp" +#include "depthai-shared/datatype/RawFeatureTrackerConfig.hpp" #include "depthai-shared/datatype/RawIMUData.hpp" #include "depthai-shared/datatype/RawImageManipConfig.hpp" #include "depthai-shared/datatype/RawImgDetections.hpp" @@ -62,7 +65,7 @@ inline std::shared_ptr parseDatatype(nlohmann::json& ser, std::vector parsePacket(streamPacketDesc_t* packet) { +std::shared_ptr StreamMessageParser::parseMessage(streamPacketDesc_t* packet) { int serializedObjectSize = readIntLE(packet->data + packet->length - 4); auto objectType = static_cast(readIntLE(packet->data + packet->length - 8)); @@ -141,12 +144,20 @@ std::shared_ptr parsePacket(streamPacketDesc_t* packet) { case DatatypeEnum::EdgeDetectorConfig: return parseDatatype(jser, data); break; + + case DatatypeEnum::TrackedFeatures: + return parseDatatype(jser, data); + break; + + case DatatypeEnum::FeatureTrackerConfig: + return parseDatatype(jser, data); + break; } throw std::runtime_error("Bad packet, couldn't parse"); } -std::shared_ptr parsePacketToADatatype(streamPacketDesc_t* packet) { +std::shared_ptr StreamMessageParser::parseMessageToADatatype(streamPacketDesc_t* packet) { int serializedObjectSize = readIntLE(packet->data + packet->length - 4); auto objectType = static_cast(readIntLE(packet->data + packet->length - 8)); @@ -223,15 +234,20 @@ std::shared_ptr parsePacketToADatatype(streamPacketDesc_t* packet) { case DatatypeEnum::EdgeDetectorConfig: return std::make_shared(parseDatatype(jser, data)); break; + + case DatatypeEnum::TrackedFeatures: + return std::make_shared(parseDatatype(jser, data)); + break; + + case DatatypeEnum::FeatureTrackerConfig: + return std::make_shared(parseDatatype(jser, data)); + break; } throw std::runtime_error("Bad packet, couldn't parse"); } -std::vector serializeData(const std::shared_ptr& data) { - std::vector ser; - if(!data) return ser; - +std::vector StreamMessageParser::serializeMessage(const RawBuffer& data) { // Serialization: // 1. fill vector with bytes from data.data // 2. serialize and append metadata @@ -240,21 +256,37 @@ std::vector serializeData(const std::shared_ptr& data) std::vector metadata; DatatypeEnum datatype; - data->serialize(metadata, datatype); + data.serialize(metadata, datatype); uint32_t metadataSize = static_cast(metadata.size()); // 4B datatype & 4B metadata size - std::uint8_t leDatatype[4]; - std::uint8_t leMetadataSize[4]; + std::array leDatatype; + std::array leMetadataSize; for(int i = 0; i < 4; i++) leDatatype[i] = (static_cast(datatype) >> (i * 8)) & 0xFF; for(int i = 0; i < 4; i++) leMetadataSize[i] = (metadataSize >> i * 8) & 0xFF; - ser.insert(ser.end(), data->data.begin(), data->data.end()); + std::vector ser; + ser.reserve(data.data.size() + metadata.size() + leDatatype.size() + leMetadataSize.size()); + ser.insert(ser.end(), data.data.begin(), data.data.end()); ser.insert(ser.end(), metadata.begin(), metadata.end()); - ser.insert(ser.end(), leDatatype, leDatatype + sizeof(leDatatype)); - ser.insert(ser.end(), leMetadataSize, leMetadataSize + sizeof(leMetadataSize)); + ser.insert(ser.end(), leDatatype.begin(), leDatatype.end()); + ser.insert(ser.end(), leMetadataSize.begin(), leMetadataSize.end()); return ser; } +std::vector StreamMessageParser::serializeMessage(const std::shared_ptr& data) { + if(!data) return {}; + return serializeMessage(*data); +} + +std::vector StreamMessageParser::serializeMessage(const ADatatype& data) { + return serializeMessage(data.serialize()); +} + +std::vector StreamMessageParser::serializeMessage(const std::shared_ptr& data) { + if(!data) return {}; + return serializeMessage(*data); +} + } // namespace dai diff --git a/src/pipeline/datatype/StreamPacketParser.hpp b/src/pipeline/datatype/StreamPacketParser.hpp deleted file mode 100644 index f1b4c9956..000000000 --- a/src/pipeline/datatype/StreamPacketParser.hpp +++ /dev/null @@ -1,24 +0,0 @@ -#pragma once - -// standard -#include - -// libraries -#include - -//project -#include "depthai/pipeline/datatype/ADatatype.hpp" - -//shared -#include "depthai-shared/datatype/RawBuffer.hpp" - -// StreamPacket structure -> || imgframepixels... , serialized_object, object_type, serialized_object_size || -// object_type -> DataType(int), serialized_object_size -> int - -namespace dai { - -std::shared_ptr parsePacket(streamPacketDesc_t* packet); -std::shared_ptr parsePacketToADatatype(streamPacketDesc_t* packet); -std::vector serializeData(const std::shared_ptr& data); - -} // namespace dai diff --git a/src/pipeline/datatype/TrackedFeatures.cpp b/src/pipeline/datatype/TrackedFeatures.cpp new file mode 100644 index 000000000..1fa34051a --- /dev/null +++ b/src/pipeline/datatype/TrackedFeatures.cpp @@ -0,0 +1,14 @@ +#include "depthai/pipeline/datatype/TrackedFeatures.hpp" + +namespace dai { + +std::shared_ptr TrackedFeatures::serialize() const { + return raw; +} + +TrackedFeatures::TrackedFeatures() + : Buffer(std::make_shared()), rawdata(*dynamic_cast(raw.get())), trackedFeatures(rawdata.trackedFeatures) {} +TrackedFeatures::TrackedFeatures(std::shared_ptr ptr) + : Buffer(std::move(ptr)), rawdata(*dynamic_cast(raw.get())), trackedFeatures(rawdata.trackedFeatures) {} + +} // namespace dai diff --git a/src/pipeline/node/EdgeDetector.cpp b/src/pipeline/node/EdgeDetector.cpp index 2c5c1a7c7..436e7faa8 100644 --- a/src/pipeline/node/EdgeDetector.cpp +++ b/src/pipeline/node/EdgeDetector.cpp @@ -8,7 +8,7 @@ namespace node { EdgeDetector::EdgeDetector(const std::shared_ptr& par, int64_t nodeId) : Node(par, nodeId), rawConfig(std::make_shared()), initialConfig(rawConfig) { inputs = {&inputConfig, &inputImage}; - outputs = {&outputImage}; + outputs = {&outputImage, &passthroughInputImage}; } std::string EdgeDetector::getName() const { diff --git a/src/pipeline/node/FeatureTracker.cpp b/src/pipeline/node/FeatureTracker.cpp new file mode 100644 index 000000000..a27c995ae --- /dev/null +++ b/src/pipeline/node/FeatureTracker.cpp @@ -0,0 +1,40 @@ +#include "depthai/pipeline/node/FeatureTracker.hpp" + +#include "spdlog/fmt/fmt.h" + +namespace dai { +namespace node { + +FeatureTracker::FeatureTracker(const std::shared_ptr& par, int64_t nodeId) + : Node(par, nodeId), rawConfig(std::make_shared()), initialConfig(rawConfig) { + inputs = {&inputConfig, &inputImage}; + outputs = {&outputFeatures, &passthroughInputImage}; +} + +std::string FeatureTracker::getName() const { + return "FeatureTracker"; +} + +nlohmann::json FeatureTracker::getProperties() { + nlohmann::json j; + properties.initialConfig = *rawConfig; + nlohmann::to_json(j, properties); + return j; +} + +// Node properties configuration +void FeatureTracker::setWaitForConfigInput(bool wait) { + properties.inputConfigSync = wait; +} + +void FeatureTracker::setHardwareResources(int numShaves, int numMemorySlices) { + properties.numShaves = numShaves; + properties.numMemorySlices = numMemorySlices; +} + +std::shared_ptr FeatureTracker::clone() { + return std::make_shared::type>(*this); +} + +} // namespace node +} // namespace dai