Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Imu sync fix noetic #358

Merged
merged 6 commits into from
Aug 7, 2023
Merged

Imu sync fix noetic #358

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
f0cde6c
imu vector fixes
Serafadam Aug 1, 2023
59fc637
remove distortion from stereo
Serafadam Aug 3, 2023
18c2d46
remove stereo distortion
Serafadam Aug 3, 2023
56d4533
update socket names
Serafadam Aug 3, 2023
8706471
format
Serafadam Aug 3, 2023
9175505
bump version
Serafadam Aug 7, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions depthai-ros/CHANGELOG.rst
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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
73 changes: 71 additions & 2 deletions depthai_bridge/include/depthai_bridge/ImuConverter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}
}
}
Expand Down Expand Up @@ -157,6 +165,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
View file
Open in desktop
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
15 changes: 8 additions & 7 deletions depthai_bridge/src/ImuConverter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,16 +97,14 @@ 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 All @@ -122,16 +120,19 @@ void ImuConverter::toRosDaiMsg(std::shared_ptr<dai::IMUData> 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);
}
}
Expand Down
2 changes: 1 addition & 1 deletion depthai_descriptions/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand Down
2 changes: 1 addition & 1 deletion depthai_descriptions/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>depthai_descriptions</name>
<version>2.7.4</version>
<version>2.7.5</version>
<description>The depthai_descriptions package</description>

<maintainer email="adam.serafin@luxonis.com">Adam Serafin</maintainer>
Expand Down
2 changes: 1 addition & 1 deletion depthai_examples/CMakeLists.txt
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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<sensor_msgs::Image, dai::ImgFrame> depthPublish(depthQueue,
pnh,
Expand Down
10 changes: 5 additions & 5 deletions depthai_examples/src/rgb_stereo_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand All @@ -70,7 +70,7 @@ dai::Pipeline createPipeline(bool lrcheck, bool extended, bool subpixel, int con
auto colorCam = pipeline.create<dai::node::ColorCamera>();
auto xlinkOut = pipeline.create<dai::node::XLinkOut>();
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);
Expand Down Expand Up @@ -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<sensor_msgs::Image, dai::ImgFrame> depthPublish(stereoQueue,
pnh,
Expand Down
32 changes: 16 additions & 16 deletions depthai_examples/src/stereo_inertial_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,10 +97,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 @@ -110,7 +110,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 @@ -123,7 +123,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);
camRgb->setFps(stereo_fps);
dai::node::ColorCamera::Properties::SensorResolution rgbResolution;

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