Additional IMU modes - Humble (#693)

Serafadam · web-flow · commit 7d66a47475ec · 2025-03-17T19:00:21.000+01:00
diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst
@@ -1,6 +1,10 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package depthai-ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+2.11.2 (2025-03-17)
+-------------------
+* Add autoexposure region settings
+* Add additional IMU parameters
 
 2.11.1 (2025-03-12)
 -------------------
diff --git 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.11.1 LANGUAGES CXX C)
+project(depthai-ros VERSION 2.11.2 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 
diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai-ros</name>
-  <version>2.11.1</version>
+  <version>2.11.2</version>
   <description>The depthai-ros package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
diff --git 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.11.1 LANGUAGES CXX C)
+project(depthai_bridge VERSION 2.11.2 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
diff --git a/depthai_bridge/package.xml b/depthai_bridge/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_bridge</name>
-  <version>2.11.1</version>
+  <version>2.11.2</version>
   <description>The depthai_bridge package</description>
 
   <maintainer email="adam.serafin@luxonis.com">Adam Serafin</maintainer>
diff --git 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.11.1)
+project(depthai_descriptions VERSION 2.11.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
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_descriptions</name>
-  <version>2.11.1</version>
+  <version>2.11.2</version>
   <description>The depthai_descriptions package</description>
 
   <maintainer email="adam.serafin@luxonis.com">Adam Serafin</maintainer>
diff --git 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.11.1 LANGUAGES CXX C)
+project(depthai_examples VERSION 2.11.2 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
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_examples</name>
-  <version>2.11.1</version>
+  <version>2.11.2</version>
   <description>The depthai_examples package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
diff --git 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.11.1 LANGUAGES CXX C)
+project(depthai_filters VERSION 2.11.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
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>depthai_filters</name>
-  <version>2.11.1</version>
+  <version>2.11.2</version>
   <description>Depthai filters package</description>
   <maintainer email="adam.serafin@luxonis.com">Adam Serafin</maintainer>
   <license>MIT</license>
diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.22)
-project(depthai_ros_driver VERSION 2.11.1)
+project(depthai_ros_driver VERSION 2.11.2)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 set(CMAKE_BUILD_SHARED_LIBS ON)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp
@@ -31,9 +31,12 @@ class ImuParamHandler : public BaseParamHandler {
     ~ImuParamHandler();
     void declareParams(std::shared_ptr<dai::node::IMU> imu, const std::string& imuType);
     dai::CameraControl setRuntimeParams(const std::vector<rclcpp::Parameter>& params) override;
-    std::unordered_map<std::string, dai::ros::ImuSyncMethod> imuSyncMethodMap;
-    std::unordered_map<std::string, imu::ImuMsgType> imuMessagetTypeMap;
+    std::unordered_map<std::string, dai::ros::ImuSyncMethod> syncMethodMap;
+    std::unordered_map<std::string, imu::ImuMsgType> messagetTypeMap;
     std::unordered_map<std::string, dai::IMUSensor> rotationVectorTypeMap;
+    std::unordered_map<std::string, dai::IMUSensor> accelerometerModeMap;
+    std::unordered_map<std::string, dai::IMUSensor> gyroscopeModeMap;
+    std::unordered_map<std::string, dai::IMUSensor> magnetometerModeMap;
     imu::ImuMsgType getMsgType();
     dai::ros::ImuSyncMethod getSyncMethod();
 };
diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>depthai_ros_driver</name>
-  <version>2.11.1</version>
+  <version>2.11.2</version>
   <description>Depthai ROS Monolithic node.</description>
   <maintainer email="adam.serafin@luxonis.com">Adam Serafin</maintainer>
 
