From 11f8784be662a09a45c0591b3d39a2d0b7c61a66 Mon Sep 17 00:00:00 2001 From: Adam Serafin Date: Fri, 12 May 2023 16:21:57 +0200 Subject: [PATCH] Imu improvements - Humble (#305) --- README.md | 23 ++ depthai-ros/CHANGELOG.rst | 3 + depthai-ros/CMakeLists.txt | 2 +- depthai-ros/package.xml | 2 +- depthai_bridge/CMakeLists.txt | 2 +- .../include/depthai_bridge/ImuConverter.hpp | 158 +++++++++++- .../include/depthai_bridge/depthaiUtility.hpp | 14 - depthai_bridge/package.xml | 2 +- depthai_bridge/src/ImuConverter.cpp | 241 ++++++------------ depthai_descriptions/CMakeLists.txt | 2 +- depthai_descriptions/package.xml | 2 +- depthai_examples/CMakeLists.txt | 2 +- .../launch/rgb_publisher.launch.py | 125 --------- .../launch/yolov4_publisher.launch.py | 2 +- depthai_examples/package.xml | 2 +- .../src/yolov4_spatial_publisher.cpp | 12 +- depthai_filters/CMakeLists.txt | 2 +- depthai_filters/package.xml | 2 +- depthai_ros_driver/CMakeLists.txt | 3 +- depthai_ros_driver/config/camera.yaml | 14 + .../dai_nodes/sensors/imu.hpp | 12 +- .../param_handlers/imu_param_handler.hpp | 10 +- depthai_ros_driver/launch/rgbd_pcl.launch.py | 9 +- depthai_ros_driver/package.xml | 4 +- .../src/dai_nodes/sensors/imu.cpp | 71 +++++- .../src/param_handlers/imu_param_handler.cpp | 45 +++- .../param_handlers/stereo_param_handler.cpp | 3 +- depthai_ros_driver/src/pipeline_generator.cpp | 2 +- depthai_ros_msgs/CMakeLists.txt | 6 +- depthai_ros_msgs/msg/ImuWithMagneticField.msg | 3 + depthai_ros_msgs/package.xml | 2 +- 31 files changed, 420 insertions(+), 362 deletions(-) delete mode 100644 depthai_examples/launch/rgb_publisher.launch.py create mode 100644 depthai_ros_msgs/msg/ImuWithMagneticField.msg diff --git a/README.md b/README.md index 4206eb88..3c949c40 100644 --- a/README.md +++ b/README.md @@ -148,6 +148,29 @@ Additionally you can set `i.output_isp: false` to use `video` output and set cus ![](docs/param_rgb.gif) #### Setting Stereo parameters ![](docs/param_stereo.gif) + +#### Setting IMU parameters +Parameters: +* `i_acc_freq: 400` - Accelerometer sensor frequency +* `i_acc_cov: 0.0` - Accelerometer covariance +* `i_batch_report_threshold: 1` - Batch report size +* `i_enable_rotation: false` - Whether to enable rotation vector & magnetometer data (available when using IMU_WITH_MAGN/ IMU_WITH_MAGN_SPLIT message type) +* `i_gyro_cov: 0.0` - Gyroscope covariance +* `i_gyro_freq: 400` - Gyroscope frequency +* `i_mag_cov: 0.0` - Magnetometer covariance +* `i_mag_freq: 100` - Magnetometer frequency +* `i_max_batch_reports: 10` - Max reports per batch +* `i_message_type: IMU` - ROS publisher type: + * `IMU` - sensor_msgs/Imu + * `IMU_WITH_MAG` - depthai_ros_msgs/ImuWithMagneticField + * `IMU_WITH_MAG_SPLIT` - two publishers - sensor_msgs/Imu & sensor_msgs/MagneticField +* `i_rot_cov: -1.0` - Rotation covariance +* `i_rot_freq: 400` - Rotation frequency +* `i_sync_method: LINEAR_INTERPOLATE_ACCEL` - sync method. Available options: + * `COPY` + * `LINEAR_INTERPOLATE_GYRO` + * `LINEAR_INTERPOLATE_ACCEL` + #### Stopping/starting camera for power saving/reconfiguration ![](docs/start_stop.gif) diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index 6250cc01..d7a61e1b 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/depthai-ros/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package depthai-ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.2 (20230-5-08) +* IMU improvements + 2.7.1 (2023-03-29) ------------------- * Add custom output size option for streams diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt index c92f39af..d69c7aa3 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.1 LANGUAGES CXX C) +project(depthai-ros VERSION 2.7.2 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml index ebf7599b..e3f6e0e6 100644 --- a/depthai-ros/package.xml +++ b/depthai-ros/package.xml @@ -1,7 +1,7 @@ depthai-ros - 2.7.1 + 2.7.2 The depthai-ros package diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index 8ea7bf87..41041145 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.1 LANGUAGES CXX C) +project(depthai_bridge VERSION 2.7.2 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 fa2b52b9..e74a6f99 100644 --- a/depthai_bridge/include/depthai_bridge/ImuConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp @@ -9,8 +9,11 @@ #include "depthai-shared/datatype/RawIMUData.hpp" #include "depthai/pipeline/datatype/IMUData.hpp" +#include "depthai_bridge/depthaiUtility.hpp" +#include "depthai_ros_msgs/msg/imu_with_magnetic_field.hpp" #include "rclcpp/time.hpp" #include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/magnetic_field.hpp" namespace dai { @@ -26,20 +29,165 @@ class ImuConverter { ImuConverter(const std::string& frameName, ImuSyncMethod syncMode = ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL, double linear_accel_cov = 0.0, - double angular_velocity_cov = 0.0); + double angular_velocity_cov = 0.0, + double rotation_cov = 0.0, + double magnetic_field_cov = 0.0, + bool enable_rotation = false); ~ImuConverter(); - void toRosMsg(std::shared_ptr inData, std::deque& outImuMsg); + void toRosMsg(std::shared_ptr inData, std::deque& outImuMsgs); + void toRosDaiMsg(std::shared_ptr inData, std::deque& outImuMsgs); + + template + T lerp(const T& a, const T& b, const double t) { + return a * (1.0 - t) + b * t; + } + + template + T lerpImu(const T& a, const T& b, const double t) { + T res; + res.x = lerp(a.x, b.x, t); + res.y = lerp(a.y, b.y, t); + res.z = lerp(a.z, b.z, t); + return res; + } private: - void FillImuData_LinearInterpolation(std::vector& imuPackets, std::deque& imuMsgs); - ImuMsgs::Imu CreateUnitMessage(dai::IMUReportAccelerometer accel, dai::IMUReportGyroscope gyro); + template + void FillImuData_LinearInterpolation(std::vector& imuPackets, std::deque& imuMsgs) { + static std::deque accelHist; + static std::deque gyroHist; + static std::deque rotationHist; + static std::deque magnHist; + + for(int i = 0; i < imuPackets.size(); ++i) { + if(accelHist.size() == 0) { + accelHist.push_back(imuPackets[i].acceleroMeter); + } else if(accelHist.back().sequence != imuPackets[i].acceleroMeter.sequence) { + accelHist.push_back(imuPackets[i].acceleroMeter); + } + + if(gyroHist.size() == 0) { + gyroHist.push_back(imuPackets[i].gyroscope); + } else if(gyroHist.back().sequence != imuPackets[i].gyroscope.sequence) { + gyroHist.push_back(imuPackets[i].gyroscope); + } + + if(_enable_rotation && rotationHist.size() == 0) { + rotationHist.push_back(imuPackets[i].rotationVector); + } else if(_enable_rotation && rotationHist.back().sequence != imuPackets[i].rotationVector.sequence) { + rotationHist.push_back(imuPackets[i].rotationVector); + } else { + rotationHist.resize(accelHist.size()); + } + + if(_enable_rotation && magnHist.size() == 0) { + magnHist.push_back(imuPackets[i].magneticField); + } else if(_enable_rotation && magnHist.back().sequence != imuPackets[i].magneticField.sequence) { + magnHist.push_back(imuPackets[i].magneticField); + } else { + magnHist.resize(accelHist.size()); + } + + if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL) { + if(accelHist.size() < 3) { + continue; + } else { + interpolate(accelHist, gyroHist, rotationHist, magnHist, imuMsgs); + } + + } else if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_GYRO) { + if(gyroHist.size() < 3) { + continue; + } else { + interpolate(gyroHist, accelHist, rotationHist, magnHist, imuMsgs); + } + } + } + } uint32_t _sequenceNum; - double _linear_accel_cov, _angular_velocity_cov; + double _linear_accel_cov, _angular_velocity_cov, _rotation_cov, _magnetic_field_cov; + bool _enable_rotation; const std::string _frameName = ""; ImuSyncMethod _syncMode; std::chrono::time_point _steadyBaseTime; rclcpp::Time _rosBaseTime; + + void fillImuMsg(dai::IMUReportAccelerometer report, ImuMsgs::Imu& msg); + void fillImuMsg(dai::IMUReportGyroscope report, ImuMsgs::Imu& msg); + void fillImuMsg(dai::IMUReportRotationVectorWAcc report, ImuMsgs::Imu& msg); + void fillImuMsg(dai::IMUReportMagneticField report, ImuMsgs::Imu& msg); + + void fillImuMsg(dai::IMUReportAccelerometer report, depthai_ros_msgs::msg::ImuWithMagneticField& msg); + void fillImuMsg(dai::IMUReportGyroscope report, depthai_ros_msgs::msg::ImuWithMagneticField& msg); + void fillImuMsg(dai::IMUReportRotationVectorWAcc report, depthai_ros_msgs::msg::ImuWithMagneticField& msg); + void fillImuMsg(dai::IMUReportMagneticField report, depthai_ros_msgs::msg::ImuWithMagneticField& msg); + + template + void CreateUnitMessage(I first, S second, T third, F fourth, M& msg, dai::Timestamp timestamp) { + fillImuMsg(first, msg); + fillImuMsg(second, msg); + fillImuMsg(third, msg); + fillImuMsg(fourth, msg); + + msg.header.frame_id = _frameName; + + msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp.get()); + } + + template + void interpolate(std::deque& interpolated, std::deque& second, std::deque& third, std::deque& fourth, std::deque& imuMsgs) { + I interp0, interp1; + S currSecond; + T currThird; + F currFourth; + 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(); + currThird = third.front(); + currFourth = fourth.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; + CreateUnitMessage(interp, currSecond, currThird, currFourth, msg, currSecond.timestamp); + imuMsgs.push_back(msg); + second.pop_front(); + third.pop_front(); + fourth.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(); + third.pop_front(); + fourth.pop_front(); + } + } + interp0 = interp1; + } + } + interpolated.push_back(interp0); + } }; } // namespace ros diff --git a/depthai_bridge/include/depthai_bridge/depthaiUtility.hpp b/depthai_bridge/include/depthai_bridge/depthaiUtility.hpp index 744edae9..cda9132c 100644 --- a/depthai_bridge/include/depthai_bridge/depthaiUtility.hpp +++ b/depthai_bridge/include/depthai_bridge/depthaiUtility.hpp @@ -84,19 +84,5 @@ inline rclcpp::Time getFrameTime(rclcpp::Time rclBaseTime, return rclStamp; } -template -T lerp(const T& a, const T& b, const double t) { - return a * (1.0 - t) + b * t; -} - -template -T lerpImu(const T& a, const T& b, const double t) { - T res; - res.x = lerp(a.x, b.x, t); - res.y = lerp(a.y, b.y, t); - res.z = lerp(a.z, b.z, t); - return res; -} - } // namespace ros } // namespace dai \ No newline at end of file diff --git a/depthai_bridge/package.xml b/depthai_bridge/package.xml index 299f2af8..49e75867 100644 --- a/depthai_bridge/package.xml +++ b/depthai_bridge/package.xml @@ -1,7 +1,7 @@ depthai_bridge - 2.7.1 + 2.7.2 The depthai_bridge package Sachin Guruswamy diff --git a/depthai_bridge/src/ImuConverter.cpp b/depthai_bridge/src/ImuConverter.cpp index e140cfd3..baa937bd 100644 --- a/depthai_bridge/src/ImuConverter.cpp +++ b/depthai_bridge/src/ImuConverter.cpp @@ -7,11 +7,20 @@ namespace dai { namespace ros { -ImuConverter::ImuConverter(const std::string& frameName, ImuSyncMethod syncMode, double linear_accel_cov, double angular_velocity_cov) +ImuConverter::ImuConverter(const std::string& frameName, + ImuSyncMethod syncMode, + double linear_accel_cov, + double angular_velocity_cov, + double rotation_cov, + double magnetic_field_cov, + bool enable_rotation) : _frameName(frameName), _syncMode(syncMode), _linear_accel_cov(linear_accel_cov), _angular_velocity_cov(angular_velocity_cov), + _rotation_cov(rotation_cov), + _magnetic_field_cov(magnetic_field_cov), + _enable_rotation(enable_rotation), _sequenceNum(0), _steadyBaseTime(std::chrono::steady_clock::now()) { _rosBaseTime = rclcpp::Clock().now(); @@ -19,195 +28,87 @@ ImuConverter::ImuConverter(const std::string& frameName, ImuSyncMethod syncMode, ImuConverter::~ImuConverter() = default; -void ImuConverter::FillImuData_LinearInterpolation(std::vector& imuPackets, std::deque& imuMsgs) { - static std::deque accelHist; - static std::deque gyroHist; - - for(int i = 0; i < imuPackets.size(); ++i) { - if(accelHist.size() == 0) { - accelHist.push_back(imuPackets[i].acceleroMeter); - } else if(accelHist.back().sequence != imuPackets[i].acceleroMeter.sequence) { - accelHist.push_back(imuPackets[i].acceleroMeter); - } +void ImuConverter::fillImuMsg(dai::IMUReportAccelerometer report, ImuMsgs::Imu& msg) { + msg.linear_acceleration.x = report.x; + msg.linear_acceleration.y = report.y; + msg.linear_acceleration.z = report.z; + msg.linear_acceleration_covariance = {_linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov}; +} - if(gyroHist.size() == 0) { - gyroHist.push_back(imuPackets[i].gyroscope); - } else if(gyroHist.back().sequence != imuPackets[i].gyroscope.sequence) { - gyroHist.push_back(imuPackets[i].gyroscope); - } +void ImuConverter::fillImuMsg(dai::IMUReportGyroscope report, ImuMsgs::Imu& msg) { + msg.angular_velocity.x = report.x; + msg.angular_velocity.y = report.y; + msg.angular_velocity.z = report.z; + msg.angular_velocity_covariance = {_angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov}; +} - if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL) { - if(accelHist.size() < 3) { - continue; - } else { - dai::IMUReportAccelerometer accel0, accel1; - dai::IMUReportGyroscope currGyro; - accel0.sequence = -1; - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", " Interpolating LINEAR_INTERPOLATE_ACCEL mode " << std::endl); - while(accelHist.size()) { - if(accel0.sequence == -1) { - accel0 = accelHist.front(); - accelHist.pop_front(); - } else { - accel1 = accelHist.front(); - accelHist.pop_front(); - - // remove std::milli to get in seconds - std::chrono::duration duration_ms = accel1.timestamp.get() - accel0.timestamp.get(); - double dt = duration_ms.count(); - - if(!gyroHist.size()) { - DEPTHAI_ROS_WARN_STREAM("IMU INTERPOLATION: ", "Gyro data not found. Dropping accel data points"); - } - while(gyroHist.size()) { - currGyro = gyroHist.front(); - - DEPTHAI_ROS_DEBUG_STREAM( - "IMU INTERPOLATION: ", - "Accel 0: Seq => " << accel0.sequence << " timeStamp => " << (accel0.timestamp.get() - _steadyBaseTime).count() << std::endl); - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", - "currGyro 0: Seq => " << currGyro.sequence << "timeStamp => " - << (currGyro.timestamp.get() - _steadyBaseTime).count() << std::endl); - DEPTHAI_ROS_DEBUG_STREAM( - "IMU INTERPOLATION: ", - "Accel 1: Seq => " << accel1.sequence << " timeStamp => " << (accel1.timestamp.get() - _steadyBaseTime).count() << std::endl); - if(currGyro.timestamp.get() > accel0.timestamp.get() && currGyro.timestamp.get() <= accel1.timestamp.get()) { - // remove std::milli to get in seconds - std::chrono::duration diff = currGyro.timestamp.get() - accel0.timestamp.get(); - const double alpha = diff.count() / dt; - dai::IMUReportAccelerometer interpAccel = lerpImu(accel0, accel1, alpha); - imuMsgs.push_back(CreateUnitMessage(interpAccel, currGyro)); - gyroHist.pop_front(); - } else if(currGyro.timestamp.get() > accel1.timestamp.get()) { - accel0 = accel1; - if(accelHist.size()) { - accel1 = accelHist.front(); - accelHist.pop_front(); - duration_ms = accel1.timestamp.get() - accel0.timestamp.get(); - dt = duration_ms.count(); - } else { - break; - } - } else { - gyroHist.pop_front(); - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", "Droppinh GYRO with old timestamps which are below accel10 \n"); - } - } - // gyroHist.push_back(currGyro); // Undecided whether this is necessary - accel0 = accel1; - } - } - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", "Count ->" << i << " Placing Accel 0 Seq Number :" << accel0.sequence << std::endl); - - accelHist.push_back(accel0); - } - } else if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_GYRO) { - if(gyroHist.size() < 3) { - continue; - } else { - dai::IMUReportGyroscope gyro0, gyro1; - dai::IMUReportAccelerometer currAccel; - gyro0.sequence = -1; - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", " Interpolating LINEAR_INTERPOLATE_GYRO mode " << std::endl); - while(gyroHist.size()) { - if(gyro0.sequence == -1) { - gyro0 = gyroHist.front(); - gyroHist.pop_front(); - } else { - gyro1 = gyroHist.front(); - gyroHist.pop_front(); - // remove std::milli to get in seconds - std::chrono::duration duration_ms = gyro1.timestamp.get() - gyro0.timestamp.get(); - double dt = duration_ms.count(); - - if(!accelHist.size()) { - DEPTHAI_ROS_WARN_STREAM("IMU INTERPOLATION: ", "Accel data not found. Dropping data"); - } - while(accelHist.size()) { - currAccel = accelHist.front(); - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", - "gyro 0: Seq => " << gyro0.sequence << std::endl - << " timeStamp => " << (gyro0.timestamp.get() - _steadyBaseTime).count() - << std::endl); - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", - "currAccel 0: Seq => " << currAccel.sequence << std::endl - << " timeStamp => " << (currAccel.timestamp.get() - _steadyBaseTime).count() - << std::endl); - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", - "gyro 1: Seq => " << gyro1.sequence << std::endl - << " timeStamp => " << (gyro1.timestamp.get() - _steadyBaseTime).count() - << std::endl); - if(currAccel.timestamp.get() > gyro0.timestamp.get() && currAccel.timestamp.get() <= gyro1.timestamp.get()) { - // remove std::milli to get in seconds - std::chrono::duration diff = currAccel.timestamp.get() - gyro0.timestamp.get(); - const double alpha = diff.count() / dt; - dai::IMUReportGyroscope interpGyro = lerpImu(gyro0, gyro1, alpha); - imuMsgs.push_back(CreateUnitMessage(currAccel, interpGyro)); - accelHist.pop_front(); - } else if(currAccel.timestamp.get() > gyro1.timestamp.get()) { - gyro0 = gyro1; - if(gyroHist.size()) { - gyro1 = gyroHist.front(); - gyroHist.pop_front(); - duration_ms = gyro1.timestamp.get() - gyro0.timestamp.get(); - dt = duration_ms.count(); - } else { - break; - } - } else { - accelHist.pop_front(); - DEPTHAI_ROS_DEBUG_STREAM("IMU INTERPOLATION: ", "Droppinh ACCEL with old timestamps which are below accel10 \n"); - } - } - gyro0 = gyro1; - } - } - gyroHist.push_back(gyro0); - } - } +void ImuConverter::fillImuMsg(dai::IMUReportRotationVectorWAcc report, ImuMsgs::Imu& msg) { + if(_enable_rotation) { + msg.orientation.x = report.i; + msg.orientation.y = report.j; + msg.orientation.z = report.k; + msg.orientation.w = report.real; + msg.orientation_covariance = {_rotation_cov, 0.0, 0.0, 0.0, _rotation_cov, 0.0, 0.0, 0.0, _rotation_cov}; + } else { + msg.orientation.x = 0.0; + msg.orientation.y = 0.0; + msg.orientation.z = 0.0; + msg.orientation.w = 1.0; + msg.orientation_covariance = {-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; } } -ImuMsgs::Imu ImuConverter::CreateUnitMessage(dai::IMUReportAccelerometer accel, dai::IMUReportGyroscope gyro) { - ImuMsgs::Imu interpMsg; - interpMsg.linear_acceleration.x = accel.x; - interpMsg.linear_acceleration.y = accel.y; - interpMsg.linear_acceleration.z = accel.z; +void ImuConverter::fillImuMsg(dai::IMUReportMagneticField report, ImuMsgs::Imu& msg) { + return; +} - interpMsg.angular_velocity.x = gyro.x; - interpMsg.angular_velocity.y = gyro.y; - interpMsg.angular_velocity.z = gyro.z; +void ImuConverter::fillImuMsg(dai::IMUReportAccelerometer report, depthai_ros_msgs::msg::ImuWithMagneticField& msg) { + fillImuMsg(report, msg.imu); +} - interpMsg.orientation.x = 0.0; - interpMsg.orientation.y = 0.0; - interpMsg.orientation.z = 0.0; - interpMsg.orientation.w = 1.0; +void ImuConverter::fillImuMsg(dai::IMUReportGyroscope report, depthai_ros_msgs::msg::ImuWithMagneticField& msg) { + fillImuMsg(report, msg.imu); +} - interpMsg.orientation_covariance = {-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - interpMsg.linear_acceleration_covariance = {_linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov}; - interpMsg.angular_velocity_covariance = {_angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov}; +void ImuConverter::fillImuMsg(dai::IMUReportRotationVectorWAcc report, depthai_ros_msgs::msg::ImuWithMagneticField& msg) { + fillImuMsg(report, msg.imu); +} - interpMsg.header.frame_id = _frameName; +void ImuConverter::fillImuMsg(dai::IMUReportMagneticField report, depthai_ros_msgs::msg::ImuWithMagneticField& msg) { + msg.field.magnetic_field.x = report.x; + msg.field.magnetic_field.y = report.y; + msg.field.magnetic_field.z = report.z; + msg.field.magnetic_field_covariance = {_magnetic_field_cov, 0.0, 0.0, 0.0, _magnetic_field_cov, 0.0, 0.0, 0.0, _magnetic_field_cov}; +} - if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL) { - interpMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, gyro.timestamp.get()); - } else if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_GYRO) { - interpMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, accel.timestamp.get()); +void ImuConverter::toRosMsg(std::shared_ptr inData, std::deque& outImuMsgs) { + if(_syncMode != ImuSyncMethod::COPY) { + FillImuData_LinearInterpolation(inData->packets, outImuMsgs); } else { - interpMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, accel.timestamp.get()); + 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; + CreateUnitMessage(accel, gyro, rot, magn, msg, accel.timestamp); + outImuMsgs.push_back(msg); + } } - - return interpMsg; } -void ImuConverter::toRosMsg(std::shared_ptr inData, std::deque& outImuMsgs) { +void ImuConverter::toRosDaiMsg(std::shared_ptr inData, std::deque& outImuMsgs) { if(_syncMode != ImuSyncMethod::COPY) { FillImuData_LinearInterpolation(inData->packets, outImuMsgs); } else { for(int i = 0; i < inData->packets.size(); ++i) { auto accel = inData->packets[i].acceleroMeter; auto gyro = inData->packets[i].gyroscope; - outImuMsgs.push_back(CreateUnitMessage(accel, gyro)); + auto rot = inData->packets[i].rotationVector; + auto magn = inData->packets[i].magneticField; + depthai_ros_msgs::msg::ImuWithMagneticField msg; + CreateUnitMessage(accel, gyro, rot, magn, msg, accel.timestamp); + outImuMsgs.push_back(msg); } } } diff --git a/depthai_descriptions/CMakeLists.txt b/depthai_descriptions/CMakeLists.txt index 4da38635..541f4678 100644 --- a/depthai_descriptions/CMakeLists.txt +++ b/depthai_descriptions/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(depthai_descriptions VERSION 2.7.1) +project(depthai_descriptions VERSION 2.7.2) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/depthai_descriptions/package.xml b/depthai_descriptions/package.xml index 07757bc8..2e74de72 100644 --- a/depthai_descriptions/package.xml +++ b/depthai_descriptions/package.xml @@ -1,7 +1,7 @@ depthai_descriptions - 2.7.1 + 2.7.2 The depthai_descriptions package Sachin Guruswamy diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index 58da94e7..c5a097b4 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.1 LANGUAGES CXX C) +project(depthai_examples VERSION 2.7.2 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/depthai_examples/launch/rgb_publisher.launch.py b/depthai_examples/launch/rgb_publisher.launch.py deleted file mode 100644 index 0e5cebab..00000000 --- a/depthai_examples/launch/rgb_publisher.launch.py +++ /dev/null @@ -1,125 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription, launch_description_sources -from launch.actions import IncludeLaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -import launch_ros.actions -import launch_ros.descriptions - - -def generate_launch_description(): - - urdf_launch_dir = os.path.join(get_package_share_directory('depthai_descriptions'), 'launch') - - - camera_model = LaunchConfiguration('camera_model', default = 'OAK-D') - tf_prefix = LaunchConfiguration('tf_prefix', default = 'oak') - base_frame = LaunchConfiguration('base_frame', default = 'oak-d_frame') - parent_frame = LaunchConfiguration('parent_frame', default = 'oak-d-base-frame') - cam_pos_x = LaunchConfiguration('cam_pos_x', default = '0.0') - cam_pos_y = LaunchConfiguration('cam_pos_y', default = '0.0') - cam_pos_z = LaunchConfiguration('cam_pos_z', default = '0.0') - cam_roll = LaunchConfiguration('cam_roll', default = '0.0') - cam_pitch = LaunchConfiguration('cam_pitch', default = '0.0') - cam_yaw = LaunchConfiguration('cam_yaw', default = '0.0') - - camera_param_uri = LaunchConfiguration('camera_param_uri', default = 'package://depthai_examples/params/camera') - - declare_camera_model_cmd = DeclareLaunchArgument( - 'camera_model', - default_value=camera_model, - description='The model of the camera. Using a wrong camera model can disable camera features. Valid models: `OAK-D, OAK-D-LITE`.') - - declare_tf_prefix_cmd = DeclareLaunchArgument( - 'tf_prefix', - default_value=tf_prefix, - description='The name of the camera. It can be different from the camera model and it will be used in naming TF.') - - declare_base_frame_cmd = DeclareLaunchArgument( - 'base_frame', - default_value=base_frame, - description='Name of the base link.') - - declare_parent_frame_cmd = DeclareLaunchArgument( - 'parent_frame', - default_value=parent_frame, - description='Name of the parent link from other a robot TF for example that can be connected to the base of the OAK.') - - declare_pos_x_cmd = DeclareLaunchArgument( - 'cam_pos_x', - default_value=cam_pos_x, - description='Position X of the camera with respect to the base frame.') - - declare_pos_y_cmd = DeclareLaunchArgument( - 'cam_pos_y', - default_value=cam_pos_y, - description='Position Y of the camera with respect to the base frame.') - - declare_pos_z_cmd = DeclareLaunchArgument( - 'cam_pos_z', - default_value=cam_pos_z, - description='Position Z of the camera with respect to the base frame.') - - declare_roll_cmd = DeclareLaunchArgument( - 'cam_roll', - default_value=cam_roll, - description='Roll orientation of the camera with respect to the base frame.') - - declare_pitch_cmd = DeclareLaunchArgument( - 'cam_pitch', - default_value=cam_pitch, - description='Pitch orientation of the camera with respect to the base frame.') - - declare_yaw_cmd = DeclareLaunchArgument( - 'cam_yaw', - default_value=cam_yaw, - description='Yaw orientation of the camera with respect to the base frame.') - - declare_camera_param_uri_cmd = DeclareLaunchArgument( - 'camera_param_uri', - default_value=camera_param_uri, - description='Sending camera yaml path') - - urdf_launch = IncludeLaunchDescription( - launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(urdf_launch_dir, 'urdf_launch.py')), - launch_arguments={'tf_prefix' : tf_prefix, - 'camera_model': camera_model, - 'base_frame' : base_frame, - 'parent_frame': parent_frame, - 'cam_pos_x' : cam_pos_x, - 'cam_pos_y' : cam_pos_y, - 'cam_pos_z' : cam_pos_z, - 'cam_roll' : cam_roll, - 'cam_pitch' : cam_pitch, - 'cam_yaw' : cam_yaw}.items()) - - mobilenet_node = launch_ros.actions.Node( - package='depthai_examples', executable='rgb_node', - output='screen', - parameters=[{'tf_prefix': tf_prefix}, - {'camera_param_uri': camera_param_uri}]) - - ld = LaunchDescription() - ld.add_action(declare_tf_prefix_cmd) - ld.add_action(declare_camera_model_cmd) - - ld.add_action(declare_base_frame_cmd) - ld.add_action(declare_parent_frame_cmd) - - ld.add_action(declare_pos_x_cmd) - ld.add_action(declare_pos_y_cmd) - ld.add_action(declare_pos_z_cmd) - ld.add_action(declare_roll_cmd) - ld.add_action(declare_pitch_cmd) - ld.add_action(declare_yaw_cmd) - - ld.add_action(declare_camera_param_uri_cmd) - - ld.add_action(mobilenet_node) - ld.add_action(urdf_launch) - - return ld - diff --git a/depthai_examples/launch/yolov4_publisher.launch.py b/depthai_examples/launch/yolov4_publisher.launch.py index d21cc6d9..683379ba 100644 --- a/depthai_examples/launch/yolov4_publisher.launch.py +++ b/depthai_examples/launch/yolov4_publisher.launch.py @@ -153,7 +153,7 @@ def generate_launch_description(): {'sync_nn': sync_nn}, {'nnName': nnName}, {'resourceBaseFolder': resourceBaseFolder}, - {'monoResolution': monoResolution}]) + {'monoResolution': monoResolution}, {'spatial_camera': spatial_camera}]) rviz_node = launch_ros.actions.Node( diff --git a/depthai_examples/package.xml b/depthai_examples/package.xml index 0c4e3e66..965e7122 100644 --- a/depthai_examples/package.xml +++ b/depthai_examples/package.xml @@ -1,7 +1,7 @@ depthai_examples - 2.7.1 + 2.7.2 The depthai_examples package diff --git a/depthai_examples/src/yolov4_spatial_publisher.cpp b/depthai_examples/src/yolov4_spatial_publisher.cpp index 21ebafaf..f6838a31 100644 --- a/depthai_examples/src/yolov4_spatial_publisher.cpp +++ b/depthai_examples/src/yolov4_spatial_publisher.cpp @@ -17,9 +17,9 @@ #include "depthai/device/Device.hpp" #include "depthai/pipeline/Pipeline.hpp" #include "depthai/pipeline/node/ColorCamera.hpp" +#include "depthai/pipeline/node/DetectionNetwork.hpp" #include "depthai/pipeline/node/MonoCamera.hpp" #include "depthai/pipeline/node/SpatialDetectionNetwork.hpp" -#include "depthai/pipeline/node/DetectionNetwork.hpp" #include "depthai/pipeline/node/StereoDepth.hpp" #include "depthai/pipeline/node/XLinkOut.hpp" @@ -38,19 +38,18 @@ dai::Pipeline createPipeline(bool spatial_camera, bool syncNN, bool subpixel, st dai::Pipeline pipeline; dai::node::MonoCamera::Properties::SensorResolution monoResolution; auto colorCam = pipeline.create(); - auto camRgb = pipeline.create(); // non spatial add in + auto camRgb = pipeline.create(); // non spatial add in auto spatialDetectionNetwork = pipeline.create(); auto detectionNetwork = pipeline.create(); auto monoLeft = pipeline.create(); auto monoRight = pipeline.create(); auto stereo = pipeline.create(); - // create xlink connections auto xoutRgb = pipeline.create(); auto xoutDepth = pipeline.create(); auto xoutNN = pipeline.create(); - auto nnOut = pipeline.create(); // non spatial add in + auto nnOut = pipeline.create(); // non spatial add in xoutRgb->setStreamName("preview"); xoutNN->setStreamName("detections"); @@ -112,12 +111,11 @@ dai::Pipeline createPipeline(bool spatial_camera, bool syncNN, bool subpixel, st else colorCam->preview.link(xoutRgb->input); - spatialDetectionNetwork->out.link(xoutNN->input); + spatialDetectionNetwork->out.link(xoutNN->input); stereo->depth.link(spatialDetectionNetwork->inputDepth); spatialDetectionNetwork->passthroughDepth.link(xoutDepth->input); - } - else { + } else { xoutRgb->setStreamName("rgb"); nnOut->setStreamName("detections"); diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index 625543c2..d40d3406 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.1 LANGUAGES CXX C) +project(depthai_filters VERSION 2.7.2 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 53f9daa1..4a7143c2 100644 --- a/depthai_filters/package.xml +++ b/depthai_filters/package.xml @@ -2,7 +2,7 @@ depthai_filters - 2.7.1 + 2.7.2 Depthai filters package Adam Serafin MIT diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index d4429456..07622d61 100644 --- a/depthai_ros_driver/CMakeLists.txt +++ b/depthai_ros_driver/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 3.22) -project(depthai_ros_driver VERSION 2.7.1) +project(depthai_ros_driver VERSION 2.7.2) set(CMAKE_POSITION_INDEPENDENT_CODE ON) +set(CMAKE_BUILD_SHARED_LIBS ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/depthai_ros_driver/config/camera.yaml b/depthai_ros_driver/config/camera.yaml index 06120fec..aa4c7ad6 100644 --- a/depthai_ros_driver/config/camera.yaml +++ b/depthai_ros_driver/config/camera.yaml @@ -14,6 +14,20 @@ i_pipeline_dump: false i_calibration_dump: false i_external_calibration_path: '' + imu: + i_acc_freq: 400 + i_acc_cov: 0.0 + i_batch_report_threshold: 1 + i_enable_rotation: false + i_gyro_cov: 0.0 + i_gyro_freq: 400 + i_mag_cov: 0.0 + i_mag_freq: 100 + i_max_batch_reports: 10 + i_message_type: IMU + i_rot_cov: -1.0 + i_rot_freq: 400 + i_sync_method: LINEAR_INTERPOLATE_ACCEL left: i_board_socket_id: 1 i_calibration_file: '' diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/imu.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/imu.hpp index ae834a9c..6d9c851c 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/imu.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/imu.hpp @@ -1,8 +1,10 @@ #pragma once #include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "depthai_ros_msgs/msg/imu_with_magnetic_field.hpp" #include "rclcpp/publisher.hpp" #include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/magnetic_field.hpp" namespace dai { class Pipeline; @@ -31,7 +33,7 @@ namespace dai_nodes { class Imu : public BaseNode { public: - explicit Imu(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline); + explicit Imu(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline, std::shared_ptr device); ~Imu(); void updateParams(const std::vector& params) override; void setupQueues(std::shared_ptr device) override; @@ -42,8 +44,12 @@ class Imu : public BaseNode { private: std::unique_ptr imuConverter; - void imuQCB(const std::string& name, const std::shared_ptr& data); - rclcpp::Publisher::SharedPtr imuPub; + void imuRosQCB(const std::string& name, const std::shared_ptr& data); + void imuDaiRosQCB(const std::string& name, const std::shared_ptr& data); + void imuMagQCB(const std::string& name, const std::shared_ptr& data); + rclcpp::Publisher::SharedPtr rosImuPub; + rclcpp::Publisher::SharedPtr magPub; + rclcpp::Publisher::SharedPtr daiImuPub; std::shared_ptr imuNode; std::unique_ptr ph; std::shared_ptr imuQ; diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp index 89313ecd..713bf148 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp @@ -5,6 +5,7 @@ #include #include "depthai/pipeline/datatype/CameraControl.hpp" +#include "depthai_bridge/ImuConverter.hpp" #include "depthai_ros_driver/param_handlers/base_param_handler.hpp" namespace dai { @@ -20,12 +21,19 @@ class Parameter; namespace depthai_ros_driver { namespace param_handlers { +namespace imu { +enum class ImuMsgType { IMU, IMU_WITH_MAG, IMU_WITH_MAG_SPLIT }; +} class ImuParamHandler : public BaseParamHandler { public: explicit ImuParamHandler(rclcpp::Node* node, const std::string& name); ~ImuParamHandler(); - void declareParams(std::shared_ptr imu); + void declareParams(std::shared_ptr imu, const std::string& imuType); dai::CameraControl setRuntimeParams(const std::vector& params) override; + std::unordered_map imuSyncMethodMap; + std::unordered_map imuMessagetTypeMap; + imu::ImuMsgType getMsgType(); + dai::ros::ImuSyncMethod getSyncMethod(); }; } // namespace param_handlers } // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/launch/rgbd_pcl.launch.py b/depthai_ros_driver/launch/rgbd_pcl.launch.py index dc156e70..2bb9c10b 100644 --- a/depthai_ros_driver/launch/rgbd_pcl.launch.py +++ b/depthai_ros_driver/launch/rgbd_pcl.launch.py @@ -5,7 +5,7 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import LoadComposableNodes, Node from launch_ros.descriptions import ComposableNode from launch.conditions import IfCondition @@ -15,7 +15,10 @@ def launch_setup(context, *args, **kwargs): depthai_prefix = get_package_share_directory("depthai_ros_driver") name = LaunchConfiguration('name').perform(context) - + rgb_topic_name = name+'/rgb/image_raw' + print(LaunchConfiguration('rectify_rgb').perform(context)) + if LaunchConfiguration('rectify_rgb').perform(context)=='true': + rgb_topic_name = name +'/rgb/image_rect' return [ IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -56,7 +59,7 @@ def launch_setup(context, *args, **kwargs): plugin='depth_image_proc::PointCloudXyzrgbNode', name='point_cloud_xyzrgb_node', remappings=[('depth_registered/image_rect', name+'/stereo/image_raw'), - ('rgb/image_rect_color', name+'/rgb/image_rect'), + ('rgb/image_rect_color', rgb_topic_name), ('rgb/camera_info', name+'/rgb/camera_info'), ('points', name+'/points')] ), diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index 3cee51f6..d37f2a4d 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -2,7 +2,7 @@ depthai_ros_driver - 2.7.1 + 2.7.2 Depthai ROS Monolithic node. Adam Serafin Sachin Guruswamy @@ -26,6 +26,8 @@ depthai depthai_bridge depthai_descriptions + depthai_ros_msgs + depthai_examples ament_cmake diff --git a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp index d06864f8..034dc95c 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp @@ -7,16 +7,19 @@ #include "depthai/pipeline/node/XLinkOut.hpp" #include "depthai_bridge/ImuConverter.hpp" #include "depthai_ros_driver/param_handlers/imu_param_handler.hpp" +#include "depthai_ros_driver/utils.hpp" +#include "depthai_ros_msgs/msg/imu_with_magnetic_field.hpp" #include "rclcpp/node.hpp" namespace depthai_ros_driver { namespace dai_nodes { -Imu::Imu(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline) : BaseNode(daiNodeName, node, pipeline) { +Imu::Imu(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline, std::shared_ptr device) + : BaseNode(daiNodeName, node, pipeline) { RCLCPP_DEBUG(node->get_logger(), "Creating node %s", daiNodeName.c_str()); setNames(); imuNode = pipeline->create(); ph = std::make_unique(node, daiNodeName); - ph->declareParams(imuNode); + ph->declareParams(imuNode, device->getConnectedIMU()); setXinXout(pipeline); RCLCPP_DEBUG(node->get_logger(), "Node %s created", daiNodeName.c_str()); } @@ -34,27 +37,77 @@ void Imu::setXinXout(std::shared_ptr pipeline) { void Imu::setupQueues(std::shared_ptr device) { imuQ = device->getOutputQueue(imuQName, ph->getParam("i_max_q_size"), false); auto tfPrefix = std::string(getROSNode()->get_name()) + "_" + getName(); - auto imuMode = static_cast(0); - imuConverter = std::make_unique(tfPrefix + "_frame", imuMode, 0.0, 0.0); - imuQ->addCallback(std::bind(&Imu::imuQCB, this, std::placeholders::_1, std::placeholders::_2)); - imuPub = getROSNode()->create_publisher("~/" + getName() + "/data", 10); + auto imuMode = ph->getSyncMethod(); + imuConverter = std::make_unique(tfPrefix + "_frame", + imuMode, + ph->getParam("i_acc_cov"), + ph->getParam("i_gyro_cov"), + ph->getParam("i_rot_cov"), + ph->getParam("i_mag_cov"), + ph->getParam("i_enable_rotation")); + param_handlers::imu::ImuMsgType msgType = ph->getMsgType(); + switch(msgType) { + case param_handlers::imu::ImuMsgType::IMU: { + rosImuPub = getROSNode()->create_publisher("~/" + getName() + "/data", 10); + imuQ->addCallback(std::bind(&Imu::imuRosQCB, this, std::placeholders::_1, std::placeholders::_2)); + break; + } + case param_handlers::imu::ImuMsgType::IMU_WITH_MAG: { + daiImuPub = getROSNode()->create_publisher("~/" + getName() + "/data", 10); + imuQ->addCallback(std::bind(&Imu::imuDaiRosQCB, this, std::placeholders::_1, std::placeholders::_2)); + break; + } + case param_handlers::imu::ImuMsgType::IMU_WITH_MAG_SPLIT: { + rosImuPub = getROSNode()->create_publisher("~/" + getName() + "/data", 10); + magPub = getROSNode()->create_publisher("~/" + getName() + "/mag", 10); + imuQ->addCallback(std::bind(&Imu::imuMagQCB, this, std::placeholders::_1, std::placeholders::_2)); + break; + } + default: { + break; + } + } } void Imu::closeQueues() { imuQ->close(); } -void Imu::imuQCB(const std::string& /*name*/, const std::shared_ptr& data) { +void Imu::imuRosQCB(const std::string& /*name*/, const std::shared_ptr& data) { auto imuData = std::dynamic_pointer_cast(data); std::deque deq; imuConverter->toRosMsg(imuData, deq); while(deq.size() > 0) { auto currMsg = deq.front(); - imuPub->publish(currMsg); + rosImuPub->publish(currMsg); + deq.pop_front(); + } +} +void Imu::imuDaiRosQCB(const std::string& /*name*/, const std::shared_ptr& data) { + auto imuData = std::dynamic_pointer_cast(data); + std::deque deq; + imuConverter->toRosDaiMsg(imuData, deq); + while(deq.size() > 0) { + auto currMsg = deq.front(); + daiImuPub->publish(currMsg); + deq.pop_front(); + } +} +void Imu::imuMagQCB(const std::string& /*name*/, const std::shared_ptr& data) { + auto imuData = std::dynamic_pointer_cast(data); + std::deque deq; + imuConverter->toRosDaiMsg(imuData, deq); + while(deq.size() > 0) { + auto currMsg = deq.front(); + sensor_msgs::msg::Imu imu = currMsg.imu; + sensor_msgs::msg::MagneticField field = currMsg.field; + imu.header = currMsg.header; + field.header = currMsg.header; + rosImuPub->publish(imu); + magPub->publish(field); deq.pop_front(); } } - void Imu::link(dai::Node::Input in, int /*linkType*/) { imuNode->out.link(in); } diff --git a/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp b/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp index d69e1fc0..8dda80cd 100644 --- a/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp @@ -1,6 +1,8 @@ #include "depthai_ros_driver/param_handlers/imu_param_handler.hpp" #include "depthai/pipeline/node/IMU.hpp" +#include "depthai_bridge/ImuConverter.hpp" +#include "depthai_ros_driver/utils.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/node.hpp" @@ -8,13 +10,44 @@ namespace depthai_ros_driver { namespace param_handlers { ImuParamHandler::ImuParamHandler(rclcpp::Node* node, const std::string& name) : BaseParamHandler(node, name) {} ImuParamHandler::~ImuParamHandler() = default; -void ImuParamHandler::declareParams(std::shared_ptr imu) { - imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 400); - imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, 400); - // imu->enableIMUSensor(dai::IMUSensor::ROTATION_VECTOR, 400); - imu->setBatchReportThreshold(1); - imu->setMaxBatchReports(10); +void ImuParamHandler::declareParams(std::shared_ptr imu, const std::string& imuType) { + imuSyncMethodMap = { + {"COPY", dai::ros::ImuSyncMethod::COPY}, + {"LINEAR_INTERPOLATE_GYRO", dai::ros::ImuSyncMethod::LINEAR_INTERPOLATE_GYRO}, + {"LINEAR_INTERPOLATE_ACCEL", dai::ros::ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL}, + }; + imuMessagetTypeMap = { + {"IMU", imu::ImuMsgType::IMU}, {"IMU_WITH_MAG", imu::ImuMsgType::IMU_WITH_MAG}, {"IMU_WITH_MAG_SPLIT", imu::ImuMsgType::IMU_WITH_MAG_SPLIT}}; + declareAndLogParam("i_message_type", "IMU"); + declareAndLogParam("i_sync_method", "LINEAR_INTERPOLATE_ACCEL"); + declareAndLogParam("i_acc_cov", 0.0); + declareAndLogParam("i_gyro_cov", 0.0); + declareAndLogParam("i_rot_cov", -1.0); + declareAndLogParam("i_mag_cov", 0.0); + bool rotationAvailable = imuType == "BNO086"; + if(declareAndLogParam("i_enable_rotation", false)) { + if(rotationAvailable) { + imu->enableIMUSensor(dai::IMUSensor::ROTATION_VECTOR, declareAndLogParam("i_rot_freq", 400)); + imu->enableIMUSensor(dai::IMUSensor::MAGNETOMETER_CALIBRATED, declareAndLogParam("i_mag_freq", 100)); + } else { + RCLCPP_ERROR(getROSNode()->get_logger(), "Rotation enabled but not available with current sensor"); + declareAndLogParam("i_enable_rotation", false, true); + } + } + imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, declareAndLogParam("i_acc_freq", 400)); + imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, declareAndLogParam("i_gyro_freq", 400)); + imu->setBatchReportThreshold(declareAndLogParam("i_batch_report_threshold", 1)); + imu->setMaxBatchReports(declareAndLogParam("i_max_batch_reports", 10)); } + +dai::ros::ImuSyncMethod ImuParamHandler::getSyncMethod() { + return utils::getValFromMap(utils::getUpperCaseStr(getParam("i_sync_method")), imuSyncMethodMap); +} + +imu::ImuMsgType ImuParamHandler::getMsgType() { + return utils::getValFromMap(utils::getUpperCaseStr(getParam("i_message_type")), imuMessagetTypeMap); +} + dai::CameraControl ImuParamHandler::setRuntimeParams(const std::vector& /*params*/) { dai::CameraControl ctrl; return ctrl; 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 db4a7005..78532081 100644 --- a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp @@ -84,8 +84,7 @@ void StereoParamHandler::declareParams(std::shared_ptr s stereo->setExtendedDisparity(declareAndLogParam("i_extended_disp", false)); stereo->setRectifyEdgeFillColor(declareAndLogParam("i_rectify_edge_fill_color", 0)); auto config = stereo->initialConfig.get(); - config.costMatching.disparityWidth = - utils::getValFromMap(declareAndLogParam("i_disparity_width", "DISPARITY_96"), disparityWidthMap); + config.costMatching.disparityWidth = utils::getValFromMap(declareAndLogParam("i_disparity_width", "DISPARITY_96"), disparityWidthMap); if(!config.algorithmControl.enableExtended) { config.costMatching.enableCompanding = declareAndLogParam("i_enable_companding", false); } diff --git a/depthai_ros_driver/src/pipeline_generator.cpp b/depthai_ros_driver/src/pipeline_generator.cpp index 053d021b..11d9eb86 100644 --- a/depthai_ros_driver/src/pipeline_generator.cpp +++ b/depthai_ros_driver/src/pipeline_generator.cpp @@ -127,7 +127,7 @@ std::vector> PipelineGenerator::createPipel } } if(enableImu) { - auto imu = std::make_unique("imu", node, pipeline); + auto imu = std::make_unique("imu", node, pipeline, device); daiNodes.push_back(std::move(imu)); } diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index 02738394..cba6e91f 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.1) +project(depthai_ros_msgs VERSION 2.7.2) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) @@ -16,6 +16,7 @@ message(STATUS "------------------------------------------") find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(vision_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) # set(BUILD_TOOL_INCLUDE_DIRS ${ament_INCLUDE_DIRS}) find_package(rosidl_default_generators REQUIRED) @@ -23,13 +24,14 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/AutoFocusCtrl.msg" "msg/HandLandmark.msg" "msg/HandLandmarkArray.msg" + "msg/ImuWithMagneticField.msg" # "msg/ImageMarker.msg" # "msg/ImageMarkerArray.msg" "msg/SpatialDetection.msg" "msg/SpatialDetectionArray.msg" "srv/TriggerNamed.srv" "srv/NormalizedImageCrop.srv" - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs vision_msgs + DEPENDENCIES builtin_interfaces geometry_msgs std_msgs vision_msgs sensor_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/depthai_ros_msgs/msg/ImuWithMagneticField.msg b/depthai_ros_msgs/msg/ImuWithMagneticField.msg new file mode 100644 index 00000000..6c41b315 --- /dev/null +++ b/depthai_ros_msgs/msg/ImuWithMagneticField.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +sensor_msgs/Imu imu +sensor_msgs/MagneticField field \ No newline at end of file diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml index 0e4401fd..16dd76d7 100644 --- a/depthai_ros_msgs/package.xml +++ b/depthai_ros_msgs/package.xml @@ -1,7 +1,7 @@ depthai_ros_msgs - 2.7.1 + 2.7.2 Package to keep interface independent of the driver Sachin Guruswamy