From 9c97b345c406d69b85bae7ed309ad04afa6c28b1 Mon Sep 17 00:00:00 2001 From: Adam Serafin Date: Fri, 11 Aug 2023 13:40:57 +0200 Subject: [PATCH] Iron update 2.7.5 (#367) --- depthai-ros/CHANGELOG.rst | 4 + depthai-ros/CMakeLists.txt | 2 +- depthai-ros/package.xml | 2 +- depthai_bridge/CMakeLists.txt | 2 +- .../include/depthai_bridge/ImageConverter.hpp | 2 +- .../include/depthai_bridge/ImuConverter.hpp | 77 ++++++++++++++++++- depthai_bridge/package.xml | 2 +- depthai_bridge/src/ImuConverter.cpp | 5 +- depthai_descriptions/CMakeLists.txt | 2 +- depthai_descriptions/package.xml | 2 +- depthai_examples/CMakeLists.txt | 2 +- depthai_examples/package.xml | 2 +- depthai_examples/src/depth_crop_control.cpp | 6 +- depthai_examples/src/rgb_stereo_node.cpp | 10 +-- .../src/stereo_inertial_publisher.cpp | 36 ++++----- depthai_examples/src/stereo_publisher.cpp | 8 +- .../src/yolov4_spatial_publisher.cpp | 10 +-- depthai_filters/CMakeLists.txt | 2 +- depthai_filters/package.xml | 2 +- depthai_filters/src/detection2d_overlay.cpp | 2 +- depthai_filters/src/segmentation_overlay.cpp | 2 +- depthai_filters/src/spatial_bb.cpp | 2 +- depthai_filters/src/utils.cpp | 2 +- depthai_filters/src/wls_filter.cpp | 2 +- depthai_ros_driver/CMakeLists.txt | 2 +- .../dai_nodes/nn/segmentation.hpp | 2 +- .../depthai_ros_driver/dai_nodes/stereo.hpp | 4 +- .../launch/example_multicam.launch.py | 1 - depthai_ros_driver/package.xml | 3 +- .../src/dai_nodes/nn/segmentation.cpp | 4 +- .../src/dai_nodes/sensors/rgb.cpp | 2 +- depthai_ros_driver/src/dai_nodes/stereo.cpp | 5 +- .../param_handlers/stereo_param_handler.cpp | 2 +- .../src/pipeline/base_types.cpp | 14 ++-- depthai_ros_msgs/CMakeLists.txt | 2 +- depthai_ros_msgs/package.xml | 2 +- 36 files changed, 153 insertions(+), 78 deletions(-) diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index 257723c2..7af7d5e7 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/depthai-ros/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package depthai-ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.5 (2023-08-07) +------------------- +* IMU sync fix + 2.7.4 (2023-06-26) ------------------- * ROS time update diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt index 442342b7..28e63452 100644 --- a/depthai-ros/CMakeLists.txt +++ b/depthai-ros/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS -project(depthai-ros VERSION 2.7.4 LANGUAGES CXX C) +project(depthai-ros VERSION 2.7.5 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml index 97732f87..2be1cd74 100644 --- a/depthai-ros/package.xml +++ b/depthai-ros/package.xml @@ -1,7 +1,7 @@ depthai-ros - 2.7.4 + 2.7.5 The depthai-ros package diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index 4d291c1e..6930d4df 100644 --- a/depthai_bridge/CMakeLists.txt +++ b/depthai_bridge/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS set(CMAKE_POSITION_INDEPENDENT_CODE ON) -project(depthai_bridge VERSION 2.7.4 LANGUAGES CXX C) +project(depthai_bridge VERSION 2.7.5 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp index 834f4e05..6df9253c 100644 --- a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp @@ -6,7 +6,7 @@ #include #include -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" #include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai-shared/common/Point2f.hpp" #include "depthai/device/CalibrationHandler.hpp" diff --git a/depthai_bridge/include/depthai_bridge/ImuConverter.hpp b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp index cc2b1546..6cf7a36d 100644 --- a/depthai_bridge/include/depthai_bridge/ImuConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp @@ -108,17 +108,25 @@ class ImuConverter { } if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL) { - if(accelHist.size() < 3) { + if(accelHist.size() < 3 && gyroHist.size() && rotationHist.size() && magnHist.size()) { continue; } else { - interpolate(accelHist, gyroHist, rotationHist, magnHist, imuMsgs); + if(_enable_rotation) { + interpolate(accelHist, gyroHist, rotationHist, magnHist, imuMsgs); + } else { + interpolate(accelHist, gyroHist, imuMsgs); + } } } else if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_GYRO) { - if(gyroHist.size() < 3) { + if(gyroHist.size() < 3 && accelHist.size() && rotationHist.size() && magnHist.size()) { continue; } else { - interpolate(gyroHist, accelHist, rotationHist, magnHist, imuMsgs); + if(_enable_rotation) { + interpolate(gyroHist, accelHist, rotationHist, magnHist, imuMsgs); + } else { + interpolate(gyroHist, accelHist, imuMsgs); + } } } } @@ -159,6 +167,67 @@ class ImuConverter { msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp); } + template + void CreateUnitMessage(I first, S second, M& msg, std::chrono::_V2::steady_clock::time_point timestamp) { + fillImuMsg(first, msg); + fillImuMsg(second, msg); + + msg.header.frame_id = _frameName; + + msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp); + } + + template + void interpolate(std::deque& interpolated, std::deque& second, std::deque& imuMsgs) { + I interp0, interp1; + S currSecond; + interp0.sequence = -1; + while(interpolated.size()) { + if(interp0.sequence == -1) { + interp0 = interpolated.front(); + interpolated.pop_front(); + } else { + interp1 = interpolated.front(); + interpolated.pop_front(); + // remove std::milli to get in seconds + std::chrono::duration duration_ms = interp1.timestamp.get() - interp0.timestamp.get(); + double dt = duration_ms.count(); + while(second.size()) { + currSecond = second.front(); + if(currSecond.timestamp.get() > interp0.timestamp.get() && currSecond.timestamp.get() <= interp1.timestamp.get()) { + // remove std::milli to get in seconds + std::chrono::duration diff = currSecond.timestamp.get() - interp0.timestamp.get(); + const double alpha = diff.count() / dt; + I interp = lerpImu(interp0, interp1, alpha); + M msg; + std::chrono::_V2::steady_clock::time_point tstamp; + if(_getBaseDeviceTimestamp) + tstamp = currSecond.getTimestampDevice(); + else + tstamp = currSecond.getTimestamp(); + CreateUnitMessage(interp, currSecond, msg, tstamp); + imuMsgs.push_back(msg); + second.pop_front(); + } else if(currSecond.timestamp.get() > interp1.timestamp.get()) { + interp0 = interp1; + if(interpolated.size()) { + interp1 = interpolated.front(); + interpolated.pop_front(); + duration_ms = interp1.timestamp.get() - interp0.timestamp.get(); + dt = duration_ms.count(); + } else { + break; + } + } else { + second.pop_front(); + } + } + interp0 = interp1; + } + } + interpolated.push_back(interp0); + } + template void interpolate(std::deque& interpolated, std::deque& second, std::deque& third, std::deque& fourth, std::deque& imuMsgs) { I interp0, interp1; diff --git a/depthai_bridge/package.xml b/depthai_bridge/package.xml index ee235e1a..9002320b 100644 --- a/depthai_bridge/package.xml +++ b/depthai_bridge/package.xml @@ -1,7 +1,7 @@ depthai_bridge - 2.7.4 + 2.7.5 The depthai_bridge package Sachin Guruswamy diff --git a/depthai_bridge/src/ImuConverter.cpp b/depthai_bridge/src/ImuConverter.cpp index 84771132..e733d0f3 100644 --- a/depthai_bridge/src/ImuConverter.cpp +++ b/depthai_bridge/src/ImuConverter.cpp @@ -97,8 +97,6 @@ void ImuConverter::toRosMsg(std::shared_ptr inData, std::dequepackets.size(); ++i) { auto accel = inData->packets[i].acceleroMeter; auto gyro = inData->packets[i].gyroscope; - auto rot = inData->packets[i].rotationVector; - auto magn = inData->packets[i].magneticField; ImuMsgs::Imu msg; std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) @@ -106,7 +104,8 @@ void ImuConverter::toRosMsg(std::shared_ptr inData, std::deque depthai_descriptions - 2.7.4 + 2.7.5 The depthai_descriptions package Sachin Guruswamy diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index 2ef1933e..84dc5876 100644 --- a/depthai_examples/CMakeLists.txt +++ b/depthai_examples/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS -project(depthai_examples VERSION 2.7.4 LANGUAGES CXX C) +project(depthai_examples VERSION 2.7.5 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/depthai_examples/package.xml b/depthai_examples/package.xml index d73e9184..37140550 100644 --- a/depthai_examples/package.xml +++ b/depthai_examples/package.xml @@ -1,7 +1,7 @@ depthai_examples - 2.7.4 + 2.7.5 The depthai_examples package diff --git a/depthai_examples/src/depth_crop_control.cpp b/depthai_examples/src/depth_crop_control.cpp index 6245912e..b1667516 100644 --- a/depthai_examples/src/depth_crop_control.cpp +++ b/depthai_examples/src/depth_crop_control.cpp @@ -102,8 +102,8 @@ int main() { } // Properties - monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); - monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C); + monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B); monoRight->setResolution(monoRes); monoLeft->setResolution(monoRes); @@ -139,7 +139,7 @@ int main() { dai::rosBridge::ImageConverter depthConverter(cameraName + "_right_camera_optical_frame", true); // TODO(sachin): Modify the calibration based on crop from service - auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight); + auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, monoWidth, monoHeight); dai::rosBridge::BridgePublisher depthPublish( depthQueue, diff --git a/depthai_examples/src/rgb_stereo_node.cpp b/depthai_examples/src/rgb_stereo_node.cpp index d2f22ee9..6576e28a 100644 --- a/depthai_examples/src/rgb_stereo_node.cpp +++ b/depthai_examples/src/rgb_stereo_node.cpp @@ -70,9 +70,9 @@ std::tuple createPipeline(bool lrcheck, // MonoCamera monoLeft->setResolution(monoResolution); - monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B); monoRight->setResolution(monoResolution); - monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C); // StereoDepth stereo->initialConfig.setConfidenceThreshold(confidence); @@ -249,9 +249,9 @@ int main(int argc, char** argv) { dai::rosBridge::ImageConverter depthConverter(tfPrefix + "_right_camera_optical_frame", true); dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", true); - auto stereoCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight); - auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, previewWidth, previewHeight); - auto videoCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, colorWidth, colorHeight); + auto stereoCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, monoWidth, monoHeight); + auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, previewWidth, previewHeight); + auto videoCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, colorWidth, colorHeight); if(useDepth) { depthPublish = std::make_unique>( diff --git a/depthai_examples/src/stereo_inertial_publisher.cpp b/depthai_examples/src/stereo_inertial_publisher.cpp index 08a60e55..a4fc8258 100644 --- a/depthai_examples/src/stereo_inertial_publisher.cpp +++ b/depthai_examples/src/stereo_inertial_publisher.cpp @@ -96,10 +96,10 @@ std::tuple createPipeline(bool enableDepth, // MonoCamera monoLeft->setResolution(monoResolution); - monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B); monoLeft->setFps(stereo_fps); monoRight->setResolution(monoResolution); - monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C); monoRight->setFps(stereo_fps); // StereoDepth @@ -109,7 +109,7 @@ std::tuple createPipeline(bool enableDepth, stereo->setLeftRightCheck(lrcheck); stereo->setExtendedDisparity(extended); stereo->setSubpixel(subpixel); - if(enableDepth && depth_aligned) stereo->setDepthAlign(dai::CameraBoardSocket::RGB); + if(enableDepth && depth_aligned) stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A); // Imu imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 500); @@ -122,7 +122,7 @@ std::tuple createPipeline(bool enableDepth, auto camRgb = pipeline.create(); auto xoutRgb = pipeline.create(); xoutRgb->setStreamName("rgb"); - camRgb->setBoardSocket(dai::CameraBoardSocket::RGB); + camRgb->setBoardSocket(dai::CameraBoardSocket::CAM_A); dai::node::ColorCamera::Properties::SensorResolution rgbResolution; if(rgbResolutionStr == "1080p") { @@ -519,8 +519,8 @@ int main(int argc, char** argv) { imuPublish.addPublisherCallback(); - // auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, monoWidth, monoHeight); - // auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight); + // auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_B, monoWidth, monoHeight); + // auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, monoWidth, monoHeight); // const std::string leftPubName = rectify ? std::string("left/image_rect") : std::string("left/image_raw"); // const std::string rightPubName = rectify ? std::string("right/image_rect") : std::string("right/image_raw"); @@ -529,9 +529,9 @@ int main(int argc, char** argv) { rgbConverter.setUpdateRosBaseTimeOnToRosMsg(); } if(enableDepth) { - auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height); + auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, width, height); auto depthCameraInfo = - depth_aligned ? rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, width, height) : rightCameraInfo; + depth_aligned ? rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, width, height) : rightCameraInfo; auto depthconverter = depth_aligned ? rgbConverter : rightconverter; dai::rosBridge::BridgePublisher depthPublish( @@ -549,7 +549,7 @@ int main(int argc, char** argv) { depthPublish.addPublisherCallback(); if(depth_aligned) { - auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, width, height); + auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, width, height); auto imgQueue = device->getOutputQueue("rgb", 30, false); dai::rosBridge::BridgePublisher rgbPublish( imgQueue, @@ -564,7 +564,7 @@ int main(int argc, char** argv) { if(enableSpatialDetection) { auto previewQueue = device->getOutputQueue("preview", 30, false); auto detectionQueue = device->getOutputQueue("detections", 30, false); - auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, previewWidth, previewHeight); + auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, previewWidth, previewHeight); dai::rosBridge::BridgePublisher previewPublish( previewQueue, @@ -588,8 +588,8 @@ int main(int argc, char** argv) { } rclcpp::spin(node); } else { - auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, width, height); - auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height); + auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_B, width, height); + auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, width, height); auto leftQueue = device->getOutputQueue("left", 30, false); auto rightQueue = device->getOutputQueue("right", 30, false); @@ -616,10 +616,10 @@ int main(int argc, char** argv) { } else { std::string tfSuffix = depth_aligned ? "_rgb_camera_optical_frame" : "_right_camera_optical_frame"; dai::rosBridge::DisparityConverter dispConverter(tfPrefix + tfSuffix, 880, 7.5, 20, 2000); // TODO(sachin): undo hardcoding of baseline - auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height); + auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, width, height); auto disparityCameraInfo = - depth_aligned ? rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, 1280, 720) : rightCameraInfo; + depth_aligned ? rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, 1280, 720) : rightCameraInfo; auto depthconverter = depth_aligned ? rgbConverter : rightconverter; dai::rosBridge::BridgePublisher dispPublish( stereoQueue, @@ -632,7 +632,7 @@ int main(int argc, char** argv) { dispPublish.addPublisherCallback(); if(depth_aligned) { - auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, width, height); + auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, width, height); auto imgQueue = device->getOutputQueue("rgb", 30, false); dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false); dai::rosBridge::BridgePublisher rgbPublish( @@ -647,7 +647,7 @@ int main(int argc, char** argv) { if(enableSpatialDetection) { auto previewQueue = device->getOutputQueue("preview", 30, false); auto detectionQueue = device->getOutputQueue("detections", 30, false); - auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, previewWidth, previewHeight); + auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, previewWidth, previewHeight); dai::rosBridge::BridgePublisher previewPublish( previewQueue, @@ -671,8 +671,8 @@ int main(int argc, char** argv) { } rclcpp::spin(node); } else { - auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, width, height); - auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height); + auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_B, width, height); + auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, width, height); auto leftQueue = device->getOutputQueue("left", 30, false); auto rightQueue = device->getOutputQueue("right", 30, false); diff --git a/depthai_examples/src/stereo_publisher.cpp b/depthai_examples/src/stereo_publisher.cpp index 8e0df7b4..73be3fad 100644 --- a/depthai_examples/src/stereo_publisher.cpp +++ b/depthai_examples/src/stereo_publisher.cpp @@ -65,9 +65,9 @@ std::tuple createPipeline( // MonoCamera monoLeft->setResolution(monoResolution); - monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B); monoRight->setResolution(monoResolution); - monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C); // StereoDepth stereo->initialConfig.setConfidenceThreshold(confidence); @@ -147,7 +147,7 @@ int main(int argc, char** argv) { } dai::rosBridge::ImageConverter converter(tfPrefix + "_left_camera_optical_frame", true); - auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, monoWidth, monoHeight); + auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_B, monoWidth, monoHeight); dai::rosBridge::BridgePublisher leftPublish( leftQueue, node, @@ -160,7 +160,7 @@ int main(int argc, char** argv) { leftPublish.addPublisherCallback(); dai::rosBridge::ImageConverter rightconverter(tfPrefix + "_right_camera_optical_frame", true); - auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight); + auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, monoWidth, monoHeight); dai::rosBridge::BridgePublisher rightPublish( rightQueue, diff --git a/depthai_examples/src/yolov4_spatial_publisher.cpp b/depthai_examples/src/yolov4_spatial_publisher.cpp index f6838a31..1d7df58e 100644 --- a/depthai_examples/src/yolov4_spatial_publisher.cpp +++ b/depthai_examples/src/yolov4_spatial_publisher.cpp @@ -75,16 +75,16 @@ dai::Pipeline createPipeline(bool spatial_camera, bool syncNN, bool subpixel, st } monoLeft->setResolution(monoResolution); - monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B); monoRight->setResolution(monoResolution); - monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C); /// setting node configs stereo->initialConfig.setConfidenceThreshold(confidence); stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->initialConfig.setLeftRightCheckThreshold(LRchecktresh); stereo->setSubpixel(subpixel); - stereo->setDepthAlign(dai::CameraBoardSocket::RGB); + stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A); spatialDetectionNetwork->setBlobPath(nnPath); spatialDetectionNetwork->setConfidenceThreshold(0.5f); @@ -225,7 +225,7 @@ int main(int argc, char** argv) { } dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false); - auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, -1, -1); + auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, -1, -1); dai::rosBridge::BridgePublisher rgbPublish(colorQueue, node, std::string("color/image"), @@ -247,7 +247,7 @@ int main(int argc, char** argv) { 30); dai::rosBridge::ImageConverter depthConverter(tfPrefix + "_right_camera_optical_frame", true); - auto rightCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height); + auto rightCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, width, height); dai::rosBridge::BridgePublisher depthPublish( depthQueue, node, diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index 1b11fd77..9a7c9327 100644 --- a/depthai_filters/CMakeLists.txt +++ b/depthai_filters/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(depthai_filters VERSION 2.7.4 LANGUAGES CXX C) +project(depthai_filters VERSION 2.7.5 LANGUAGES CXX C) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/depthai_filters/package.xml b/depthai_filters/package.xml index e5afa787..a4684998 100644 --- a/depthai_filters/package.xml +++ b/depthai_filters/package.xml @@ -2,7 +2,7 @@ depthai_filters - 2.7.4 + 2.7.5 Depthai filters package Adam Serafin MIT diff --git a/depthai_filters/src/detection2d_overlay.cpp b/depthai_filters/src/detection2d_overlay.cpp index 8ad65b36..c6f667ab 100644 --- a/depthai_filters/src/detection2d_overlay.cpp +++ b/depthai_filters/src/detection2d_overlay.cpp @@ -1,6 +1,6 @@ #include "depthai_filters/detection2d_overlay.hpp" -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" #include "depthai_filters/utils.hpp" namespace depthai_filters { diff --git a/depthai_filters/src/segmentation_overlay.cpp b/depthai_filters/src/segmentation_overlay.cpp index 7f939b2f..a0668cfe 100644 --- a/depthai_filters/src/segmentation_overlay.cpp +++ b/depthai_filters/src/segmentation_overlay.cpp @@ -1,6 +1,6 @@ #include "depthai_filters/segmentation_overlay.hpp" -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" #include "depthai_filters/utils.hpp" namespace depthai_filters { diff --git a/depthai_filters/src/spatial_bb.cpp b/depthai_filters/src/spatial_bb.cpp index 5a3a7f0f..5ebe2363 100644 --- a/depthai_filters/src/spatial_bb.cpp +++ b/depthai_filters/src/spatial_bb.cpp @@ -1,6 +1,6 @@ #include "depthai_filters/spatial_bb.hpp" -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" #include "depthai_filters/utils.hpp" #include "geometry_msgs/msg/point32.hpp" #include "opencv2/opencv.hpp" diff --git a/depthai_filters/src/utils.cpp b/depthai_filters/src/utils.cpp index 1e6b9cea..5e32e8cf 100644 --- a/depthai_filters/src/utils.cpp +++ b/depthai_filters/src/utils.cpp @@ -1,6 +1,6 @@ #include "depthai_filters/utils.hpp" -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" #include "rclcpp/rclcpp.hpp" namespace depthai_filters { diff --git a/depthai_filters/src/wls_filter.cpp b/depthai_filters/src/wls_filter.cpp index b00a5ccb..a01193bf 100644 --- a/depthai_filters/src/wls_filter.cpp +++ b/depthai_filters/src/wls_filter.cpp @@ -2,7 +2,7 @@ #include -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" #include "depthai_filters/utils.hpp" namespace depthai_filters { diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index 920dc33f..71502987 100644 --- a/depthai_ros_driver/CMakeLists.txt +++ b/depthai_ros_driver/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.22) -project(depthai_ros_driver VERSION 2.7.4) +project(depthai_ros_driver VERSION 2.7.5) set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_BUILD_SHARED_LIBS ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/segmentation.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/segmentation.hpp index ff5ef19b..c59acca1 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/segmentation.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/segmentation.hpp @@ -4,7 +4,7 @@ #include #include -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" #include "depthai_ros_driver/dai_nodes/base_node.hpp" #include "image_transport/camera_publisher.hpp" #include "image_transport/image_transport.hpp" diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp index 1973efd2..c3d7f062 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp @@ -53,8 +53,8 @@ class Stereo : public BaseNode { rclcpp::Node* node, std::shared_ptr pipeline, std::shared_ptr device, - StereoSensorInfo leftInfo = StereoSensorInfo{"left", dai::CameraBoardSocket::LEFT}, - StereoSensorInfo rightInfo = StereoSensorInfo{"right", dai::CameraBoardSocket::RIGHT}); + StereoSensorInfo leftInfo = StereoSensorInfo{"left", dai::CameraBoardSocket::CAM_B}, + StereoSensorInfo rightInfo = StereoSensorInfo{"right", dai::CameraBoardSocket::CAM_C}); ~Stereo(); void updateParams(const std::vector& params) override; void setupQueues(std::shared_ptr device) override; diff --git a/depthai_ros_driver/launch/example_multicam.launch.py b/depthai_ros_driver/launch/example_multicam.launch.py index 77806be2..f9258642 100644 --- a/depthai_ros_driver/launch/example_multicam.launch.py +++ b/depthai_ros_driver/launch/example_multicam.launch.py @@ -16,7 +16,6 @@ def launch_setup(context, *args, **kwargs): depthai_prefix = get_package_share_directory("depthai_ros_driver") params_file = os.path.join(depthai_prefix, "config", "multicam_example.yaml") - # put mx_ids here cams = ["oak_d_w", "oak_d_lite"] nodes = [] i=0.0 diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index 99b05e63..a137d208 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -2,7 +2,7 @@ depthai_ros_driver - 2.7.4 + 2.7.5 Depthai ROS Monolithic node. Adam Serafin Sachin Guruswamy @@ -29,6 +29,7 @@ depthai_ros_msgs depthai_examples pluginlib + ament_cmake diff --git a/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp b/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp index b1211e0b..91885caf 100644 --- a/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp +++ b/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp @@ -1,7 +1,7 @@ #include "depthai_ros_driver/dai_nodes/nn/segmentation.hpp" #include "camera_info_manager/camera_info_manager.hpp" -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" #include "depthai/device/DataQueue.hpp" #include "depthai/device/Device.hpp" #include "depthai/pipeline/Pipeline.hpp" @@ -66,7 +66,7 @@ void Segmentation::setupQueues(std::shared_ptr device) { infoManager->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), *imageConverter, device, - dai::CameraBoardSocket::RGB, + dai::CameraBoardSocket::CAM_A, imageManip->initialConfig.getResizeWidth(), imageManip->initialConfig.getResizeWidth())); diff --git a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp index 52425001..44fdcbd2 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp @@ -20,7 +20,7 @@ namespace dai_nodes { RGB::RGB(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline, - dai::CameraBoardSocket socket = dai::CameraBoardSocket::RGB, + dai::CameraBoardSocket socket = dai::CameraBoardSocket::CAM_A, sensor_helpers::ImageSensor sensor = {"IMX378", {"12mp", "4k"}, true}, bool publish = true) : BaseNode(daiNodeName, node, pipeline) { diff --git a/depthai_ros_driver/src/dai_nodes/stereo.cpp b/depthai_ros_driver/src/dai_nodes/stereo.cpp index 6e087ccf..49a681d7 100644 --- a/depthai_ros_driver/src/dai_nodes/stereo.cpp +++ b/depthai_ros_driver/src/dai_nodes/stereo.cpp @@ -1,7 +1,7 @@ #include "depthai_ros_driver/dai_nodes/stereo.hpp" #include "camera_info_manager/camera_info_manager.hpp" -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" #include "depthai/device/DataQueue.hpp" #include "depthai/device/DeviceBase.hpp" #include "depthai/pipeline/Pipeline.hpp" @@ -161,6 +161,9 @@ void Stereo::setupStereoQueue(std::shared_ptr device) { ph->getParam("i_height")); auto calibHandler = device->readCalibration(); info.p[3] = calibHandler.getBaselineDistance() * 10.0; // baseline in mm + for(auto& d : info.d) { + d = 0.0; + } stereoIM->setCameraInfo(info); if(ph->getParam("i_low_bandwidth")) { diff --git a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp index 7400a46d..e2018de5 100644 --- a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp @@ -57,7 +57,7 @@ void StereoParamHandler::declareParams(std::shared_ptr s stereo->setLeftRightCheck(declareAndLogParam("i_lr_check", true)); int width = 1280; int height = 720; - dai::CameraBoardSocket socket = dai::CameraBoardSocket::RIGHT; + dai::CameraBoardSocket socket = dai::CameraBoardSocket::CAM_C; if(declareAndLogParam("i_align_depth", true)) { try { width = getROSNode()->get_parameter("rgb.i_width").as_int(); diff --git a/depthai_ros_driver/src/pipeline/base_types.cpp b/depthai_ros_driver/src/pipeline/base_types.cpp index 6432fb72..41ba908e 100644 --- a/depthai_ros_driver/src/pipeline/base_types.cpp +++ b/depthai_ros_driver/src/pipeline/base_types.cpp @@ -25,7 +25,7 @@ std::vector> RGB::createPipeline(rclcpp::No auto nType = utils::getValFromMap(nTypeUpCase, nnTypeMap); std::vector> daiNodes; - auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::RGB); + auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::CAM_A); switch(nType) { case NNType::None: break; @@ -51,7 +51,7 @@ std::vector> RGBD::createPipeline(rclcpp::N auto nType = utils::getValFromMap(nTypeUpCase, nnTypeMap); std::vector> daiNodes; - auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::RGB); + auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::CAM_A); auto stereo = std::make_unique("stereo", node, pipeline, device); switch(nType) { case NNType::None: @@ -81,9 +81,9 @@ std::vector> RGBStereo::createPipeline(rclc auto nType = utils::getValFromMap(nTypeUpCase, nnTypeMap); std::vector> daiNodes; - auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::RGB); - auto left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::LEFT); - auto right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::RIGHT); + auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::CAM_A); + auto left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::CAM_B); + auto right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::CAM_C); switch(nType) { case NNType::None: break; @@ -108,8 +108,8 @@ std::vector> Stereo::createPipeline(rclcpp: std::shared_ptr pipeline, const std::string& /*nnType*/) { std::vector> daiNodes; - auto left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::LEFT); - auto right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::RIGHT); + auto left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::CAM_B); + auto right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::CAM_C); daiNodes.push_back(std::move(left)); daiNodes.push_back(std::move(right)); return daiNodes; diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index fd4c0851..9210b25b 100644 --- a/depthai_ros_msgs/CMakeLists.txt +++ b/depthai_ros_msgs/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS -project(depthai_ros_msgs VERSION 2.7.4) +project(depthai_ros_msgs VERSION 2.7.5) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml index 5f7a3663..9b444079 100644 --- a/depthai_ros_msgs/package.xml +++ b/depthai_ros_msgs/package.xml @@ -1,7 +1,7 @@ depthai_ros_msgs - 2.7.4 + 2.7.5 Package to keep interface independent of the driver Sachin Guruswamy