diff --git a/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp b/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp
@@ -9,57 +9,93 @@
 namespace depthai_ros_driver {
 namespace param_handlers {
 ImuParamHandler::ImuParamHandler(std::shared_ptr<rclcpp::Node> node, const std::string& name) : BaseParamHandler(node, name) {
-    imuSyncMethodMap = {
+    syncMethodMap = {
         {"COPY", dai::ros::ImuSyncMethod::COPY},
         {"LINEAR_INTERPOLATE_GYRO", dai::ros::ImuSyncMethod::LINEAR_INTERPOLATE_GYRO},
         {"LINEAR_INTERPOLATE_ACCEL", dai::ros::ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL},
     };
-    imuMessagetTypeMap = {
+    messagetTypeMap = {
         {"IMU", imu::ImuMsgType::IMU}, {"IMU_WITH_MAG", imu::ImuMsgType::IMU_WITH_MAG}, {"IMU_WITH_MAG_SPLIT", imu::ImuMsgType::IMU_WITH_MAG_SPLIT}};
     rotationVectorTypeMap = {{"ROTATION_VECTOR", dai::IMUSensor::ROTATION_VECTOR},
                              {"GAME_ROTATION_VECTOR", dai::IMUSensor::GAME_ROTATION_VECTOR},
                              {"GEOMAGNETIC_ROTATION_VECTOR", dai::IMUSensor::GEOMAGNETIC_ROTATION_VECTOR},
                              {"ARVR_STABILIZED_ROTATION_VECTOR", dai::IMUSensor::ARVR_STABILIZED_ROTATION_VECTOR},
                              {"ARVR_STABILIZED_GAME_ROTATION_VECTOR", dai::IMUSensor::ARVR_STABILIZED_GAME_ROTATION_VECTOR}};
+    accelerometerModeMap = {{"ACCELEROMETER_RAW", dai::IMUSensor::ACCELEROMETER_RAW},
+                            {"ACCELEROMETER", dai::IMUSensor::ACCELEROMETER},
+                            {"LINEAR_ACCELERATION", dai::IMUSensor::LINEAR_ACCELERATION},
+                            {"GRAVITY", dai::IMUSensor::GRAVITY}};
+    gyroscopeModeMap = {{"GYROSCOPE_RAW", dai::IMUSensor::GYROSCOPE_RAW},
+                        {"GYROSCOPE_CALIBRATED", dai::IMUSensor::GYROSCOPE_CALIBRATED},
+                        {"GYROSCOPE_UNCALIBRATED", dai::IMUSensor::GYROSCOPE_UNCALIBRATED}};
+    magnetometerModeMap = {{"MAGNETOMETER_RAW", dai::IMUSensor::MAGNETOMETER_RAW},
+                           {"MAGNETOMETER_CALIBRATED", dai::IMUSensor::MAGNETOMETER_CALIBRATED},
+                           {"MAGNETOMETER_UNCALIBRATED", dai::IMUSensor::MAGNETOMETER_UNCALIBRATED}};
 }
 ImuParamHandler::~ImuParamHandler() = default;
 void ImuParamHandler::declareParams(std::shared_ptr<dai::node::IMU> imu, const std::string& imuType) {
     declareAndLogParam<bool>("i_get_base_device_timestamp", false);
     declareAndLogParam<int>("i_max_q_size", 8);
     auto messageType = declareAndLogParam<std::string>("i_message_type", "IMU");
     declareAndLogParam<std::string>("i_sync_method", "LINEAR_INTERPOLATE_ACCEL");
-    declareAndLogParam<float>("i_acc_cov", 0.0);
-    declareAndLogParam<float>("i_gyro_cov", 0.0);
-    declareAndLogParam<float>("i_rot_cov", -1.0);
-    declareAndLogParam<float>("i_mag_cov", 0.0);
     declareAndLogParam<bool>("i_update_ros_base_time_on_ros_msg", false);
-    bool rotationAvailable = imuType == "BNO086";
-    if(declareAndLogParam<bool>("i_enable_rotation", false)) {
+    if(declareAndLogParam<bool>("i_enable_acc", true)) {
+        const std::string accelerometerModeName = utils::getUpperCaseStr(declareAndLogParam<std::string>("i_acc_mode", "ACCELEROMETER_RAW"));
+        const dai::IMUSensor accelerometerMode = utils::getValFromMap(accelerometerModeName, accelerometerModeMap);
+        const int accelerometerFreq = declareAndLogParam<int>("i_acc_freq", 400);
+        declareAndLogParam<float>("i_acc_cov", 0.0);
+
+        imu->enableIMUSensor(accelerometerMode, accelerometerFreq);
+    }
+
+    if(declareAndLogParam<bool>("i_enable_gyro", true)) {
+        const std::string gyroscopeModeName = utils::getUpperCaseStr(declareAndLogParam<std::string>("i_gyro_mode", "GYROSCOPE_RAW"));
+        const dai::IMUSensor gyroscopeMode = utils::getValFromMap(gyroscopeModeName, gyroscopeModeMap);
+        const int gyroscopeFreq = declareAndLogParam<int>("i_gyro_freq", 400);
+        declareAndLogParam<float>("i_gyro_cov", 0.0);
+
+        imu->enableIMUSensor(gyroscopeMode, gyroscopeFreq);
+    }
+
+    const bool magnetometerAvailable = imuType == "BNO086";
+    if(declareAndLogParam<bool>("i_enable_mag", magnetometerAvailable)) {
+        if(magnetometerAvailable) {
+            const std::string magnetometerModeName = utils::getUpperCaseStr(declareAndLogParam<std::string>("i_mag_mode", "MAGNETOMETER_RAW"));
+            const dai::IMUSensor magnetometerMode = utils::getValFromMap(magnetometerModeName, magnetometerModeMap);
+            const int magnetometerFreq = declareAndLogParam<int>("i_mag_freq", 100);
+            declareAndLogParam<float>("i_mag_cov", 0.0);
+
+            imu->enableIMUSensor(magnetometerMode, magnetometerFreq);
+        } else {
+            RCLCPP_ERROR(getROSNode()->get_logger(), "Magnetometer enabled but not available with current sensor");
+            declareAndLogParam<bool>("i_enable_mag", false, true);
+        }
+    }
+
+    const bool rotationAvailable = imuType == "BNO086";
+    if(declareAndLogParam<bool>("i_enable_rotation", rotationAvailable)) {
         if(rotationAvailable) {
-            auto rotationVecType = utils::getValFromMap(utils::getUpperCaseStr(declareAndLogParam<std::string>("i_rotation_vector_type", "ROTATION_VECTOR")),
-                                                        rotationVectorTypeMap);
-            imu->enableIMUSensor(rotationVecType, declareAndLogParam<int>("i_rot_freq", 400));
-            // if imu message type is IMU_WITH_MAG or IMU_WITH_MAG_SPLIT, enable magnetometer
-            if(messageType == "IMU_WITH_MAG" || messageType == "IMU_WITH_MAG_SPLIT") {
-                imu->enableIMUSensor(dai::IMUSensor::MAGNETOMETER_CALIBRATED, declareAndLogParam<int>("i_mag_freq", 100));
-            }
+            const std::string rotationModeName = utils::getUpperCaseStr(declareAndLogParam<std::string>("i_rot_mode", "ROTATION_VECTOR"));
+            const dai::IMUSensor rotationMode = utils::getValFromMap(rotationModeName, rotationVectorTypeMap);
+            const int rotationFreq = declareAndLogParam<int>("i_rot_freq", 400);
+            declareAndLogParam<float>("i_rot_cov", -1.0);
+
+            imu->enableIMUSensor(rotationMode, rotationFreq);
         } else {
             RCLCPP_ERROR(getROSNode()->get_logger(), "Rotation enabled but not available with current sensor");
             declareAndLogParam<bool>("i_enable_rotation", false, true);
         }
+        imu->setBatchReportThreshold(declareAndLogParam<int>("i_batch_report_threshold", 5));
+        imu->setMaxBatchReports(declareAndLogParam<int>("i_max_batch_reports", 10));
     }
-    imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, declareAndLogParam<int>("i_acc_freq", 400));
-    imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, declareAndLogParam<int>("i_gyro_freq", 400));
-    imu->setBatchReportThreshold(declareAndLogParam<int>("i_batch_report_threshold", 5));
-    imu->setMaxBatchReports(declareAndLogParam<int>("i_max_batch_reports", 10));
 }
 
 dai::ros::ImuSyncMethod ImuParamHandler::getSyncMethod() {
-    return utils::getValFromMap(utils::getUpperCaseStr(getParam<std::string>("i_sync_method")), imuSyncMethodMap);
+    return utils::getValFromMap(utils::getUpperCaseStr(getParam<std::string>("i_sync_method")), syncMethodMap);
 }
 
 imu::ImuMsgType ImuParamHandler::getMsgType() {
-    return utils::getValFromMap(utils::getUpperCaseStr(getParam<std::string>("i_message_type")), imuMessagetTypeMap);
+    return utils::getValFromMap(utils::getUpperCaseStr(getParam<std::string>("i_message_type")), messagetTypeMap);
 }
 
 dai::CameraControl ImuParamHandler::setRuntimeParams(const std::vector<rclcpp::Parameter>& /*params*/) {
diff --git 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.11.1)
+project(depthai_ros_msgs VERSION 2.11.2)
 
 if(POLICY CMP0057)
     cmake_policy(SET CMP0057 NEW)
diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_ros_msgs</name>
-  <version>2.11.1</version>
+  <version>2.11.2</version>
   <description>Package to keep interface independent of the driver</description>
 
   <maintainer email="adam.serafin@luxonis.com">Adam Serafin</maintainer>
