Skip to content

Commit

Permalink
Iron update 2.7.5 (#367)
Browse files Browse the repository at this point in the history
  • Loading branch information
Serafadam authored Aug 11, 2023
1 parent d2a4438 commit 9c97b34
Show file tree
Hide file tree
Showing 36 changed files with 153 additions and 78 deletions.
4 changes: 4 additions & 0 deletions depthai-ros/CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion depthai-ros/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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)

Expand Down
2 changes: 1 addition & 1 deletion depthai-ros/package.xml
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="3">
<name>depthai-ros</name>
<version>2.7.4</version>
<version>2.7.5</version>
<description>The depthai-ros package</description>

<!-- One maintainer tag required, multiple allowed, one person per tag -->
Expand Down
2 changes: 1 addition & 1 deletion depthai_bridge/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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)
Expand Down
2 changes: 1 addition & 1 deletion depthai_bridge/include/depthai_bridge/ImageConverter.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
#include <tuple>
#include <unordered_map>

#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"
Expand Down
77 changes: 73 additions & 4 deletions depthai_bridge/include/depthai_bridge/ImuConverter.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}
}
}
Expand Down Expand Up @@ -159,6 +167,67 @@ class ImuConverter {
msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp);
}

template <typename I, typename S, typename M>
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 <typename I, typename S, typename M>
void interpolate(std::deque<I>& interpolated, std::deque<S>& second, std::deque<M>& 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<double, std::milli> 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<double, std::milli> 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 <typename I, typename S, typename T, typename F, typename M>
void interpolate(std::deque<I>& interpolated, std::deque<S>& second, std::deque<T>& third, std::deque<F>& fourth, std::deque<M>& imuMsgs) {
I interp0, interp1;
Expand Down
2 changes: 1 addition & 1 deletion depthai_bridge/package.xml
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="3">
<name>depthai_bridge</name>
<version>2.7.4</version>
<version>2.7.5</version>
<description>The depthai_bridge package</description>

<maintainer email="sachin@luxonis.com">Sachin Guruswamy</maintainer>
Expand Down
5 changes: 2 additions & 3 deletions depthai_bridge/src/ImuConverter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -97,16 +97,15 @@ void ImuConverter::toRosMsg(std::shared_ptr<dai::IMUData> inData, std::deque<Imu
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;
ImuMsgs::Imu 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);
CreateUnitMessage(accel, gyro, msg, tstamp);

outImuMsgs.push_back(msg);
}
}
Expand Down
2 changes: 1 addition & 1 deletion depthai_descriptions/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.8)
project(depthai_descriptions VERSION 2.7.4)
project(depthai_descriptions VERSION 2.7.5)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
Expand Down
2 changes: 1 addition & 1 deletion depthai_descriptions/package.xml
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="3">
<name>depthai_descriptions</name>
<version>2.7.4</version>
<version>2.7.5</version>
<description>The depthai_descriptions package</description>

<maintainer email="sachin@luxonis.com">Sachin Guruswamy</maintainer>
Expand Down
2 changes: 1 addition & 1 deletion depthai_examples/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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)
Expand Down
2 changes: 1 addition & 1 deletion depthai_examples/package.xml
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="3">
<name>depthai_examples</name>
<version>2.7.4</version>
<version>2.7.5</version>
<description>The depthai_examples package</description>

<!-- One maintainer tag required, multiple allowed, one person per tag -->
Expand Down
6 changes: 3 additions & 3 deletions depthai_examples/src/depth_crop_control.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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<sensor_msgs::msg::Image, dai::ImgFrame> depthPublish(
depthQueue,
Expand Down
10 changes: 5 additions & 5 deletions depthai_examples/src/rgb_stereo_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -70,9 +70,9 @@ std::tuple<dai::Pipeline, int, int> 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);
Expand Down Expand Up @@ -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<dai::rosBridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame>>(
Expand Down
36 changes: 18 additions & 18 deletions depthai_examples/src/stereo_inertial_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -96,10 +96,10 @@ std::tuple<dai::Pipeline, int, int> 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
Expand All @@ -109,7 +109,7 @@ std::tuple<dai::Pipeline, int, int> 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);
Expand All @@ -122,7 +122,7 @@ std::tuple<dai::Pipeline, int, int> createPipeline(bool enableDepth,
auto camRgb = pipeline.create<dai::node::ColorCamera>();
auto xoutRgb = pipeline.create<dai::node::XLinkOut>();
xoutRgb->setStreamName("rgb");
camRgb->setBoardSocket(dai::CameraBoardSocket::RGB);
camRgb->setBoardSocket(dai::CameraBoardSocket::CAM_A);
dai::node::ColorCamera::Properties::SensorResolution rgbResolution;

if(rgbResolutionStr == "1080p") {
Expand Down Expand Up @@ -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");

Expand All @@ -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<sensor_msgs::msg::Image, dai::ImgFrame> depthPublish(
Expand All @@ -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<sensor_msgs::msg::Image, dai::ImgFrame> rgbPublish(
imgQueue,
Expand All @@ -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<sensor_msgs::msg::Image, dai::ImgFrame> previewPublish(
previewQueue,
Expand All @@ -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);
Expand All @@ -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<stereo_msgs::msg::DisparityImage, dai::ImgFrame> dispPublish(
stereoQueue,
Expand All @@ -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<sensor_msgs::msg::Image, dai::ImgFrame> rgbPublish(
Expand All @@ -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<sensor_msgs::msg::Image, dai::ImgFrame> previewPublish(
previewQueue,
Expand All @@ -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);
Expand Down
Loading

0 comments on commit 9c97b34

Please sign in to comment.