From 1454cb694019ba36e430784d32515aa2b8d96d39 Mon Sep 17 00:00:00 2001 From: Adam Serafin Date: Mon, 7 Aug 2023 14:47:18 +0200 Subject: [PATCH] Imu sync fix noetic (#358) --- depthai-ros/CHANGELOG.rst | 4 + depthai-ros/CMakeLists.txt | 2 +- depthai-ros/package.xml | 2 +- depthai_bridge/CMakeLists.txt | 2 +- .../include/depthai_bridge/ImuConverter.hpp | 73 ++++++++++++++++++- depthai_bridge/package.xml | 2 +- depthai_bridge/src/ImuConverter.cpp | 15 ++-- 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 | 32 ++++---- depthai_examples/src/stereo_nodelet.cpp | 8 +- depthai_examples/src/stereo_publisher.cpp | 8 +- .../src/yolov4_spatial_publisher.cpp | 10 +-- depthai_filters/CMakeLists.txt | 2 +- depthai_filters/package.xml | 2 +- .../dai_nodes/nn/detection.hpp | 2 +- .../dai_nodes/nn/spatial_detection.hpp | 6 +- .../depthai_ros_driver/dai_nodes/stereo.hpp | 4 +- depthai_ros_driver/package.xml | 2 +- .../src/dai_nodes/nn/segmentation.cpp | 2 +- .../src/dai_nodes/sensors/rgb.cpp | 2 +- depthai_ros_driver/src/dai_nodes/stereo.cpp | 3 + .../param_handlers/stereo_param_handler.cpp | 4 +- .../src/pipeline/base_types.cpp | 14 ++-- depthai_ros_msgs/CMakeLists.txt | 2 +- depthai_ros_msgs/package.xml | 2 +- 30 files changed, 153 insertions(+), 76 deletions(-) diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index edb4acb9..5ae3c75e 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 f57eb7fa..840622f6 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 7d23703a..4180a17f 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 cb73f9ee..2be1ec1b 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/ImuConverter.hpp b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp index 84efadf7..b16e1590 100644 --- a/depthai_bridge/include/depthai_bridge/ImuConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp @@ -109,14 +109,22 @@ class ImuConverter { if(accelHist.size() < 3) { 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) { continue; } else { - interpolate(gyroHist, accelHist, rotationHist, magnHist, imuMsgs); + if(_enable_rotation) { + interpolate(gyroHist, accelHist, rotationHist, magnHist, imuMsgs); + } else { + interpolate(gyroHist, accelHist, imuMsgs); + } } } } @@ -157,6 +165,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 2f0a979d..1b869381 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 326bced1..8633ab17 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,7 @@ void ImuConverter::toRosMsg(std::shared_ptr inData, std::deque inData, std::deque< for(int i = 0; i < inData->packets.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; depthai_ros_msgs::ImuWithMagneticField msg; std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) tstamp = accel.getTimestampDevice(); else tstamp = accel.getTimestamp(); - - CreateUnitMessage(accel, gyro, rot, magn, msg, tstamp); + if(_enable_rotation) { + auto rot = inData->packets[i].rotationVector; + auto magn = inData->packets[i].magneticField; + CreateUnitMessage(accel, gyro, rot, magn, msg, tstamp); + } else { + CreateUnitMessage(accel, gyro, msg, tstamp); + } outImuMsgs.push_back(msg); } } diff --git a/depthai_descriptions/CMakeLists.txt b/depthai_descriptions/CMakeLists.txt index 12b551b9..b9d01a50 100644 --- a/depthai_descriptions/CMakeLists.txt +++ b/depthai_descriptions/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) -project(depthai_descriptions VERSION 2.7.4 LANGUAGES CXX C) +project(depthai_descriptions VERSION 2.7.5 LANGUAGES CXX C) find_package(catkin REQUIRED diff --git a/depthai_descriptions/package.xml b/depthai_descriptions/package.xml index 188540dd..3c589b6f 100644 --- a/depthai_descriptions/package.xml +++ b/depthai_descriptions/package.xml @@ -1,7 +1,7 @@ depthai_descriptions - 2.7.4 + 2.7.5 The depthai_descriptions package Adam Serafin diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index 38166a8f..61562fa1 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 f027e60c..28d071df 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 a686e52f..2a43e9b5 100644 --- a/depthai_examples/src/depth_crop_control.cpp +++ b/depthai_examples/src/depth_crop_control.cpp @@ -100,8 +100,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); @@ -137,7 +137,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, pnh, diff --git a/depthai_examples/src/rgb_stereo_node.cpp b/depthai_examples/src/rgb_stereo_node.cpp index e37c70a6..6930474f 100644 --- a/depthai_examples/src/rgb_stereo_node.cpp +++ b/depthai_examples/src/rgb_stereo_node.cpp @@ -48,9 +48,9 @@ dai::Pipeline createPipeline(bool lrcheck, bool extended, bool subpixel, int con // 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); @@ -60,7 +60,7 @@ dai::Pipeline createPipeline(bool lrcheck, bool extended, bool subpixel, int con stereo->setLeftRightCheck(lrcheck); stereo->setExtendedDisparity(extended); stereo->setSubpixel(subpixel); - // stereo->setDepthAlign(dai::CameraBoardSocket::RGB); + // stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A); // // Link plugins CAM -> STEREO -> XLINK monoLeft->out.link(stereo->left); monoRight->out.link(stereo->right); @@ -70,7 +70,7 @@ dai::Pipeline createPipeline(bool lrcheck, bool extended, bool subpixel, int con auto colorCam = pipeline.create(); auto xlinkOut = pipeline.create(); xlinkOut->setStreamName("video"); - colorCam->setBoardSocket(dai::CameraBoardSocket::RGB); + colorCam->setBoardSocket(dai::CameraBoardSocket::CAM_A); colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); colorCam->setInterleaved(true); @@ -123,7 +123,7 @@ int main(int argc, char** argv) { std::string color_uri = camera_param_uri + "/" + "color.yaml"; dai::rosBridge::ImageConverter depthConverter(tfPrefix + "_right_camera_optical_frame", true); - auto rgbCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, 1280, 720); + auto rgbCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, 1280, 720); dai::rosBridge::BridgePublisher depthPublish(stereoQueue, pnh, diff --git a/depthai_examples/src/stereo_inertial_publisher.cpp b/depthai_examples/src/stereo_inertial_publisher.cpp index abd924e7..b16b3471 100644 --- a/depthai_examples/src/stereo_inertial_publisher.cpp +++ b/depthai_examples/src/stereo_inertial_publisher.cpp @@ -97,10 +97,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 @@ -110,7 +110,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); @@ -123,7 +123,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); camRgb->setFps(stereo_fps); dai::node::ColorCamera::Properties::SensorResolution rgbResolution; @@ -494,10 +494,10 @@ 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( @@ -515,7 +515,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, @@ -530,7 +530,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, @@ -555,8 +555,8 @@ int main(int argc, char** argv) { ros::spin(); } 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); @@ -584,9 +584,9 @@ int main(int argc, char** argv) { 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, width, height) : rightCameraInfo; + depth_aligned ? rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, width, height) : rightCameraInfo; auto depthconverter = depth_aligned ? rgbConverter : rightconverter; dai::rosBridge::BridgePublisher dispPublish( stereoQueue, @@ -599,7 +599,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( @@ -615,7 +615,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, @@ -640,8 +640,8 @@ int main(int argc, char** argv) { ros::spin(); } 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_nodelet.cpp b/depthai_examples/src/stereo_nodelet.cpp index 20b6056a..5407116c 100644 --- a/depthai_examples/src/stereo_nodelet.cpp +++ b/depthai_examples/src/stereo_nodelet.cpp @@ -93,7 +93,7 @@ class StereoNodelet : public nodelet::Nodelet { } leftConverter = std::make_unique(tfPrefix + "_left_camera_optical_frame", true); - auto leftCameraInfo = leftConverter->calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, monoWidth, monoHeight); + auto leftCameraInfo = leftConverter->calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_B, monoWidth, monoHeight); leftPublish = std::make_unique>( leftQueue, @@ -108,7 +108,7 @@ class StereoNodelet : public nodelet::Nodelet { leftPublish->addPublisherCallback(); rightConverter = std::make_unique(tfPrefix + "_right_camera_optical_frame", true); - auto rightCameraInfo = rightConverter->calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight); + auto rightCameraInfo = rightConverter->calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, monoWidth, monoHeight); rightPublish = std::make_unique>( rightQueue, @@ -187,9 +187,9 @@ class StereoNodelet : public nodelet::Nodelet { // 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); // int maxDisp = 96; // if (extended) maxDisp *= 2; diff --git a/depthai_examples/src/stereo_publisher.cpp b/depthai_examples/src/stereo_publisher.cpp index 2c69eb7b..4c6719fe 100644 --- a/depthai_examples/src/stereo_publisher.cpp +++ b/depthai_examples/src/stereo_publisher.cpp @@ -70,9 +70,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); @@ -154,7 +154,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, pnh, @@ -167,7 +167,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 68a73cfd..fe56f1a9 100644 --- a/depthai_examples/src/yolov4_spatial_publisher.cpp +++ b/depthai_examples/src/yolov4_spatial_publisher.cpp @@ -69,16 +69,16 @@ dai::Pipeline createPipeline(bool syncNN, bool subpixel, std::string nnPath, int } 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); @@ -181,7 +181,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, 416, 416); + auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, 416, 416); rgbPublish = std::make_unique>( colorQueue, pnh, @@ -204,7 +204,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); depthPublish = std::make_unique>( depthQueue, pnh, diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index c7009179..c5a27a9c 100644 --- a/depthai_filters/CMakeLists.txt +++ b/depthai_filters/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) -project(depthai_filters VERSION 2.7.4 LANGUAGES CXX C) +project(depthai_filters VERSION 2.7.5 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/depthai_filters/package.xml b/depthai_filters/package.xml index 5e869716..d63ae397 100644 --- a/depthai_filters/package.xml +++ b/depthai_filters/package.xml @@ -1,7 +1,7 @@ depthai_filters - 2.7.4 + 2.7.5 The depthai_filters package Adam Serafin diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp index c39a6967..aad3a991 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp @@ -64,7 +64,7 @@ class Detection : public BaseNode { imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); infoManager = std::make_shared(ros::NodeHandle(getROSNode(), getName()), "/" + getName()); - infoManager->setCameraInfo(sensor_helpers::getCalibInfo(*imageConverter, device, dai::CameraBoardSocket::RGB, width, height)); + infoManager->setCameraInfo(sensor_helpers::getCalibInfo(*imageConverter, device, dai::CameraBoardSocket::CAM_A, width, height)); ptPub = it.advertiseCamera(getName() + "/passthrough/image_raw", 1); ptQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, ptPub, infoManager)); diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp index 46175bbe..089db08c 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp @@ -67,19 +67,19 @@ class SpatialDetection : public BaseNode { ptImageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); ptImageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); ptInfoMan = std::make_shared(ros::NodeHandle(getROSNode(), getName()), "/" + getName()); - ptInfoMan->setCameraInfo(sensor_helpers::getCalibInfo(*ptImageConverter, device, dai::CameraBoardSocket::RGB, width, height)); + ptInfoMan->setCameraInfo(sensor_helpers::getCalibInfo(*ptImageConverter, device, dai::CameraBoardSocket::CAM_A, width, height)); ptPub = it.advertiseCamera(getName() + "/passthrough/image_raw", 1); ptQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *ptImageConverter, ptPub, ptInfoMan)); } if(ph->getParam("i_enable_passthrough_depth")) { - dai::CameraBoardSocket socket = dai::CameraBoardSocket::RGB; + dai::CameraBoardSocket socket = dai::CameraBoardSocket::CAM_A; bool align; getROSNode().getParam("stereo_i_align_depth", align); if(!align) { tfPrefix = getTFPrefix("right"); - socket = dai::CameraBoardSocket::RIGHT; + socket = dai::CameraBoardSocket::CAM_C; }; ptDepthQ = device->getOutputQueue(ptDepthQName, ph->getParam("i_max_q_size"), false); ptDepthImageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); 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 13d6febe..1d3783d7 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 @@ -54,8 +54,8 @@ class Stereo : public BaseNode { ros::NodeHandle 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(parametersConfig& config) override; void setupQueues(std::shared_ptr device) override; diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index 30c133a8..24e299aa 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 diff --git a/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp b/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp index 6aaee17a..a798ab91 100644 --- a/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp +++ b/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp @@ -63,7 +63,7 @@ void Segmentation::setupQueues(std::shared_ptr device) { imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); infoManager = std::make_shared(ros::NodeHandle(getROSNode(), getName()), "/" + getName()); infoManager->setCameraInfo(sensor_helpers::getCalibInfo( - *imageConverter, device, dai::CameraBoardSocket::RGB, imageManip->initialConfig.getResizeWidth(), imageManip->initialConfig.getResizeWidth())); + *imageConverter, device, dai::CameraBoardSocket::CAM_A, imageManip->initialConfig.getResizeWidth(), imageManip->initialConfig.getResizeWidth())); ptPub = it.advertiseCamera(getName() + "/passthrough/image_raw", 1); ptQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, ptPub, infoManager)); diff --git a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp index 5257c630..0121c216 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, ros::NodeHandle 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), it(node) { diff --git a/depthai_ros_driver/src/dai_nodes/stereo.cpp b/depthai_ros_driver/src/dai_nodes/stereo.cpp index 7c358926..55c2c6cf 100644 --- a/depthai_ros_driver/src/dai_nodes/stereo.cpp +++ b/depthai_ros_driver/src/dai_nodes/stereo.cpp @@ -156,6 +156,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 4439f570..7fe748cc 100644 --- a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp @@ -39,14 +39,14 @@ void StereoParamHandler::declareParams(std::shared_ptr s stereo->setLeftRightCheck(getParam("i_lr_check")); int width = 1280; int height = 720; - dai::CameraBoardSocket socket = dai::CameraBoardSocket::RGB; + dai::CameraBoardSocket socket = dai::CameraBoardSocket::CAM_A; if(getParam("i_align_depth")) { width = getOtherNodeParam("rgb", "i_width", width); height = getOtherNodeParam("rgb", "i_height", height); } else { width = getOtherNodeParam(rightName, "i_width", width); height = getOtherNodeParam(rightName, "i_height", height); - socket = dai::CameraBoardSocket::RIGHT; + socket = dai::CameraBoardSocket::CAM_C; } stereo->setDepthAlign(socket); setParam("i_width", width); diff --git a/depthai_ros_driver/src/pipeline/base_types.cpp b/depthai_ros_driver/src/pipeline/base_types.cpp index 6ce63c97..1a3502bf 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(ros::NodeH 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(ros::Node 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(ros: 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(ros::No 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 20e4916b..ed9fbc2f 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 a206ab5f..8baf22f7 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