diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 00000000..adac85f7 --- /dev/null +++ b/.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. diff --git a/README.md b/README.md index 5ac1a893..b3aa80f4 100644 --- a/README.md +++ b/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_EXPORT_CLASS(test_plugins::Test, depthai_ros_driver::pipeline_gen::BasePipeline) +``` +Add plugin definition: +```xml + + + Test Pipeline. + + +``` + +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. diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index d7a61e1b..257723c2 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/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) diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt index d69c7aa3..442342b7 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.2 LANGUAGES CXX C) +project(depthai-ros VERSION 2.7.4 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml index e3f6e0e6..97732f87 100644 --- a/depthai-ros/package.xml +++ b/depthai-ros/package.xml @@ -1,7 +1,7 @@ depthai-ros - 2.7.2 + 2.7.4 The depthai-ros package diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index 41041145..4d291c1e 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.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) diff --git a/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp b/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp index 21105176..99f793bb 100644 --- a/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp +++ b/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::BridgePublisher(std::shared_ptr typename rclcpp::Publisher::SharedPtr BridgePublisher::advertise(int queueSize, std::false_type) { - return _node->create_publisher(_rosTopic, queueSize); + rclcpp::PublisherOptions options; + options.qos_overriding_options = rclcpp::QosOverridingOptions(); + return _node->create_publisher(_rosTopic, queueSize, options); } template @@ -158,7 +161,9 @@ std::shared_ptr BridgePublisher::adv if(_cameraParamUri.empty()) { _camInfoManager->setCameraInfo(_cameraInfoData); } - _cameraInfoPublisher = _node->create_publisher(_cameraName + "/camera_info", queueSize); + rclcpp::PublisherOptions options; + options.qos_overriding_options = rclcpp::QosOverridingOptions(); + _cameraInfoPublisher = _node->create_publisher(_cameraName + "/camera_info", queueSize, options); } return std::make_shared(_it.advertise(_rosTopic, queueSize)); } diff --git a/depthai_bridge/include/depthai_bridge/DisparityConverter.hpp b/depthai_bridge/include/depthai_bridge/DisparityConverter.hpp index 6432520e..568844b6 100644 --- a/depthai_bridge/include/depthai_bridge/DisparityConverter.hpp +++ b/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 inData, std::deque& outImageMsg); DisparityImagePtr toRosMsgPtr(std::shared_ptr 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 _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 diff --git a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp index 9dc09523..834f4e05 100644 --- a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp +++ b/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 inData, std::deque& 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 diff --git a/depthai_bridge/include/depthai_bridge/ImgDetectionConverter.hpp b/depthai_bridge/include/depthai_bridge/ImgDetectionConverter.hpp index ce74a758..e6fe15b3 100644 --- a/depthai_bridge/include/depthai_bridge/ImgDetectionConverter.hpp +++ b/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 inNetData, std::deque& opDetectionMsgs); Detection2DArrayPtr toRosMsgPtr(std::shared_ptr inNetData); @@ -31,6 +49,10 @@ class ImgDetectionConverter { std::chrono::time_point _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 ? diff --git a/depthai_bridge/include/depthai_bridge/ImuConverter.hpp b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp index e74a6f99..cc2b1546 100644 --- a/depthai_bridge/include/depthai_bridge/ImuConverter.hpp +++ b/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 inData, std::deque& outImuMsgs); void toRosDaiMsg(std::shared_ptr inData, std::deque& outImuMsgs); @@ -112,6 +131,11 @@ class ImuConverter { ImuSyncMethod _syncMode; std::chrono::time_point _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,7 +148,7 @@ class ImuConverter { 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) { + 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); @@ -132,7 +156,7 @@ class ImuConverter { msg.header.frame_id = _frameName; - msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp.get()); + msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp); } template @@ -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(); diff --git a/depthai_bridge/include/depthai_bridge/SpatialDetectionConverter.hpp b/depthai_bridge/include/depthai_bridge/SpatialDetectionConverter.hpp index 76285987..f9eab50e 100644 --- a/depthai_bridge/include/depthai_bridge/SpatialDetectionConverter.hpp +++ b/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 inNetData, std::deque& opDetectionMsg); void toRosVisionMsg(std::shared_ptr inNetData, std::deque& 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 ? diff --git a/depthai_bridge/include/depthai_bridge/depthaiUtility.hpp b/depthai_bridge/include/depthai_bridge/depthaiUtility.hpp index cda9132c..c2d315a8 100644 --- a/depthai_bridge/include/depthai_bridge/depthaiUtility.hpp +++ b/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 steadyBaseTime, std::chrono::time_point currTimePoint) { @@ -84,5 +86,23 @@ inline rclcpp::Time getFrameTime(rclcpp::Time rclBaseTime, return rclStamp; } +inline void updateBaseTime(std::chrono::time_point steadyBaseTime, rclcpp::Time rclBaseTime, int64_t& totalNsChange) { + rclcpp::Time currentRosTime = rclcpp::Clock().now(); + std::chrono::time_point currentSteadyTime = std::chrono::steady_clock::now(); + // In nanoseconds + auto expectedOffset = std::chrono::duration_cast(currentSteadyTime - steadyBaseTime).count(); + uint64_t previousBaseTimeNs = rclBaseTime.nanoseconds(); + rclBaseTime = rclcpp::Time(currentRosTime.nanoseconds() - expectedOffset); + uint64_t newBaseTimeNs = rclBaseTime.nanoseconds(); + int64_t diff = static_cast(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 \ No newline at end of file diff --git a/depthai_bridge/package.xml b/depthai_bridge/package.xml index 49e75867..ee235e1a 100644 --- a/depthai_bridge/package.xml +++ b/depthai_bridge/package.xml @@ -1,7 +1,7 @@ depthai_bridge - 2.7.2 + 2.7.4 The depthai_bridge package Sachin Guruswamy diff --git a/depthai_bridge/src/DisparityConverter.cpp b/depthai_bridge/src/DisparityConverter.cpp index 90367591..107e4e1d 100644 --- a/depthai_bridge/src/DisparityConverter.cpp +++ b/depthai_bridge/src/DisparityConverter.cpp @@ -21,7 +21,14 @@ DisparityConverter::DisparityConverter( DisparityConverter::~DisparityConverter() = default; +void DisparityConverter::updateRosBaseTime() { + updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange); +} + void DisparityConverter::toRosMsg(std::shared_ptr inData, std::deque& outDispImageMsgs) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) tstamp = inData->getTimestampDevice(); diff --git a/depthai_bridge/src/ImageConverter.cpp b/depthai_bridge/src/ImageConverter.cpp index 6025663c..3ab623b8 100644 --- a/depthai_bridge/src/ImageConverter.cpp +++ b/depthai_bridge/src/ImageConverter.cpp @@ -36,10 +36,17 @@ ImageConverter::ImageConverter(const std::string frameName, bool interleaved, bo ImageConverter::~ImageConverter() = default; +void ImageConverter::updateRosBaseTime() { + updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange); +} + void ImageConverter::toRosMsgFromBitStream(std::shared_ptr inData, std::deque& outImageMsgs, dai::RawImgFrame::Type type, const sensor_msgs::msg::CameraInfo& info) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) tstamp = inData->getTimestampDevice(); @@ -95,6 +102,9 @@ void ImageConverter::toRosMsgFromBitStream(std::shared_ptr inData } void ImageConverter::toRosMsg(std::shared_ptr inData, std::deque& outImageMsgs) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) tstamp = inData->getTimestampDevice(); diff --git a/depthai_bridge/src/ImgDetectionConverter.cpp b/depthai_bridge/src/ImgDetectionConverter.cpp index 4dead990..0b18d41f 100644 --- a/depthai_bridge/src/ImgDetectionConverter.cpp +++ b/depthai_bridge/src/ImgDetectionConverter.cpp @@ -18,8 +18,14 @@ ImgDetectionConverter::ImgDetectionConverter(std::string frameName, int width, i ImgDetectionConverter::~ImgDetectionConverter() = default; +void ImgDetectionConverter::updateRosBaseTime() { + updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange); +} + void ImgDetectionConverter::toRosMsg(std::shared_ptr inNetData, std::deque& opDetectionMsgs) { - // setting the header + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) tstamp = inNetData->getTimestampDevice(); diff --git a/depthai_bridge/src/ImuConverter.cpp b/depthai_bridge/src/ImuConverter.cpp index baa937bd..84771132 100644 --- a/depthai_bridge/src/ImuConverter.cpp +++ b/depthai_bridge/src/ImuConverter.cpp @@ -13,7 +13,8 @@ ImuConverter::ImuConverter(const std::string& frameName, double angular_velocity_cov, double rotation_cov, double magnetic_field_cov, - bool enable_rotation) + bool enable_rotation, + bool getBaseDeviceTimestamp) : _frameName(frameName), _syncMode(syncMode), _linear_accel_cov(linear_accel_cov), @@ -22,12 +23,17 @@ ImuConverter::ImuConverter(const std::string& frameName, _magnetic_field_cov(magnetic_field_cov), _enable_rotation(enable_rotation), _sequenceNum(0), - _steadyBaseTime(std::chrono::steady_clock::now()) { + _steadyBaseTime(std::chrono::steady_clock::now()), + _getBaseDeviceTimestamp(getBaseDeviceTimestamp) { _rosBaseTime = rclcpp::Clock().now(); } ImuConverter::~ImuConverter() = default; +void ImuConverter::updateRosBaseTime() { + updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange); +} + void ImuConverter::fillImuMsg(dai::IMUReportAccelerometer report, ImuMsgs::Imu& msg) { msg.linear_acceleration.x = report.x; msg.linear_acceleration.y = report.y; @@ -82,6 +88,9 @@ void ImuConverter::fillImuMsg(dai::IMUReportMagneticField report, depthai_ros_ms } void ImuConverter::toRosMsg(std::shared_ptr inData, std::deque& outImuMsgs) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } if(_syncMode != ImuSyncMethod::COPY) { FillImuData_LinearInterpolation(inData->packets, outImuMsgs); } else { @@ -91,13 +100,22 @@ void ImuConverter::toRosMsg(std::shared_ptr inData, std::dequepackets[i].rotationVector; auto magn = inData->packets[i].magneticField; ImuMsgs::Imu msg; - CreateUnitMessage(accel, gyro, rot, magn, msg, accel.timestamp); + std::chrono::_V2::steady_clock::time_point tstamp; + if(_getBaseDeviceTimestamp) + tstamp = accel.getTimestampDevice(); + else + tstamp = accel.getTimestamp(); + + CreateUnitMessage(accel, gyro, rot, magn, msg, tstamp); outImuMsgs.push_back(msg); } } } void ImuConverter::toRosDaiMsg(std::shared_ptr inData, std::deque& outImuMsgs) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } if(_syncMode != ImuSyncMethod::COPY) { FillImuData_LinearInterpolation(inData->packets, outImuMsgs); } else { @@ -107,7 +125,13 @@ void ImuConverter::toRosDaiMsg(std::shared_ptr inData, std::deque< 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); + std::chrono::_V2::steady_clock::time_point tstamp; + if(_getBaseDeviceTimestamp) + tstamp = accel.getTimestampDevice(); + else + tstamp = accel.getTimestamp(); + + CreateUnitMessage(accel, gyro, rot, magn, msg, tstamp); outImuMsgs.push_back(msg); } } diff --git a/depthai_bridge/src/SpatialDetectionConverter.cpp b/depthai_bridge/src/SpatialDetectionConverter.cpp index 84e91bc8..a4f62840 100644 --- a/depthai_bridge/src/SpatialDetectionConverter.cpp +++ b/depthai_bridge/src/SpatialDetectionConverter.cpp @@ -17,8 +17,15 @@ SpatialDetectionConverter::SpatialDetectionConverter(std::string frameName, int SpatialDetectionConverter::~SpatialDetectionConverter() = default; +void SpatialDetectionConverter::updateRosBaseTime() { + updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange); +} + void SpatialDetectionConverter::toRosMsg(std::shared_ptr inNetData, std::deque& opDetectionMsgs) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) tstamp = inNetData->getTimestampDevice(); @@ -79,6 +86,9 @@ SpatialDetectionArrayPtr SpatialDetectionConverter::toRosMsgPtr(std::shared_ptr< void SpatialDetectionConverter::toRosVisionMsg(std::shared_ptr inNetData, std::deque& opDetectionMsgs) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) tstamp = inNetData->getTimestampDevice(); diff --git a/depthai_descriptions/CMakeLists.txt b/depthai_descriptions/CMakeLists.txt index 541f4678..958e11c5 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.2) +project(depthai_descriptions VERSION 2.7.4) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/depthai_descriptions/launch/urdf.launch b/depthai_descriptions/launch/urdf.launch deleted file mode 100644 index 2d9dc6af..00000000 --- a/depthai_descriptions/launch/urdf.launch +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/depthai_descriptions/package.xml b/depthai_descriptions/package.xml index 2e74de72..b7264dec 100644 --- a/depthai_descriptions/package.xml +++ b/depthai_descriptions/package.xml @@ -1,7 +1,7 @@ depthai_descriptions - 2.7.2 + 2.7.4 The depthai_descriptions package Sachin Guruswamy diff --git a/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro b/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro index fc1c9e6f..df541ec7 100644 --- a/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro +++ b/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro @@ -24,7 +24,6 @@ - diff --git a/depthai_descriptions/urdf/models/OAK-D-PRO-W.stl b/depthai_descriptions/urdf/models/OAK-D-PRO-W.stl new file mode 100644 index 00000000..8540d645 Binary files /dev/null and b/depthai_descriptions/urdf/models/OAK-D-PRO-W.stl differ diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index c5a097b4..2ef1933e 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.2 LANGUAGES CXX C) +project(depthai_examples VERSION 2.7.4 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/depthai_examples/launch/stereo_inertial_node.launch.py b/depthai_examples/launch/stereo_inertial_node.launch.py index 1cc34900..e6febeeb 100644 --- a/depthai_examples/launch/stereo_inertial_node.launch.py +++ b/depthai_examples/launch/stereo_inertial_node.launch.py @@ -75,6 +75,7 @@ def generate_launch_description(): enableFloodLight = LaunchConfiguration('enableFloodLight', default = False) dotProjectormA = LaunchConfiguration('dotProjectormA', default = 200.0) floodLightmA = LaunchConfiguration('floodLightmA', default = 200.0) + enableRosBaseTimeUpdate = LaunchConfiguration('enableRosBaseTimeUpdate', default = False) enableRviz = LaunchConfiguration('enableRviz', default = True) @@ -153,6 +154,7 @@ def generate_launch_description(): 'cam_yaw', default_value=cam_yaw, description='Yaw orientation of the camera with respect to the base frame.') + declare_lrcheck_cmd = DeclareLaunchArgument( 'lrcheck', @@ -293,11 +295,17 @@ def generate_launch_description(): 'floodLightmA', default_value=floodLightmA, description='Set the mA at which you intend to drive the FloodLight. Default is set to 200mA.') + declare_enableRosBaseTimeUpdate_cmd = DeclareLaunchArgument( + 'enableRosBaseTimeUpdate', + default_value=enableRosBaseTimeUpdate, + description='Whether to update ROS time on each message.') + declare_enableRviz_cmd = DeclareLaunchArgument( 'enableRviz', default_value=enableRviz, description='When True create a RVIZ window.') + urdf_launch = IncludeLaunchDescription( @@ -357,7 +365,8 @@ def generate_launch_description(): {'enableDotProjector': enableDotProjector}, {'enableFloodLight': enableFloodLight}, {'dotProjectormA': dotProjectormA}, - {'floodLightmA': floodLightmA} + {'floodLightmA': floodLightmA}, + {'enableRosBaseTimeUpdate': enableRosBaseTimeUpdate} ]) depth_metric_converter = launch_ros.descriptions.ComposableNode( diff --git a/depthai_examples/package.xml b/depthai_examples/package.xml index 965e7122..d73e9184 100644 --- a/depthai_examples/package.xml +++ b/depthai_examples/package.xml @@ -1,7 +1,7 @@ depthai_examples - 2.7.2 + 2.7.4 The depthai_examples package diff --git a/depthai_examples/scripts/markerPublisher.py b/depthai_examples/scripts/markerPublisher.py index 62e36582..0b990e33 100644 --- a/depthai_examples/scripts/markerPublisher.py +++ b/depthai_examples/scripts/markerPublisher.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 import rospy from depthai_ros_msgs.msg import SpatialDetectionArray from foxglove_msgs.msg import ImageMarkerArray @@ -75,4 +76,4 @@ def main(): if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/depthai_examples/src/stereo_inertial_publisher.cpp b/depthai_examples/src/stereo_inertial_publisher.cpp index 7a0cb155..08a60e55 100644 --- a/depthai_examples/src/stereo_inertial_publisher.cpp +++ b/depthai_examples/src/stereo_inertial_publisher.cpp @@ -296,6 +296,7 @@ int main(int argc, char** argv) { bool usb2Mode, poeMode, syncNN; double angularVelCovariance, linearAccelCovariance; double dotProjectormA, floodLightmA; + bool enableRosBaseTimeUpdate; std::string nnName(BLOB_NAME); // Set your blob name for the model here node->declare_parameter("mxId", ""); @@ -338,6 +339,7 @@ int main(int argc, char** argv) { node->declare_parameter("enableFloodLight", false); node->declare_parameter("dotProjectormA", 200.0); node->declare_parameter("floodLightmA", 200.0); + node->declare_parameter("enableRosBaseTimeUpdate", false); // updating parameters if defined in launch file. @@ -380,6 +382,7 @@ int main(int argc, char** argv) { node->get_parameter("enableFloodLight", enableFloodLight); node->get_parameter("dotProjectormA", dotProjectormA); node->get_parameter("floodLightmA", floodLightmA); + node->get_parameter("enableRosBaseTimeUpdate", enableRosBaseTimeUpdate); if(resourceBaseFolder.empty()) { throw std::runtime_error("Send the path to the resouce folder containing NNBlob in \'resourceBaseFolder\' "); @@ -491,11 +494,20 @@ int main(int argc, char** argv) { } dai::rosBridge::ImageConverter converter(tfPrefix + "_left_camera_optical_frame", true); + if(enableRosBaseTimeUpdate) { + converter.setUpdateRosBaseTimeOnToRosMsg(); + } dai::rosBridge::ImageConverter rightconverter(tfPrefix + "_right_camera_optical_frame", true); + if(enableRosBaseTimeUpdate) { + rightconverter.setUpdateRosBaseTimeOnToRosMsg(); + } const std::string leftPubName = rectify ? std::string("left/image_rect") : std::string("left/image_raw"); const std::string rightPubName = rectify ? std::string("right/image_rect") : std::string("right/image_raw"); dai::rosBridge::ImuConverter imuConverter(tfPrefix + "_imu_frame", imuMode, linearAccelCovariance, angularVelCovariance); + if(enableRosBaseTimeUpdate) { + imuConverter.setUpdateRosBaseTimeOnToRosMsg(); + } dai::rosBridge::BridgePublisher imuPublish( imuQueue, node, @@ -513,6 +525,9 @@ int main(int argc, char** argv) { // const std::string rightPubName = rectify ? std::string("right/image_rect") : std::string("right/image_raw"); dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false); + if(enableRosBaseTimeUpdate) { + rgbConverter.setUpdateRosBaseTimeOnToRosMsg(); + } if(enableDepth) { auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height); auto depthCameraInfo = diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index d40d3406..1b11fd77 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.2 LANGUAGES CXX C) +project(depthai_filters VERSION 2.7.4 LANGUAGES CXX C) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -16,6 +16,7 @@ ament_auto_add_library( ${PROJECT_NAME} SHARED src/detection2d_overlay.cpp src/segmentation_overlay.cpp + src/spatial_bb.cpp src/wls_filter.cpp src/utils.cpp ) @@ -27,6 +28,7 @@ target_link_libraries( rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::Detection2DOverlay") rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::SegmentationOverlay") rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::WLSFilter") +rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::SpatialBB") ament_export_include_directories(include) install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) install( diff --git a/depthai_filters/config/spatial_bb.yaml b/depthai_filters/config/spatial_bb.yaml new file mode 100644 index 00000000..ed3be208 --- /dev/null +++ b/depthai_filters/config/spatial_bb.yaml @@ -0,0 +1,16 @@ +/oak: + ros__parameters: + camera: + i_nn_type: spatial + rgb: + i_enable_preview: true + i_keep_preview_aspect_ratio: false + nn: + i_enable_passthrough: true + i_disable_resize: true + stereo: + i_subpixel: true +/spatial_bb_node: + ros__parameters: + desqueeze: true + diff --git a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp index c39d84ee..2b79ef75 100644 --- a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp +++ b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp @@ -22,9 +22,9 @@ class Detection2DOverlay : public rclcpp::Node { typedef message_filters::sync_policies::ApproximateTime syncPolicy; std::unique_ptr> sync; rclcpp::Publisher::SharedPtr overlayPub; - const std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", - "car", "cat", "chair", "cow", "diningtable", "dog", "horse", - "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; }; } // namespace depthai_filters \ No newline at end of file diff --git a/depthai_filters/include/depthai_filters/segmentation_overlay.hpp b/depthai_filters/include/depthai_filters/segmentation_overlay.hpp index bcd67615..3ddc7a91 100644 --- a/depthai_filters/include/depthai_filters/segmentation_overlay.hpp +++ b/depthai_filters/include/depthai_filters/segmentation_overlay.hpp @@ -20,9 +20,9 @@ class SegmentationOverlay : public rclcpp::Node { typedef message_filters::sync_policies::ApproximateTime syncPolicy; std::unique_ptr> sync; rclcpp::Publisher::SharedPtr overlayPub; - const std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", - "car", "cat", "chair", "cow", "diningtable", "dog", "horse", - "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; }; } // namespace depthai_filters \ No newline at end of file diff --git a/depthai_filters/include/depthai_filters/spatial_bb.hpp b/depthai_filters/include/depthai_filters/spatial_bb.hpp new file mode 100644 index 00000000..2eaf92d3 --- /dev/null +++ b/depthai_filters/include/depthai_filters/spatial_bb.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/synchronizer.h" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "vision_msgs/msg/detection3_d_array.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +namespace depthai_filters { +class SpatialBB : public rclcpp::Node { + public: + explicit SpatialBB(const rclcpp::NodeOptions& options); + void onInit(); + + void overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info, + const vision_msgs::msg::Detection3DArray::ConstSharedPtr& detections); + + message_filters::Subscriber previewSub; + message_filters::Subscriber detSub; + message_filters::Subscriber infoSub; + + typedef message_filters::sync_policies::ApproximateTime + syncPolicy; + std::unique_ptr> sync; + rclcpp::Publisher::SharedPtr markerPub; + rclcpp::Publisher::SharedPtr overlayPub; + std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + bool desqueeze = false; +}; + +} // namespace depthai_filters \ No newline at end of file diff --git a/depthai_filters/include/depthai_filters/utils.hpp b/depthai_filters/include/depthai_filters/utils.hpp index 4e148fe1..2fa9122a 100644 --- a/depthai_filters/include/depthai_filters/utils.hpp +++ b/depthai_filters/include/depthai_filters/utils.hpp @@ -10,5 +10,6 @@ class Logger; namespace depthai_filters { namespace utils { cv::Mat msgToMat(const rclcpp::Logger& logger, const sensor_msgs::msg::Image::ConstSharedPtr& img, const std::string& encoding); +void addTextToFrame(cv::Mat& frame, const std::string& text, int x, int y); } // namespace utils } // namespace depthai_filters \ No newline at end of file diff --git a/depthai_filters/launch/spatial_bb.launch.py b/depthai_filters/launch/spatial_bb.launch.py new file mode 100644 index 00000000..b8ccdef5 --- /dev/null +++ b/depthai_filters/launch/spatial_bb.launch.py @@ -0,0 +1,52 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +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.descriptions import ComposableNode + + +def launch_setup(context, *args, **kwargs): + params_file = LaunchConfiguration("params_file") + depthai_prefix = get_package_share_directory("depthai_ros_driver") + name = LaunchConfiguration('name').perform(context) + + return [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(depthai_prefix, 'launch', 'rgbd_pcl.launch.py')), + launch_arguments={"name": name, + "params_file": params_file, + "rectify_rgb": "true"}.items()), + + LoadComposableNodes( + target_container=name+"_container", + composable_node_descriptions=[ + ComposableNode( + package="depthai_filters", + plugin="depthai_filters::SpatialBB", + remappings=[ + ('stereo/camera_info', name+'/stereo/camera_info'), + ('nn/spatial_detections', name+'/nn/spatial_detections'), + ('rgb/preview/image_raw', name+'/rgb/preview/image_raw')] + ), + ], + ), + + ] + + +def generate_launch_description(): + depthai_filters_prefix = get_package_share_directory("depthai_filters") + + declared_arguments = [ + DeclareLaunchArgument("name", default_value="oak"), + DeclareLaunchArgument("params_file", default_value=os.path.join(depthai_filters_prefix, 'config', 'spatial_bb.yaml')), + ] + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) \ No newline at end of file diff --git a/depthai_filters/package.xml b/depthai_filters/package.xml index 4a7143c2..e5afa787 100644 --- a/depthai_filters/package.xml +++ b/depthai_filters/package.xml @@ -2,7 +2,7 @@ depthai_filters - 2.7.2 + 2.7.4 Depthai filters package Adam Serafin MIT @@ -17,6 +17,7 @@ libopencv-dev cv_bridge image_transport + visualization_msgs ament_cmake diff --git a/depthai_filters/src/detection2d_overlay.cpp b/depthai_filters/src/detection2d_overlay.cpp index 84815d68..8ad65b36 100644 --- a/depthai_filters/src/detection2d_overlay.cpp +++ b/depthai_filters/src/detection2d_overlay.cpp @@ -14,14 +14,13 @@ void Detection2DOverlay::onInit() { sync = std::make_unique>(syncPolicy(10), previewSub, detSub); sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2)); overlayPub = this->create_publisher("overlay", 10); + labelMap = this->declare_parameter>("label_map", labelMap); } void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections) { cv::Mat previewMat = utils::msgToMat(this->get_logger(), preview, sensor_msgs::image_encodings::BGR8); - auto white = cv::Scalar(255, 255, 255); - auto black = cv::Scalar(0, 0, 0); auto blue = cv::Scalar(255, 0, 0); for(auto& detection : detections->detections) { @@ -31,12 +30,10 @@ void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr auto y2 = detection.bbox.center.position.y + detections->detections[0].bbox.size_y / 2.0; auto labelStr = labelMap[stoi(detection.results[0].hypothesis.class_id)]; auto confidence = detection.results[0].hypothesis.score; - cv::putText(previewMat, labelStr, cv::Point(x1 + 10, y1 + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, white, 3); - cv::putText(previewMat, labelStr, cv::Point(x1 + 10, y1 + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, black); + utils::addTextToFrame(previewMat, labelStr, x1 + 10, y1 + 20); std::stringstream confStr; confStr << std::fixed << std::setprecision(2) << confidence * 100; - cv::putText(previewMat, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, white, 3); - cv::putText(previewMat, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, black); + utils::addTextToFrame(previewMat, confStr.str(), x1 + 10, y1 + 40); cv::rectangle(previewMat, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), blue); } sensor_msgs::msg::Image outMsg; diff --git a/depthai_filters/src/segmentation_overlay.cpp b/depthai_filters/src/segmentation_overlay.cpp index a49460ce..7f939b2f 100644 --- a/depthai_filters/src/segmentation_overlay.cpp +++ b/depthai_filters/src/segmentation_overlay.cpp @@ -14,6 +14,7 @@ void SegmentationOverlay::onInit() { sync = std::make_unique>(syncPolicy(10), previewSub, segSub); sync->registerCallback(std::bind(&SegmentationOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2)); overlayPub = this->create_publisher("overlay", 10); + labelMap = this->declare_parameter>("label_map", labelMap); } void SegmentationOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, const sensor_msgs::msg::Image::ConstSharedPtr& segmentation) { diff --git a/depthai_filters/src/spatial_bb.cpp b/depthai_filters/src/spatial_bb.cpp new file mode 100644 index 00000000..5a3a7f0f --- /dev/null +++ b/depthai_filters/src/spatial_bb.cpp @@ -0,0 +1,148 @@ +#include "depthai_filters/spatial_bb.hpp" + +#include "cv_bridge/cv_bridge.h" +#include "depthai_filters/utils.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "opencv2/opencv.hpp" + +namespace depthai_filters { + +SpatialBB::SpatialBB(const rclcpp::NodeOptions& options) : rclcpp::Node("spatial_bb_node", options) { + onInit(); +} +void SpatialBB::onInit() { + previewSub.subscribe(this, "rgb/preview/image_raw"); + infoSub.subscribe(this, "stereo/camera_info"); + detSub.subscribe(this, "nn/spatial_detections"); + sync = std::make_unique>(syncPolicy(10), previewSub, infoSub, detSub); + sync->registerCallback(std::bind(&SpatialBB::overlayCB, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + markerPub = this->create_publisher("spatial_bb", 10); + overlayPub = this->create_publisher("overlay", 10); + desqueeze = this->declare_parameter("desqueeze", false); + labelMap = this->declare_parameter>("label_map", labelMap); +} + +void SpatialBB::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info, + const vision_msgs::msg::Detection3DArray::ConstSharedPtr& detections) { + cv::Mat previewMat = utils::msgToMat(this->get_logger(), preview, sensor_msgs::image_encodings::BGR8); + auto blue = cv::Scalar(255, 0, 0); + + double ratioY = double(info->height) / double(previewMat.rows); + double ratioX; + int offsetX; + if(desqueeze) { + ratioX = double(info->width) / double(previewMat.cols); + offsetX = 0; + } else { + ratioX = ratioY; + offsetX = (info->width - info->height) / 2.0; + } + int offsetY = 0; + visualization_msgs::msg::MarkerArray marker_array; + double fx = info->k[0]; + double fy = info->k[4]; + double cx = info->k[2]; + double cy = info->k[5]; + int id = 0; + for(auto& detection : detections->detections) { + auto x1 = detection.bbox.center.position.x - detections->detections[0].bbox.size.x / 2.0; + auto x2 = detection.bbox.center.position.x + detections->detections[0].bbox.size.x / 2.0; + auto y1 = detection.bbox.center.position.y - detections->detections[0].bbox.size.y / 2.0; + auto y2 = detection.bbox.center.position.y + detections->detections[0].bbox.size.y / 2.0; + + cv::rectangle(previewMat, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), blue); + auto labelStr = labelMap[stoi(detection.results[0].hypothesis.class_id)]; + utils::addTextToFrame(previewMat, labelStr, x1 + 10, y1 + 10); + auto confidence = detection.results[0].hypothesis.score; + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << confidence * 100; + utils::addTextToFrame(previewMat, confStr.str(), x1 + 10, y1 + 40); + + std::stringstream depthX; + depthX << "X: " << detection.results[0].pose.pose.position.x << " mm"; + utils::addTextToFrame(previewMat, depthX.str(), x1 + 10, y1 + 60); + + std::stringstream depthY; + depthY << "Y: " << detection.results[0].pose.pose.position.y << " mm"; + utils::addTextToFrame(previewMat, depthY.str(), x1 + 10, y1 + 75); + std::stringstream depthZ; + depthZ << "Z: " << detection.results[0].pose.pose.position.z << " mm"; + utils::addTextToFrame(previewMat, depthZ.str(), x1 + 10, y1 + 90); + + // Marker publishing + const auto& bbox = detection.bbox; + auto bbox_size_x = bbox.size.x * ratioX; + auto bbox_size_y = bbox.size.y * ratioY; + + auto bbox_center_x = bbox.center.position.x * ratioX + offsetX; + auto bbox_center_y = bbox.center.position.y * ratioY + offsetY; + visualization_msgs::msg::Marker box_marker; + box_marker.header.frame_id = info->header.frame_id; + box_marker.header.stamp = this->get_clock()->now(); + box_marker.ns = "detections"; + box_marker.id = id++; + box_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + box_marker.action = visualization_msgs::msg::Marker::ADD; + + box_marker.scale.x = 0.05; // Line width + box_marker.color.g = 1.0; + box_marker.color.a = 1.0; + + // Define bbox corner points in depth image frame + geometry_msgs::msg::Point32 corners[4]; + corners[0].x = bbox_center_x - bbox_size_x / 2.0; + corners[0].y = bbox_center_y - bbox_size_y / 2.0; + corners[1].x = bbox_center_x + bbox_size_x / 2.0; + corners[1].y = bbox_center_y - bbox_size_y / 2.0; + corners[2].x = bbox_center_x + bbox_size_x / 2.0; + corners[2].y = bbox_center_y + bbox_size_y / 2.0; + corners[3].x = bbox_center_x - bbox_size_x / 2.0; + corners[3].y = bbox_center_y + bbox_size_y / 2.0; + // The polygon points are a rectangle, so we need 5 points to close the loop + box_marker.points.resize(5); + for(int i = 0; i < 4; ++i) { + auto& point = corners[i]; + point.z = detection.results[0].pose.pose.position.z; + box_marker.points[i].x = (point.x - cx) * point.z / fx; + box_marker.points[i].y = (point.y - cy) * point.z / fy; + box_marker.points[i].z = point.z; // assuming depth_val is in millimeters + } + // Repeat the first point to close the loop + box_marker.points[4] = box_marker.points[0]; + marker_array.markers.push_back(box_marker); + + // Create a text marker for the label + visualization_msgs::msg::Marker text_marker; + text_marker.header.frame_id = info->header.frame_id; + text_marker.header.stamp = this->get_clock()->now(); + text_marker.ns = "detections_label"; + text_marker.id = id++; + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.action = visualization_msgs::msg::Marker::ADD; + + text_marker.scale.z = 0.3; // Text size + text_marker.color.r = 1.0; + text_marker.color.g = 1.0; + text_marker.color.b = 1.0; + text_marker.color.a = 1.0; + + // Position the text above the bounding box + text_marker.pose.position.x = box_marker.points[0].x; + text_marker.pose.position.y = box_marker.points[0].y; + text_marker.pose.position.z = box_marker.points[0].z + 0.1; // Adjust this value to position the text above the box + + // Set the text to the detection label + text_marker.text = labelMap[stoi(detection.results[0].hypothesis.class_id)]; + marker_array.markers.push_back(text_marker); + } + markerPub->publish(marker_array); + sensor_msgs::msg::Image outMsg; + cv_bridge::CvImage(preview->header, sensor_msgs::image_encodings::BGR8, previewMat).toImageMsg(outMsg); + + overlayPub->publish(outMsg); +} + +} // namespace depthai_filters +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::SpatialBB); \ No newline at end of file diff --git a/depthai_filters/src/utils.cpp b/depthai_filters/src/utils.cpp index 24e63513..1e6b9cea 100644 --- a/depthai_filters/src/utils.cpp +++ b/depthai_filters/src/utils.cpp @@ -14,5 +14,12 @@ cv::Mat msgToMat(const rclcpp::Logger& logger, const sensor_msgs::msg::Image::Co } return mat; } +void addTextToFrame(cv::Mat& frame, const std::string& text, int x, int y) { + auto white = cv::Scalar(255, 255, 255); + auto black = cv::Scalar(0, 0, 0); + + cv::putText(frame, text, cv::Point(x, y), cv::FONT_HERSHEY_TRIPLEX, 0.5, white, 3); + cv::putText(frame, text, cv::Point(x, y), cv::FONT_HERSHEY_TRIPLEX, 0.5, black); +} } // namespace utils } // namespace depthai_filters \ No newline at end of file diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index 07622d61..920dc33f 100644 --- 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.7.2) +project(depthai_ros_driver VERSION 2.7.4) set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_BUILD_SHARED_LIBS ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) @@ -52,6 +52,7 @@ depthai rclcpp rclcpp_components std_srvs +pluginlib ) @@ -103,7 +104,8 @@ target_link_libraries( add_library( ${PROJECT_NAME} SHARED src/camera.cpp - src/pipeline_generator.cpp + src/pipeline/pipeline_generator.cpp + src/pipeline/base_types.cpp ) ament_target_dependencies(${PROJECT_NAME} ${CAM_DEPS}) @@ -116,18 +118,28 @@ target_link_libraries( rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::Camera") +pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${COMMON_DEPS} ${SENSOR_DEPS} ${NN_DEPS} ${CAM_DEPS}) + install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) -install( - TARGETS - ${PROJECT_NAME} ${COMMON_LIB_NAME} ${SENSOR_LIB_NAME} ${NN_LIB_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin +install(TARGETS ${PROJECT_NAME} ${SENSOR_LIB_NAME} +${NN_LIB_NAME} +${COMMON_LIB_NAME} +EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) +install(EXPORT ${PROJECT_NAME}Targets + DESTINATION share/${PROJECT_NAME}/cmake) + ament_python_install_package(${PROJECT_NAME}) # Install Python executables install( @@ -139,6 +151,7 @@ install( ament_export_include_directories( include ) + install( DIRECTORY include/ DESTINATION include diff --git a/depthai_ros_driver/config/camera.yaml b/depthai_ros_driver/config/camera.yaml index aa4c7ad6..2ebd09db 100644 --- a/depthai_ros_driver/config/camera.yaml +++ b/depthai_ros_driver/config/camera.yaml @@ -18,6 +18,7 @@ i_acc_freq: 400 i_acc_cov: 0.0 i_batch_report_threshold: 1 + i_get_base_device_timestamp: false i_enable_rotation: false i_gyro_cov: 0.0 i_gyro_freq: 400 @@ -31,7 +32,7 @@ left: i_board_socket_id: 1 i_calibration_file: '' - i_get_base_device_timestamp: true + i_get_base_device_timestamp: false i_fps: 30.0 i_height: 720 i_low_bandwidth: false @@ -53,7 +54,7 @@ rgb: i_board_socket_id: 0 i_simulate_from_topic: false - i_get_base_device_timestamp: true + i_get_base_device_timestamp: false i_disable_node: false i_calibration_file: '' i_enable_preview: false @@ -82,7 +83,7 @@ right: i_board_socket_id: 2 i_calibration_file: '' - i_get_base_device_timestamp: true + i_get_base_device_timestamp: false i_fps: 30.0 i_height: 720 i_low_bandwidth: false @@ -98,7 +99,7 @@ r_set_man_exposure: false stereo: i_align_depth: true - i_get_base_device_timestamp: true + i_get_base_device_timestamp: false i_output_disparity: false i_bilateral_sigma: 0 i_board_socket_id: 0 @@ -112,6 +113,9 @@ i_enable_speckle_filter: false i_enable_temporal_filter: false i_enable_threshold_filter: false + i_enable_brightness_filter: false + i_brightness_filter_min_brightness: 1 + i_brightness_filter_max_brightness: 256 i_extended_disp: false i_height: 720 i_low_bandwidth: false diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp index e222d53c..26728ad9 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp @@ -43,25 +43,35 @@ class Detection : public BaseNode { void setupQueues(std::shared_ptr device) override { nnQ = device->getOutputQueue(nnQName, ph->getParam("i_max_q_size"), false); auto tfPrefix = getTFPrefix("rgb"); - detConverter = std::make_unique(tfPrefix + "_camera_optical_frame", - imageManip->initialConfig.getResizeConfig().width, - imageManip->initialConfig.getResizeConfig().height, - false, - ph->getParam("i_get_base_device_timestamp")); - detPub = getROSNode()->template create_publisher("~/" + getName() + "/detections", 10); + int width; + int height; + if(ph->getParam("i_disable_resize")) { + width = getROSNode()->get_parameter("rgb.i_preview_size").as_int(); + height = getROSNode()->get_parameter("rgb.i_preview_size").as_int(); + } else { + width = imageManip->initialConfig.getResizeConfig().width; + height = imageManip->initialConfig.getResizeConfig().height; + } + detConverter = std::make_unique( + tfPrefix + "_camera_optical_frame", width, height, false, ph->getParam("i_get_base_device_timestamp")); + detConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); + rclcpp::PublisherOptions options; + options.qos_overriding_options = rclcpp::QosOverridingOptions(); + detPub = getROSNode()->template create_publisher("~/" + getName() + "/detections", 10, options); nnQ->addCallback(std::bind(&Detection::detectionCB, this, std::placeholders::_1, std::placeholders::_2)); if(ph->getParam("i_enable_passthrough")) { ptQ = device->getOutputQueue(ptQName, ph->getParam("i_max_q_size"), false); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); + imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); infoManager = std::make_shared( getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); infoManager->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), *imageConverter, device, - dai::CameraBoardSocket::RGB, - imageManip->initialConfig.getResizeWidth(), - imageManip->initialConfig.getResizeWidth())); + static_cast(ph->getParam("i_board_socket_id")), + width, + height)); ptPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/passthrough/image_raw"); ptQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, ptPub, infoManager)); diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp index 9f4a8c2b..5cba6324 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp @@ -45,38 +45,48 @@ class SpatialDetection : public BaseNode { void setupQueues(std::shared_ptr device) override { nnQ = device->getOutputQueue(nnQName, ph->getParam("i_max_q_size"), false); auto tfPrefix = getTFPrefix("rgb"); - detConverter = std::make_unique(tfPrefix + "_camera_optical_frame", - imageManip->initialConfig.getResizeConfig().width, - imageManip->initialConfig.getResizeConfig().height, - false, - ph->getParam("i_get_base_device_timestamp")); + int width; + int height; + if(ph->getParam("i_disable_resize")) { + width = getROSNode()->get_parameter("rgb.i_preview_size").as_int(); + height = getROSNode()->get_parameter("rgb.i_preview_size").as_int(); + } else { + width = imageManip->initialConfig.getResizeConfig().width; + height = imageManip->initialConfig.getResizeConfig().height; + } + detConverter = std::make_unique( + tfPrefix + "_camera_optical_frame", width, height, false, ph->getParam("i_get_base_device_timestamp")); + detConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); nnQ->addCallback(std::bind(&SpatialDetection::spatialCB, this, std::placeholders::_1, std::placeholders::_2)); - detPub = getROSNode()->template create_publisher("~/" + getName() + "/spatial_detections", 10); + rclcpp::PublisherOptions options; + options.qos_overriding_options = rclcpp::QosOverridingOptions(); + detPub = getROSNode()->template create_publisher("~/" + getName() + "/spatial_detections", 10, options); if(ph->getParam("i_enable_passthrough")) { ptQ = device->getOutputQueue(ptQName, ph->getParam("i_max_q_size"), false); ptImageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); + ptImageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); ptInfoMan = std::make_shared( getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); ptInfoMan->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), *ptImageConverter, device, - dai::CameraBoardSocket::RGB, - imageManip->initialConfig.getResizeWidth(), - imageManip->initialConfig.getResizeWidth())); + static_cast(ph->getParam("i_board_socket_id")), + width, + height)); ptPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/passthrough/image_raw"); ptQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *ptImageConverter, ptPub, ptInfoMan)); } if(ph->getParam("i_enable_passthrough_depth")) { - dai::CameraBoardSocket socket = dai::CameraBoardSocket::RGB; + dai::CameraBoardSocket socket = static_cast(getROSNode()->get_parameter("stereo.i_board_socket_id").as_int()); if(!getROSNode()->get_parameter("stereo.i_align_depth").as_bool()) { tfPrefix = getTFPrefix("right"); - socket = dai::CameraBoardSocket::RIGHT; }; ptDepthQ = device->getOutputQueue(ptDepthQName, ph->getParam("i_max_q_size"), false); ptDepthImageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); + ptDepthImageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); ptDepthInfoMan = std::make_shared( getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); ptDepthInfoMan->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp index 38f888a4..7e6f45a0 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp @@ -7,6 +7,7 @@ #include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai/pipeline/Node.hpp" #include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" #include "rclcpp/subscription.hpp" #include "sensor_msgs/msg/image.hpp" @@ -48,6 +49,7 @@ class SensorWrapper : public BaseNode { void setNames() override; void setXinXout(std::shared_ptr pipeline) override; void closeQueues() override; + sensor_helpers::ImageSensor getSensorData(); private: void subCB(const sensor_msgs::msg::Image& img); @@ -59,6 +61,7 @@ class SensorWrapper : public BaseNode { std::shared_ptr inQ; std::string inQName; int socketID; + sensor_helpers::ImageSensor sensorData; }; } // namespace dai_nodes diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp index 123b97e4..1973efd2 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp @@ -4,7 +4,8 @@ #include #include -#include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "depthai-shared/common/CameraBoardSocket.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp" #include "image_transport/camera_publisher.hpp" #include "image_transport/image_transport.hpp" namespace dai { @@ -40,9 +41,20 @@ namespace dai_nodes { namespace link_types { enum class StereoLinkType { left, right }; }; + +struct StereoSensorInfo { + std::string name; + dai::CameraBoardSocket socket; +}; + class Stereo : public BaseNode { public: - explicit Stereo(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline, std::shared_ptr device); + explicit Stereo(const std::string& daiNodeName, + rclcpp::Node* node, + std::shared_ptr pipeline, + std::shared_ptr device, + StereoSensorInfo leftInfo = StereoSensorInfo{"left", dai::CameraBoardSocket::LEFT}, + StereoSensorInfo rightInfo = StereoSensorInfo{"right", dai::CameraBoardSocket::RIGHT}); ~Stereo(); void updateParams(const std::vector& params) override; void setupQueues(std::shared_ptr device) override; @@ -53,17 +65,21 @@ class Stereo : public BaseNode { void closeQueues() override; private: - std::unique_ptr imageConverter; - image_transport::CameraPublisher stereoPub; - std::shared_ptr infoManager; + void setupStereoQueue(std::shared_ptr device); + void setupLeftRectQueue(std::shared_ptr device); + void setupRightRectQueue(std::shared_ptr device); + std::unique_ptr stereoConv, leftRectConv, rightRectConv; + image_transport::CameraPublisher stereoPub, leftRectPub, rightRectPub; + std::shared_ptr stereoIM, leftRectIM, rightRectIM; std::shared_ptr stereoCamNode; - std::shared_ptr videoEnc; - std::unique_ptr left; - std::unique_ptr right; + std::shared_ptr stereoEnc, leftRectEnc, rightRectEnc; + std::unique_ptr left; + std::unique_ptr right; std::unique_ptr ph; - std::shared_ptr stereoQ; - std::shared_ptr xoutStereo; - std::string stereoQName; + std::shared_ptr stereoQ, leftRectQ, rightRectQ; + std::shared_ptr xoutStereo, xoutLeftRect, xoutRightRect; + std::string stereoQName, leftRectQName, rightRectQName; + StereoSensorInfo leftSensInfo, rightSensInfo; }; } // namespace dai_nodes diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/base_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/base_param_handler.hpp index 8340df18..cb00f4a6 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/base_param_handler.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/base_param_handler.hpp @@ -27,12 +27,23 @@ class BaseParamHandler { template T getParam(const std::string paramName) { T value; - baseNode->get_parameter(baseName + "." + paramName, value); + baseNode->get_parameter(getFullParamName(paramName), value); return value; } + template + T getOtherNodeParam(const std::string& daiNodeName, const std::string& paramName) { + T value; + baseNode->get_parameter(getFullParamName(daiNodeName, paramName), value); + return value; + } + std::string getFullParamName(const std::string& paramName) { return baseName + "." + paramName; } + std::string getFullParamName(const std::string& daiNodeName, const std::string& paramName) { + std::string name = std::string(baseNode->get_namespace()) + "/" + daiNodeName + "." + paramName; + return name; + } protected: rclcpp::Node* getROSNode() { diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp index 3dec9c08..2fc65c79 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp @@ -42,6 +42,7 @@ class NNParamHandler : public BaseParamHandler { declareAndLogParam("i_enable_passthrough", false); declareAndLogParam("i_enable_passthrough_depth", false); declareAndLogParam("i_get_base_device_timestamp", false); + declareAndLogParam("i_update_ros_base_time_on_ros_msg", false); auto nn_path = getParam("i_nn_config_path"); using json = nlohmann::json; std::ifstream f(nn_path); diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/sensor_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/sensor_param_handler.hpp index 8d34b7b1..87aa3ed2 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/sensor_param_handler.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/sensor_param_handler.hpp @@ -42,6 +42,7 @@ class SensorParamHandler : public BaseParamHandler { private: std::unordered_map monoResolutionMap; std::unordered_map rgbResolutionMap; + std::unordered_map fSyncModeMap; }; } // namespace param_handlers } // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/stereo_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/stereo_param_handler.hpp index 7d2a1c18..6b0e9857 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/stereo_param_handler.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/stereo_param_handler.hpp @@ -20,7 +20,7 @@ class StereoParamHandler : public BaseParamHandler { public: explicit StereoParamHandler(rclcpp::Node* node, const std::string& name); ~StereoParamHandler(); - void declareParams(std::shared_ptr stereo); + void declareParams(std::shared_ptr stereo, const std::string& rightName); dai::CameraControl setRuntimeParams(const std::vector& params) override; private: diff --git a/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_pipeline.hpp b/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_pipeline.hpp new file mode 100644 index 00000000..2959e68b --- /dev/null +++ b/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_pipeline.hpp @@ -0,0 +1,62 @@ +#pragma once + +#include +#include +#include +#include + +#include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "depthai_ros_driver/dai_nodes/nn/nn_helpers.hpp" +#include "depthai_ros_driver/dai_nodes/nn/nn_wrapper.hpp" +#include "depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" +#include "depthai_ros_driver/dai_nodes/stereo.hpp" + +namespace dai { +class Pipeline; +class Device; +} // namespace dai + +namespace rclcpp { +class Node; +} + +namespace depthai_ros_driver { +namespace pipeline_gen { +enum class NNType { None, RGB, Spatial }; +class BasePipeline { + public: + ~BasePipeline() = default; + std::unique_ptr createNN(rclcpp::Node* node, std::shared_ptr pipeline, dai_nodes::BaseNode& daiNode) { + auto nn = std::make_unique("nn", node, pipeline); + daiNode.link(nn->getInput(), static_cast(dai_nodes::link_types::RGBLinkType::preview)); + return nn; + } + std::unique_ptr createSpatialNN(rclcpp::Node* node, + std::shared_ptr pipeline, + dai_nodes::BaseNode& daiNode, + dai_nodes::BaseNode& daiStereoNode) { + auto nn = std::make_unique("nn", node, pipeline); + daiNode.link(nn->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::input)), + static_cast(dai_nodes::link_types::RGBLinkType::preview)); + daiStereoNode.link(nn->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::inputDepth))); + return nn; + } + + virtual std::vector> createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) = 0; + + protected: + BasePipeline(){}; + const std::string alphabet = "abcdefghijklmnopqrstuvwxyz"; + std::unordered_map nnTypeMap = { + {"", NNType::None}, + {"NONE", NNType::None}, + {"RGB", NNType::RGB}, + {"SPATIAL", NNType::Spatial}, + }; +}; +} // namespace pipeline_gen +} // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_types.hpp b/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_types.hpp new file mode 100644 index 00000000..a78c6333 --- /dev/null +++ b/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_types.hpp @@ -0,0 +1,65 @@ +#pragma once + +#include +#include +#include +#include + +#include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "depthai_ros_driver/pipeline/base_pipeline.hpp" + +namespace dai { +class Pipeline; +class Device; +} // namespace dai + +namespace rclcpp { +class Node; +} + +namespace depthai_ros_driver { +namespace pipeline_gen { +class RGB : public BasePipeline { + public: + std::vector> createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) override; +}; +class RGBD : public BasePipeline { + public: + std::vector> createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) override; +}; +class RGBStereo : public BasePipeline { + public: + std::vector> createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) override; +}; +class Stereo : public BasePipeline { + public: + std::vector> createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) override; +}; +class Depth : public BasePipeline { + public: + std::vector> createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) override; +}; +class CamArray : public BasePipeline { + public: + std::vector> createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) override; +}; +} // namespace pipeline_gen +} // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/include/depthai_ros_driver/pipeline_generator.hpp b/depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp similarity index 68% rename from depthai_ros_driver/include/depthai_ros_driver/pipeline_generator.hpp rename to depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp index 6a9ff91c..ff7fdefd 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/pipeline_generator.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp @@ -19,17 +19,11 @@ class Node; namespace depthai_ros_driver { namespace pipeline_gen { enum class PipelineType { RGB, RGBD, RGBStereo, Stereo, Depth, CamArray }; -enum class NNType { None, RGB, Spatial }; + class PipelineGenerator { public: - PipelineGenerator(){}; ~PipelineGenerator() = default; PipelineType validatePipeline(rclcpp::Node* node, PipelineType type, int sensorNum); - std::unique_ptr createNN(rclcpp::Node* node, std::shared_ptr pipeline, dai_nodes::BaseNode& daiNode); - std::unique_ptr createSpatialNN(rclcpp::Node* node, - std::shared_ptr pipeline, - dai_nodes::BaseNode& daiNode, - dai_nodes::BaseNode& daiStereoNode); std::vector> createPipeline(rclcpp::Node* node, std::shared_ptr device, std::shared_ptr pipeline, @@ -37,20 +31,19 @@ class PipelineGenerator { const std::string& nnType, bool enableImu); - private: + protected: + std::unordered_map pluginTypeMap{{"RGB", "depthai_ros_driver::pipeline_gen::RGB"}, + {"RGBD", "depthai_ros_driver::pipeline_gen::RGBD"}, + {"RGBSTEREO", "depthai_ros_driver::pipeline_gen::RGBStereo"}, + {"STEREO", "depthai_ros_driver::pipeline_gen::Stereo"}, + {"DEPTH", "depthai_ros_driver::pipeline_gen::Depth"}, + {"CAMARRAY", "depthai_ros_driver::pipeline_gen::CamArray"}}; std::unordered_map pipelineTypeMap{{"RGB", PipelineType::RGB}, {"RGBD", PipelineType::RGBD}, {"RGBSTEREO", PipelineType::RGBStereo}, {"STEREO", PipelineType::Stereo}, {"DEPTH", PipelineType::Depth}, {"CAMARRAY", PipelineType::CamArray}}; - std::unordered_map nnTypeMap = { - {"", NNType::None}, - {"NONE", NNType::None}, - {"RGB", NNType::RGB}, - {"SPATIAL", NNType::Spatial}, - }; - const std::string alphabet = "abcdefghijklmnopqrstuvwxyz"; }; } // namespace pipeline_gen } // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp index b99730ec..b370dd3b 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp @@ -19,7 +19,7 @@ T getValFromMap(const std::string& name, const std::unordered_mapfirst << "\n"; } - throw std::runtime_error(stream.str()); + throw std::out_of_range(stream.str()); } } std::string getUpperCaseStr(const std::string& string); diff --git a/depthai_ros_driver/launch/rtabmap.launch.py b/depthai_ros_driver/launch/rtabmap.launch.py index 47849a77..bd36584a 100644 --- a/depthai_ros_driver/launch/rtabmap.launch.py +++ b/depthai_ros_driver/launch/rtabmap.launch.py @@ -59,8 +59,8 @@ def launch_setup(context, *args, **kwargs): target_container=name+"_container", composable_node_descriptions=[ ComposableNode( - package='rtabmap_ros', - plugin='rtabmap_ros::RGBDOdometry', + package='rtabmap_odom', + plugin='rtabmap_odom::RGBDOdometry', name='rgbd_odometry', parameters=parameters, remappings=remappings, @@ -72,8 +72,8 @@ def launch_setup(context, *args, **kwargs): target_container=name+"_container", composable_node_descriptions=[ ComposableNode( - package='rtabmap_ros', - plugin='rtabmap_ros::CoreWrapper', + package='rtabmap_slam', + plugin='rtabmap_slam::CoreWrapper', name='rtabmap', parameters=parameters, remappings=remappings, @@ -82,8 +82,8 @@ def launch_setup(context, *args, **kwargs): ), Node( - package="rtabmap_ros", - executable="rtabmapviz", + package="rtabmap_viz", + executable="rtabmap_viz", output="screen", parameters=parameters, remappings=remappings, diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index d37f2a4d..99b05e63 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -2,7 +2,7 @@ depthai_ros_driver - 2.7.2 + 2.7.4 Depthai ROS Monolithic node. Adam Serafin Sachin Guruswamy @@ -28,6 +28,7 @@ depthai_descriptions depthai_ros_msgs depthai_examples + pluginlib ament_cmake diff --git a/depthai_ros_driver/plugins.xml b/depthai_ros_driver/plugins.xml new file mode 100644 index 00000000..1dfd7d40 --- /dev/null +++ b/depthai_ros_driver/plugins.xml @@ -0,0 +1,20 @@ + + + RGB Pipeline. + + + RGBD Pipeline. + + + RGBStereo Pipeline. + + + Stereo Pipeline. + + + Depth Pipeline. + + + CamArray Pipeline. + + \ No newline at end of file diff --git a/depthai_ros_driver/src/camera.cpp b/depthai_ros_driver/src/camera.cpp index f6c56620..e0d94a72 100644 --- a/depthai_ros_driver/src/camera.cpp +++ b/depthai_ros_driver/src/camera.cpp @@ -5,7 +5,7 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "depthai/device/Device.hpp" #include "depthai/pipeline/Pipeline.hpp" -#include "depthai_ros_driver/pipeline_generator.hpp" +#include "depthai_ros_driver/pipeline/pipeline_generator.hpp" namespace depthai_ros_driver { diff --git a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp index 034dc95c..78f6cc7a 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp @@ -38,28 +38,32 @@ 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 = ph->getSyncMethod(); + rclcpp::PublisherOptions options; + options.qos_overriding_options = rclcpp::QosOverridingOptions(); 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")); + ph->getParam("i_enable_rotation"), + ph->getParam("i_get_base_device_timestamp")); + imuConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); param_handlers::imu::ImuMsgType msgType = ph->getMsgType(); switch(msgType) { case param_handlers::imu::ImuMsgType::IMU: { - rosImuPub = getROSNode()->create_publisher("~/" + getName() + "/data", 10); + rosImuPub = getROSNode()->create_publisher("~/" + getName() + "/data", 10, options); 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); + daiImuPub = getROSNode()->create_publisher("~/" + getName() + "/data", 10, options); 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); + rosImuPub = getROSNode()->create_publisher("~/" + getName() + "/data", 10, options); + magPub = getROSNode()->create_publisher("~/" + getName() + "/mag", 10, options); imuQ->addCallback(std::bind(&Imu::imuMagQCB, this, std::placeholders::_1, std::placeholders::_2)); break; } diff --git a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp index 66de75a1..7266a08b 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp @@ -60,6 +60,7 @@ void Mono::setupQueues(std::shared_ptr device) { auto tfPrefix = getTFPrefix(getName()); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); + imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); monoPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw"); infoManager = std::make_shared( getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); diff --git a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp index 2ac136a5..52425001 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp @@ -73,6 +73,7 @@ void RGB::setupQueues(std::shared_ptr device) { getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); + imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); if(ph->getParam("i_calibration_file").empty()) { infoManager->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), *imageConverter, @@ -104,6 +105,7 @@ void RGB::setupQueues(std::shared_ptr device) { getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + previewQName).get(), previewQName); auto tfPrefix = getTFPrefix(getName()); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); + imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); if(ph->getParam("i_calibration_file").empty()) { previewInfoManager->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), *imageConverter, diff --git a/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp b/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp index 4edad260..06566462 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp @@ -51,6 +51,7 @@ SensorWrapper::SensorWrapper(const std::string& daiNodeName, throw std::runtime_error("Sensor not supported!"); } RCLCPP_DEBUG(node->get_logger(), "Node %s has sensor %s", daiNodeName.c_str(), sensorName.c_str()); + sensorData = *sensorIt; if((*sensorIt).color) { sensorNode = std::make_unique(daiNodeName, node, pipeline, socket, (*sensorIt), publish); } else { @@ -62,6 +63,10 @@ SensorWrapper::SensorWrapper(const std::string& daiNodeName, } SensorWrapper::~SensorWrapper() = default; +sensor_helpers::ImageSensor SensorWrapper::getSensorData() { + return sensorData; +} + void SensorWrapper::subCB(const sensor_msgs::msg::Image& img) { dai::ImgFrame data; converter->toDaiMsg(img, data); diff --git a/depthai_ros_driver/src/dai_nodes/stereo.cpp b/depthai_ros_driver/src/dai_nodes/stereo.cpp index ebe7e860..6e087ccf 100644 --- a/depthai_ros_driver/src/dai_nodes/stereo.cpp +++ b/depthai_ros_driver/src/dai_nodes/stereo.cpp @@ -18,16 +18,21 @@ namespace depthai_ros_driver { namespace dai_nodes { -Stereo::Stereo(const std::string& daiNodeName, rclcpp::Node* node, std::shared_ptr pipeline, std::shared_ptr device) - : BaseNode(daiNodeName, node, pipeline) { +Stereo::Stereo(const std::string& daiNodeName, + rclcpp::Node* node, + std::shared_ptr pipeline, + std::shared_ptr device, + StereoSensorInfo leftInfo, + StereoSensorInfo rightInfo) + : BaseNode(daiNodeName, node, pipeline), leftSensInfo(leftInfo), rightSensInfo(rightInfo) { RCLCPP_DEBUG(node->get_logger(), "Creating node %s", daiNodeName.c_str()); setNames(); stereoCamNode = pipeline->create(); - left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::LEFT, false); - right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::RIGHT, false); + left = std::make_unique(leftInfo.name, node, pipeline, device, leftInfo.socket, false); + right = std::make_unique(rightInfo.name, node, pipeline, device, rightInfo.socket, false); ph = std::make_unique(node, daiNodeName); - ph->declareParams(stereoCamNode); + ph->declareParams(stereoCamNode, rightInfo.name); setXinXout(pipeline); left->link(stereoCamNode->left); right->link(stereoCamNode->right); @@ -36,79 +41,175 @@ Stereo::Stereo(const std::string& daiNodeName, rclcpp::Node* node, std::shared_p Stereo::~Stereo() = default; void Stereo::setNames() { stereoQName = getName() + "_stereo"; + leftRectQName = getName() + "_left_rect"; + rightRectQName = getName() + "_right_rect"; } void Stereo::setXinXout(std::shared_ptr pipeline) { - xoutStereo = pipeline->create(); - xoutStereo->setStreamName(stereoQName); - if(ph->getParam("i_low_bandwidth")) { - videoEnc = sensor_helpers::createEncoder(pipeline, ph->getParam("i_low_bandwidth_quality")); - stereoCamNode->disparity.link(videoEnc->input); - videoEnc->bitstream.link(xoutStereo->input); - } else { - if(ph->getParam("i_output_disparity")) { - stereoCamNode->disparity.link(xoutStereo->input); + if(ph->getParam("i_publish_topic")) { + xoutStereo = pipeline->create(); + xoutStereo->setStreamName(stereoQName); + if(ph->getParam("i_low_bandwidth")) { + stereoEnc = sensor_helpers::createEncoder(pipeline, ph->getParam("i_low_bandwidth_quality")); + stereoCamNode->disparity.link(stereoEnc->input); + stereoEnc->bitstream.link(xoutStereo->input); + } else { + if(ph->getParam("i_output_disparity")) { + stereoCamNode->disparity.link(xoutStereo->input); + } else { + stereoCamNode->depth.link(xoutStereo->input); + } + } + } + if(ph->getParam("i_publish_left_rect")) { + xoutLeftRect = pipeline->create(); + xoutLeftRect->setStreamName(leftRectQName); + if(ph->getParam("i_left_rect_low_bandwidth")) { + leftRectEnc = sensor_helpers::createEncoder(pipeline, ph->getParam("i_left_rect_low_bandwidth_quality")); + stereoCamNode->rectifiedLeft.link(leftRectEnc->input); + leftRectEnc->bitstream.link(xoutLeftRect->input); } else { - stereoCamNode->depth.link(xoutStereo->input); + stereoCamNode->rectifiedLeft.link(xoutLeftRect->input); + } + } + + if(ph->getParam("i_publish_right_rect")) { + xoutRightRect = pipeline->create(); + xoutRightRect->setStreamName(rightRectQName); + if(ph->getParam("i_right_rect_low_bandwidth")) { + rightRectEnc = sensor_helpers::createEncoder(pipeline, ph->getParam("i_right_rect_low_bandwidth_quality")); + stereoCamNode->rectifiedRight.link(rightRectEnc->input); + rightRectEnc->bitstream.link(xoutRightRect->input); + } else { + stereoCamNode->rectifiedRight.link(xoutRightRect->input); } } } -void Stereo::setupQueues(std::shared_ptr device) { - left->setupQueues(device); - right->setupQueues(device); +void Stereo::setupLeftRectQueue(std::shared_ptr device) { + leftRectQ = device->getOutputQueue(leftRectQName, ph->getOtherNodeParam(leftSensInfo.name, "i_max_q_size"), false); + auto tfPrefix = getTFPrefix(leftSensInfo.name); + leftRectConv = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); + leftRectConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); + leftRectIM = std::make_shared( + getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + leftSensInfo.name).get(), "/rect"); + auto info = sensor_helpers::getCalibInfo(getROSNode()->get_logger(), + *leftRectConv, + device, + leftSensInfo.socket, + ph->getOtherNodeParam(leftSensInfo.name, "i_width"), + ph->getOtherNodeParam(leftSensInfo.name, "i_height")); + leftRectIM->setCameraInfo(info); + leftRectPub = image_transport::create_camera_publisher(getROSNode(), "~/" + leftSensInfo.name + "/image_rect"); + dai::RawImgFrame::Type encType = dai::RawImgFrame::Type::GRAY8; + if(left->getSensorData().color) { + encType = dai::RawImgFrame::Type::BGR888i; + } + if(ph->getParam("i_left_rect_low_bandwidth")) { + leftRectQ->addCallback( + std::bind(sensor_helpers::compressedImgCB, std::placeholders::_1, std::placeholders::_2, *leftRectConv, leftRectPub, leftRectIM, encType)); + } else { + leftRectQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *leftRectConv, leftRectPub, leftRectIM)); + } +} + +void Stereo::setupRightRectQueue(std::shared_ptr device) { + rightRectQ = device->getOutputQueue(rightRectQName, ph->getOtherNodeParam(rightSensInfo.name, "i_max_q_size"), false); + auto tfPrefix = getTFPrefix(rightSensInfo.name); + rightRectConv = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); + rightRectConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); + rightRectIM = std::make_shared( + getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + rightSensInfo.name).get(), "/rect"); + auto info = sensor_helpers::getCalibInfo(getROSNode()->get_logger(), + *rightRectConv, + device, + rightSensInfo.socket, + ph->getOtherNodeParam(rightSensInfo.name, "i_width"), + ph->getOtherNodeParam(rightSensInfo.name, "i_height")); + rightRectIM->setCameraInfo(info); + rightRectPub = image_transport::create_camera_publisher(getROSNode(), "~/" + rightSensInfo.name + "/image_rect"); + dai::RawImgFrame::Type encType = dai::RawImgFrame::Type::GRAY8; + if(right->getSensorData().color) { + encType = dai::RawImgFrame::Type::BGR888i; + } + if(ph->getParam("i_right_rect_low_bandwidth")) { + rightRectQ->addCallback( + std::bind(sensor_helpers::compressedImgCB, std::placeholders::_1, std::placeholders::_2, *rightRectConv, rightRectPub, rightRectIM, encType)); + } else { + rightRectQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *rightRectConv, rightRectPub, rightRectIM)); + } +} + +void Stereo::setupStereoQueue(std::shared_ptr device) { stereoQ = device->getOutputQueue(stereoQName, ph->getParam("i_max_q_size"), false); std::string tfPrefix; if(ph->getParam("i_align_depth")) { tfPrefix = getTFPrefix("rgb"); } else { - tfPrefix = getTFPrefix("right"); + tfPrefix = getTFPrefix(rightSensInfo.name); } - imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); - + stereoConv = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); + stereoConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); stereoPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw"); - infoManager = std::make_shared( + stereoIM = std::make_shared( getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); auto info = sensor_helpers::getCalibInfo(getROSNode()->get_logger(), - *imageConverter, + *stereoConv, device, static_cast(ph->getParam("i_board_socket_id")), ph->getParam("i_width"), ph->getParam("i_height")); auto calibHandler = device->readCalibration(); info.p[3] = calibHandler.getBaselineDistance() * 10.0; // baseline in mm - infoManager->setCameraInfo(info); + stereoIM->setCameraInfo(info); if(ph->getParam("i_low_bandwidth")) { if(ph->getParam("i_output_disparity")) { stereoQ->addCallback(std::bind(sensor_helpers::compressedImgCB, std::placeholders::_1, std::placeholders::_2, - *imageConverter, + *stereoConv, stereoPub, - infoManager, + stereoIM, dai::RawImgFrame::Type::GRAY8)); } else { // converting disp->depth - stereoQ->addCallback(std::bind(sensor_helpers::compressedImgCB, - std::placeholders::_1, - std::placeholders::_2, - *imageConverter, - stereoPub, - infoManager, - dai::RawImgFrame::Type::RAW8)); + stereoQ->addCallback(std::bind( + sensor_helpers::compressedImgCB, std::placeholders::_1, std::placeholders::_2, *stereoConv, stereoPub, stereoIM, dai::RawImgFrame::Type::RAW8)); } } else { if(ph->getParam("i_output_disparity")) { - stereoQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, stereoPub, infoManager)); + stereoQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *stereoConv, stereoPub, stereoIM)); } - stereoQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *imageConverter, stereoPub, infoManager)); + stereoQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *stereoConv, stereoPub, stereoIM)); + } +} + +void Stereo::setupQueues(std::shared_ptr device) { + left->setupQueues(device); + right->setupQueues(device); + if(ph->getParam("i_publish_topic")) { + setupStereoQueue(device); + } + if(ph->getParam("i_publish_left_rect")) { + setupLeftRectQueue(device); + } + if(ph->getParam("i_publish_right_rect")) { + setupRightRectQueue(device); } } void Stereo::closeQueues() { left->closeQueues(); right->closeQueues(); - stereoQ->close(); + if(ph->getParam("i_publish_topic")) { + stereoQ->close(); + } + if(ph->getParam("i_publish_left_rect")) { + leftRectQ->close(); + } + if(ph->getParam("i_publish_right_rect")) { + rightRectQ->close(); + } } void Stereo::link(dai::Node::Input in, int /*linkType*/) { 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 8dda80cd..7dcdb3b1 100644 --- a/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp @@ -18,12 +18,14 @@ void ImuParamHandler::declareParams(std::shared_ptr imu, const s }; 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_get_base_device_timestamp", false); 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); + declareAndLogParam("i_update_ros_base_time_on_ros_msg", false); bool rotationAvailable = imuType == "BNO086"; if(declareAndLogParam("i_enable_rotation", false)) { if(rotationAvailable) { diff --git a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp index d1fb3165..18a75206 100644 --- a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp @@ -26,6 +26,12 @@ void SensorParamHandler::declareCommonParams() { declareAndLogParam("i_disable_node", false); declareAndLogParam("i_get_base_device_timestamp", false); declareAndLogParam("i_board_socket_id", 0); + declareAndLogParam("i_update_ros_base_time_on_ros_msg", false); + fSyncModeMap = { + {"OFF", dai::CameraControl::FrameSyncMode::OFF}, + {"OUTPUT", dai::CameraControl::FrameSyncMode::OUTPUT}, + {"INPUT", dai::CameraControl::FrameSyncMode::INPUT}, + }; } void SensorParamHandler::declareParams(std::shared_ptr monoCam, @@ -49,9 +55,15 @@ void SensorParamHandler::declareParams(std::shared_ptr mo size_t iso = declareAndLogParam("r_iso", 800, getRangedIntDescriptor(100, 1600)); size_t exposure = declareAndLogParam("r_exposure", 1000, getRangedIntDescriptor(1, 33000)); - if(declareAndLogParam("r_set_man_exposure", false)) { + if(declareAndLogParam("r_set_man_exposure", false)) { monoCam->initialControl.setManualExposure(exposure, iso); } + if(declareAndLogParam("i_fsync_continuous", false)) { + monoCam->initialControl.setFrameSyncMode(utils::getValFromMap(declareAndLogParam("i_fsync_mode", "INPUT"), fSyncModeMap)); + } + if(declareAndLogParam("i_fsync_trigger", false)) { + monoCam->initialControl.setExternalTrigger(declareAndLogParam("i_num_frames_burst", 1), declareAndLogParam("i_num_frames_discard", 0)); + } } void SensorParamHandler::declareParams(std::shared_ptr colorCam, dai::CameraBoardSocket socket, @@ -113,6 +125,12 @@ void SensorParamHandler::declareParams(std::shared_ptr c if(declareAndLogParam("r_set_man_whitebalance", false)) { colorCam->initialControl.setManualWhiteBalance(whitebalance); } + if(declareAndLogParam("i_fsync_continuous", false)) { + colorCam->initialControl.setFrameSyncMode(utils::getValFromMap(declareAndLogParam("i_fsync_mode", "INPUT"), fSyncModeMap)); + } + if(declareAndLogParam("i_fsync_trigger", false)) { + colorCam->initialControl.setExternalTrigger(declareAndLogParam("i_num_frames_burst", 1), declareAndLogParam("i_num_frames_discard", 0)); + } } dai::CameraControl SensorParamHandler::setRuntimeParams(const std::vector& params) { dai::CameraControl 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 78532081..7400a46d 100644 --- a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp @@ -37,35 +37,48 @@ StereoParamHandler::StereoParamHandler(rclcpp::Node* node, const std::string& na } StereoParamHandler::~StereoParamHandler() = default; -void StereoParamHandler::declareParams(std::shared_ptr stereo) { +void StereoParamHandler::declareParams(std::shared_ptr stereo, const std::string& rightName) { declareAndLogParam("i_max_q_size", 30); declareAndLogParam("i_low_bandwidth", false); declareAndLogParam("i_low_bandwidth_quality", 50); declareAndLogParam("i_output_disparity", false); declareAndLogParam("i_get_base_device_timestamp", false); + declareAndLogParam("i_update_ros_base_time_on_ros_msg", false); + declareAndLogParam("i_publish_topic", true); + + declareAndLogParam("i_publish_left_rect", false); + declareAndLogParam("i_left_rect_low_bandwidth", false); + declareAndLogParam("i_left_rect_low_bandwidth_quality", 50); + + declareAndLogParam("i_publish_right_rect", false); + declareAndLogParam("i_right_rect_low_bandwidth", false); + declareAndLogParam("i_right_rect_low_bandwidth_quality", 50); + stereo->setLeftRightCheck(declareAndLogParam("i_lr_check", true)); int width = 1280; int height = 720; + dai::CameraBoardSocket socket = dai::CameraBoardSocket::RIGHT; if(declareAndLogParam("i_align_depth", true)) { - declareAndLogParam("i_board_socket_id", static_cast(dai::CameraBoardSocket::RGB)); - stereo->setDepthAlign(dai::CameraBoardSocket::RGB); try { width = getROSNode()->get_parameter("rgb.i_width").as_int(); height = getROSNode()->get_parameter("rgb.i_height").as_int(); + socket = static_cast(getROSNode()->get_parameter("rgb.i_board_socket_id").as_int()); } catch(rclcpp::exceptions::ParameterNotDeclaredException& e) { RCLCPP_ERROR(getROSNode()->get_logger(), "RGB parameters not set, defaulting to 1280x720 unless specified otherwise."); } } else { - declareAndLogParam("i_board_socket_id", static_cast(dai::CameraBoardSocket::RIGHT)); try { - width = getROSNode()->get_parameter("right.i_width").as_int(); - height = getROSNode()->get_parameter("right.i_height").as_int(); + width = getROSNode()->get_parameter(rightName + ".i_width").as_int(); + height = getROSNode()->get_parameter(rightName + ".i_height").as_int(); + socket = static_cast(getROSNode()->get_parameter(rightName + ".i_board_socket_id").as_int()); } catch(rclcpp::exceptions::ParameterNotDeclaredException& e) { RCLCPP_ERROR(getROSNode()->get_logger(), "Right parameters not set, defaulting to 1280x720 unless specified otherwise."); } - stereo->setDepthAlign(dai::CameraBoardSocket::RIGHT); } + declareAndLogParam("i_board_socket_id", static_cast(socket)); + stereo->setDepthAlign(socket); + if(declareAndLogParam("i_set_input_size", false)) { stereo->setInputResolution(declareAndLogParam("i_input_width", 1280), declareAndLogParam("i_input_height", 720)); } @@ -81,13 +94,11 @@ void StereoParamHandler::declareParams(std::shared_ptr s stereo->initialConfig.setSubpixel(true); stereo->initialConfig.setSubpixelFractionalBits(declareAndLogParam("i_subpixel_fractional_bits", 3)); } - 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); - if(!config.algorithmControl.enableExtended) { - config.costMatching.enableCompanding = declareAndLogParam("i_enable_companding", false); - } + stereo->setExtendedDisparity(declareAndLogParam("i_extended_disp", false)); + config.costMatching.enableCompanding = declareAndLogParam("i_enable_companding", false); config.postProcessing.temporalFilter.enable = declareAndLogParam("i_enable_temporal_filter", false); if(config.postProcessing.temporalFilter.enable) { config.postProcessing.temporalFilter.alpha = declareAndLogParam("i_temporal_filter_alpha", 0.4); @@ -110,6 +121,10 @@ void StereoParamHandler::declareParams(std::shared_ptr s config.postProcessing.thresholdFilter.minRange = declareAndLogParam("i_threshold_filter_min_range", 400); config.postProcessing.thresholdFilter.maxRange = declareAndLogParam("i_threshold_filter_max_range", 15000); } + if(declareAndLogParam("i_enable_brightness_filter", false)) { + config.postProcessing.brightnessFilter.minBrightness = declareAndLogParam("i_brightness_filter_min_brightness", 0); + config.postProcessing.brightnessFilter.maxBrightness = declareAndLogParam("i_brightness_filter_max_brightness", 256); + } if(declareAndLogParam("i_enable_decimation_filter", false)) { config.postProcessing.decimationFilter.decimationMode = utils::getValFromMap(declareAndLogParam("i_decimation_filter_decimation_mode", "PIXEL_SKIPPING"), decimationModeMap); diff --git a/depthai_ros_driver/src/pipeline/base_types.cpp b/depthai_ros_driver/src/pipeline/base_types.cpp new file mode 100644 index 00000000..6432fb72 --- /dev/null +++ b/depthai_ros_driver/src/pipeline/base_types.cpp @@ -0,0 +1,155 @@ +#include "depthai_ros_driver/pipeline/base_types.hpp" + +#include "depthai/device/Device.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "depthai_ros_driver/dai_nodes/nn/nn_helpers.hpp" +#include "depthai_ros_driver/dai_nodes/nn/nn_wrapper.hpp" +#include "depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/imu.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp" +#include "depthai_ros_driver/dai_nodes/stereo.hpp" +#include "depthai_ros_driver/pipeline/base_pipeline.hpp" +#include "depthai_ros_driver/utils.hpp" +#include "rclcpp/node.hpp" + +namespace depthai_ros_driver { +namespace pipeline_gen { + +std::vector> RGB::createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) { + std::string nTypeUpCase = utils::getUpperCaseStr(nnType); + auto nType = utils::getValFromMap(nTypeUpCase, nnTypeMap); + + std::vector> daiNodes; + auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::RGB); + switch(nType) { + case NNType::None: + break; + case NNType::RGB: { + auto nn = createNN(node, pipeline, *rgb); + daiNodes.push_back(std::move(nn)); + break; + } + case NNType::Spatial: { + RCLCPP_WARN(node->get_logger(), "Spatial NN selected, but configuration is RGB."); + } + default: + break; + } + daiNodes.push_back(std::move(rgb)); + return daiNodes; +} +std::vector> RGBD::createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) { + std::string nTypeUpCase = utils::getUpperCaseStr(nnType); + auto nType = utils::getValFromMap(nTypeUpCase, nnTypeMap); + + std::vector> daiNodes; + auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::RGB); + auto stereo = std::make_unique("stereo", node, pipeline, device); + switch(nType) { + case NNType::None: + break; + case NNType::RGB: { + auto nn = createNN(node, pipeline, *rgb); + daiNodes.push_back(std::move(nn)); + break; + } + case NNType::Spatial: { + auto nn = createSpatialNN(node, pipeline, *rgb, *stereo); + daiNodes.push_back(std::move(nn)); + break; + } + default: + break; + } + daiNodes.push_back(std::move(rgb)); + daiNodes.push_back(std::move(stereo)); + return daiNodes; +} +std::vector> RGBStereo::createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) { + std::string nTypeUpCase = utils::getUpperCaseStr(nnType); + auto nType = utils::getValFromMap(nTypeUpCase, nnTypeMap); + + std::vector> daiNodes; + auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::RGB); + auto left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::LEFT); + auto right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::RIGHT); + switch(nType) { + case NNType::None: + break; + case NNType::RGB: { + auto nn = createNN(node, pipeline, *rgb); + daiNodes.push_back(std::move(nn)); + break; + } + case NNType::Spatial: { + RCLCPP_WARN(node->get_logger(), "Spatial NN selected, but configuration is RGBStereo."); + } + default: + break; + } + daiNodes.push_back(std::move(rgb)); + daiNodes.push_back(std::move(left)); + daiNodes.push_back(std::move(right)); + return daiNodes; +} +std::vector> Stereo::createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& /*nnType*/) { + std::vector> daiNodes; + auto left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::LEFT); + auto right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::RIGHT); + daiNodes.push_back(std::move(left)); + daiNodes.push_back(std::move(right)); + return daiNodes; +} +std::vector> Depth::createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& /*nnType*/) { + std::vector> daiNodes; + auto stereo = std::make_unique("stereo", node, pipeline, device); + daiNodes.push_back(std::move(stereo)); + return daiNodes; +} +std::vector> CamArray::createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& /*nnType*/) { + std::vector> daiNodes; + int i = 0; + int j = 0; + for(auto& sensor : device->getCameraSensorNames()) { + // append letter for greater sensor number + if(i % alphabet.size() == 0) { + j++; + } + std::string nodeName(j, alphabet[i % alphabet.size()]); + auto daiNode = std::make_unique(nodeName, node, pipeline, device, sensor.first); + daiNodes.push_back(std::move(daiNode)); + i++; + }; + return daiNodes; +} +} // namespace pipeline_gen +} // namespace depthai_ros_driver + +#include + +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::RGB, depthai_ros_driver::pipeline_gen::BasePipeline) +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::RGBD, depthai_ros_driver::pipeline_gen::BasePipeline) +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::RGBStereo, depthai_ros_driver::pipeline_gen::BasePipeline) +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::Stereo, depthai_ros_driver::pipeline_gen::BasePipeline) +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::Depth, depthai_ros_driver::pipeline_gen::BasePipeline) +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::CamArray, depthai_ros_driver::pipeline_gen::BasePipeline) \ No newline at end of file diff --git a/depthai_ros_driver/src/pipeline/pipeline_generator.cpp b/depthai_ros_driver/src/pipeline/pipeline_generator.cpp new file mode 100644 index 00000000..d9a6be32 --- /dev/null +++ b/depthai_ros_driver/src/pipeline/pipeline_generator.cpp @@ -0,0 +1,71 @@ +#include "depthai_ros_driver/pipeline/pipeline_generator.hpp" + +#include "depthai/device/Device.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/imu.hpp" +#include "depthai_ros_driver/pipeline/base_pipeline.hpp" +#include "depthai_ros_driver/utils.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/node.hpp" + +namespace depthai_ros_driver { +namespace pipeline_gen { +std::vector> PipelineGenerator::createPipeline(rclcpp::Node* node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& pipelineType, + const std::string& nnType, + bool enableImu) { + RCLCPP_INFO(node->get_logger(), "Pipeline type: %s", pipelineType.c_str()); + std::string pluginType = pipelineType; + std::vector> daiNodes; + // Check if one of the default types. + try { + std::string pTypeUpCase = utils::getUpperCaseStr(pipelineType); + auto pType = utils::getValFromMap(pTypeUpCase, pipelineTypeMap); + pType = validatePipeline(node, pType, device->getCameraSensorNames().size()); + pluginType = utils::getValFromMap(pTypeUpCase, pluginTypeMap); + } catch(std::out_of_range& e) { + RCLCPP_DEBUG(node->get_logger(), "Pipeline type [%s] not found in base types, trying to load as a plugin.", pipelineType.c_str()); + } + pluginlib::ClassLoader pipelineLoader("depthai_ros_driver", "depthai_ros_driver::pipeline_gen::BasePipeline"); + + try { + std::shared_ptr pipelinePlugin = pipelineLoader.createSharedInstance(pluginType); + daiNodes = pipelinePlugin->createPipeline(node, device, pipeline, nnType); + } catch(pluginlib::PluginlibException& ex) { + RCLCPP_ERROR(node->get_logger(), "The plugin failed to load for some reason. Error: %s\n", ex.what()); + } + + if(enableImu) { + if(device->getConnectedIMU() == "NONE") { + RCLCPP_WARN(node->get_logger(), "IMU enabled but not available!"); + } else { + auto imu = std::make_unique("imu", node, pipeline, device); + daiNodes.push_back(std::move(imu)); + } + } + + RCLCPP_INFO(node->get_logger(), "Finished setting up pipeline."); + return daiNodes; +} + +PipelineType PipelineGenerator::validatePipeline(rclcpp::Node* node, PipelineType type, int sensorNum) { + if(sensorNum == 1) { + if(type != PipelineType::RGB) { + RCLCPP_ERROR(node->get_logger(), "Wrong pipeline chosen for camera as it has only one sensor. Switching to RGB."); + return PipelineType::RGB; + } + } else if(sensorNum == 2) { + if(type != PipelineType::Stereo && type != PipelineType::Depth) { + RCLCPP_ERROR(node->get_logger(), "Wrong pipeline chosen for camera as it has only stereo pair. Switching to Stereo."); + return PipelineType::Stereo; + } + } else if(sensorNum > 3 && type != PipelineType::CamArray) { + RCLCPP_ERROR(node->get_logger(), "For cameras with more than three sensors you can only use CamArray. Switching to CamArray."); + return PipelineType::CamArray; + } + return type; +} +} // namespace pipeline_gen +} // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/src/pipeline_generator.cpp b/depthai_ros_driver/src/pipeline_generator.cpp deleted file mode 100644 index 11d9eb86..00000000 --- a/depthai_ros_driver/src/pipeline_generator.cpp +++ /dev/null @@ -1,170 +0,0 @@ -#include "depthai_ros_driver/pipeline_generator.hpp" - -#include "depthai/device/Device.hpp" -#include "depthai/pipeline/Pipeline.hpp" -#include "depthai_ros_driver/dai_nodes/nn/nn_helpers.hpp" -#include "depthai_ros_driver/dai_nodes/nn/nn_wrapper.hpp" -#include "depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp" -#include "depthai_ros_driver/dai_nodes/sensors/imu.hpp" -#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" -#include "depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp" -#include "depthai_ros_driver/dai_nodes/stereo.hpp" -#include "depthai_ros_driver/utils.hpp" -#include "rclcpp/node.hpp" - -namespace depthai_ros_driver { -namespace pipeline_gen { -std::vector> PipelineGenerator::createPipeline(rclcpp::Node* node, - std::shared_ptr device, - std::shared_ptr pipeline, - const std::string& pipelineType, - const std::string& nnType, - bool enableImu) { - RCLCPP_INFO(node->get_logger(), "Pipeline type: %s", pipelineType.c_str()); - std::string pTypeUpCase = utils::getUpperCaseStr(pipelineType); - std::string nTypeUpCase = utils::getUpperCaseStr(nnType); - auto pType = utils::getValFromMap(pTypeUpCase, pipelineTypeMap); - pType = validatePipeline(node, pType, device->getCameraSensorNames().size()); - auto nType = utils::getValFromMap(nTypeUpCase, nnTypeMap); - std::vector> daiNodes; - switch(pType) { - case PipelineType::RGB: { - auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::RGB); - switch(nType) { - case NNType::None: - break; - case NNType::RGB: { - auto nn = createNN(node, pipeline, *rgb); - daiNodes.push_back(std::move(nn)); - break; - } - case NNType::Spatial: { - RCLCPP_WARN(node->get_logger(), "Spatial NN selected, but configuration is RGB."); - } - default: - break; - } - daiNodes.push_back(std::move(rgb)); - break; - } - - case PipelineType::RGBD: { - auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::RGB); - auto stereo = std::make_unique("stereo", node, pipeline, device); - switch(nType) { - case NNType::None: - break; - case NNType::RGB: { - auto nn = createNN(node, pipeline, *rgb); - daiNodes.push_back(std::move(nn)); - break; - } - case NNType::Spatial: { - auto nn = createSpatialNN(node, pipeline, *rgb, *stereo); - daiNodes.push_back(std::move(nn)); - break; - } - default: - break; - } - daiNodes.push_back(std::move(rgb)); - daiNodes.push_back(std::move(stereo)); - break; - } - case PipelineType::RGBStereo: { - auto rgb = std::make_unique("rgb", node, pipeline, device, dai::CameraBoardSocket::RGB); - auto left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::LEFT); - auto right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::RIGHT); - switch(nType) { - case NNType::None: - break; - case NNType::RGB: { - auto nn = createNN(node, pipeline, *rgb); - daiNodes.push_back(std::move(nn)); - break; - } - case NNType::Spatial: { - RCLCPP_WARN(node->get_logger(), "Spatial NN selected, but configuration is RGBStereo."); - } - default: - break; - } - daiNodes.push_back(std::move(rgb)); - daiNodes.push_back(std::move(left)); - daiNodes.push_back(std::move(right)); - break; - } - case PipelineType::Stereo: { - auto left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::LEFT); - auto right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::RIGHT); - daiNodes.push_back(std::move(left)); - daiNodes.push_back(std::move(right)); - break; - } - case PipelineType::Depth: { - auto stereo = std::make_unique("stereo", node, pipeline, device); - daiNodes.push_back(std::move(stereo)); - break; - } - case PipelineType::CamArray: { - int i = 0; - int j = 0; - for(auto& sensor : device->getCameraSensorNames()) { - // append letter for greater sensor number - if(i % alphabet.size() == 0) { - j++; - } - std::string nodeName(j, alphabet[i % alphabet.size()]); - auto daiNode = std::make_unique(nodeName, node, pipeline, device, sensor.first); - daiNodes.push_back(std::move(daiNode)); - i++; - }; - break; - } - default: { - std::string configuration = pipelineType; - throw std::runtime_error("UNKNOWN PIPELINE TYPE SPECIFIED/CAMERA DOESN'T SUPPORT GIVEN PIPELINE. Configuration: " + configuration); - } - } - if(enableImu) { - auto imu = std::make_unique("imu", node, pipeline, device); - daiNodes.push_back(std::move(imu)); - } - - RCLCPP_INFO(node->get_logger(), "Finished setting up pipeline."); - return daiNodes; -} -std::unique_ptr PipelineGenerator::createNN(rclcpp::Node* node, std::shared_ptr pipeline, dai_nodes::BaseNode& daiNode) { - auto nn = std::make_unique("nn", node, pipeline); - daiNode.link(nn->getInput(), static_cast(dai_nodes::link_types::RGBLinkType::preview)); - return nn; -} -std::unique_ptr PipelineGenerator::createSpatialNN(rclcpp::Node* node, - std::shared_ptr pipeline, - dai_nodes::BaseNode& daiNode, - dai_nodes::BaseNode& daiStereoNode) { - auto nn = std::make_unique("nn", node, pipeline); - daiNode.link(nn->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::input)), - static_cast(dai_nodes::link_types::RGBLinkType::preview)); - daiStereoNode.link(nn->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::inputDepth))); - return nn; -} -PipelineType PipelineGenerator::validatePipeline(rclcpp::Node* node, PipelineType type, int sensorNum) { - if(sensorNum == 1) { - if(type != PipelineType::RGB) { - RCLCPP_ERROR(node->get_logger(), "Wrong pipeline chosen for camera as it has only one sensor. Switching to RGB."); - return PipelineType::RGB; - } - } else if(sensorNum == 2) { - if(type != PipelineType::Stereo && type != PipelineType::Depth) { - RCLCPP_ERROR(node->get_logger(), "Wrong pipeline chosen for camera as it has only stereo pair. Switching to Stereo."); - return PipelineType::Stereo; - } - } else if(sensorNum > 3 && type != PipelineType::CamArray) { - RCLCPP_ERROR(node->get_logger(), "For cameras with more than three sensors you can only use CamArray. Switching to CamArray."); - return PipelineType::CamArray; - } - return type; -} -} // namespace pipeline_gen -} // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index cba6e91f..fd4c0851 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.2) +project(depthai_ros_msgs VERSION 2.7.4) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml index 16dd76d7..5f7a3663 100644 --- a/depthai_ros_msgs/package.xml +++ b/depthai_ros_msgs/package.xml @@ -1,7 +1,7 @@ depthai_ros_msgs - 2.7.2 + 2.7.4 Package to keep interface independent of the driver Sachin Guruswamy