diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index 9d5a11f3..098dafbf 100644 --- a/depthai_bridge/CMakeLists.txt +++ b/depthai_bridge/CMakeLists.txt @@ -47,6 +47,7 @@ FILE(GLOB LIB_SRC "src/ImageConverter.cpp" "src/ImgDetectionConverter.cpp" "src/SpatialDetectionConverter.cpp" +"src/TrackletConverter.cpp" "src/ImuConverter.cpp" ) diff --git a/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp b/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp new file mode 100644 index 00000000..6e13e7df --- /dev/null +++ b/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include +#include +#include + +#include "depthai/pipeline/datatype/Tracklets.hpp" +#include "depthai_ros_msgs/TrackletArray.h" +#include "ros/ros.h" + +namespace dai { + +namespace ros { +namespace TrackletMessages = depthai_ros_msgs; +using TrackletArrayPtr = TrackletMessages::TrackletArray::Ptr; +class TrackletConverter { + public: + TrackletConverter(std::string frameName, + int width_srcImgDetection, + int height_srcImgDetection, + int width_srcImgTracker, + int height_srcImgTracker, + bool normalized_srcImgDetection = false, + bool normalized_srcImgTracker = false, + bool getBaseDeviceTimestamp = false); + void toRosMsg(std::shared_ptr trackData, std::deque& trackletsMsg); + TrackletArrayPtr toRosMsgPtr(std::shared_ptr trackData); + + private: + int _width_srcImgDetection, _height_srcImgDetection, _width_srcImgTracker, _height_srcImgTracker; + const std::string _frameName; + bool _normalized_srcImgDetection, _normalized_srcImgTracker; + std::chrono::time_point _steadyBaseTime; + ::ros::Time _rosBaseTime; + bool _getBaseDeviceTimestamp; +}; + +} // namespace ros + +namespace rosBridge = ros; + +} // namespace dai diff --git a/depthai_bridge/src/TrackletConverter.cpp b/depthai_bridge/src/TrackletConverter.cpp new file mode 100644 index 00000000..8af518f3 --- /dev/null +++ b/depthai_bridge/src/TrackletConverter.cpp @@ -0,0 +1,108 @@ +#include "depthai_bridge/TrackletConverter.hpp" +#include "depthai_bridge/depthaiUtility.hpp" + +namespace dai { +namespace ros { + +TrackletConverter::TrackletConverter(std::string frameName, + int width_srcImgDetection, + int height_srcImgDetection, + int width_srcImgTracker, + int height_srcImgTracker, + bool normalized_srcImgDetection, + bool normalized_srcImgTracker, + bool getBaseDeviceTimestamp) + : _frameName(frameName), + _width_srcImgDetection(width_srcImgDetection), + _height_srcImgDetection(height_srcImgDetection), + _width_srcImgTracker(width_srcImgTracker), + _height_srcImgTracker(height_srcImgTracker), + _normalized_srcImgDetection(normalized_srcImgDetection), + _normalized_srcImgTracker(normalized_srcImgTracker), + _steadyBaseTime(std::chrono::steady_clock::now()), + _getBaseDeviceTimestamp(getBaseDeviceTimestamp) { + _rosBaseTime = ::ros::Time::now(); +} + +void TrackletConverter::toRosMsg(std::shared_ptr trackData, std::deque& trackletMsgs) { + // setting the header + std::chrono::_V2::steady_clock::time_point tstamp; + if(_getBaseDeviceTimestamp) + tstamp = trackData->getTimestampDevice(); + else + tstamp = trackData->getTimestamp(); + + TrackletMessages::TrackletArray trackletsMsg; + + trackletsMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp); + trackletsMsg.header.frame_id = _frameName; + trackletsMsg.tracklets.resize(trackData->tracklets.size()); + + // publishing + for(int i = 0; i < trackData->tracklets.size(); ++i) { + // srcImgDetection -> inputDetectionsFrame + int xMin_srcImgDetection, yMin_srcImgDetection, xMax_srcImgDetection, yMax_srcImgDetection; + if(_normalized_srcImgDetection) { + xMin_srcImgDetection = trackData->tracklets[i].srcImgDetection.xmin; + yMin_srcImgDetection = trackData->tracklets[i].srcImgDetection.ymin; + xMax_srcImgDetection = trackData->tracklets[i].srcImgDetection.xmax; + yMax_srcImgDetection = trackData->tracklets[i].srcImgDetection.ymax; + } else { + xMin_srcImgDetection = trackData->tracklets[i].srcImgDetection.xmin * _width_srcImgDetection; + yMin_srcImgDetection = trackData->tracklets[i].srcImgDetection.ymin * _height_srcImgDetection; + xMax_srcImgDetection = trackData->tracklets[i].srcImgDetection.xmax * _width_srcImgDetection; + yMax_srcImgDetection = trackData->tracklets[i].srcImgDetection.ymax * _height_srcImgDetection; + } + auto xSize_srcImgDetection = xMax_srcImgDetection - xMin_srcImgDetection; + auto ySize_srcImgDetection = yMax_srcImgDetection - yMin_srcImgDetection; + auto xCenter_srcImgDetection = xMin_srcImgDetection + xSize_srcImgDetection / 2; + auto yCenter_srcImgDetection = yMin_srcImgDetection + ySize_srcImgDetection / 2; + + // roi -> inputTrackerFrame + dai::Rect roi; + if(_normalized_srcImgTracker) { + roi = trackData->tracklets[i].roi; + } else { + roi = trackData->tracklets[i].roi.denormalize(_width_srcImgTracker, _height_srcImgTracker); + } + auto xSize_srcImgTracker = roi.size().width; + auto ySize_srcImgTracker = roi.size().height; + auto xCenter_srcImgTracker = roi.topLeft().x + roi.width / 2; + auto yCenter_srcImgTracker = roi.topLeft().y + roi.height / 2; + + trackletsMsg.tracklets[i].roi.center.x = xCenter_srcImgTracker; + trackletsMsg.tracklets[i].roi.center.y = yCenter_srcImgTracker; + trackletsMsg.tracklets[i].roi.size_x = xSize_srcImgTracker; + trackletsMsg.tracklets[i].roi.size_y = ySize_srcImgTracker; + + trackletsMsg.tracklets[i].id = trackData->tracklets[i].id; + trackletsMsg.tracklets[i].label = trackData->tracklets[i].label; + trackletsMsg.tracklets[i].age = trackData->tracklets[i].age; + trackletsMsg.tracklets[i].status = static_cast::type>(trackData->tracklets[i].status); + + trackletsMsg.tracklets[i].srcImgDetectionHypothesis.score = trackData->tracklets[i].srcImgDetection.confidence; + trackletsMsg.tracklets[i].srcImgDetectionHypothesis.id = trackData->tracklets[i].srcImgDetection.label; + trackletsMsg.tracklets[i].srcImgDetectionBBox.center.x = xCenter_srcImgDetection; + trackletsMsg.tracklets[i].srcImgDetectionBBox.center.y = yCenter_srcImgDetection; + trackletsMsg.tracklets[i].srcImgDetectionBBox.size_x = xSize_srcImgDetection; + trackletsMsg.tracklets[i].srcImgDetectionBBox.size_y = ySize_srcImgDetection; + + // converting mm to meters since per ros rep-103 lenght should always be in meters + trackletsMsg.tracklets[i].spatialCoordinates.x = trackData->tracklets[i].spatialCoordinates.x / 1000; + trackletsMsg.tracklets[i].spatialCoordinates.y = trackData->tracklets[i].spatialCoordinates.y / 1000; + trackletsMsg.tracklets[i].spatialCoordinates.z = trackData->tracklets[i].spatialCoordinates.z / 1000; + } + + trackletMsgs.push_back(trackletsMsg); +} + +TrackletArrayPtr TrackletConverter::toRosMsgPtr(std::shared_ptr trackData) { + std::deque msgQueue; + toRosMsg(trackData, msgQueue); + auto msg = msgQueue.front(); + TrackletArrayPtr ptr = boost::make_shared(msg); + return ptr; +} + +} // namespace ros +} // namespace dai \ No newline at end of file diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index 214fb9cf..2ba04c0f 100644 --- a/depthai_examples/CMakeLists.txt +++ b/depthai_examples/CMakeLists.txt @@ -111,9 +111,11 @@ dai_add_node(stereo_inertial_node src/stereo_inertial_publisher.cpp) dai_add_node(stereo_node src/stereo_publisher.cpp) dai_add_node(yolov4_spatial_node src/yolov4_spatial_publisher.cpp) +dai_add_node(tracker_node src/tracker_publisher.cpp) target_compile_definitions(mobilenet_node PRIVATE BLOB_NAME="${mobilenet_blob_name}") target_compile_definitions(yolov4_spatial_node PRIVATE BLOB_NAME="${tiny_yolo_v4_blob_name}") +target_compile_definitions(tracker_node PRIVATE BLOB_NAME="${tiny_yolo_v4_blob_name}") target_compile_definitions(stereo_inertial_node PRIVATE BLOB_NAME="${tiny_yolo_v4_blob_name}") catkin_install_python(PROGRAMS @@ -131,6 +133,7 @@ install(TARGETS stereo_inertial_node stereo_node yolov4_spatial_node + tracker_node LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/depthai_examples/launch/tracker_publisher.launch b/depthai_examples/launch/tracker_publisher.launch new file mode 100644 index 00000000..aff73741 --- /dev/null +++ b/depthai_examples/launch/tracker_publisher.launch @@ -0,0 +1,81 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/depthai_examples/src/tracker_publisher.cpp b/depthai_examples/src/tracker_publisher.cpp new file mode 100644 index 00000000..3a77aba0 --- /dev/null +++ b/depthai_examples/src/tracker_publisher.cpp @@ -0,0 +1,409 @@ +#include +#include +#include +#include + +#include "camera_info_manager/camera_info_manager.h" +#include "depthai_bridge/BridgePublisher.hpp" +#include "depthai_bridge/ImageConverter.hpp" +#include "depthai_bridge/SpatialDetectionConverter.hpp" +#include "depthai_bridge/TrackletConverter.hpp" +#include "depthai_bridge/DisparityConverter.hpp" +#include "depthai_bridge/ImuConverter.hpp" +#include "depthai_bridge/depthaiUtility.hpp" +#include "ros/ros.h" + +// Inludes common necessary includes for development using depthai library +#include "depthai/device/DataQueue.hpp" +#include "depthai/device/Device.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/node/ColorCamera.hpp" +#include "depthai/pipeline/node/MonoCamera.hpp" +#include "depthai/pipeline/node/SpatialDetectionNetwork.hpp" +#include "depthai/pipeline/node/StereoDepth.hpp" +#include "depthai/pipeline/node/XLinkOut.hpp" +#include "depthai/pipeline/node/IMU.hpp" +#include "depthai/pipeline/node/XLinkIn.hpp" +#include "depthai/pipeline/node/ObjectTracker.hpp" + +#include "depthai_ros_msgs/SpatialDetectionArray.h" +#include "sensor_msgs/Imu.h" +#include "stereo_msgs/DisparityImage.h" +#include "sensor_msgs/Image.h" +#include "vision_msgs/Detection2DArray.h" + + +const std::vector label_map = { + "person", "bicycle", "car", "motorbike", "aeroplane", "bus", "train", "truck", "boat", + "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", + "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", + "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", + "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", + "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", + "donut", "cake", "chair", "sofa", "pottedplant", "bed", "diningtable", "toilet", "tvmonitor", + "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", + "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; + +std::tuple createPipeline(bool syncNN, + bool subpixel, + bool lrcheck, + bool extended, + std::string nnPath, + int confidence, + int LRchecktresh, + int mono_fps, + int rgb_fps, + int previewWidth, + int previewHeight, + int rgbScaleNumerator, + int rgbScaleDinominator, + std::string monoResolutionStr, + std::string rgbResolutionStr) { + dai::Pipeline pipeline; + pipeline.setXLinkChunkSize(0); + + auto controlIn = pipeline.create(); + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto colorCam = pipeline.create(); + auto stereo = pipeline.create(); + auto spatialDetectionNetwork = pipeline.create(); + auto objectTracker = pipeline.create(); + auto imu = pipeline.create(); + + // create xlink connections + auto xoutRgb = pipeline.create(); + auto xoutDepth = pipeline.create(); + auto xoutNN = pipeline.create(); + auto xoutPreview = pipeline.create(); + auto xoutTracker = pipeline.create(); + auto xoutImu = pipeline.create(); + + controlIn->setStreamName("control"); + xoutPreview->setStreamName("preview"); + xoutRgb->setStreamName("rgb"); + xoutDepth->setStreamName("depth"); + xoutNN->setStreamName("detections"); + xoutTracker->setStreamName("tracklets"); + xoutImu->setStreamName("imu"); + + // MonoCameras + dai::node::MonoCamera::Properties::SensorResolution monoResolution; + int monoWidth, monoHeight; + if(monoResolutionStr == "720p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_720_P; + monoWidth = 1280; + monoHeight = 720; + } else if(monoResolutionStr == "400p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_400_P; + monoWidth = 640; + monoHeight = 400; + } else if(monoResolutionStr == "800p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_800_P; + monoWidth = 1280; + monoHeight = 800; + } else if(monoResolutionStr == "480p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_480_P; + monoWidth = 640; + monoHeight = 480; + } else { + ROS_ERROR("Invalid parameter. -> monoResolution: %s", monoResolutionStr.c_str()); + throw std::runtime_error("Invalid mono camera resolution."); + } + monoLeft->setResolution(monoResolution); + monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoLeft->setFps(mono_fps); + monoRight->setResolution(monoResolution); + monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + monoRight->setFps(mono_fps); + + // StereoDepth + stereo->initialConfig.setConfidenceThreshold(confidence); + stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout + stereo->initialConfig.setLeftRightCheckThreshold(LRchecktresh); + stereo->setSubpixel(subpixel); + stereo->setLeftRightCheck(lrcheck); + stereo->setExtendedDisparity(extended); + stereo->setDepthAlign(dai::CameraBoardSocket::RGB); + stereo->setOutputSize(monoLeft->getResolutionWidth(), monoLeft->getResolutionHeight()); + // stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY); + + // ObjectTracker + // objectTracker->setDetectionLabelsToTrack({15}); // track only person + objectTracker->setTrackerType(dai::TrackerType::ZERO_TERM_COLOR_HISTOGRAM); + objectTracker->setTrackerIdAssignmentPolicy(dai::TrackerIdAssignmentPolicy::UNIQUE_ID); + + // IMU + imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 500); + imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, 400); + imu->setBatchReportThreshold(5); + imu->setMaxBatchReports(20); // Get one message only for now. + + // ColorCamera + int rgbWidth, rgbHeight; + dai::node::ColorCamera::Properties::SensorResolution rgbResolution; + if(rgbResolutionStr == "1080p") { + rgbResolution = dai::node::ColorCamera::Properties::SensorResolution::THE_1080_P; + rgbWidth = 1920; + rgbHeight = 1080; + } else if(rgbResolutionStr == "4K") { + rgbResolution = dai::node::ColorCamera::Properties::SensorResolution::THE_4_K; + rgbWidth = 3840; + rgbHeight = 2160; + } else if(rgbResolutionStr == "12MP") { + rgbResolution = dai::node::ColorCamera::Properties::SensorResolution::THE_12_MP; + rgbWidth = 4056; + rgbHeight = 3040; + } else if(rgbResolutionStr == "13MP") { + rgbResolution = dai::node::ColorCamera::Properties::SensorResolution::THE_13_MP; + rgbWidth = 4208; + rgbHeight = 3120; + } else { + ROS_ERROR("Invalid parameter. -> rgbResolution: %s", rgbResolutionStr.c_str()); + throw std::runtime_error("Invalid color camera resolution."); + } + colorCam->setBoardSocket(dai::CameraBoardSocket::RGB); + colorCam->setPreviewSize(previewWidth, previewHeight); + colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + colorCam->setInterleaved(false); + colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + colorCam->setFps(rgb_fps); + colorCam->setIspScale(rgbScaleNumerator, rgbScaleDinominator); + rgbWidth = rgbWidth * rgbScaleNumerator / rgbScaleDinominator; + rgbHeight = rgbHeight * rgbScaleNumerator / rgbScaleDinominator; + + // SpatialDetections + spatialDetectionNetwork->setBlobPath(nnPath); + spatialDetectionNetwork->setConfidenceThreshold(0.5f); + spatialDetectionNetwork->input.setBlocking(false); + spatialDetectionNetwork->setBoundingBoxScaleFactor(0.5); + spatialDetectionNetwork->setDepthLowerThreshold(100); + spatialDetectionNetwork->setDepthUpperThreshold(5000); + + // yolo specific parameters + spatialDetectionNetwork->setNumClasses(80); + spatialDetectionNetwork->setCoordinateSize(4); + spatialDetectionNetwork->setAnchors({10, 14, 23, 27, 37, 58, 81, 82, 135, 169, 344, 319}); + spatialDetectionNetwork->setAnchorMasks({{"side13", {3, 4, 5}}, {"side26", {1, 2, 3}}}); + spatialDetectionNetwork->setIouThreshold(0.5f); + + // Link Control + controlIn->out.link(monoRight->inputControl); + controlIn->out.link(monoLeft->inputControl); + + // Link MonoCameras + monoLeft->out.link(stereo->left); + monoRight->out.link(stereo->right); + + // Link StereoCamera + stereo->depth.link(spatialDetectionNetwork->inputDepth); + + // Link ColorCamera + colorCam->preview.link(spatialDetectionNetwork->input); + colorCam->isp.link(objectTracker->inputTrackerFrame); + + // Link SpatialDetectionNetwork + spatialDetectionNetwork->passthroughDepth.link(xoutDepth->input); + spatialDetectionNetwork->passthrough.link(objectTracker->inputDetectionFrame); + spatialDetectionNetwork->passthrough.link(objectTracker->inputTrackerFrame); + spatialDetectionNetwork->out.link(objectTracker->inputDetections); + + // Link ObjectTracker + // objectTracker->setDetectionLabelsToTrack({47}); + objectTracker->passthroughDetectionFrame.link(xoutPreview->input); + objectTracker->passthroughDetections.link(xoutNN->input); + objectTracker->passthroughTrackerFrame.link(xoutRgb->input); + objectTracker->out.link(xoutTracker->input); + objectTracker->inputTrackerFrame.setBlocking(false); + objectTracker->inputTrackerFrame.setQueueSize(2); + + // Link IMU + imu->out.link(xoutImu->input); + + return std::make_tuple(pipeline, rgbWidth, rgbHeight, monoWidth, monoHeight); +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "tracker_node"); + ros::NodeHandle pnh("~"); + + std::string tfPrefix, resourceBaseFolder, nnPath; + std::string camera_param_uri; + std::string nnName(BLOB_NAME); // Set your blob name for the model here + bool lrcheck, extended, syncNN, subpixel, enableDotProjector, enableFloodLight; + int rgbScaleNumerator, rgbScaleDinominator; + double dotProjectormA, floodLightmA; + double angularVelCovariance, linearAccelCovariance; + int imuModeParam = 1; + int badParams = 0; + int confidence = 200; + int LRchecktresh = 5; + int monoFPS = 30; + int rgbFPS = 30; + int previewHeight = 416; + int previewWidth = 416; + std::string monoResolution = "720p", rgbResolution = "1080p"; + + badParams += !pnh.getParam("tf_prefix", tfPrefix); + badParams += !pnh.getParam("camera_param_uri", camera_param_uri); + badParams += !pnh.getParam("sync_nn", syncNN); + badParams += !pnh.getParam("subpixel", subpixel); + badParams += !pnh.getParam("confidence", confidence); + badParams += !pnh.getParam("LRchecktresh", LRchecktresh); + badParams += !pnh.getParam("monoResolution", monoResolution); + badParams += !pnh.getParam("resourceBaseFolder", resourceBaseFolder); + badParams += !pnh.getParam("monoFPS", monoFPS); + badParams += !pnh.getParam("rgbFPS", rgbFPS); + badParams += !pnh.getParam("previewHeight", previewHeight); + badParams += !pnh.getParam("previewWidth", previewWidth); + badParams += !pnh.getParam("lrcheck", lrcheck); + badParams += !pnh.getParam("extended", extended); + badParams += !pnh.getParam("rgbScaleNumerator", rgbScaleNumerator); + badParams += !pnh.getParam("rgbScaleDinominator", rgbScaleDinominator); + badParams += !pnh.getParam("monoResolution", monoResolution); + badParams += !pnh.getParam("rgbResolution", rgbResolution); + + badParams += !pnh.getParam("angularVelCovariance", angularVelCovariance); + badParams += !pnh.getParam("linearAccelCovariance", linearAccelCovariance); + + badParams += !pnh.getParam("enableDotProjector", enableDotProjector); + badParams += !pnh.getParam("enableFloodLight", enableFloodLight); + badParams += !pnh.getParam("dotProjectormA", dotProjectormA); + badParams += !pnh.getParam("floodLightmA", floodLightmA); + + if(badParams > 0) { + throw std::runtime_error("Couldn't find one of the parameters"); + } + + std::string nnParam; + pnh.getParam("nnName", nnParam); + if(nnParam != "x") { + pnh.getParam("nnName", nnName); + } + + if(resourceBaseFolder.empty()) { + throw std::runtime_error("Send the path to the resouce folder containing NNBlob in \'resourceBaseFolder\' "); + } + nnPath = resourceBaseFolder + "/" + nnName; + + dai::Pipeline pipeline; + int rgbWidth, rgbHeight, monoWidth, monoHeight; + std::tie(pipeline, rgbWidth, rgbHeight, monoWidth, monoHeight) = createPipeline(syncNN, + subpixel, + lrcheck, + extended, + nnPath, + confidence, + LRchecktresh, + monoFPS, + rgbFPS, + previewWidth, + previewHeight, + rgbScaleNumerator, + rgbScaleDinominator, + monoResolution, + rgbResolution); + + // Device + std::shared_ptr device; + device = std::make_shared(pipeline); + + // Get Input/Output Queues + auto previewQueue = device->getOutputQueue("preview", 30, false); + auto colorQueue = device->getOutputQueue("rgb", 30, false); + auto detectionQueue = device->getOutputQueue("detections", 30, false); + auto depthQueue = device->getOutputQueue("depth", 30, false); + auto imuQueue = device->getOutputQueue("imu", 30, false); + auto controlQueue = device->getInputQueue("control"); + auto trackletQueue = device->getOutputQueue("tracklets", 30, false); + + auto calibrationHandler = device->readCalibration(); + + std::vector> irDrivers = device->getIrDrivers(); + if(!irDrivers.empty()) { + if(enableDotProjector) device->setIrLaserDotProjectorBrightness(dotProjectormA); // in mA, 0..1200 + if(enableFloodLight) device->setIrFloodLightBrightness(floodLightmA); // in mA, 0..1500 + } + + auto boardName = calibrationHandler.getEepromData().boardName; + if(monoHeight > 480 && boardName == "OAK-D-LITE") { + ROS_ERROR("Invalid parameter. -> monoResolution: %s", monoResolution.c_str()); + throw std::runtime_error("Invalid mono camera resolution for OAK-D-LITE."); + } + + // IMU + dai::ros::ImuSyncMethod imuMode = static_cast(imuModeParam); + dai::rosBridge::ImuConverter imuConverter(tfPrefix + "_imu_frame", imuMode, linearAccelCovariance, angularVelCovariance); + dai::rosBridge::BridgePublisher imuPublish( + imuQueue, + pnh, + std::string("imu"), + std::bind(&dai::rosBridge::ImuConverter::toRosMsg, &imuConverter, std::placeholders::_1, std::placeholders::_2), + 30, + "", + "imu"); + imuPublish.addPublisherCallback(); + + // RGB + dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false); + auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, rgbWidth, rgbHeight); + dai::rosBridge::BridgePublisher rgbPublish( + colorQueue, + pnh, + std::string("color/image"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &rgbConverter, std::placeholders::_1, std::placeholders::_2), + 30, + rgbCameraInfo, + "color"); + rgbPublish.addPublisherCallback(); + + // Depth + dai::rosBridge::ImageConverter depthConverter(tfPrefix + "_rgb_camera_optical_frame", false); + auto depthCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight); + dai::rosBridge::BridgePublisher depthPublish( + depthQueue, + pnh, + std::string("stereo/depth"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &depthConverter, std::placeholders::_1, std::placeholders::_2), + 30, + depthCameraInfo, + "stereo"); + depthPublish.addPublisherCallback(); + + // Preview + dai::rosBridge::ImageConverter previewConverter(tfPrefix + "_rgb_camera_optical_frame", false); + auto previewCameraInfo = previewConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, previewWidth, previewHeight); + dai::rosBridge::BridgePublisher previewPublish( + previewQueue, + pnh, + std::string("color/preview/image"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &previewConverter, std::placeholders::_1, std::placeholders::_2), + 30, + previewCameraInfo, + "color/preview"); + previewPublish.addPublisherCallback(); + + // SpatialDetections + dai::rosBridge::SpatialDetectionConverter detConverter(tfPrefix + "_rgb_camera_optical_frame", previewWidth, previewHeight, false); + dai::rosBridge::BridgePublisher detectionPublish( + detectionQueue, + pnh, + std::string("detections/yolov4_spatial"), + std::bind(&dai::rosBridge::SpatialDetectionConverter::toRosMsg, &detConverter, std::placeholders::_1, std::placeholders::_2), + 30); + detectionPublish.addPublisherCallback(); + + // Tracklets + dai::rosBridge::TrackletConverter trackConverter(tfPrefix + "_rgb_camera_optical_frame", previewWidth, previewHeight, rgbWidth, rgbHeight, false, false); + dai::rosBridge::BridgePublisher trackletPublish( + trackletQueue, + pnh, + std::string("tracker/tracklets"), + std::bind(&dai::rosBridge::TrackletConverter::toRosMsg, &trackConverter, std::placeholders::_1, std::placeholders::_2), + 30); + trackletPublish.addPublisherCallback(); + + ros::spin(); + + return 0; +} diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index d0338bdf..afe29a4f 100644 --- a/depthai_ros_msgs/CMakeLists.txt +++ b/depthai_ros_msgs/CMakeLists.txt @@ -31,6 +31,8 @@ add_message_files ( SpatialDetection.msg SpatialDetectionArray.msg ImuWithMagneticField.msg + Tracklet.msg + TrackletArray.msg ) ## Generate services in the 'srv' folder diff --git a/depthai_ros_msgs/msg/Tracklet.msg b/depthai_ros_msgs/msg/Tracklet.msg new file mode 100644 index 00000000..1b1ec0b1 --- /dev/null +++ b/depthai_ros_msgs/msg/Tracklet.msg @@ -0,0 +1,27 @@ +# TrackingStatus +uint8 NEW=0 +uint8 TRACKED=1 +uint8 LOST=2 +uint8 REMOVED=3 + +# Rect roi +vision_msgs/BoundingBox2D roi + +# Tracklet's ID. +int32 id + +# Tracklet's label ID. +int32 label + +# Number of frames it is being tracked for. +int32 age + +# Status of tracklet. +int32 status + +# srcImgDetection detection that is tracked. +vision_msgs/ObjectHypothesis srcImgDetectionHypothesis +vision_msgs/BoundingBox2D srcImgDetectionBBox + +# Spatial coordinates of tracklet. +geometry_msgs/Point spatialCoordinates diff --git a/depthai_ros_msgs/msg/TrackletArray.msg b/depthai_ros_msgs/msg/TrackletArray.msg new file mode 100644 index 00000000..38749ba1 --- /dev/null +++ b/depthai_ros_msgs/msg/TrackletArray.msg @@ -0,0 +1,5 @@ +# header +std_msgs/Header header + +# tracklets +Tracklet[] tracklets \ No newline at end of file