Iron update v2.7.4 (#341)

* Added shebang so it's executable (#316)

* Stereo updates (#320)

* add brightness filter (#284)

* Pipeline generation as plugins (#321)

* Update pull_request_template.md

* 3D bounding boxes in Humble (#336)

* Qos overrides (#339)

* Ros time update humble (#340)

* Pre release v2.7.4 (#343)

---------

Co-authored-by: Matt Williamson <Matt@aimatt.com>
Co-authored-by: nelson <kaichie.lee@gmail.com>
Co-authored-by: Sachin Guruswamy <43363595+saching13@users.noreply.github.com>

.github/pull_request_template.md

@@ -0,0 +1,19 @@
+## Overview
+Author: 
+
+## Issue 
+Issue link (if present):
+Issue description:
+Related PRs
+
+## Changes
+ROS distro:
+List of changes:
+
+## Testing
+Hardware used:
+Depthai library version:
+
+
+## Visuals from testing
+Add screenshots/gifs/videos from RVIZ or other visualizers demonstrating the effect of the changes when applicable. 

README.md

@@ -186,6 +186,26 @@ As for the parameters themselves, there are a few crucial ones that decide on ho
   * `Depth` - Publishes only depth stream, no NN available
   * `CamArray` - Publishes streams for all detected sensors, no NN available
 This tells the camera whether it should load stereo components. Default set to `RGBD`.
+It is also possible to create a custom pipeline since all types are defined as plugins. 
+
+To do that, you can create a custom package (let's say `test_plugins`), create an executable in that package (`test_plugins.cpp`). Inside that file, define a cusom plugin that inherits from `depthai_ros_driver::pipeline_gen::BasePipeline` and overrides `createPipeline` method.
+
+After that export plugin, for example:
+
+```c++
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(test_plugins::Test, depthai_ros_driver::pipeline_gen::BasePipeline)
+```
+Add plugin definition:
+```xml
+<library path="test_plugins">
+    <class type="test_plugins::Test" base_class_type="depthai_ros_driver::pipeline_gen::BasePipeline">
+        <description>Test Pipeline.</description>
+    </class>
+</library>
+```
+
+Now you can use created plugin as pipeline, just set `camera.i_pipeline_type` to `test_plugins::Test`.
 
 * `camera.i_nn_type` can be either `none`, `rgb` or `spatial`. This is responsible for whether the NN that we load should also take depth information (and for example provide detections in 3D format). Default set to `spatial`
 * `camera.i_mx_id`/`camera.i_ip`/`camera.i_usb_port_id` are for connecting to a specific camera. If not set, it automatically connects to the next available device. You can get those parameters from logs by running the default launch file.

depthai-ros/CHANGELOG.rst

@@ -2,7 +2,21 @@
 Changelog for package depthai-ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-2.7.2 (20230-5-08)
+2.7.4 (2023-06-26)
+-------------------
+* ROS time update
+* Minor bugfixes
+
+2.7.3 (2023-06-16)
+-------------------
+* Pipeline generation as a plugin
+* Fixed bounding box generation issue
+* Stereo rectified streams publishing
+* Camera trigger mechanisms
+* Brightness filter
+
+2.7.2 (2023-05-08)
+-------------------
 * IMU improvements
 
 2.7.1 (2023-03-29)

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.2 LANGUAGES CXX C)
+project(depthai-ros VERSION 2.7.4 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 

depthai-ros/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai-ros</name>
-  <version>2.7.2</version>
+  <version>2.7.4</version>
   <description>The depthai-ros package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->

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.2 LANGUAGES CXX C)
+project(depthai_bridge VERSION 2.7.4 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)

depthai_bridge/include/depthai_bridge/BridgePublisher.hpp

@@ -8,6 +8,7 @@
 #include "depthai/device/DataQueue.hpp"
 #include "image_transport/image_transport.hpp"
 #include "rclcpp/node.hpp"
+#include "rclcpp/qos.hpp"
 #include "sensor_msgs/msg/camera_info.hpp"
 #include "sensor_msgs/msg/image.hpp"
 #include "std_msgs/msg/header.hpp"
@@ -147,7 +148,9 @@ BridgePublisher<RosMsg, SimMsg>::BridgePublisher(std::shared_ptr<dai::DataOutput
 
 template <class RosMsg, class SimMsg>
 typename rclcpp::Publisher<RosMsg>::SharedPtr BridgePublisher<RosMsg, SimMsg>::advertise(int queueSize, std::false_type) {
-    return _node->create_publisher<RosMsg>(_rosTopic, queueSize);
+    rclcpp::PublisherOptions options;
+    options.qos_overriding_options = rclcpp::QosOverridingOptions();
+    return _node->create_publisher<RosMsg>(_rosTopic, queueSize, options);
 }
 
 template <class RosMsg, class SimMsg>
@@ -158,7 +161,9 @@ std::shared_ptr<image_transport::Publisher> BridgePublisher<RosMsg, SimMsg>::adv
         if(_cameraParamUri.empty()) {
             _camInfoManager->setCameraInfo(_cameraInfoData);
         }
-        _cameraInfoPublisher = _node->create_publisher<ImageMsgs::CameraInfo>(_cameraName + "/camera_info", queueSize);
+        rclcpp::PublisherOptions options;
+        options.qos_overriding_options = rclcpp::QosOverridingOptions();
+        _cameraInfoPublisher = _node->create_publisher<ImageMsgs::CameraInfo>(_cameraName + "/camera_info", queueSize, options);
     }
     return std::make_shared<image_transport::Publisher>(_it.advertise(_rosTopic, queueSize));
 }

depthai_bridge/include/depthai_bridge/DisparityConverter.hpp

@@ -25,17 +25,37 @@ class DisparityConverter {
     DisparityConverter(
         const std::string frameName, float focalLength, float baseline = 7.5, float minDepth = 80, float maxDepth = 1100, bool getBaseDeviceTimestamp = false);
     ~DisparityConverter();
+
+    /**
+     * @brief Handles cases in which the ROS time shifts forward or backward
+     *  Should be called at regular intervals or on-change of ROS time, depending
+     *  on monitoring.
+     *
+     */
+    void updateRosBaseTime();
+
+    /**
+     * @brief Commands the converter to automatically update the ROS base time on message conversion based on variable
+     *
+     * @param update: bool whether to automatically update the ROS base time on message conversion
+     */
+    void setUpdateRosBaseTimeOnToRosMsg(bool update = true) {
+        _updateRosBaseTimeOnToRosMsg = update;
+    }
+
     void toRosMsg(std::shared_ptr<dai::ImgFrame> inData, std::deque<DisparityMsgs::DisparityImage>& outImageMsg);
     DisparityImagePtr toRosMsgPtr(std::shared_ptr<dai::ImgFrame> inData);
 
-    // void toDaiMsg(const DisparityMsgs::DisparityImage& inMsg, dai::ImgFrame& outData);
-
    private:
     const std::string _frameName = "";
     const float _focalLength = 882.2, _baseline = 7.5, _minDepth = 80, _maxDepth;
     std::chrono::time_point<std::chrono::steady_clock> _steadyBaseTime;
     rclcpp::Time _rosBaseTime;
     bool _getBaseDeviceTimestamp;
+    // For handling ROS time shifts and debugging
+    int64_t _totalNsChange{0};
+    // Whether to update the ROS base time on each message conversion
+    bool _updateRosBaseTimeOnToRosMsg{false};
 };
 
 }  // namespace ros

depthai_bridge/include/depthai_bridge/ImageConverter.hpp

@@ -37,6 +37,24 @@ class ImageConverter {
     ImageConverter(const std::string frameName, bool interleaved, bool getBaseDeviceTimestamp = false);
     ~ImageConverter();
     ImageConverter(bool interleaved, bool getBaseDeviceTimestamp = false);
+
+    /**
+     * @brief Handles cases in which the ROS time shifts forward or backward
+     *  Should be called at regular intervals or on-change of ROS time, depending
+     *  on monitoring.
+     *
+     */
+    void updateRosBaseTime();
+
+    /**
+     * @brief Commands the converter to automatically update the ROS base time on message conversion based on variable
+     *
+     * @param update: bool whether to automatically update the ROS base time on message conversion
+     */
+    void setUpdateRosBaseTimeOnToRosMsg(bool update = true) {
+        _updateRosBaseTimeOnToRosMsg = update;
+    }
+
     void toRosMsgFromBitStream(std::shared_ptr<dai::ImgFrame> inData,
                                std::deque<ImageMsgs::Image>& outImageMsgs,
                                dai::RawImgFrame::Type type,
@@ -73,6 +91,10 @@ class ImageConverter {
 
     rclcpp::Time _rosBaseTime;
     bool _getBaseDeviceTimestamp;
+    // For handling ROS time shifts and debugging
+    int64_t _totalNsChange{0};
+    // Whether to update the ROS base time on each message conversion
+    bool _updateRosBaseTimeOnToRosMsg{false};
 };
 
 }  // namespace ros

depthai_bridge/include/depthai_bridge/ImgDetectionConverter.hpp

@@ -20,6 +20,24 @@ class ImgDetectionConverter {
     // DetectionConverter() = default;
     ImgDetectionConverter(std::string frameName, int width, int height, bool normalized = false, bool getBaseDeviceTimestamp = false);
     ~ImgDetectionConverter();
+
+    /**
+     * @brief Handles cases in which the ROS time shifts forward or backward
+     *  Should be called at regular intervals or on-change of ROS time, depending
+     *  on monitoring.
+     *
+     */
+    void updateRosBaseTime();
+
+    /**
+     * @brief Commands the converter to automatically update the ROS base time on message conversion based on variable
+     *
+     * @param update: bool whether to automatically update the ROS base time on message conversion
+     */
+    void setUpdateRosBaseTimeOnToRosMsg(bool update = true) {
+        _updateRosBaseTimeOnToRosMsg = update;
+    }
+
     void toRosMsg(std::shared_ptr<dai::ImgDetections> inNetData, std::deque<VisionMsgs::Detection2DArray>& opDetectionMsgs);
 
     Detection2DArrayPtr toRosMsgPtr(std::shared_ptr<dai::ImgDetections> inNetData);
@@ -31,6 +49,10 @@ class ImgDetectionConverter {
     std::chrono::time_point<std::chrono::steady_clock> _steadyBaseTime;
     rclcpp::Time _rosBaseTime;
     bool _getBaseDeviceTimestamp;
+    // For handling ROS time shifts and debugging
+    int64_t _totalNsChange{0};
+    // Whether to update the ROS base time on each message conversion
+    bool _updateRosBaseTimeOnToRosMsg{false};
 };
 
 /** TODO(sachin): Do we need to have ros msg -> dai bounding box ?

depthai_bridge/include/depthai_bridge/ImuConverter.hpp

@@ -32,8 +32,27 @@ class ImuConverter {
                  double angular_velocity_cov = 0.0,
                  double rotation_cov = 0.0,
                  double magnetic_field_cov = 0.0,
-                 bool enable_rotation = false);
+                 bool enable_rotation = false,
+                 bool getBaseDeviceTimestamp = false);
     ~ImuConverter();
+
+    /**
+     * @brief Handles cases in which the ROS time shifts forward or backward
+     *  Should be called at regular intervals or on-change of ROS time, depending
+     *  on monitoring.
+     *
+     */
+    void updateRosBaseTime();
+
+    /**
+     * @brief Commands the converter to automatically update the ROS base time on message conversion based on variable
+     *
+     * @param update: bool whether to automatically update the ROS base time on message conversion
+     */
+    void setUpdateRosBaseTimeOnToRosMsg(bool update = true) {
+        _updateRosBaseTimeOnToRosMsg = update;
+    }
+
     void toRosMsg(std::shared_ptr<dai::IMUData> inData, std::deque<ImuMsgs::Imu>& outImuMsgs);
     void toRosDaiMsg(std::shared_ptr<dai::IMUData> inData, std::deque<depthai_ros_msgs::msg::ImuWithMagneticField>& outImuMsgs);
 
@@ -112,6 +131,11 @@ class ImuConverter {
     ImuSyncMethod _syncMode;
     std::chrono::time_point<std::chrono::steady_clock> _steadyBaseTime;
     rclcpp::Time _rosBaseTime;
+    bool _getBaseDeviceTimestamp;
+    // For handling ROS time shifts and debugging
+    int64_t _totalNsChange{0};
+    // Whether to update the ROS base time on each message conversion
+    bool _updateRosBaseTimeOnToRosMsg{false};
 
     void fillImuMsg(dai::IMUReportAccelerometer report, ImuMsgs::Imu& msg);
     void fillImuMsg(dai::IMUReportGyroscope report, ImuMsgs::Imu& msg);
@@ -124,15 +148,15 @@ class ImuConverter {
     void fillImuMsg(dai::IMUReportMagneticField report, depthai_ros_msgs::msg::ImuWithMagneticField& msg);
 
     template <typename I, typename S, typename T, typename F, typename M>
-    void CreateUnitMessage(I first, S second, T third, F fourth, M& msg, dai::Timestamp timestamp) {
+    void CreateUnitMessage(I first, S second, T third, F fourth, M& msg, std::chrono::_V2::steady_clock::time_point 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());
+        msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp);
     }
 
     template <typename I, typename S, typename T, typename F, typename M>
@@ -162,7 +186,12 @@ class ImuConverter {
                         const double alpha = diff.count() / dt;
                         I interp = lerpImu(interp0, interp1, alpha);
                         M msg;
-                        CreateUnitMessage(interp, currSecond, currThird, currFourth, msg, currSecond.timestamp);
+                        std::chrono::_V2::steady_clock::time_point tstamp;
+                        if(_getBaseDeviceTimestamp)
+                            tstamp = currSecond.getTimestampDevice();
+                        else
+                            tstamp = currSecond.getTimestamp();
+                        CreateUnitMessage(interp, currSecond, currThird, currFourth, msg, tstamp);
                         imuMsgs.push_back(msg);
                         second.pop_front();
                         third.pop_front();

depthai_bridge/include/depthai_bridge/SpatialDetectionConverter.hpp

@@ -20,6 +20,24 @@ class SpatialDetectionConverter {
    public:
     SpatialDetectionConverter(std::string frameName, int width, int height, bool normalized = false, bool getBaseDeviceTimestamp = false);
     ~SpatialDetectionConverter();
+
+    /**
+     * @brief Handles cases in which the ROS time shifts forward or backward
+     *  Should be called at regular intervals or on-change of ROS time, depending
+     *  on monitoring.
+     *
+     */
+    void updateRosBaseTime();
+
+    /**
+     * @brief Commands the converter to automatically update the ROS base time on message conversion based on variable
+     *
+     * @param update: bool whether to automatically update the ROS base time on message conversion
+     */
+    void setUpdateRosBaseTimeOnToRosMsg(bool update = true) {
+        _updateRosBaseTimeOnToRosMsg = update;
+    }
+
     void toRosMsg(std::shared_ptr<dai::SpatialImgDetections> inNetData, std::deque<SpatialMessages::SpatialDetectionArray>& opDetectionMsg);
     void toRosVisionMsg(std::shared_ptr<dai::SpatialImgDetections> inNetData, std::deque<vision_msgs::msg::Detection3DArray>& opDetectionMsg);
 
@@ -33,6 +51,10 @@ class SpatialDetectionConverter {
 
     rclcpp::Time _rosBaseTime;
     bool _getBaseDeviceTimestamp;
+    // For handling ROS time shifts and debugging
+    int64_t _totalNsChange{0};
+    // Whether to update the ROS base time on each message conversion
+    bool _updateRosBaseTimeOnToRosMsg{false};
 };
 
 /** TODO(sachin): Do we need to have ros msg -> dai bounding box ?

depthai_bridge/include/depthai_bridge/depthaiUtility.hpp

@@ -74,6 +74,8 @@ enum LogLevel { DEBUG, INFO, WARN, ERROR, FATAL };
 
 #define DEPTHAI_ROS_FATAL_STREAM_ONCE(loggerName, args) DEPTHAI_ROS_LOG_STREAM(loggerName, dai::ros::LogLevel::FATAL, true, args)
 
+static const int64_t ZERO_TIME_DELTA_NS{100};
+
 inline rclcpp::Time getFrameTime(rclcpp::Time rclBaseTime,
                                  std::chrono::time_point<std::chrono::steady_clock> steadyBaseTime,
                                  std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration> currTimePoint) {
@@ -84,5 +86,23 @@ inline rclcpp::Time getFrameTime(rclcpp::Time rclBaseTime,
     return rclStamp;
 }
 
+inline void updateBaseTime(std::chrono::time_point<std::chrono::steady_clock> steadyBaseTime, rclcpp::Time rclBaseTime, int64_t& totalNsChange) {
+    rclcpp::Time currentRosTime = rclcpp::Clock().now();
+    std::chrono::time_point<std::chrono::steady_clock> currentSteadyTime = std::chrono::steady_clock::now();
+    // In nanoseconds
+    auto expectedOffset = std::chrono::duration_cast<std::chrono::nanoseconds>(currentSteadyTime - steadyBaseTime).count();
+    uint64_t previousBaseTimeNs = rclBaseTime.nanoseconds();
+    rclBaseTime = rclcpp::Time(currentRosTime.nanoseconds() - expectedOffset);
+    uint64_t newBaseTimeNs = rclBaseTime.nanoseconds();
+    int64_t diff = static_cast<int64_t>(newBaseTimeNs - previousBaseTimeNs);
+    totalNsChange += diff;
+    if(::abs(diff) > ZERO_TIME_DELTA_NS) {
+        // Has been updated
+        DEPTHAI_ROS_DEBUG_STREAM("ROS BASE TIME CHANGE: ",
+                                 "ROS base time changed by " << std::to_string(diff) << " ns. Total change: " << std::to_string(totalNsChange)
+                                                             << " ns. New time: " << std::to_string(rclBaseTime.nanoseconds()) << " ns.");
+    }
+}
+
 }  // namespace ros
 }  // namespace dai

depthai_bridge/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_bridge</name>
-  <version>2.7.2</version>
+  <version>2.7.4</version>
   <description>The depthai_bridge package</description>
 
   <maintainer email="sachin@luxonis.com">Sachin Guruswamy</maintainer>