diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index 089a2dec..053db04b 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/depthai-ros/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package depthai-ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.1 (2024-09-18) +------------------- +* Fix ToF synced publishing +* Add camera_info publishing when publishing compressed images +* Catch errors when starting the device + +2.10.0 (2024-08-29) +------------------- +* Adding stl files for SR and LR models by @danilo-pejovic in https://github.com/luxonis/depthai-ros/pull/491 +* No imu fix Humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/500 +* Tracking converter for ROS2 Humble by @daniqsilva25 in https://github.com/luxonis/depthai-ros/pull/505 +* Added Env Hooks so that depthai xacro can be used with gazebo sim by @r4hul77 in https://github.com/luxonis/depthai-ros/pull/507 +* Fix resource paths for Ignition Gazebo by @Nibanovic in https://github.com/luxonis/depthai-ros/pull/511 +* Use simulation flag to decide how to load meshes. by @destogl in https://github.com/luxonis/depthai-ros/pull/524 +* Add new launch file for starting multiple rgbd cameras on robots. by @destogl in https://github.com/luxonis/depthai-ros/pull/532 +* Missing fields in detection messages Humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/574 +* Ip autodiscovery fix Humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/561 +* RS Mode & Sync - Humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/578 +* Compressed image publishers by @Serafadam in https://github.com/luxonis/depthai-ros/pull/580 +* ToF Support Humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/581 +* WLS fix humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/582 +* Syncing & RS updates Humble by @Serafadam in https://github.com/luxonis/depthai-ros/pull/586 + 2.9.0 (2024-01-24) ------------------- diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt index eb0bc15d..3913f5ac 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.9.0 LANGUAGES CXX C) +project(depthai-ros VERSION 2.10.1 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml index 6e12193e..e6993e2d 100644 --- a/depthai-ros/package.xml +++ b/depthai-ros/package.xml @@ -1,13 +1,10 @@ depthai-ros - 2.9.0 + 2.10.1 The depthai-ros package - - - - sachin + Adam Serafin Sachin Guruswamy MIT diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index 17cc2235..f9b6e51c 100644 --- a/depthai_bridge/CMakeLists.txt +++ b/depthai_bridge/CMakeLists.txt @@ -1,10 +1,11 @@ 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.9.0 LANGUAGES CXX C) +project(depthai_bridge VERSION 2.10.1 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # Update the policy setting to avoid an error when loading the ament_cmake package # at the current cmake version level @@ -55,6 +56,8 @@ FILE(GLOB LIB_SRC "src/ImuConverter.cpp" "src/TFPublisher.cpp" "src/TrackedFeaturesConverter.cpp" +"src/TrackedDetectionConverter.cpp" +"src/TrackedSpatialDetectionConverter.cpp" ) catkin_package( diff --git a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp index fa5b0b8a..66359eaa 100644 --- a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp @@ -9,9 +9,12 @@ #include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai-shared/common/Point2f.hpp" #include "depthai/device/CalibrationHandler.hpp" +#include "depthai/pipeline/datatype/EncodedFrame.hpp" #include "depthai/pipeline/datatype/ImgFrame.hpp" +#include "depthai_ros_msgs/FFMPEGPacket.h" #include "ros/time.h" #include "sensor_msgs/CameraInfo.h" +#include "sensor_msgs/CompressedImage.h" #include "sensor_msgs/Image.h" #include "std_msgs/Header.h" @@ -21,8 +24,11 @@ namespace ros { namespace StdMsgs = std_msgs; namespace ImageMsgs = sensor_msgs; +namespace DepthAiRosMsgs = depthai_ros_msgs; using ImagePtr = ImageMsgs::ImagePtr; using TimePoint = std::chrono::time_point; +using FFMPegImagePtr = DepthAiRosMsgs::FFMPEGPacketPtr; +using CompImagePtr = ImageMsgs::CompressedImagePtr; class ImageConverter { public: @@ -44,7 +50,7 @@ class ImageConverter { * @param update: bool whether to automatically update the ROS base time on message conversion */ void setUpdateRosBaseTimeOnToRosMsg(bool update = true) { - _updateRosBaseTimeOnToRosMsg = update; + updateRosBaseTimeOnToRosMsg = update; } /** @@ -77,10 +83,20 @@ class ImageConverter { */ void setAlphaScaling(double alphaScalingFactor = 0.0); + /** + * @brief Sets the encoding of the image when converting to FFMPEG message. Default is libx264. + * @param encoding: The encoding to be used. + */ + void setFFMPEGEncoding(const std::string& encoding); + ImageMsgs::Image toRosMsgRawPtr(std::shared_ptr inData, const sensor_msgs::CameraInfo& info = sensor_msgs::CameraInfo()); void toRosMsg(std::shared_ptr inData, std::deque& outImageMsgs); ImagePtr toRosMsgPtr(std::shared_ptr inData); + DepthAiRosMsgs::FFMPEGPacket toRosFFMPEGPacket(std::shared_ptr inData); + + ImageMsgs::CompressedImage toRosCompressedMsg(std::shared_ptr inData); + void toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outData); /** TODO(sachin): Add support for ros msg to cv mat since we have some @@ -99,29 +115,31 @@ class ImageConverter { static std::unordered_map encodingEnumMap; static std::unordered_map planarEncodingEnumMap; - // dai::RawImgFrame::Type _srcType; - bool _daiInterleaved; + bool daiInterleaved; // bool c - const std::string _frameName = ""; + const std::string frameName = ""; void planarToInterleaved(const std::vector& srcData, std::vector& destData, int w, int h, int numPlanes, int bpp); void interleavedToPlanar(const std::vector& srcData, std::vector& destData, int w, int h, int numPlanes, int bpp); - std::chrono::time_point _steadyBaseTime; + std::chrono::time_point steadyBaseTime; - ::ros::Time _rosBaseTime; - bool _getBaseDeviceTimestamp; + ::ros::Time rosBaseTime; + bool getBaseDeviceTimestamp; // For handling ROS time shifts and debugging - int64_t _totalNsChange{0}; + int64_t totalNsChange{0}; // Whether to update the ROS base time on each message conversion - bool _updateRosBaseTimeOnToRosMsg{false}; - dai::RawImgFrame::Type _srcType; - bool _fromBitstream = false; - bool _convertDispToDepth = false; - bool _addExpOffset = false; - dai::CameraExposureOffset _expOffset; - bool _reverseStereoSocketOrder = false; - double _baseline; - bool _alphaScalingEnabled = false; - double _alphaScalingFactor = 0.0; + bool updateRosBaseTimeOnToRosMsg{false}; + dai::RawImgFrame::Type srcType; + bool fromBitstream = false; + bool dispToDepth = false; + bool addExpOffset = false; + dai::CameraExposureOffset expOffset; + bool reversedStereoSocketOrder = false; + double baseline; + bool alphaScalingEnabled = false; + double alphaScalingFactor = 0.0; + int camHeight = -1; + int camWidth = -1; + std::string ffmpegEncoding = "libx264"; }; } // namespace ros diff --git a/depthai_bridge/include/depthai_bridge/TFPublisher.hpp b/depthai_bridge/include/depthai_bridge/TFPublisher.hpp index c8fcdb47..64c404ec 100644 --- a/depthai_bridge/include/depthai_bridge/TFPublisher.hpp +++ b/depthai_bridge/include/depthai_bridge/TFPublisher.hpp @@ -65,23 +65,23 @@ class TFPublisher { */ bool modelNameAvailable(); std::string getCamSocketName(int socketNum); - std::shared_ptr _tfPub; - std::shared_ptr _rsp; - std::string _camName; - std::string _camModel; - std::string _baseFrame; - std::string _parentFrame; - std::string _camPosX; - std::string _camPosY; - std::string _camPosZ; - std::string _camRoll; - std::string _camPitch; - std::string _camYaw; - std::string _imuFromDescr; - std::string _customURDFLocation; - std::string _customXacroArgs; - std::vector _camFeatures; - const std::unordered_map _socketNameMap = { + std::shared_ptr tfPub; + std::shared_ptr rsp; + std::string camName; + std::string camModel; + std::string baseFrame; + std::string parentFrame; + std::string camPosX; + std::string camPosY; + std::string camPosZ; + std::string camRoll; + std::string camPitch; + std::string camYaw; + std::string imuFromDescr; + std::string customURDFLocation; + std::string customXacroArgs; + std::vector camFeatures; + const std::unordered_map socketNameMap = { {dai::CameraBoardSocket::AUTO, "rgb"}, {dai::CameraBoardSocket::CAM_A, "rgb"}, {dai::CameraBoardSocket::CAM_B, "left"}, @@ -91,4 +91,4 @@ class TFPublisher { }; }; } // namespace ros -} // namespace dai \ No newline at end of file +} // namespace dai diff --git a/depthai_bridge/include/depthai_bridge/TrackDetectionConverter.hpp b/depthai_bridge/include/depthai_bridge/TrackDetectionConverter.hpp new file mode 100644 index 00000000..fa341f3d --- /dev/null +++ b/depthai_bridge/include/depthai_bridge/TrackDetectionConverter.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include +#include +#include + +#include "depthai/pipeline/datatype/Tracklets.hpp" +#include "depthai_ros_msgs/TrackDetection2DArray.h" +#include "ros/time.h" +#include "vision_msgs/Detection2DArray.h" + +namespace dai { + +namespace ros { + +class TrackDetectionConverter { + public: + TrackDetectionConverter(std::string frameName, int width, int height, bool normalized, float thresh, bool getBaseDeviceTimestamp = false); + ~TrackDetectionConverter() = default; + + /** + * @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 trackData, std::deque& opDetectionMsgs); + + depthai_ros_msgs::TrackDetection2DArray::Ptr toRosMsgPtr(std::shared_ptr trackData); + + private: + int _width, _height; + const std::string _frameName; + bool _normalized; + float _thresh; + std::chrono::time_point _steadyBaseTime; + ::ros::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 +} // namespace dai diff --git a/depthai_bridge/include/depthai_bridge/TrackSpatialDetectionConverter.hpp b/depthai_bridge/include/depthai_bridge/TrackSpatialDetectionConverter.hpp new file mode 100644 index 00000000..a8f67ec4 --- /dev/null +++ b/depthai_bridge/include/depthai_bridge/TrackSpatialDetectionConverter.hpp @@ -0,0 +1,60 @@ +#pragma once + +#include +#include +#include + +#include "depthai/pipeline/datatype/Tracklets.hpp" +#include "depthai_ros_msgs/TrackDetection2DArray.h" +#include "ros/time.h" +#include "vision_msgs/Detection2DArray.h" + +namespace dai { + +namespace ros { + +class TrackSpatialDetectionConverter { + public: + TrackSpatialDetectionConverter(std::string frameName, int width, int height, bool normalized, bool getBaseDeviceTimestamp = false); + ~TrackSpatialDetectionConverter() = default; + + /** + * @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 trackData, std::deque& opDetectionMsgs); + + depthai_ros_msgs::TrackDetection2DArray::Ptr toRosMsgPtr(std::shared_ptr trackData); + + private: + int _width, _height; + const std::string _frameName; + bool _normalized; + float _thresh; + std::chrono::time_point _steadyBaseTime; + ::ros::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 + +namespace rosBridge = ros; + +} // namespace dai diff --git a/depthai_bridge/package.xml b/depthai_bridge/package.xml index 96564a77..472f02e9 100644 --- a/depthai_bridge/package.xml +++ b/depthai_bridge/package.xml @@ -1,10 +1,10 @@ depthai_bridge - 2.9.0 + 2.10.1 The depthai_bridge package - Sachin Guruswamy + Adam Serafin Sachin Guruswamy MIT diff --git a/depthai_bridge/src/ImageConverter.cpp b/depthai_bridge/src/ImageConverter.cpp index 8f45ff23..14b9ea56 100644 --- a/depthai_bridge/src/ImageConverter.cpp +++ b/depthai_bridge/src/ImageConverter.cpp @@ -26,69 +26,73 @@ std::unordered_map ImageConverter::planarEn {dai::RawImgFrame::Type::YUV420p, "rgb8"}}; ImageConverter::ImageConverter(bool interleaved, bool getBaseDeviceTimestamp) - : _daiInterleaved(interleaved), _steadyBaseTime(std::chrono::steady_clock::now()), _getBaseDeviceTimestamp(getBaseDeviceTimestamp) { - _rosBaseTime = ::ros::Time::now(); + : daiInterleaved(interleaved), steadyBaseTime(std::chrono::steady_clock::now()), getBaseDeviceTimestamp(getBaseDeviceTimestamp) { + rosBaseTime = ::ros::Time::now(); } ImageConverter::ImageConverter(const std::string frameName, bool interleaved, bool getBaseDeviceTimestamp) - : _frameName(frameName), _daiInterleaved(interleaved), _steadyBaseTime(std::chrono::steady_clock::now()), _getBaseDeviceTimestamp(getBaseDeviceTimestamp) { - _rosBaseTime = ::ros::Time::now(); + : frameName(frameName), daiInterleaved(interleaved), steadyBaseTime(std::chrono::steady_clock::now()), getBaseDeviceTimestamp(getBaseDeviceTimestamp) { + rosBaseTime = ::ros::Time::now(); } void ImageConverter::updateRosBaseTime() { - updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange); + updateBaseTime(steadyBaseTime, rosBaseTime, totalNsChange); } void ImageConverter::convertFromBitstream(dai::RawImgFrame::Type srcType) { - _fromBitstream = true; - _srcType = srcType; + fromBitstream = true; + this->srcType = srcType; } void ImageConverter::convertDispToDepth(double baseline) { - _convertDispToDepth = true; - _baseline = baseline; + dispToDepth = true; + this->baseline = baseline; } void ImageConverter::reverseStereoSocketOrder() { - _reverseStereoSocketOrder = true; + reversedStereoSocketOrder = true; } void ImageConverter::addExposureOffset(dai::CameraExposureOffset& offset) { - _expOffset = offset; - _addExpOffset = true; + expOffset = offset; + addExpOffset = true; } void ImageConverter::setAlphaScaling(double alphaScalingFactor) { - _alphaScalingEnabled = true; - _alphaScalingFactor = alphaScalingFactor; + alphaScalingEnabled = true; + alphaScalingFactor = alphaScalingFactor; +} + +void ImageConverter::setFFMPEGEncoding(const std::string& encoding) { + ffmpegEncoding = encoding; } ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr inData, const sensor_msgs::CameraInfo& info) { - if(_updateRosBaseTimeOnToRosMsg) { + if(updateRosBaseTimeOnToRosMsg) { updateRosBaseTime(); } std::chrono::_V2::steady_clock::time_point tstamp; - if(_getBaseDeviceTimestamp) - if(_addExpOffset) - tstamp = inData->getTimestampDevice(_expOffset); + if(getBaseDeviceTimestamp) + if(addExpOffset) + tstamp = inData->getTimestampDevice(expOffset); else tstamp = inData->getTimestampDevice(); - else if(_addExpOffset) - tstamp = inData->getTimestamp(_expOffset); + else if(addExpOffset) + tstamp = inData->getTimestamp(expOffset); else tstamp = inData->getTimestamp(); ImageMsgs::Image outImageMsg; StdMsgs::Header header; - header.frame_id = _frameName; + header.frame_id = frameName; - header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp); + header.stamp = getFrameTime(rosBaseTime, steadyBaseTime, tstamp); - if(_fromBitstream) { + if(fromBitstream) { std::string encoding; int decodeFlags; int channels; cv::Mat output; - switch(_srcType) { + switch(srcType) { case dai::RawImgFrame::Type::BGR888i: { encoding = sensor_msgs::image_encodings::BGR8; decodeFlags = cv::IMREAD_COLOR; @@ -108,7 +112,7 @@ ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr i break; } default: { - std::cout << _frameName << static_cast(_srcType) << std::endl; + std::cout << frameName << static_cast(srcType) << std::endl; throw(std::runtime_error("Converted type not supported!")); } } @@ -116,8 +120,8 @@ ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr i output = cv::imdecode(cv::Mat(inData->getData()), decodeFlags); // converting disparity - if(_convertDispToDepth) { - auto factor = std::abs(_baseline * 10) * info.P[0]; + if(dispToDepth) { + auto factor = std::abs(baseline * 10) * info.P[0]; cv::Mat depthOut = cv::Mat(cv::Size(output.cols, output.rows), CV_16UC1); depthOut.forEach([&output, &factor](uint16_t& pixel, const int* position) -> void { auto disp = output.at(position); @@ -206,6 +210,80 @@ ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr i return outImageMsg; } +std::chrono::time_point getOffsetTimestamp( + std::chrono::time_point ts, + CameraExposureOffset offset, + std::chrono::microseconds expTime) { + switch(offset) { + case CameraExposureOffset::START: + return ts - expTime; + case CameraExposureOffset::MIDDLE: + return ts - expTime / 2; + case CameraExposureOffset::END: + default: + return ts; + } +} + +ImageMsgs::CompressedImage ImageConverter::toRosCompressedMsg(std::shared_ptr inData) { + if(updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } + std::chrono::_V2::steady_clock::time_point tstamp; + if(getBaseDeviceTimestamp) + if(addExpOffset) + tstamp = getOffsetTimestamp(inData->getTimestampDevice(), expOffset, inData->getExposureTime()); + else + tstamp = inData->getTimestampDevice(); + else if(addExpOffset) + tstamp = getOffsetTimestamp(inData->getTimestamp(), expOffset, inData->getExposureTime()); + else + tstamp = inData->getTimestamp(); + + ImageMsgs::CompressedImage outImageMsg; + StdMsgs::Header header; + header.frame_id = frameName; + header.stamp = getFrameTime(rosBaseTime, steadyBaseTime, tstamp); + + outImageMsg.header = header; + outImageMsg.format = "jpeg"; + outImageMsg.data = inData->getData(); + return outImageMsg; +} + +DepthAiRosMsgs::FFMPEGPacket ImageConverter::toRosFFMPEGPacket(std::shared_ptr inData) { + if(updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } + std::chrono::_V2::steady_clock::time_point tstamp; + if(getBaseDeviceTimestamp) + if(addExpOffset) + tstamp = getOffsetTimestamp(inData->getTimestampDevice(), expOffset, inData->getExposureTime()); + else + tstamp = inData->getTimestampDevice(); + else if(addExpOffset) + tstamp = getOffsetTimestamp(inData->getTimestamp(), expOffset, inData->getExposureTime()); + else + tstamp = inData->getTimestamp(); + + DepthAiRosMsgs::FFMPEGPacket outFrameMsg; + StdMsgs::Header header; + header.frame_id = frameName; + header.stamp = getFrameTime(rosBaseTime, steadyBaseTime, tstamp); + outFrameMsg.header = header; + auto ft = inData->getFrameType(); + + outFrameMsg.width = camWidth; + outFrameMsg.height = camHeight; + outFrameMsg.encoding = ffmpegEncoding; + outFrameMsg.pts = header.stamp.sec * 1000000000 + header.stamp.nsec; // in nanoseconds + outFrameMsg.flags = (int)(ft == RawEncodedFrame::FrameType::I); + outFrameMsg.is_bigendian = false; + outFrameMsg.data = inData->getData(); + + return outFrameMsg; +} + void ImageConverter::toRosMsg(std::shared_ptr inData, std::deque& outImageMsgs) { auto outImageMsg = toRosMsgRawPtr(inData); outImageMsgs.push_back(outImageMsg); @@ -221,7 +299,7 @@ ImagePtr ImageConverter::toRosMsgPtr(std::shared_ptr inData) { // TODO(sachin): Not tested void ImageConverter::toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outData) { std::unordered_map::iterator revEncodingIter; - if(_daiInterleaved) { + if(daiInterleaved) { revEncodingIter = std::find_if(encodingEnumMap.begin(), encodingEnumMap.end(), [&](const std::pair& pair) { return pair.second == inMsg.encoding; }); @@ -247,19 +325,6 @@ void ImageConverter::toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outD outData.setData(opData); } - /** FIXME(sachin) : is this time convertion correct ??? - * Print the original time and ros time in seconds in - * ImageFrame::toRosMsg(std::shared_ptr inData, - *ImageMsgs::Image& opMsg) to cross verify.. - **/ - /* #ifdef IS_ROS2 - TimePoint ts(std::chrono::seconds((int)inMsg.header.stamp.seconds ()) + std::chrono::nanoseconds(inMsg.header.stamp.nanoseconds())); - #else - TimePoint ts(std::chrono::seconds((int)inMsg.header.stamp.toSec()) + std::chrono::nanoseconds(inMsg.header.stamp.toNSec())); - #endif - - outData.setTimestamp(ts); - outData.setSequenceNum(inMsg.header.seq); */ outData.setWidth(inMsg.width); outData.setHeight(inMsg.height); outData.setType(revEncodingIter->first); @@ -341,6 +406,9 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo( cameraData.height = static_cast(height); } + camWidth = cameraData.width; + camHeight = cameraData.height; + camIntrinsics = calibHandler.getCameraIntrinsics(cameraId, cameraData.width, cameraData.height, topLeftPixelId, bottomRightPixelId); flatIntrinsics.resize(9); @@ -371,7 +439,7 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo( std::vector> stereoIntrinsics = calibHandler.getCameraIntrinsics( calibHandler.getStereoRightCameraId(), cameraData.width, cameraData.height, topLeftPixelId, bottomRightPixelId); - if(_alphaScalingEnabled) { + if(alphaScalingEnabled) { cv::Mat cameraMatrix = cv::Mat(3, 3, CV_64F); for(int i = 0; i < 3; i++) { for(int j = 0; j < 3; j++) { @@ -380,7 +448,7 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo( } cv::Mat distCoefficients(distCoeffs); - cv::Mat newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoefficients, cv::Size(width, height), _alphaScalingFactor); + cv::Mat newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoefficients, cv::Size(width, height), alphaScalingFactor); // Copying the contents of newCameraMatrix to stereoIntrinsics for(int i = 0; i < 3; i++) { for(int j = 0; j < 3; j++) { @@ -400,7 +468,7 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo( dai::CameraBoardSocket stereoSocketFirst = calibHandler.getStereoLeftCameraId(); dai::CameraBoardSocket stereoSocketSecond = calibHandler.getStereoRightCameraId(); double factor = 1.0; - if(_reverseStereoSocketOrder) { + if(reversedStereoSocketOrder) { stereoSocketFirst = calibHandler.getStereoRightCameraId(); stereoSocketSecond = calibHandler.getStereoLeftCameraId(); factor = -1.0; @@ -430,4 +498,4 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo( } } // namespace ros -} // namespace dai \ No newline at end of file +} // namespace dai diff --git a/depthai_bridge/src/TFPublisher.cpp b/depthai_bridge/src/TFPublisher.cpp index f25f30dc..f1a11335 100644 --- a/depthai_bridge/src/TFPublisher.cpp +++ b/depthai_bridge/src/TFPublisher.cpp @@ -39,27 +39,27 @@ TFPublisher::TFPublisher(::ros::NodeHandle node, const std::string& imuFromDescr, const std::string& customURDFLocation, const std::string& customXacroArgs) - : _camName(camName), - _camModel(camModel), - _baseFrame(baseFrame), - _parentFrame(parentFrame), - _camPosX(camPosX), - _camPosY(camPosY), - _camPosZ(camPosZ), - _camRoll(camRoll), - _camPitch(camPitch), - _camYaw(camYaw), - _camFeatures(camFeatures), - _imuFromDescr(imuFromDescr), - _customURDFLocation(customURDFLocation), - _customXacroArgs(customXacroArgs) { - _tfPub = std::make_shared(); + : camName(camName), + camModel(camModel), + baseFrame(baseFrame), + parentFrame(parentFrame), + camPosX(camPosX), + camPosY(camPosY), + camPosZ(camPosZ), + camRoll(camRoll), + camPitch(camPitch), + camYaw(camYaw), + camFeatures(camFeatures), + imuFromDescr(imuFromDescr), + customURDFLocation(customURDFLocation), + customXacroArgs(customXacroArgs) { + tfPub = std::make_shared(); auto json = calHandler.eepromToJson(); auto camData = json["cameraData"]; publishDescription(node); publishCamTransforms(camData, node); - if(_imuFromDescr != "true") { + if(imuFromDescr != "true") { publishImuTransform(json, node); } } @@ -73,8 +73,8 @@ void TFPublisher::publishDescription(::ros::NodeHandle node) { ROS_ERROR("Failed to extract kdl tree from xml robot description"); throw std::runtime_error("Failed to extract kdl tree from xml robot description"); } - _rsp = std::make_shared(tree, model); - _rsp->publishFixedTransforms(true); + rsp = std::make_shared(tree, model); + rsp->publishFixedTransforms(true); node.setParam("robot_description", urdf); ROS_INFO("Published URDF"); } @@ -91,26 +91,26 @@ void TFPublisher::publishCamTransforms(nlohmann::json camData, ::ros::NodeHandle ts.transform.translation = transFromExtr(extrinsics["translation"]); std::string name = getCamSocketName(cam[0]); - ts.child_frame_id = _baseFrame + std::string("_") + name + std::string("_camera_frame"); + ts.child_frame_id = baseFrame + std::string("_") + name + std::string("_camera_frame"); // check if the camera is at the end of the chain if(extrinsics["toCameraSocket"] != -1) { - ts.header.frame_id = _baseFrame + std::string("_") + getCamSocketName(extrinsics["toCameraSocket"].get()) + std::string("_camera_frame"); + ts.header.frame_id = baseFrame + std::string("_") + getCamSocketName(extrinsics["toCameraSocket"].get()) + std::string("_camera_frame"); } else { - ts.header.frame_id = _baseFrame; + ts.header.frame_id = baseFrame; ts.transform.rotation.w = 1.0; ts.transform.rotation.x = 0.0; ts.transform.rotation.y = 0.0; ts.transform.rotation.z = 0.0; } // rotate optical fransform - opticalTS.child_frame_id = _baseFrame + std::string("_") + name + std::string("_camera_optical_frame"); + opticalTS.child_frame_id = baseFrame + std::string("_") + name + std::string("_camera_optical_frame"); opticalTS.header.frame_id = ts.child_frame_id; opticalTS.transform.rotation.w = 0.5; opticalTS.transform.rotation.x = -0.5; opticalTS.transform.rotation.y = 0.5; opticalTS.transform.rotation.z = -0.5; - _tfPub->sendTransform(ts); - _tfPub->sendTransform(opticalTS); + tfPub->sendTransform(ts); + tfPub->sendTransform(opticalTS); } } void TFPublisher::publishImuTransform(nlohmann::json json, ::ros::NodeHandle node) { @@ -118,11 +118,11 @@ void TFPublisher::publishImuTransform(nlohmann::json json, ::ros::NodeHandle nod ts.header.stamp = ::ros::Time::now(); auto imuExtr = json["imuExtrinsics"]; if(imuExtr["toCameraSocket"] != -1) { - ts.header.frame_id = _baseFrame + std::string("_") + getCamSocketName(imuExtr["toCameraSocket"].get()) + std::string("_camera_frame"); + ts.header.frame_id = baseFrame + std::string("_") + getCamSocketName(imuExtr["toCameraSocket"].get()) + std::string("_camera_frame"); } else { - ts.header.frame_id = _baseFrame; + ts.header.frame_id = baseFrame; } - ts.child_frame_id = _baseFrame + std::string("_imu_frame"); + ts.child_frame_id = baseFrame + std::string("_imu_frame"); ts.transform.rotation = quatFromRotM(imuExtr["rotationMatrix"]); ts.transform.translation = transFromExtr(imuExtr["translation"]); @@ -133,11 +133,11 @@ void TFPublisher::publishImuTransform(nlohmann::json json, ::ros::NodeHandle nod ts.transform.rotation.w = 1.0; ts.transform.rotation.x = 0.0; } - _tfPub->sendTransform(ts); + tfPub->sendTransform(ts); } std::string TFPublisher::getCamSocketName(int socketNum) { - return _socketNameMap.at(static_cast(socketNum)); + return socketNameMap.at(static_cast(socketNum)); } geometry_msgs::Vector3 TFPublisher::transFromExtr(nlohmann::json translation) { @@ -176,7 +176,7 @@ bool TFPublisher::modelNameAvailable() { while((ent = readdir(dir)) != NULL) { auto name = std::string(ent->d_name); ROS_DEBUG("Found model: %s", name.c_str()); - if(name == _camModel + ".stl") { + if(name == camModel + ".stl") { return true; } } @@ -188,59 +188,59 @@ bool TFPublisher::modelNameAvailable() { } std::string TFPublisher::prepareXacroArgs() { - if(!_customURDFLocation.empty() || !modelNameAvailable()) { + if(!customURDFLocation.empty() || !modelNameAvailable()) { ROS_ERROR( "Model name %s not found in depthai_descriptions package. If camera model is autodetected, please notify developers. Using default model: OAK-D", - _camModel.c_str()); - _camModel = "OAK-D"; + camModel.c_str()); + camModel = "OAK-D"; } - std::string xacroArgs = "camera_name:=" + _camName; - xacroArgs += " camera_model:=" + _camModel; - xacroArgs += " base_frame:=" + _baseFrame; - xacroArgs += " parent_frame:=" + _parentFrame; - xacroArgs += " cam_pos_x:=" + _camPosX; - xacroArgs += " cam_pos_y:=" + _camPosY; - xacroArgs += " cam_pos_z:=" + _camPosZ; - xacroArgs += " cam_roll:=" + _camRoll; - xacroArgs += " cam_pitch:=" + _camPitch; - xacroArgs += " cam_yaw:=" + _camYaw; - xacroArgs += " has_imu:=" + _imuFromDescr; + std::string xacroArgs = "camera_name:=" + camName; + xacroArgs += " camera_model:=" + camModel; + xacroArgs += " base_frame:=" + baseFrame; + xacroArgs += " parent_frame:=" + parentFrame; + xacroArgs += " cam_pos_x:=" + camPosX; + xacroArgs += " cam_pos_y:=" + camPosY; + xacroArgs += " cam_pos_z:=" + camPosZ; + xacroArgs += " cam_roll:=" + camRoll; + xacroArgs += " cam_pitch:=" + camPitch; + xacroArgs += " cam_yaw:=" + camYaw; + xacroArgs += " has_imu:=" + imuFromDescr; return xacroArgs; } void TFPublisher::convertModelName() { - if(_camModel.find("OAK-D-PRO-POE") != std::string::npos || _camModel.find("OAK-D-PRO-W-POE") != std::string::npos - || _camModel.find("OAK-D-S2-POE") != std::string::npos) { - _camModel = "OAK-D-POE"; - } else if(_camModel.find("OAK-D-LITE") != std::string::npos) { - _camModel = "OAK-D-PRO"; - } else if(_camModel.find("OAK-D-S2") != std::string::npos) { - _camModel = "OAK-D-PRO"; - } else if(_camModel.find("OAK-D-PRO-W") != std::string::npos) { - _camModel = "OAK-D-PRO"; - } else if(_camModel.find("OAK-D-PRO") != std::string::npos) { - _camModel = "OAK-D-PRO"; - } else if(_camModel.find("OAK-D-POE")) { - _camModel = "OAK-D-POE"; - } else if(_camModel.find("OAK-D") != std::string::npos) { - _camModel = "OAK-D"; + if(camModel.find("OAK-D-PRO-POE") != std::string::npos || camModel.find("OAK-D-PRO-W-POE") != std::string::npos + || camModel.find("OAK-D-S2-POE") != std::string::npos) { + camModel = "OAK-D-POE"; + } else if(camModel.find("OAK-D-LITE") != std::string::npos) { + camModel = "OAK-D-PRO"; + } else if(camModel.find("OAK-D-S2") != std::string::npos) { + camModel = "OAK-D-PRO"; + } else if(camModel.find("OAK-D-PRO-W") != std::string::npos) { + camModel = "OAK-D-PRO"; + } else if(camModel.find("OAK-D-PRO") != std::string::npos) { + camModel = "OAK-D-PRO"; + } else if(camModel.find("OAK-D-POE")) { + camModel = "OAK-D-POE"; + } else if(camModel.find("OAK-D") != std::string::npos) { + camModel = "OAK-D"; } else { - ROS_WARN("Unable to match model name: %s to available model family.", _camModel.c_str()); + ROS_WARN("Unable to match model name: %s to available model family.", camModel.c_str()); } } std::string TFPublisher::getURDF() { std::string args, path; - if(_customXacroArgs.empty()) { + if(customXacroArgs.empty()) { args = prepareXacroArgs(); } else { - args = _customXacroArgs; + args = customXacroArgs; } - if(_customURDFLocation.empty()) { + if(customURDFLocation.empty()) { path = ::ros::package::getPath("depthai_descriptions") + "/urdf/base_descr.urdf.xacro "; } else { - path = _customURDFLocation + " "; + path = customURDFLocation + " "; } std::string cmd = "xacro " + path + args; ROS_DEBUG("Xacro command: %s", cmd.c_str()); @@ -256,4 +256,4 @@ std::string TFPublisher::getURDF() { return result; } } // namespace ros -} // namespace dai \ No newline at end of file +} // namespace dai diff --git a/depthai_bridge/src/TrackDetectionConverter.cpp b/depthai_bridge/src/TrackDetectionConverter.cpp new file mode 100644 index 00000000..89276e04 --- /dev/null +++ b/depthai_bridge/src/TrackDetectionConverter.cpp @@ -0,0 +1,92 @@ +#include "depthai_bridge/TrackDetectionConverter.hpp" + +#include "depthai/depthai.hpp" +#include "depthai_bridge/depthaiUtility.hpp" + +namespace dai { + +namespace ros { + +TrackDetectionConverter::TrackDetectionConverter(std::string frameName, int width, int height, bool normalized, float thresh, bool getBaseDeviceTimestamp) + : _frameName(frameName), + _width(width), + _height(height), + _normalized(normalized), + _thresh(thresh), + _steadyBaseTime(std::chrono::steady_clock::now()), + _getBaseDeviceTimestamp(getBaseDeviceTimestamp) { + _rosBaseTime = ::ros::Time::now(); +} + +void TrackDetectionConverter::updateRosBaseTime() { + updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange); +} + +void TrackDetectionConverter::toRosMsg(std::shared_ptr trackData, std::deque& opDetectionMsgs) { + // setting the header + std::chrono::_V2::steady_clock::time_point tstamp; + if(_getBaseDeviceTimestamp) + tstamp = trackData->getTimestampDevice(); + else + tstamp = trackData->getTimestamp(); + + depthai_ros_msgs::TrackDetection2DArray opDetectionMsg; + opDetectionMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp); + opDetectionMsg.header.frame_id = _frameName; + opDetectionMsg.detections.resize(trackData->tracklets.size()); + + // publishing + for(int i = 0; i < trackData->tracklets.size(); ++i) { + dai::Tracklet t = trackData->tracklets[i]; + dai::Rect roi; + float xMin, yMin, xMax, yMax; + + if(_normalized) + roi = t.roi; + else + roi = t.roi.denormalize(_width, _height); + + xMin = roi.topLeft().x; + yMin = roi.topLeft().y; + xMax = roi.bottomRight().x; + yMax = roi.bottomRight().y; + + float xSize = xMax - xMin; + float ySize = yMax - yMin; + float xCenter = xMin + xSize / 2.; + float yCenter = yMin + ySize / 2.; + + opDetectionMsg.detections[i].results.resize(1); + + opDetectionMsg.detections[i].results[0].id = t.label; + opDetectionMsg.detections[i].results[0].score = _thresh; + + opDetectionMsg.detections[i].bbox.center.x = xCenter; + opDetectionMsg.detections[i].bbox.center.y = yCenter; + opDetectionMsg.detections[i].bbox.size_x = xSize; + opDetectionMsg.detections[i].bbox.size_y = ySize; + + opDetectionMsg.detections[i].is_tracking = true; + std::stringstream track_id_str; + track_id_str << "" << t.id; + opDetectionMsg.detections[i].tracking_id = track_id_str.str(); + opDetectionMsg.detections[i].tracking_age = t.age; + opDetectionMsg.detections[i].tracking_status = static_cast(t.status); + } + + opDetectionMsgs.push_back(opDetectionMsg); +} + +depthai_ros_msgs::TrackDetection2DArray::Ptr TrackDetectionConverter::toRosMsgPtr(std::shared_ptr trackData) { + std::deque msgQueue; + toRosMsg(trackData, msgQueue); + auto msg = msgQueue.front(); + + depthai_ros_msgs::TrackDetection2DArray::Ptr ptr = boost::make_shared(msg); + + return ptr; +} + +} // namespace ros + +} // namespace dai diff --git a/depthai_bridge/src/TrackSpatialDetectionConverter.cpp b/depthai_bridge/src/TrackSpatialDetectionConverter.cpp new file mode 100644 index 00000000..bad2f679 --- /dev/null +++ b/depthai_bridge/src/TrackSpatialDetectionConverter.cpp @@ -0,0 +1,98 @@ +#include "depthai_bridge/TrackSpatialDetectionConverter.hpp" + +#include "depthai/depthai.hpp" +#include "depthai_bridge/depthaiUtility.hpp" + +namespace dai { + +namespace ros { + +TrackSpatialDetectionConverter::TrackSpatialDetectionConverter( + std::string frameName, int width, int height, bool normalized, float thresh, bool getBaseDeviceTimestamp) + : _frameName(frameName), + _width(width), + _height(height), + _normalized(normalized), + _thresh(thresh), + _steadyBaseTime(std::chrono::steady_clock::now()), + _getBaseDeviceTimestamp(getBaseDeviceTimestamp) { + _rosBaseTime = ::ros::Time::now(); +} + +void TrackSpatialDetectionConverter::updateRosBaseTime() { + updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange); +} + +void TrackSpatialDetectionConverter::toRosMsg(std::shared_ptr trackData, std::deque& opDetectionMsgs) { + // setting the header + std::chrono::_V2::steady_clock::time_point tstamp; + if(_getBaseDeviceTimestamp) + tstamp = trackData->getTimestampDevice(); + else + tstamp = trackData->getTimestamp(); + + depthai_ros_msgs::TrackDetection2DArray opDetectionMsg; + opDetectionMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp); + opDetectionMsg.header.frame_id = _frameName; + opDetectionMsg.detections.resize(trackData->tracklets.size()); + + // publishing + for(int i = 0; i < trackData->tracklets.size(); ++i) { + dai::Tracklet t = trackData->tracklets[i]; + dai::Rect roi; + float xMin, yMin, xMax, yMax; + + if(_normalized) + roi = t.roi; + else + roi = t.roi.denormalize(_width, _height); + + xMin = roi.topLeft().x; + yMin = roi.topLeft().y; + xMax = roi.bottomRight().x; + yMax = roi.bottomRight().y; + + float xSize = xMax - xMin; + float ySize = yMax - yMin; + float xCenter = xMin + xSize / 2.; + float yCenter = yMin + ySize / 2.; + + opDetectionMsg.detections[i].results.resize(1); + + opDetectionMsg.detections[i].results[0].id = t.label; + opDetectionMsg.detections[i].results[0].score = _thresh; + + opDetectionMsg.detections[i].bbox.center.x = xCenter; + opDetectionMsg.detections[i].bbox.center.y = yCenter; + opDetectionMsg.detections[i].bbox.size_x = xSize; + opDetectionMsg.detections[i].bbox.size_y = ySize; + + opDetectionMsg.detections[i].is_tracking = true; + std::stringstream track_id_str; + track_id_str << "" << t.id; + opDetectionMsg.detections[i].tracking_id = track_id_str.str(); + opDetectionMsg.detections[i].tracking_age = t.age; + opDetectionMsg.detections[i].tracking_status = static_cast(t.status); + + // converting mm to meters since per ros rep-103 lenght should always be in meters + opDetectionMsg.detections[i].results[0].pose.pose.position.x = t.spatialCoordinates.x / 1000.0; + opDetectionMsg.detections[i].results[0].pose.pose.position.y = t.spatialCoordinates.y / 1000.0; + opDetectionMsg.detections[i].results[0].pose.pose.position.z = t.spatialCoordinates.z / 1000.0; + } + + opDetectionMsgs.push_back(opDetectionMsg); +} + +depthai_ros_msgs::TrackDetection2DArray::Ptr TrackSpatialDetectionConverter::toRosMsgPtr(std::shared_ptr trackData) { + std::deque msgQueue; + toRosMsg(trackData, msgQueue); + auto msg = msgQueue.front(); + + depthai_ros_msgs::TrackDetection2DArray::Ptr ptr = boost::make_shared(msg); + + return ptr; +} + +} // namespace ros + +} // namespace dai diff --git a/depthai_bridge/src/TrackedFeaturesConverter.cpp b/depthai_bridge/src/TrackedFeaturesConverter.cpp index 75f7ef38..56ea62be 100644 --- a/depthai_bridge/src/TrackedFeaturesConverter.cpp +++ b/depthai_bridge/src/TrackedFeaturesConverter.cpp @@ -31,7 +31,7 @@ void TrackedFeaturesConverter::toRosMsg(std::shared_ptr in msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp); msg.header.frame_id = _frameName; - msg.features.resize(inFeatures->trackedFeatures.size()); + msg.features.reserve(inFeatures->trackedFeatures.size()); for(const auto& feature : inFeatures->trackedFeatures) { depthai_ros_msgs::TrackedFeature ft; @@ -48,4 +48,4 @@ void TrackedFeaturesConverter::toRosMsg(std::shared_ptr in } } // namespace ros -} // namespace dai \ No newline at end of file +} // namespace dai diff --git a/depthai_descriptions/CMakeLists.txt b/depthai_descriptions/CMakeLists.txt index 25a37c5e..cabcef61 100644 --- a/depthai_descriptions/CMakeLists.txt +++ b/depthai_descriptions/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) -project(depthai_descriptions VERSION 2.9.0 LANGUAGES CXX C) +project(depthai_descriptions VERSION 2.10.1 LANGUAGES CXX C) find_package(catkin REQUIRED diff --git a/depthai_descriptions/launch/urdf.launch b/depthai_descriptions/launch/urdf.launch index 91ebd924..c68e043b 100644 --- a/depthai_descriptions/launch/urdf.launch +++ b/depthai_descriptions/launch/urdf.launch @@ -1,22 +1,23 @@ - - - + + + - - + + - - - - - - + + + + + + + - + cam_yaw:=$(arg cam_yaw) + rs_compat:=$(arg rs_compat)" /> - - - + + + diff --git a/depthai_descriptions/package.xml b/depthai_descriptions/package.xml index 35612189..13a68388 100644 --- a/depthai_descriptions/package.xml +++ b/depthai_descriptions/package.xml @@ -1,7 +1,7 @@ depthai_descriptions - 2.9.0 + 2.10.1 The depthai_descriptions package Adam Serafin diff --git a/depthai_descriptions/urdf/base_descr.urdf.xacro b/depthai_descriptions/urdf/base_descr.urdf.xacro index e5d04601..47190853 100644 --- a/depthai_descriptions/urdf/base_descr.urdf.xacro +++ b/depthai_descriptions/urdf/base_descr.urdf.xacro @@ -1,21 +1,25 @@ - - - - - - - - - - - + + + + + + + + + + + - + - - + + diff --git a/depthai_descriptions/urdf/depthai_descr.urdf.xacro b/depthai_descriptions/urdf/depthai_descr.urdf.xacro index 9fba2d7e..39e9fac3 100644 --- a/depthai_descriptions/urdf/depthai_descr.urdf.xacro +++ b/depthai_descriptions/urdf/depthai_descr.urdf.xacro @@ -1,20 +1,25 @@ - - - - - - - - - - + + + + + + + + + + + - + - - + + diff --git a/depthai_descriptions/urdf/include/base_macro.urdf.xacro b/depthai_descriptions/urdf/include/base_macro.urdf.xacro index e46e3bca..93a73660 100644 --- a/depthai_descriptions/urdf/include/base_macro.urdf.xacro +++ b/depthai_descriptions/urdf/include/base_macro.urdf.xacro @@ -1,78 +1,110 @@ + xmlns:xacro="http://ros.org/wiki/xacro"> - - - - - + cam_roll cam_pitch cam_yaw has_imu r:=0.8 g:=0.8 b:=0.8 a:=0.8 simulation:=false"> - - - - - + + + + + + - - - - - - - - - - - - + + + + - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + - - - - - - - - - - - - - - - - + - - - - - - - - + + + + + + + + - - - - - - - - + + + + + + + + - - \ No newline at end of file + + + + + + + + + + + + + + + + + + + + diff --git a/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro b/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro index 5eda7e07..45d16958 100644 --- a/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro +++ b/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro @@ -1,120 +1,177 @@ + xmlns:xacro="http://ros.org/wiki/xacro"> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file + cam_roll cam_pitch cam_yaw r:=0.8 g:=0.8 b:=0.8 a:=0.8 simulation:=false rs_compat:=false"> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/depthai_descriptions/urdf/models/OAK-D-SR-POE.stl b/depthai_descriptions/urdf/models/OAK-D-SR-POE.stl new file mode 100644 index 00000000..fd72d704 Binary files /dev/null and b/depthai_descriptions/urdf/models/OAK-D-SR-POE.stl differ diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index cf0c27ad..e3861e33 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.9.0 LANGUAGES CXX C) +project(depthai_examples VERSION 2.10.1 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/depthai_examples/package.xml b/depthai_examples/package.xml index 9cb495ea..7e0f988c 100644 --- a/depthai_examples/package.xml +++ b/depthai_examples/package.xml @@ -1,26 +1,21 @@ depthai_examples - 2.9.0 + 2.10.1 The depthai_examples package - - - - sachin + Adam Serafin Sachin Guruswamy MIT catkin - ament_cmake roscpp rospy message_filters - rclcpp cv_bridge camera_info_manager @@ -46,7 +41,6 @@ catkin - ament_cmake diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index 454764e9..a2d330a1 100644 --- a/depthai_filters/CMakeLists.txt +++ b/depthai_filters/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2) -project(depthai_filters VERSION 2.9.0 LANGUAGES CXX C) +project(depthai_filters VERSION 2.10.1 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/depthai_filters/cfg/wls.cfg b/depthai_filters/cfg/wls.cfg index 268dce64..879d501c 100644 --- a/depthai_filters/cfg/wls.cfg +++ b/depthai_filters/cfg/wls.cfg @@ -10,7 +10,8 @@ wls = gen.add_group("wls") wls.add("lambda", int_t, 0, "WLS Lambda", 8000, 0, 20000) wls.add("sigma", double_t, 0, "WLS Sigma", 1.5, 0.0, 2.0) +wls.add("max_disparity", double_t, 0, "WLS Max Disparity", 760.0, 0.0, 10000.0) -exit(gen.generate(PACKAGE, "depthai_filters", "wls")) \ No newline at end of file +exit(gen.generate(PACKAGE, "depthai_filters", "wls")) diff --git a/depthai_filters/config/wls.yaml b/depthai_filters/config/wls.yaml index e3caaf24..05fab721 100644 --- a/depthai_filters/config/wls.yaml +++ b/depthai_filters/config/wls.yaml @@ -2,4 +2,5 @@ camera_i_nn_type: none left_i_publish_topic: true stereo_i_output_disparity: true - stereo_i_subpixel: true \ No newline at end of file + stereo_i_subpixel: true + stereo_i_board_socket_id: 1 diff --git a/depthai_filters/include/depthai_filters/wls_filter.hpp b/depthai_filters/include/depthai_filters/wls_filter.hpp index a053de4d..9b14050f 100644 --- a/depthai_filters/include/depthai_filters/wls_filter.hpp +++ b/depthai_filters/include/depthai_filters/wls_filter.hpp @@ -31,5 +31,6 @@ class WLSFilter : public nodelet::Nodelet { cv::Ptr filter; image_transport::CameraPublisher depthPub; std::shared_ptr it; + double maxDisparity; }; -} // namespace depthai_filters \ No newline at end of file +} // namespace depthai_filters diff --git a/depthai_filters/package.xml b/depthai_filters/package.xml index 3cb8a66a..3ff34453 100644 --- a/depthai_filters/package.xml +++ b/depthai_filters/package.xml @@ -1,7 +1,7 @@ depthai_filters - 2.9.0 + 2.10.1 The depthai_filters package Adam Serafin diff --git a/depthai_filters/src/wls_filter.cpp b/depthai_filters/src/wls_filter.cpp index bfc68641..08b4b6cf 100644 --- a/depthai_filters/src/wls_filter.cpp +++ b/depthai_filters/src/wls_filter.cpp @@ -28,41 +28,48 @@ void WLSFilter::onInit() { void WLSFilter::parameterCB(wlsConfig& config, uint32_t level) { filter->setLambda(config.lambda); filter->setSigmaColor(config.sigma); + maxDisparity = config.max_disparity; } void WLSFilter::wlsCB(const sensor_msgs::ImageConstPtr& disp, const sensor_msgs::CameraInfoConstPtr& disp_info, const sensor_msgs::ImageConstPtr& leftImg) { cv::Mat leftFrame = utils::msgToMat(leftImg, sensor_msgs::image_encodings::MONO8); cv::Mat dispFrame; - if(disp->encoding == sensor_msgs::image_encodings::TYPE_16UC1) { - dispFrame = utils::msgToMat(disp, sensor_msgs::image_encodings::TYPE_16UC1); - } else { - dispFrame = utils::msgToMat(disp, sensor_msgs::image_encodings::MONO8); - } + dispFrame = utils::msgToMat(disp, disp->encoding); cv::Mat dispFiltered; sensor_msgs::CameraInfo depthInfo = *disp_info; filter->filter(dispFrame, leftFrame, dispFiltered); sensor_msgs::Image depth; - auto factor = (disp_info->K[0] * disp_info->P[3]); - cv::Mat depthOut = cv::Mat(cv::Size(dispFiltered.cols, dispFiltered.rows), CV_16UC1); - depthOut.forEach([&dispFiltered, &factor, &disp](short& pixel, const int* position) -> void { - if(disp->encoding == sensor_msgs::image_encodings::TYPE_16UC1) { - auto dispPoint = dispFiltered.at(position); - if(dispPoint == 0) + auto factor = abs(depthInfo.P[3]) * 100.0; + // set distortion to 0 + if(disp->encoding == sensor_msgs::image_encodings::MONO8) { + auto dispMultiplier = 255.0 / maxDisparity; + cv::Mat depthOut = cv::Mat(dispFiltered.size(), CV_8UC1); + depthOut.forEach([&dispFiltered, &factor, &dispMultiplier](uint8_t& pixel, const int* position) -> void { + auto disp = dispFiltered.at(position); + if(disp == 0) { pixel = 0; - else - pixel = factor / dispPoint; - } else { - auto dispPoint = dispFiltered.at(position); - if(dispPoint == 0) + } else { + pixel = factor / disp * dispMultiplier; + } + }); + cv_bridge::CvImage(disp->header, sensor_msgs::image_encodings::MONO8, depthOut).toImageMsg(depth); + depthPub.publish(depth, depthInfo); + return; + } else { + cv::Mat depthOut = cv::Mat(dispFiltered.size(), CV_16UC1); + auto dispMultiplier = 255.0 * 255.0 / maxDisparity; + depthOut.forEach([&dispFiltered, &factor, &dispMultiplier](uint16_t& pixel, const int* position) -> void { + auto disp = dispFiltered.at(position); + if(disp == 0) { pixel = 0; - else - pixel = factor / dispPoint; - } - }); - - cv_bridge::CvImage(disp->header, sensor_msgs::image_encodings::TYPE_16UC1, depthOut).toImageMsg(depth); - depthPub.publish(depth, depthInfo); + } else { + pixel = factor / disp * dispMultiplier; + } + }); + cv_bridge::CvImage(disp->header, sensor_msgs::image_encodings::TYPE_16UC1, depthOut).toImageMsg(depth); + depthPub.publish(depth, depthInfo); + } } } // namespace depthai_filters diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index 4737c50c..7c2b9924 100644 --- a/depthai_ros_driver/CMakeLists.txt +++ b/depthai_ros_driver/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16.3) -project(depthai_ros_driver) +project(depthai_ros_driver VERSION 2.10.1) if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() @@ -87,6 +87,7 @@ add_library( src/utils.cpp src/dai_nodes/base_node.cpp src/dai_nodes/sys_logger.cpp + src/dai_nodes/sensors/img_pub.cpp src/dai_nodes/sensors/sensor_helpers.cpp # TODO: Figure out different place for this src/param_handlers/camera_param_handler.cpp src/param_handlers/imu_param_handler.cpp @@ -94,6 +95,9 @@ add_library( src/param_handlers/sensor_param_handler.cpp src/param_handlers/feature_tracker_param_handler.cpp src/param_handlers/stereo_param_handler.cpp + src/param_handlers/tof_param_handler.cpp + src/param_handlers/pipeline_gen_param_handler.cpp + src/param_handlers/sync_param_handler.cpp ) add_dependencies(${COMMON_LIB_NAME} @@ -116,7 +120,9 @@ add_library( src/dai_nodes/sensors/mono.cpp src/dai_nodes/sensors/feature_tracker.cpp src/dai_nodes/sensors/sensor_wrapper.cpp - src/dai_nodes/stereo.cpp + src/dai_nodes/sensors/stereo.cpp + src/dai_nodes/sensors/tof.cpp + src/dai_nodes/sensors/sync.cpp ) target_link_libraries( diff --git a/depthai_ros_driver/config/sr_poe_rgbd.yaml b/depthai_ros_driver/config/sr_poe_rgbd.yaml new file mode 100644 index 00000000..7e2abea3 --- /dev/null +++ b/depthai_ros_driver/config/sr_poe_rgbd.yaml @@ -0,0 +1,10 @@ + +/oak: + camera_i_enable_imu: true + camera_i_enable_ir: false + camera_i_nn_type: none + camera_i_pipeline_type: rgbtof + pipeline_gen_i_enable_sync: true + right_i_low_bandwidth: true + right_i_synced: true + tof_i_synced: true diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp index 762215ba..4dba2967 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp @@ -3,12 +3,18 @@ #include #include +#include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai/pipeline/Node.hpp" #include "depthai_ros_driver/parametersConfig.h" +#include "depthai_ros_driver/utils.hpp" namespace dai { class Pipeline; class Device; +namespace node { +class XLinkOut; +class VideoEncoder; +} // namespace node } // namespace dai namespace ros { @@ -18,6 +24,10 @@ class Parameter; namespace depthai_ros_driver { namespace dai_nodes { +namespace sensor_helpers { +class ImagePublisher; +struct VideoEncoderConfig; +} // namespace sensor_helpers class BaseNode { public: /** @@ -32,6 +42,8 @@ class BaseNode { virtual void updateParams(parametersConfig& config); virtual void link(dai::Node::Input in, int linkType = 0); virtual dai::Node::Input getInput(int linkType = 0); + virtual dai::Node::Input getInputByName(const std::string& name = ""); + virtual std::vector> getPublishers(); virtual void setupQueues(std::shared_ptr device) = 0; /** * @brief Sets the names of the queues. @@ -43,8 +55,15 @@ class BaseNode { * @param pipeline The pipeline */ virtual void setXinXout(std::shared_ptr pipeline) = 0; + std::shared_ptr setupOutput(std::shared_ptr pipeline, + const std::string& qName, + std::function nodeLink, + bool isSynced = false, + const utils::VideoEncoderConfig& encoderConfig = {}); virtual void closeQueues() = 0; + std::shared_ptr setupXout(std::shared_ptr pipeline, const std::string& name); + void setNodeName(const std::string& daiNodeName); void setROSNodePointer(ros::NodeHandle node); /** @@ -65,10 +84,18 @@ class BaseNode { * @param[in] frameName The frame name */ std::string getTFPrefix(const std::string& frameName = ""); + /** + * @brief Append ROS node name to the frameName given and append optical frame suffix to it. + * + * @param[in] frameName The frame name + */ + std::string getOpticalTFPrefix(const std::string& frameName = ""); + std::string getSocketName(dai::CameraBoardSocket socket); + bool rsCompabilityMode(); private: ros::NodeHandle baseNode; std::string baseDAINodeName; }; } // namespace dai_nodes -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver 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 bf7492ed..8adab488 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 @@ -15,6 +15,7 @@ #include "depthai_bridge/ImageConverter.hpp" #include "depthai_bridge/ImgDetectionConverter.hpp" #include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/img_pub.hpp" #include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" #include "depthai_ros_driver/param_handlers/nn_param_handler.hpp" #include "depthai_ros_driver/parametersConfig.h" @@ -42,7 +43,7 @@ class Detection : public BaseNode { ros::NodeHandle node, std::shared_ptr pipeline, const dai::CameraBoardSocket& socket = dai::CameraBoardSocket::CAM_A) - : BaseNode(daiNodeName, node, pipeline), it(node) { + : BaseNode(daiNodeName, node, pipeline) { ROS_DEBUG("Creating node %s", daiNodeName.c_str()); setNames(); detectionNode = pipeline->create(); @@ -62,8 +63,8 @@ class Detection : public BaseNode { */ void setupQueues(std::shared_ptr device) override { nnQ = device->getOutputQueue(nnQName, ph->getParam("i_max_q_size"), false); - std::string socketName = utils::getSocketName(static_cast(ph->getParam("i_board_socket_id"))); - auto tfPrefix = getTFPrefix(socketName); + std::string socketName = getSocketName(static_cast(ph->getParam("i_board_socket_id"))); + auto tfPrefix = getOpticalTFPrefix(socketName); int width; int height; if(ph->getParam("i_disable_resize")) { @@ -73,21 +74,24 @@ class Detection : public BaseNode { 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 = std::make_unique(tfPrefix, width, height, false, ph->getParam("i_get_base_device_timestamp")); detConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); detPub = getROSNode().template advertise(getName() + "/detections", 10); 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(ros::NodeHandle(getROSNode(), getName()), "/" + getName()); - infoManager->setCameraInfo(sensor_helpers::getCalibInfo(*imageConverter, device, dai::CameraBoardSocket::CAM_A, width, height)); + utils::ImgConverterConfig convConf; + convConf.getBaseDeviceTimestamp = ph->getParam("i_get_base_device_timestamp"); + convConf.tfPrefix = tfPrefix; + convConf.updateROSBaseTimeOnRosMsg = ph->getParam("i_update_ros_base_time_on_ros_msg"); - ptPub = it.advertiseCamera(getName() + "/passthrough/image_raw", 1); - ptQ->addCallback(std::bind(sensor_helpers::basicCameraPub, std::placeholders::_1, std::placeholders::_2, *imageConverter, ptPub, infoManager)); + utils::ImgPublisherConfig pubConf; + pubConf.daiNodeName = getName(); + pubConf.topicName = "~/" + getName(); + pubConf.topicSuffix = "passthrough"; + pubConf.socket = static_cast(ph->getParam("i_board_socket_id")); + + ptPub->setup(device, convConf, pubConf); } }; /** @@ -126,9 +130,7 @@ class Detection : public BaseNode { xoutNN->setStreamName(nnQName); detectionNode->out.link(xoutNN->input); if(ph->getParam("i_enable_passthrough")) { - xoutPT = pipeline->create(); - xoutPT->setStreamName(ptQName); - detectionNode->passthrough.link(xoutPT->input); + ptPub = setupOutput(pipeline, ptQName, [&](dai::Node::Input input) { detectionNode->passthrough.link(input); }); } }; /** @@ -137,7 +139,7 @@ class Detection : public BaseNode { void closeQueues() override { nnQ->close(); if(ph->getParam("i_enable_passthrough")) { - ptQ->close(); + ptPub->closeQueue(); } }; @@ -163,12 +165,9 @@ class Detection : public BaseNode { } }; std::unique_ptr detConverter; - image_transport::ImageTransport it; std::vector labelNames; ros::Publisher detPub; - std::unique_ptr imageConverter; - image_transport::CameraPublisher ptPub; - std::shared_ptr infoManager; + std::shared_ptr ptPub; std::shared_ptr detectionNode; std::shared_ptr imageManip; std::unique_ptr ph; @@ -179,4 +178,4 @@ class Detection : public BaseNode { } // namespace nn } // namespace dai_nodes -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/segmentation.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/segmentation.hpp index d336a059..e449c50c 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/segmentation.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/segmentation.hpp @@ -60,7 +60,7 @@ class Segmentation : public BaseNode { std::vector labelNames; image_transport::CameraPublisher nnPub, ptPub; sensor_msgs::CameraInfo nnInfo; - std::unique_ptr imageConverter; + std::shared_ptr imageConverter; std::shared_ptr infoManager; std::shared_ptr segNode; std::shared_ptr imageManip; @@ -72,4 +72,4 @@ class Segmentation : public BaseNode { } // namespace nn } // namespace dai_nodes -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver 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 baff3ff6..1729ea5b 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 @@ -16,6 +16,7 @@ #include "depthai_bridge/SpatialDetectionConverter.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/sensors/img_pub.hpp" #include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" #include "depthai_ros_driver/param_handlers/nn_param_handler.hpp" #include "depthai_ros_driver/parametersConfig.h" @@ -34,7 +35,7 @@ class SpatialDetection : public BaseNode { ros::NodeHandle node, std::shared_ptr pipeline, const dai::CameraBoardSocket& socket = dai::CameraBoardSocket::CAM_A) - : BaseNode(daiNodeName, node, pipeline), it(node) { + : BaseNode(daiNodeName, node, pipeline) { ROS_DEBUG("Creating node %s", daiNodeName.c_str()); setNames(); spatialNode = pipeline->create(); @@ -51,9 +52,9 @@ class SpatialDetection : public BaseNode { }; void setupQueues(std::shared_ptr device) override { nnQ = device->getOutputQueue(nnQName, ph->getParam("i_max_q_size"), false); - std::string socketName = utils::getSocketName(static_cast(ph->getParam("i_board_socket_id"))); + std::string socketName = getSocketName(static_cast(ph->getParam("i_board_socket_id"))); - auto tfPrefix = getTFPrefix(socketName); + auto tfPrefix = getOpticalTFPrefix(socketName); int width; int height; if(ph->getParam("i_disable_resize")) { @@ -63,40 +64,47 @@ class SpatialDetection : public BaseNode { 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 = std::make_unique(tfPrefix, 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 advertise(getName() + "/spatial_detections", 10); 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(ros::NodeHandle(getROSNode(), getName()), "/" + getName()); - ptInfoMan->setCameraInfo(sensor_helpers::getCalibInfo(*ptImageConverter, device, dai::CameraBoardSocket::CAM_A, width, height)); + utils::ImgConverterConfig convConf; + convConf.tfPrefix = tfPrefix; + convConf.getBaseDeviceTimestamp = ph->getParam("i_get_base_device_timestamp"); + convConf.updateROSBaseTimeOnRosMsg = ph->getParam("i_update_ros_base_time_on_ros_msg"); - ptPub = it.advertiseCamera(getName() + "/passthrough/image_raw", 1); - ptQ->addCallback(std::bind(sensor_helpers::basicCameraPub, std::placeholders::_1, std::placeholders::_2, *ptImageConverter, ptPub, ptInfoMan)); + utils::ImgPublisherConfig pubConf; + pubConf.width = width; + pubConf.height = height; + pubConf.daiNodeName = getName(); + pubConf.topicName = "~/" + getName(); + pubConf.topicSuffix = "passthrough"; + pubConf.socket = static_cast(ph->getParam("i_board_socket_id")); + + ptPub->setup(device, convConf, pubConf); } if(ph->getParam("i_enable_passthrough_depth")) { dai::CameraBoardSocket socket = static_cast(ph->getOtherNodeParam("stereo", "i_board_socket_id")); if(!ph->getOtherNodeParam("stereo", "i_align_depth")) { tfPrefix = getTFPrefix("right"); - socket = dai::CameraBoardSocket::CAM_C; }; - 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(ros::NodeHandle(getROSNode(), getName()), "/" + getName()); - int width = ph->getOtherNodeParam("stereo", "i_width"); - int height = ph->getOtherNodeParam("stereo", "i_height"); - ptDepthInfoMan->setCameraInfo(sensor_helpers::getCalibInfo(*ptDepthImageConverter, device, socket, width, height)); + utils::ImgConverterConfig convConf; + convConf.tfPrefix = tfPrefix; + convConf.getBaseDeviceTimestamp = ph->getParam("i_get_base_device_timestamp"); + convConf.updateROSBaseTimeOnRosMsg = ph->getParam("i_update_ros_base_time_on_ros_msg"); + + utils::ImgPublisherConfig pubConf; + pubConf.width = ph->getOtherNodeParam(sensor_helpers::getNodeName(getROSNode(), sensor_helpers::NodeNameEnum::Stereo), "i_width"); + pubConf.height = ph->getOtherNodeParam(sensor_helpers::getNodeName(getROSNode(), sensor_helpers::NodeNameEnum::Stereo), "i_height"); + pubConf.daiNodeName = getName(); + pubConf.topicName = "~/" + getName(); + pubConf.topicSuffix = "/passthrough_depth"; + pubConf.socket = socket; - ptDepthPub = it.advertiseCamera(getName() + "/passthrough_depth/image_raw", 1); - ptDepthQ->addCallback( - std::bind(sensor_helpers::basicCameraPub, std::placeholders::_1, std::placeholders::_2, *ptDepthImageConverter, ptDepthPub, ptDepthInfoMan)); + ptDepthPub->setup(device, convConf, pubConf); } }; void link(dai::Node::Input in, int /*linkType = 0*/) override { @@ -122,23 +130,19 @@ class SpatialDetection : public BaseNode { xoutNN->setStreamName(nnQName); spatialNode->out.link(xoutNN->input); if(ph->getParam("i_enable_passthrough")) { - xoutPT = pipeline->create(); - xoutPT->setStreamName(ptQName); - spatialNode->passthrough.link(xoutPT->input); + ptPub = setupOutput(pipeline, ptQName, [&](dai::Node::Input input) { spatialNode->passthrough.link(input); }); } if(ph->getParam("i_enable_passthrough_depth")) { - xoutPTDepth = pipeline->create(); - xoutPTDepth->setStreamName(ptDepthQName); - spatialNode->passthroughDepth.link(xoutPTDepth->input); + ptDepthPub = setupOutput(pipeline, ptDepthQName, [&](dai::Node::Input input) { spatialNode->passthroughDepth.link(input); }); } }; void closeQueues() override { nnQ->close(); if(ph->getParam("i_enable_passthrough")) { - ptQ->close(); + ptPub->closeQueue(); } if(ph->getParam("i_enable_passthrough_depth")) { - ptDepthQ->close(); + ptDepthPub->closeQueue(); } }; @@ -154,13 +158,9 @@ class SpatialDetection : public BaseNode { } }; std::unique_ptr detConverter; - image_transport::ImageTransport it; std::vector labelNames; ros::Publisher detPub; - std::unique_ptr ptImageConverter, ptDepthImageConverter; - image_transport::CameraPublisher ptPub, ptDepthPub; - sensor_msgs::CameraInfo ptInfo, ptDepthInfo; - std::shared_ptr ptInfoMan, ptDepthInfoMan; + std::shared_ptr ptPub, ptDepthPub; std::shared_ptr spatialNode; std::shared_ptr imageManip; std::unique_ptr ph; @@ -171,4 +171,4 @@ class SpatialDetection : public BaseNode { } // namespace nn } // namespace dai_nodes -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/img_pub.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/img_pub.hpp new file mode 100644 index 00000000..60701661 --- /dev/null +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/img_pub.hpp @@ -0,0 +1,110 @@ +#pragma once + +#include + +#include "depthai/pipeline/Node.hpp" +#include "depthai/pipeline/datatype/ADatatype.hpp" +#include "depthai_ros_driver/utils.hpp" +#include "depthai_ros_msgs/FFMPEGPacket.h" +#include "image_transport/camera_publisher.h" +#include "image_transport/image_transport.h" +#include "sensor_msgs/CameraInfo.h" +#include "sensor_msgs/CompressedImage.h" +#include "sensor_msgs/Image.h" + +namespace dai { +class Device; +class Pipeline; +class DataOutputQueue; +namespace node { +class VideoEncoder; +class XLinkOut; +} // namespace node +namespace ros { +class ImageConverter; +} +} // namespace dai + +namespace camera_info_manager { +class CameraInfoManager; +} +namespace depthai_ros_driver { +namespace dai_nodes { + +namespace sensor_helpers { +/** + * @brief Image struct + * + * This struct is used to store the image, camera info, header and ffmpeg packet. + */ +class Image { + public: + sensor_msgs::Image::Ptr image; + sensor_msgs::CameraInfo::Ptr info; + depthai_ros_msgs::FFMPEGPacket::Ptr ffmpegPacket; + sensor_msgs::CompressedImage::Ptr compressedImg; +}; +/** + * @brief ImagePublisher class + * + * This class is used to publish images from the device to ROS2. It creates a publisher for the image and camera info topics. + */ +class ImagePublisher { + public: + /** + * @brief Construct a new Image Publisher object + * + * Creates XLinkOut if synced and VideoEncoder if lowBandwidth is enabled. linkFunc is stored and returned when link is called. + */ + ImagePublisher(ros::NodeHandle node, + std::shared_ptr pipeline, + const std::string& qName, + std::function linkFunc, + bool synced = false, + const utils::VideoEncoderConfig& encoderConfig = {}); + + ~ImagePublisher(); + /** + * @brief Setup the image publisher + * + * Creates Publishers, ImageConverter and CameraInfoManager. Creates a Queue and adds a callback if not synced. + */ + void setup(std::shared_ptr device, const utils::ImgConverterConfig& convConf, const utils::ImgPublisherConfig& pubConf); + void createImageConverter(std::shared_ptr device); + void createInfoManager(std::shared_ptr device); + void addQueueCB(const std::shared_ptr& queue); + void closeQueue(); + std::shared_ptr getQueue(); + void link(dai::Node::Input in); + std::string getQueueName(); + void publish(const std::shared_ptr& data); + void publish(std::shared_ptr img); + void publish(std::shared_ptr img, ros::Time timestamp); + std::shared_ptr convertData(const std::shared_ptr& data); + std::shared_ptr createEncoder(std::shared_ptr pipeline, const utils::VideoEncoderConfig& encoderConfig); + + private: + bool detectSubscription(const ros::Publisher pub, const ros::Publisher infoPub); + ros::NodeHandle node; + image_transport::ImageTransport it; + utils::VideoEncoderConfig encConfig; + utils::ImgPublisherConfig pubConfig; + utils::ImgConverterConfig convConfig; + std::shared_ptr infoManager; + std::shared_ptr converter; + std::shared_ptr xout; + std::shared_ptr encoder; + std::function linkCB; + ros::Publisher infoPub; + ros::Publisher ffmpegPub; + ros::Publisher compressedImgPub; + image_transport::CameraPublisher imgPubIT; + std::shared_ptr dataQ; + int cbID; + std::string qName; + bool ipcEnabled; + bool synced; +}; +} // namespace sensor_helpers +} // namespace dai_nodes +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/mono.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/mono.hpp index 4a54f15a..5c869d6c 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/mono.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/mono.hpp @@ -1,42 +1,30 @@ #pragma once #include "depthai_ros_driver/dai_nodes/base_node.hpp" -#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" -#include "image_transport/camera_publisher.h" -#include "image_transport/image_transport.h" -#include "sensor_msgs/CameraInfo.h" -#include "sensor_msgs/Image.h" - namespace dai { class Pipeline; class Device; -class DataOutputQueue; class DataInputQueue; class ADatatype; namespace node { class MonoCamera; class XLinkIn; -class XLinkOut; -class VideoEncoder; } // namespace node -namespace ros { -class ImageConverter; -} } // namespace dai namespace ros { class NodeHandle; } // namespace ros -namespace camera_info_manager { -class CameraInfoManager; -} - namespace depthai_ros_driver { namespace param_handlers { class SensorParamHandler; } namespace dai_nodes { +namespace sensor_helpers { +struct ImageSensor; +class ImagePublisher; +} // namespace sensor_helpers class Mono : public BaseNode { public: @@ -53,21 +41,16 @@ class Mono : public BaseNode { void setNames() override; void setXinXout(std::shared_ptr pipeline) override; void closeQueues() override; + std::vector> getPublishers() override; private: - std::unique_ptr imageConverter; - image_transport::ImageTransport it; - image_transport::CameraPublisher monoPubIT; - std::shared_ptr infoManager; + std::shared_ptr imagePublisher; std::shared_ptr monoCamNode; - std::shared_ptr videoEnc; std::unique_ptr ph; - std::shared_ptr monoQ; std::shared_ptr controlQ; - std::shared_ptr xoutMono; std::shared_ptr xinControl; std::string monoQName, controlQName; }; } // namespace dai_nodes -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/rgb.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/rgb.hpp index 24b1eedd..96c1b4f7 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/rgb.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/rgb.hpp @@ -2,37 +2,23 @@ #include "depthai_ros_driver/dai_nodes/base_node.hpp" #include "depthai_ros_driver/parametersConfig.h" -#include "image_transport/camera_publisher.h" -#include "image_transport/image_transport.h" -#include "sensor_msgs/CameraInfo.h" -#include "sensor_msgs/Image.h" namespace dai { class Pipeline; class Device; -class DataOutputQueue; class DataInputQueue; enum class CameraBoardSocket; class ADatatype; namespace node { class ColorCamera; class XLinkIn; -class XLinkOut; -class VideoEncoder; } // namespace node -namespace ros { -class ImageConverter; -} } // namespace dai namespace ros { class NodeHandle; } // namespace ros -namespace camera_info_manager { -class CameraInfoManager; -} - namespace depthai_ros_driver { namespace param_handlers { class SensorParamHandler; @@ -41,7 +27,8 @@ namespace dai_nodes { namespace sensor_helpers { struct ImageSensor; -} +class ImagePublisher; +} // namespace sensor_helpers class RGB : public BaseNode { public: @@ -58,21 +45,16 @@ class RGB : public BaseNode { void setNames() override; void setXinXout(std::shared_ptr pipeline) override; void closeQueues() override; + std::vector> getPublishers() override; private: - std::unique_ptr imageConverter; - image_transport::ImageTransport it; - image_transport::CameraPublisher rgbPubIT, previewPubIT; - std::shared_ptr infoManager, previewInfoManager; + std::shared_ptr rgbPub, previewPub; std::shared_ptr colorCamNode; - std::shared_ptr videoEnc; std::unique_ptr ph; - std::shared_ptr colorQ, previewQ; std::shared_ptr controlQ; - std::shared_ptr xoutColor, xoutPreview; std::shared_ptr xinControl; std::string ispQName, previewQName, controlQName; }; } // namespace dai_nodes -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp index d4a6644c..0e1c838c 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp @@ -34,6 +34,7 @@ namespace link_types { enum class RGBLinkType { video, isp, preview }; } namespace sensor_helpers { +enum class NodeNameEnum { RGB, Left, Right, Stereo, IMU, NN }; struct ImageSensor { std::string name; std::string defaultResolution; @@ -44,11 +45,16 @@ struct ImageSensor { extern std::vector availableSensors; extern const std::unordered_map socketNameMap; +extern const std::unordered_map rsSocketNameMap; +extern const std::unordered_map rsNodeNameMap; +extern const std::unordered_map NodeNameMap; extern const std::unordered_map monoResolutionMap; extern const std::unordered_map rgbResolutionMap; extern const std::unordered_map fSyncModeMap; extern const std::unordered_map cameraImageOrientationMap; - +bool rsCompabilityMode(ros::NodeHandle node); +std::string getSocketName(ros::NodeHandle node, dai::CameraBoardSocket socket); +std::string getNodeName(ros::NodeHandle node, NodeNameEnum name); void basicCameraPub(const std::string& /*name*/, const std::shared_ptr& data, dai::ros::ImageConverter& converter, @@ -63,11 +69,11 @@ void cameraPub(const std::string& /*name*/, bool lazyPub = true); sensor_msgs::CameraInfo getCalibInfo( - dai::ros::ImageConverter& converter, std::shared_ptr device, dai::CameraBoardSocket socket, int width = 0, int height = 0); + std::shared_ptr converter, std::shared_ptr device, dai::CameraBoardSocket socket, int width = 0, int height = 0); std::shared_ptr createEncoder(std::shared_ptr pipeline, int quality, dai::VideoEncoderProperties::Profile profile = dai::VideoEncoderProperties::Profile::MJPEG); } // namespace sensor_helpers } // namespace dai_nodes -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver 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 6c264791..3e324aa0 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 @@ -49,6 +49,7 @@ class SensorWrapper : public BaseNode { void setXinXout(std::shared_ptr pipeline) override; void closeQueues() override; sensor_helpers::ImageSensor getSensorData(); + std::vector> getPublishers() override; private: void subCB(const sensor_msgs::Image::ConstPtr& img); @@ -65,4 +66,4 @@ class SensorWrapper : public BaseNode { }; } // namespace dai_nodes -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/stereo.hpp similarity index 67% rename from depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp rename to depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/stereo.hpp index 15519c41..1b0e9598 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/stereo.hpp @@ -8,29 +8,20 @@ #include "depthai_ros_driver/dai_nodes/base_node.hpp" #include "depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp" #include "depthai_ros_driver/parametersConfig.h" -#include "image_transport/camera_publisher.h" -#include "image_transport/image_transport.h" namespace dai { class Pipeline; class Device; -class DataOutputQueue; class ADatatype; class ImgFrame; +class DataOutputQueue; namespace node { class StereoDepth; -class XLinkOut; -class VideoEncoder; } // namespace node namespace ros { -class ImageConverter; class Timer; } // namespace ros } // namespace dai -namespace camera_info_manager { -class CameraInfoManager; -} - namespace depthai_ros_driver { namespace param_handlers { class StereoParamHandler; @@ -38,9 +29,12 @@ class StereoParamHandler; namespace dai_nodes { namespace link_types { -enum class StereoLinkType { left, right }; +enum class StereoLinkType { stereo, left, right }; } +namespace sensor_helpers { +class ImagePubliser; +} class Stereo : public BaseNode { public: explicit Stereo(const std::string& daiNodeName, @@ -53,35 +47,26 @@ class Stereo : public BaseNode { void updateParams(parametersConfig& config) override; void setupQueues(std::shared_ptr device) override; void link(dai::Node::Input in, int linkType = 0) override; - dai::Node::Input getInput(int linkType = 0); + dai::Node::Input getInput(int linkType = 0) override; void setNames() override; void setXinXout(std::shared_ptr pipeline) override; void closeQueues() override; + std::vector> getPublishers() override; private: void setupStereoQueue(std::shared_ptr device); void setupLeftRectQueue(std::shared_ptr device); void setupRightRectQueue(std::shared_ptr device); - void setupRectQueue(std::shared_ptr device, - dai::CameraFeatures& sensorInfo, - const std::string& queueName, - std::unique_ptr& conv, - std::shared_ptr& im, - std::shared_ptr& q, - image_transport::CameraPublisher& pubIT, - bool isLeft); + void setupRectQueue(std::shared_ptr device, dai::CameraFeatures& sensorInfo, std::shared_ptr pub, bool isLeft); + /** * This callback is used to synchronize left and right rectified frames * It is called every 10ms and it publishes the frames if they are synchronized * If they are not synchronized, it prints a warning message */ void syncTimerCB(); - image_transport::ImageTransport it; - std::unique_ptr stereoConv, leftRectConv, rightRectConv; - image_transport::CameraPublisher stereoPubIT, leftRectPubIT, rightRectPubIT; - std::shared_ptr stereoIM, leftRectIM, rightRectIM; + std::shared_ptr stereoPub, leftRectPub, rightRectPub; std::shared_ptr stereoCamNode; - std::shared_ptr stereoEnc, leftRectEnc, rightRectEnc; std::unique_ptr left; std::unique_ptr right; std::unique_ptr featureTrackerLeftR, featureTrackerRightR, nnNode; @@ -94,4 +79,4 @@ class Stereo : public BaseNode { }; } // namespace dai_nodes -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sync.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sync.hpp new file mode 100644 index 00000000..a395c3f4 --- /dev/null +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sync.hpp @@ -0,0 +1,55 @@ + +#pragma once + +#include +#include +#include + +#include "depthai_ros_driver/dai_nodes/base_node.hpp" + +namespace dai { +class Pipeline; +class Device; +class DataOutputQueue; +class ADatatype; +class ImgFrame; +namespace node { +class Sync; +class XLinkOut; +} // namespace node +} // namespace dai + +namespace ros { +class NodeHandle; +class Parameter; +} // namespace ros + +namespace depthai_ros_driver { +namespace param_handlers { +class SyncParamHandler; +} +namespace dai_nodes { +class Sync : public BaseNode { + public: + explicit Sync(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline); + ~Sync(); + void setupQueues(std::shared_ptr device) override; + void link(dai::Node::Input in, int linkType = 0) override; + dai::Node::Input getInputByName(const std::string& name = "") override; + void setNames() override; + void setXinXout(std::shared_ptr pipeline) override; + void closeQueues() override; + void addPublishers(const std::vector>& pubs); + + private: + std::unique_ptr paramHandler; + std::shared_ptr syncNode; + std::string syncOutputName; + std::shared_ptr xoutFrame; + std::shared_ptr outQueue; + void publishOutputs(); + std::vector> publishers; + std::vector syncNames; +}; +} // namespace dai_nodes +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/tof.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/tof.hpp new file mode 100644 index 00000000..ccee8205 --- /dev/null +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/tof.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" + +namespace dai { +class Pipeline; +class Device; +class DataInputQueue; +class ADatatype; +namespace node { +class Camera; +class ToF; +class ImageAlign; +class XLinkIn; +} // namespace node +} // namespace dai + +namespace ros { +class NodeHandle; +class Parameter; +} // namespace ros + +namespace depthai_ros_driver { +namespace param_handlers { +class ToFParamHandler; +} +namespace dai_nodes { + +namespace sensor_helpers { +class ImagePublisher; +} // namespace sensor_helpers +class ToF : public BaseNode { + public: + explicit ToF(const std::string& daiNodeName, + ros::NodeHandle node, + std::shared_ptr pipeline, + dai::CameraBoardSocket boardSocket = dai::CameraBoardSocket::CAM_A); + ~ToF(); + void setupQueues(std::shared_ptr device) override; + void link(dai::Node::Input in, int linkType = 0) override; + dai::Node::Input getInput(int linkType = 0) override; + void setNames() override; + void setXinXout(std::shared_ptr pipeline) override; + std::vector> getPublishers() override; + void closeQueues() override; + + private: + std::shared_ptr tofPub; + std::shared_ptr camNode; + std::shared_ptr tofNode; + std::shared_ptr alignNode; + std::unique_ptr ph; + dai::CameraBoardSocket boardSocket; + std::string tofQName; +}; + +} // namespace dai_nodes +} // namespace depthai_ros_driver 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 9ce03c78..ef3579f8 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 @@ -1,5 +1,7 @@ #pragma once +#include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai/pipeline/datatype/CameraControl.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" #include "depthai_ros_driver/parametersConfig.h" #include "ros/node_handle.h" @@ -11,10 +13,7 @@ inline std::pair getRangedIntDescriptor(uint16_t min, uint16_t max) { class BaseParamHandler { public: - BaseParamHandler(ros::NodeHandle node, const std::string& name) { - baseName = name; - baseNode = node; - }; + BaseParamHandler(ros::NodeHandle node, const std::string& name) : baseName(name), baseNode(node){}; virtual ~BaseParamHandler(){}; virtual dai::CameraControl setRuntimeParams(parametersConfig& config) = 0; std::string getName() { @@ -23,6 +22,9 @@ class BaseParamHandler { template T getParam(const std::string& paramName) { T value; + if(!baseNode.hasParam(getFullParamName(paramName))) { + ROS_ERROR("Param %s not found", getFullParamName(paramName).c_str()); + } baseNode.getParam(getFullParamName(paramName), value); logParam(getFullParamName(paramName), value); return value; @@ -30,6 +32,9 @@ class BaseParamHandler { template T getParam(const std::string& paramName, T defaultVal) { T value; + if(!baseNode.hasParam(getFullParamName(paramName))) { + ROS_ERROR("Param %s not found", getFullParamName(paramName).c_str()); + } if(!baseNode.param(getFullParamName(paramName), value, defaultVal)) { baseNode.setParam(getFullParamName(paramName), defaultVal); value = defaultVal; @@ -66,6 +71,9 @@ class BaseParamHandler { std::string name = daiNodeName + "_" + paramName; return name; } + std::string getSocketName(dai::CameraBoardSocket socket) { + return dai_nodes::sensor_helpers::getSocketName(getROSNode(), socket); + } protected: ros::NodeHandle getROSNode() { diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/pipeline_gen_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/pipeline_gen_param_handler.hpp new file mode 100644 index 00000000..698c3a45 --- /dev/null +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/pipeline_gen_param_handler.hpp @@ -0,0 +1,29 @@ + +#pragma once + +#include +#include + +#include "depthai_ros_driver/param_handlers/base_param_handler.hpp" + +namespace dai { +enum class UsbSpeed; +} + +namespace ros { +class NodeHandle; +class Parameter; +} // namespace ros + +namespace depthai_ros_driver { +namespace param_handlers { + +class PipelineGenParamHandler : public BaseParamHandler { + public: + explicit PipelineGenParamHandler(ros::NodeHandle node, const std::string& name); + ~PipelineGenParamHandler(); + void declareParams(); + dai::CameraControl setRuntimeParams(parametersConfig& config) override; +}; +} // namespace param_handlers +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/sync_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/sync_param_handler.hpp new file mode 100644 index 00000000..ceac21a7 --- /dev/null +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/sync_param_handler.hpp @@ -0,0 +1,32 @@ + +#pragma once + +#include +#include +#include + +#include "depthai/pipeline/datatype/CameraControl.hpp" +#include "depthai_ros_driver/param_handlers/base_param_handler.hpp" + +namespace dai { +namespace node { +class Sync; +} +} // namespace dai + +namespace ros { +class NodeHandle; +class Parameter; +} // namespace ros + +namespace depthai_ros_driver { +namespace param_handlers { +class SyncParamHandler : public BaseParamHandler { + public: + explicit SyncParamHandler(ros::NodeHandle node, const std::string& name); + ~SyncParamHandler(); + void declareParams(std::shared_ptr sync); + dai::CameraControl setRuntimeParams(parametersConfig& config) override; +}; +} // namespace param_handlers +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/tof_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/tof_param_handler.hpp new file mode 100644 index 00000000..7b35c7f9 --- /dev/null +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/tof_param_handler.hpp @@ -0,0 +1,36 @@ + +#pragma once + +#include +#include +#include + +#include "depthai/pipeline/datatype/CameraControl.hpp" +#include "depthai/pipeline/datatype/ToFConfig.hpp" +#include "depthai_ros_driver/param_handlers/base_param_handler.hpp" + +namespace dai { +namespace node { +class Camera; +class ToF; +} // namespace node +} // namespace dai + +namespace rclcpp { +class Node; +class Parameter; +} // namespace rclcpp + +namespace depthai_ros_driver { +namespace param_handlers { + +class ToFParamHandler : public BaseParamHandler { + public: + explicit ToFParamHandler(ros::NodeHandle node, const std::string& name); + ~ToFParamHandler(); + void declareParams(std::shared_ptr cam, std::shared_ptr tof); + dai::CameraControl setRuntimeParams(parametersConfig& config) override; + std::unordered_map medianFilterMap; +}; +} // namespace param_handlers +} // namespace depthai_ros_driver 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 index a780e3dd..56551d58 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_pipeline.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_pipeline.hpp @@ -10,7 +10,7 @@ #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" +#include "depthai_ros_driver/dai_nodes/sensors/stereo.hpp" namespace dai { class Pipeline; @@ -58,4 +58,4 @@ class BasePipeline { }; }; } // namespace pipeline_gen -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver 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 index dd1aa805..22d1091d 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_types.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/pipeline/base_types.hpp @@ -61,5 +61,33 @@ class CamArray : public BasePipeline { std::shared_ptr pipeline, const std::string& nnType) override; }; +class DepthToF : public BasePipeline { + public: + std::vector> createPipeline(ros::NodeHandle node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) override; +}; +class StereoToF : public BasePipeline { + public: + std::vector> createPipeline(ros::NodeHandle node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) override; +}; +class ToF : public BasePipeline { + public: + std::vector> createPipeline(ros::NodeHandle node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& nnType) override; +}; +class RGBToF : public BasePipeline { + public: + std::vector> createPipeline(ros::NodeHandle 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 +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp b/depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp index 86cd6a77..cd430509 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp @@ -17,22 +17,26 @@ class NodeHandle; } namespace depthai_ros_driver { +namespace param_handlers { +class PipelineGenParamHandler; +} // namespace param_handlers namespace pipeline_gen { -enum class PipelineType { RGB, RGBD, RGBStereo, Depth, Stereo, CamArray }; +enum class PipelineType { RGB, RGBD, RGBStereo, Stereo, Depth, CamArray, DepthToF, StereoToF, ToF, RGBToF }; class PipelineGenerator { public: - PipelineGenerator(){}; - ~PipelineGenerator() = default; + PipelineGenerator(); + ~PipelineGenerator(); /** * @brief Validates the pipeline type. If the pipeline type is not valid for the number of sensors, it will be changed to the default type. * * @param[in] type The type * @param[in] sensorNum The sensor number + * @param[in] deviceName The device name * * @return The validated pipeline type. */ - std::string validatePipeline(const std::string& typeStr, int sensorNum); + std::string validatePipeline(const std::string& typeStr, int sensorNum, const std::string& deviceName); /** * @brief Creates the pipeline by using a plugin. Plugin types need to be of type depthai_ros_driver::pipeline_gen::BasePipeline. @@ -42,7 +46,6 @@ class PipelineGenerator { * @param pipeline The pipeline * @param[in] pipelineType The pipeline type name (plugin name or one of the default types) * @param[in] nnType The neural network type (none, rgb, spatial) - * @param[in] enableImu Indicates if IMU is enabled * * @return Vector BaseNodes created. */ @@ -50,23 +53,14 @@ class PipelineGenerator { std::shared_ptr device, std::shared_ptr pipeline, const std::string& pipelineType, - const std::string& nnType, - bool enableImu); + const std::string& nnType); - 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 pluginTypeMap; + std::unordered_map pipelineTypeMap; + + private: + std::unique_ptr ph; }; } // namespace pipeline_gen -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp index 2e618e9e..59f67fb0 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/utils.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/utils.hpp @@ -1,14 +1,20 @@ #pragma once -#include #include #include #include #include -#include + +#include "depthai-shared/common/CameraBoardSocket.hpp" +#include "depthai-shared/datatype/RawImgFrame.hpp" +#include "depthai-shared/properties/VideoEncoderProperties.hpp" +#include "depthai/common/CameraExposureOffset.hpp" namespace dai { -enum class CameraBoardSocket; +class Pipeline; +namespace node { +class XLinkOut; +} // namespace node } // namespace dai namespace depthai_ros_driver { @@ -24,10 +30,51 @@ 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); -std::string getSocketName(dai::CameraBoardSocket socket); +struct VideoEncoderConfig { + bool enabled = false; + int quality = 50; + dai::VideoEncoderProperties::Profile profile = dai::VideoEncoderProperties::Profile::MJPEG; + int frameFreq = 30; + int bitrate = 0; +}; +struct ImgConverterConfig { + std::string tfPrefix = ""; + bool interleaved = false; + bool getBaseDeviceTimestamp = false; + bool updateROSBaseTimeOnRosMsg = false; + bool lowBandwidth = false; + dai::RawImgFrame::Type encoding = dai::RawImgFrame::Type::BGR888i; + bool addExposureOffset = false; + dai::CameraExposureOffset expOffset = dai::CameraExposureOffset::START; + bool reverseSocketOrder = false; + bool alphaScalingEnabled = false; + double alphaScaling = 1.0; + bool outputDisparity = false; + std::string ffmpegEncoder = "libx264"; +}; + +struct ImgPublisherConfig { + std::string daiNodeName = ""; + std::string topicName = ""; + bool lazyPub = false; + dai::CameraBoardSocket socket = dai::CameraBoardSocket::AUTO; + dai::CameraBoardSocket leftSocket = dai::CameraBoardSocket::CAM_B; + dai::CameraBoardSocket rightSocket = dai::CameraBoardSocket::CAM_C; + std::string calibrationFile = ""; + std::string topicSuffix = "/image_raw"; + std::string compressedTopicSuffix = "/image_raw/compressed"; + std::string infoMgrSuffix = ""; + bool rectified = false; + int width = 0; + int height = 0; + int maxQSize = 8; + bool qBlocking = false; + bool publishCompressed = false; +}; +std::shared_ptr setupXout(std::shared_ptr pipeline, const std::string& name); } // namespace utils -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/launch/camera.launch b/depthai_ros_driver/launch/camera.launch index 092aca1a..937224b4 100644 --- a/depthai_ros_driver/launch/camera.launch +++ b/depthai_ros_driver/launch/camera.launch @@ -1,70 +1,172 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/depthai_ros_driver/launch/rgbd_pcl.launch b/depthai_ros_driver/launch/rgbd_pcl.launch index e116b26e..03719c1a 100644 --- a/depthai_ros_driver/launch/rgbd_pcl.launch +++ b/depthai_ros_driver/launch/rgbd_pcl.launch @@ -35,26 +35,12 @@ + + - - - - - - - - - - - - - - - \ No newline at end of file + diff --git a/depthai_ros_driver/launch/sr_poe_rgbd.launch b/depthai_ros_driver/launch/sr_poe_rgbd.launch new file mode 100644 index 00000000..cc78d207 --- /dev/null +++ b/depthai_ros_driver/launch/sr_poe_rgbd.launch @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index 522c4d9b..59e50067 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -2,10 +2,9 @@ depthai_ros_driver - 2.9.0 + 2.10.1 Depthai ROS Monolithic node. Adam Serafin - Sachin Guruswamy Adam Serafin MIT diff --git a/depthai_ros_driver/plugins.xml b/depthai_ros_driver/plugins.xml index c5886876..c61a05c1 100644 --- a/depthai_ros_driver/plugins.xml +++ b/depthai_ros_driver/plugins.xml @@ -1,20 +1,42 @@ - - RGB Pipeline. - - - RGBD Pipeline. - - - RGBStereo Pipeline. - - - Stereo Pipeline. - - - Depth Pipeline. - - - CamArray Pipeline. - - \ No newline at end of file + + RGB Pipeline. + + + RGBD Pipeline. + + + RGBStereo Pipeline. + + + Stereo Pipeline. + + + Depth Pipeline. + + + CamArray Pipeline. + + + Depth + ToF Pipeline. + + + Stereo + ToF Pipeline. + + + ToF Pipeline. + + + RGB + ToF Pipeline. + + diff --git a/depthai_ros_driver/src/camera.cpp b/depthai_ros_driver/src/camera.cpp index 03421efc..f8c36b93 100644 --- a/depthai_ros_driver/src/camera.cpp +++ b/depthai_ros_driver/src/camera.cpp @@ -19,7 +19,7 @@ void Camera::onInit() { ph->declareParams(); paramServer = std::make_shared>(pNH); paramServer->setCallback(std::bind(&Camera::parameterCB, this, std::placeholders::_1, std::placeholders::_2)); - onConfigure(); + start(); startSrv = pNH.advertiseService("start_camera", &Camera::startCB, this); stopSrv = pNH.advertiseService("stop_camera", &Camera::stopCB, this); savePipelineSrv = pNH.advertiseService("save_pipeline", &Camera::startCB, this); @@ -68,14 +68,22 @@ void Camera::diagCB(const diagnostic_msgs::DiagnosticArray::ConstPtr& msg) { } void Camera::start() { - ROS_INFO("Starting camera."); - if(!camRunning) { - onConfigure(); - } else { - ROS_INFO("Camera already running!."); + bool success = false; + while(!success) { + try { + ROS_INFO("Starting camera."); + if(!camRunning) { + onConfigure(); + } else { + ROS_INFO("Camera already running!."); + } + success = true; + } catch(const std::exception& e) { + ROS_ERROR("Exception occurred: %s. Retry", e.what()); + camRunning = false; + } } } - void Camera::stop() { ROS_INFO("Stopping camera."); if(camRunning) { @@ -152,10 +160,7 @@ void Camera::parameterCB(parametersConfig& config, uint32_t /*level*/) { enableIR = config.camera_i_enable_ir; floodlightBrighness = config.camera_i_floodlight_brightness; laserDotBrightness = config.camera_i_laser_dot_brightness; - if(camRunning && enableIR && !device->getIrDrivers().empty()) { - device->setIrFloodLightBrightness(floodlightBrighness); - device->setIrLaserDotProjectorBrightness(laserDotBrightness); - } + setIR(); if(!daiNodes.empty()) { for(const auto& node : daiNodes) { try { @@ -175,7 +180,7 @@ void Camera::onConfigure() { setupQueues(); setIR(); } catch(const std::runtime_error& e) { - ROS_ERROR(e.what()); + ROS_ERROR("%s", e.what()); throw std::runtime_error("Failed to start up the camera."); } ROS_INFO("Camera ready!"); @@ -199,8 +204,7 @@ void Camera::getDeviceType() { void Camera::createPipeline() { auto generator = std::make_unique(); - daiNodes = generator->createPipeline( - pNH, device, pipeline, ph->getParam("i_pipeline_type"), ph->getParam("i_nn_type"), ph->getParam("i_enable_imu")); + daiNodes = generator->createPipeline(pNH, device, pipeline, ph->getParam("i_pipeline_type"), ph->getParam("i_nn_type")); if(ph->getParam("i_pipeline_dump")) { savePipeline(); } @@ -282,8 +286,16 @@ void Camera::startDevice() { void Camera::setIR() { if(camRunning && enableIR && !device->getIrDrivers().empty()) { - device->setIrFloodLightBrightness(floodlightBrighness); - device->setIrLaserDotProjectorBrightness(laserDotBrightness); + float laserdotIntensity = float(laserDotBrightness); + if(laserdotIntensity > 1.0) { + laserdotIntensity = laserdotIntensity / 1200.0; + } + device->setIrLaserDotProjectorIntensity(laserdotIntensity); + float floodlightIntensity = float(floodlightBrighness); + if(floodlightIntensity > 1.0) { + floodlightIntensity = floodlightIntensity / 1500.0; + } + device->setIrFloodLightIntensity(floodlightIntensity); } } diff --git a/depthai_ros_driver/src/dai_nodes/base_node.cpp b/depthai_ros_driver/src/dai_nodes/base_node.cpp index 58d1335f..72608be7 100644 --- a/depthai_ros_driver/src/dai_nodes/base_node.cpp +++ b/depthai_ros_driver/src/dai_nodes/base_node.cpp @@ -2,6 +2,8 @@ #include "depthai/device/Device.hpp" #include "depthai/pipeline/Pipeline.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/img_pub.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" #include "ros/node_handle.h" namespace depthai_ros_driver { @@ -23,17 +25,45 @@ ros::NodeHandle BaseNode::getROSNode() { std::string BaseNode::getName() { return baseDAINodeName; } +bool BaseNode::rsCompabilityMode() { + return sensor_helpers::rsCompabilityMode(getROSNode()); +} +std::string BaseNode::getSocketName(dai::CameraBoardSocket socket) { + return sensor_helpers::getSocketName(getROSNode(), socket); +} std::string BaseNode::getTFPrefix(const std::string& frameName) { auto prefix = std::string(getROSNode().getNamespace()) + "_" + frameName; prefix.erase(0, 1); return prefix; } +std::string BaseNode::getOpticalTFPrefix(const std::string& frameName) { + std::string suffix = "_camera_optical_frame"; + if(sensor_helpers::rsCompabilityMode(getROSNode())) { + suffix = "_optical_frame"; + } + return getTFPrefix(frameName) + suffix; +} dai::Node::Input BaseNode::getInput(int /*linkType*/) { throw(std::runtime_error("getInput() not implemented")); } +dai::Node::Input BaseNode::getInputByName(const std::string& /*name*/) { + throw(std::runtime_error("getInputByName() not implemented")); +}; + void BaseNode::closeQueues() { throw(std::runtime_error("closeQueues() not implemented")); } +std::shared_ptr BaseNode::setupXout(std::shared_ptr pipeline, const std::string& name) { + return utils::setupXout(pipeline, name); +}; + +std::shared_ptr BaseNode::setupOutput(std::shared_ptr pipeline, + const std::string& qName, + std::function nodeLink, + bool isSynced, + const utils::VideoEncoderConfig& encoderConfig) { + return std::make_shared(getROSNode(), pipeline, qName, nodeLink, isSynced, encoderConfig); +}; void BaseNode::setNames() { throw(std::runtime_error("setNames() not implemented")); @@ -50,9 +80,12 @@ void BaseNode::setupQueues(std::shared_ptr /*device*/) { void BaseNode::link(dai::Node::Input /*in*/, int /*linkType = 0*/) { throw(std::runtime_error("link() not implemented")); } +std::vector> BaseNode::getPublishers() { + return std::vector>(); +}; void BaseNode::updateParams(parametersConfig& /*config*/) { return; } } // namespace dai_nodes -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp b/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp index 8f75dadc..d3de8d5f 100644 --- a/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp +++ b/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp @@ -58,13 +58,13 @@ void Segmentation::setupQueues(std::shared_ptr device) { nnPub = it.advertiseCamera(getName() + "/image_raw", 1); nnQ->addCallback(std::bind(&Segmentation::segmentationCB, this, std::placeholders::_1, std::placeholders::_2)); if(ph->getParam("i_enable_passthrough")) { - auto tfPrefix = getTFPrefix(utils::getSocketName(static_cast(ph->getParam("i_board_socket_id")))); + auto tfPrefix = getOpticalTFPrefix(getSocketName(static_cast(ph->getParam("i_board_socket_id")))); ptQ = device->getOutputQueue(ptQName, ph->getParam("i_max_q_size"), false); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); infoManager = std::make_shared(ros::NodeHandle(getROSNode(), getName()), "/" + getName()); infoManager->setCameraInfo(sensor_helpers::getCalibInfo( - *imageConverter, device, dai::CameraBoardSocket::CAM_A, imageManip->initialConfig.getResizeWidth(), imageManip->initialConfig.getResizeWidth())); + imageConverter, device, dai::CameraBoardSocket::CAM_A, imageManip->initialConfig.getResizeWidth(), imageManip->initialConfig.getResizeWidth())); ptPub = it.advertiseCamera(getName() + "/passthrough/image_raw", 1); ptQ->addCallback(std::bind(sensor_helpers::basicCameraPub, std::placeholders::_1, std::placeholders::_2, *imageConverter, ptPub, infoManager)); @@ -126,4 +126,4 @@ void Segmentation::updateParams(parametersConfig& config) { } // namespace nn } // namespace dai_nodes -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp new file mode 100644 index 00000000..763c0e33 --- /dev/null +++ b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp @@ -0,0 +1,211 @@ +#include "depthai_ros_driver/dai_nodes/sensors/img_pub.hpp" + +#include + +#include "camera_info_manager/camera_info_manager.h" +#include "depthai/device/Device.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/node/VideoEncoder.hpp" +#include "depthai/pipeline/node/XLinkOut.hpp" +#include "depthai_bridge/ImageConverter.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" +#include "depthai_ros_driver/utils.hpp" +#include "image_transport/image_transport.h" + +namespace depthai_ros_driver { +namespace dai_nodes { +namespace sensor_helpers { +ImagePublisher::ImagePublisher(ros::NodeHandle node, + std::shared_ptr pipeline, + const std::string& qName, + std::function linkFunc, + bool synced, + const utils::VideoEncoderConfig& encoderConfig) + : node(node), encConfig(encoderConfig), qName(qName), synced(synced), it(node) { + if(!synced) { + xout = utils::setupXout(pipeline, qName); + } + + linkCB = linkFunc; + if(encoderConfig.enabled) { + encoder = createEncoder(pipeline, encoderConfig); + linkFunc(encoder->input); + + if(!synced) { + if(encoderConfig.profile == dai::VideoEncoderProperties::Profile::MJPEG) { + encoder->bitstream.link(xout->input); + } else { + encoder->out.link(xout->input); + } + } else { + if(encoderConfig.profile == dai::VideoEncoderProperties::Profile::MJPEG) { + linkCB = [&](dai::Node::Input input) { encoder->bitstream.link(input); }; + } else { + linkCB = [&](dai::Node::Input input) { encoder->out.link(input); }; + } + } + } else { + if(!synced) { + linkFunc(xout->input); + } + } +} +void ImagePublisher::setup(std::shared_ptr device, const utils::ImgConverterConfig& convConf, const utils::ImgPublisherConfig& pubConf) { + convConfig = convConf; + pubConfig = pubConf; + createImageConverter(device); + createInfoManager(device); + if(pubConfig.topicName.empty()) { + throw std::runtime_error("Topic name cannot be empty!"); + } + if(pubConfig.publishCompressed) { + if(encConfig.profile == dai::VideoEncoderProperties::Profile::MJPEG) { + compressedImgPub = node.advertise(pubConfig.topicName + pubConfig.compressedTopicSuffix, 10); + } else { + ffmpegPub = node.advertise(pubConfig.topicName + pubConfig.compressedTopicSuffix, 10); + } + infoPub = node.advertise(pubConfig.topicName + "/camera_info", 10); + } else { + imgPubIT = it.advertiseCamera(pubConfig.topicName + pubConfig.topicSuffix, 10); + } + if(!synced) { + dataQ = device->getOutputQueue(getQueueName(), pubConf.maxQSize, pubConf.qBlocking); + addQueueCB(dataQ); + } +} + +void ImagePublisher::createImageConverter(std::shared_ptr device) { + converter = std::make_shared(convConfig.tfPrefix, convConfig.interleaved, convConfig.getBaseDeviceTimestamp); + converter->setUpdateRosBaseTimeOnToRosMsg(convConfig.updateROSBaseTimeOnRosMsg); + if(convConfig.lowBandwidth) { + converter->convertFromBitstream(convConfig.encoding); + } + if(convConfig.addExposureOffset) { + converter->addExposureOffset(convConfig.expOffset); + } + if(convConfig.reverseSocketOrder) { + converter->reverseStereoSocketOrder(); + } + if(convConfig.alphaScalingEnabled) { + converter->setAlphaScaling(convConfig.alphaScaling); + } + if(convConfig.outputDisparity) { + auto calHandler = device->readCalibration(); + double baseline = calHandler.getBaselineDistance(pubConfig.leftSocket, pubConfig.rightSocket, false); + if(convConfig.reverseSocketOrder) { + baseline = calHandler.getBaselineDistance(pubConfig.rightSocket, pubConfig.leftSocket, false); + } + converter->convertDispToDepth(baseline); + } + converter->setFFMPEGEncoding(convConfig.ffmpegEncoder); +} + +std::shared_ptr ImagePublisher::createEncoder(std::shared_ptr pipeline, + const utils::VideoEncoderConfig& encoderConfig) { + auto enc = pipeline->create(); + enc->setQuality(encoderConfig.quality); + enc->setProfile(encoderConfig.profile); + enc->setBitrate(encoderConfig.bitrate); + enc->setKeyframeFrequency(encoderConfig.frameFreq); + return enc; +} +void ImagePublisher::createInfoManager(std::shared_ptr device) { + infoManager = std::make_shared(ros::NodeHandle(node, pubConfig.daiNodeName), + "/" + pubConfig.daiNodeName + pubConfig.infoMgrSuffix); + if(pubConfig.calibrationFile.empty()) { + auto info = sensor_helpers::getCalibInfo(converter, device, pubConfig.socket, pubConfig.width, pubConfig.height); + if(pubConfig.rectified) { + std::fill(info.D.begin(), info.D.end(), 0.0); + std::fill(info.K.begin(), info.K.end(), 0.0); + info.R[0] = info.R[4] = info.R[8] = 1.0; + } + infoManager->setCameraInfo(info); + } else { + infoManager->loadCameraInfo(pubConfig.calibrationFile); + } +}; +ImagePublisher::~ImagePublisher() { + closeQueue(); +}; + +void ImagePublisher::closeQueue() { + if(dataQ) dataQ->close(); +} +void ImagePublisher::link(dai::Node::Input in) { + linkCB(in); +} +std::shared_ptr ImagePublisher::getQueue() { + return dataQ; +} +void ImagePublisher::addQueueCB(const std::shared_ptr& queue) { + dataQ = queue; + qName = queue->getName(); + cbID = dataQ->addCallback([this](const std::shared_ptr& data) { publish(data); }); +} + +std::string ImagePublisher::getQueueName() { + return qName; +} +std::shared_ptr ImagePublisher::convertData(const std::shared_ptr& data) { + auto info = infoManager->getCameraInfo(); + auto img = std::make_shared(); + if(pubConfig.publishCompressed) { + if(encConfig.profile == dai::VideoEncoderProperties::Profile::MJPEG) { + auto daiImg = std::dynamic_pointer_cast(data); + auto rawMsg = converter->toRosCompressedMsg(daiImg); + img->compressedImg = std::make_unique(rawMsg); + } else { + auto daiImg = std::dynamic_pointer_cast(data); + auto rawMsg = converter->toRosFFMPEGPacket(daiImg); + img->ffmpegPacket = std::make_unique(rawMsg); + } + } else { + auto daiImg = std::dynamic_pointer_cast(data); + auto rawMsg = converter->toRosMsgRawPtr(daiImg, info); + info.header = rawMsg.header; + sensor_msgs::Image::Ptr msg = std::make_unique(rawMsg); + img->image = std::move(msg); + } + sensor_msgs::CameraInfo::Ptr infoMsg = std::make_unique(info); + img->info = std::move(infoMsg); + return img; +} +void ImagePublisher::publish(std::shared_ptr img) { + if(pubConfig.publishCompressed) { + if(encConfig.profile == dai::VideoEncoderProperties::Profile::MJPEG) { + compressedImgPub.publish(std::move(img->compressedImg)); + } else { + ffmpegPub.publish(std::move(img->ffmpegPacket)); + } + infoPub.publish(std::move(img->info)); + } else { + if(!pubConfig.lazyPub || imgPubIT.getNumSubscribers() > 0) imgPubIT.publish(*img->image, *img->info); + } +} +void ImagePublisher::publish(std::shared_ptr img, ros::Time timestamp) { + img->info->header.stamp = timestamp; + if(pubConfig.publishCompressed) { + if(encConfig.profile == dai::VideoEncoderProperties::Profile::MJPEG) { + img->compressedImg->header.stamp = timestamp; + } else { + img->ffmpegPacket->header.stamp = timestamp; + } + } else { + img->image->header.stamp = timestamp; + } + publish(img); +} + +void ImagePublisher::publish(const std::shared_ptr& data) { + if(ros::ok()) { + auto img = convertData(data); + publish(img); + } +} + +bool ImagePublisher::detectSubscription(const ros::Publisher pub, const ros::Publisher infoPub) { + return (pub.getNumSubscribers() > 0 || infoPub.getNumSubscribers() > 0); +} +} // namespace sensor_helpers +} // namespace dai_nodes +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp index 81331971..25347b92 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp @@ -1,19 +1,14 @@ #include "depthai_ros_driver/dai_nodes/sensors/mono.hpp" -#include "camera_info_manager/camera_info_manager.h" #include "depthai/device/DataQueue.hpp" #include "depthai/device/Device.hpp" #include "depthai/pipeline/Pipeline.hpp" #include "depthai/pipeline/node/MonoCamera.hpp" -#include "depthai/pipeline/node/VideoEncoder.hpp" #include "depthai/pipeline/node/XLinkIn.hpp" -#include "depthai/pipeline/node/XLinkOut.hpp" -#include "depthai_bridge/ImageConverter.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/img_pub.hpp" #include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" #include "depthai_ros_driver/param_handlers/sensor_param_handler.hpp" #include "depthai_ros_driver/utils.hpp" -#include "image_transport/camera_publisher.h" -#include "image_transport/image_transport.h" #include "ros/node_handle.h" namespace depthai_ros_driver { @@ -24,7 +19,7 @@ Mono::Mono(const std::string& daiNodeName, dai::CameraBoardSocket socket, dai_nodes::sensor_helpers::ImageSensor sensor, bool publish = true) - : BaseNode(daiNodeName, node, pipeline), it(node) { + : BaseNode(daiNodeName, node, pipeline) { ROS_DEBUG("Creating node %s", daiNodeName.c_str()); setNames(); monoCamNode = pipeline->create(); @@ -41,15 +36,15 @@ void Mono::setNames() { void Mono::setXinXout(std::shared_ptr pipeline) { if(ph->getParam("i_publish_topic")) { - xoutMono = pipeline->create(); - xoutMono->setStreamName(monoQName); - if(ph->getParam("i_low_bandwidth")) { - videoEnc = sensor_helpers::createEncoder(pipeline, ph->getParam("i_low_bandwidth_quality")); - monoCamNode->out.link(videoEnc->input); - videoEnc->bitstream.link(xoutMono->input); - } else { - monoCamNode->out.link(xoutMono->input); - } + utils::VideoEncoderConfig encConfig; + encConfig.profile = static_cast(ph->getParam("i_low_bandwidth_profile")); + encConfig.bitrate = ph->getParam("i_low_bandwidth_bitrate"); + encConfig.frameFreq = ph->getParam("i_low_bandwidth_frame_freq"); + encConfig.quality = ph->getParam("i_low_bandwidth_quality"); + encConfig.enabled = ph->getParam("i_low_bandwidth"); + + imagePublisher = setupOutput( + pipeline, monoQName, [&](auto input) { monoCamNode->out.link(input); }, ph->getParam("i_synced"), encConfig); } xinControl = pipeline->create(); xinControl->setStreamName(controlQName); @@ -58,44 +53,36 @@ void Mono::setXinXout(std::shared_ptr pipeline) { void Mono::setupQueues(std::shared_ptr device) { if(ph->getParam("i_publish_topic")) { - monoQ = device->getOutputQueue(monoQName, ph->getParam("i_max_q_size"), false); - auto tfPrefix = getTFPrefix(utils::getSocketName(static_cast(ph->getParam("i_board_socket_id")))); - 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_low_bandwidth")) { - imageConverter->convertFromBitstream(dai::RawImgFrame::Type::GRAY8); - } - if(ph->getParam("i_add_exposure_offset")) { - auto offset = static_cast(ph->getParam("i_exposure_offset")); - imageConverter->addExposureOffset(offset); - } - if(ph->getParam("i_reverse_stereo_socket_order")) { - imageConverter->reverseStereoSocketOrder(); - } - monoPubIT = it.advertiseCamera(getName() + "/image_raw", 1); - infoManager = std::make_shared(ros::NodeHandle(getROSNode(), getName()), "/" + getName()); - if(ph->getParam("i_calibration_file").empty()) { - infoManager->setCameraInfo(sensor_helpers::getCalibInfo(*imageConverter, - device, - static_cast(ph->getParam("i_board_socket_id")), - ph->getParam("i_width"), - ph->getParam("i_height"))); - } else { - infoManager->loadCameraInfo(ph->getParam("i_calibration_file")); - } - monoQ->addCallback(std::bind(sensor_helpers::cameraPub, - std::placeholders::_1, - std::placeholders::_2, - *imageConverter, - monoPubIT, - infoManager, - ph->getParam("i_enable_lazy_publisher"))); + auto tfPrefix = getOpticalTFPrefix(getSocketName(static_cast(ph->getParam("i_board_socket_id")))); + utils::ImgConverterConfig convConf; + convConf.tfPrefix = tfPrefix; + convConf.getBaseDeviceTimestamp = ph->getParam("i_get_base_device_timestamp"); + convConf.updateROSBaseTimeOnRosMsg = ph->getParam("i_update_ros_base_time_on_ros_msg"); + convConf.lowBandwidth = ph->getParam("i_low_bandwidth"); + convConf.encoding = dai::RawImgFrame::Type::GRAY8; + convConf.addExposureOffset = ph->getParam("i_add_exposure_offset"); + convConf.expOffset = static_cast(ph->getParam("i_exposure_offset")); + convConf.reverseSocketOrder = ph->getParam("i_reverse_stereo_socket_order"); + + utils::ImgPublisherConfig pubConf; + pubConf.daiNodeName = getName(); + pubConf.topicName = getName(); + pubConf.lazyPub = ph->getParam("i_enable_lazy_publisher"); + pubConf.socket = static_cast(ph->getParam("i_board_socket_id")); + pubConf.calibrationFile = ph->getParam("i_calibration_file"); + pubConf.rectified = false; + pubConf.width = ph->getParam("i_width"); + pubConf.height = ph->getParam("i_height"); + pubConf.maxQSize = ph->getParam("i_max_q_size"); + pubConf.publishCompressed = ph->getParam("i_publish_compressed"); + + imagePublisher->setup(device, convConf, pubConf); } controlQ = device->getInputQueue(controlQName); } void Mono::closeQueues() { if(ph->getParam("i_publish_topic")) { - monoQ->close(); + imagePublisher->closeQueue(); } controlQ->close(); } @@ -104,6 +91,13 @@ void Mono::link(dai::Node::Input in, int /*linkType*/) { monoCamNode->out.link(in); } +std::vector> Mono::getPublishers() { + std::vector> publishers; + if(ph->getParam("i_publish_topic") && ph->getParam("i_synced")) { + publishers.push_back(imagePublisher); + } + return publishers; +} void Mono::updateParams(parametersConfig& config) { auto ctrl = ph->setRuntimeParams(config); controlQ->send(ctrl); diff --git a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp index 49ee3650..673dd2de 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp @@ -1,19 +1,14 @@ #include "depthai_ros_driver/dai_nodes/sensors/rgb.hpp" -#include "camera_info_manager/camera_info_manager.h" #include "depthai/device/DataQueue.hpp" #include "depthai/device/Device.hpp" #include "depthai/pipeline/Pipeline.hpp" #include "depthai/pipeline/node/ColorCamera.hpp" -#include "depthai/pipeline/node/VideoEncoder.hpp" #include "depthai/pipeline/node/XLinkIn.hpp" -#include "depthai/pipeline/node/XLinkOut.hpp" -#include "depthai_bridge/ImageConverter.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/img_pub.hpp" #include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" #include "depthai_ros_driver/param_handlers/sensor_param_handler.hpp" #include "depthai_ros_driver/utils.hpp" -#include "image_transport/camera_publisher.h" -#include "image_transport/image_transport.h" #include "ros/node_handle.h" namespace depthai_ros_driver { @@ -24,7 +19,7 @@ RGB::RGB(const std::string& daiNodeName, dai::CameraBoardSocket socket = dai::CameraBoardSocket::CAM_A, sensor_helpers::ImageSensor sensor = {"IMX378", "4k", {"12mp", "4k"}, true}, bool publish = true) - : BaseNode(daiNodeName, node, pipeline), it(node) { + : BaseNode(daiNodeName, node, pipeline) { ROS_DEBUG("Creating node %s", daiNodeName.c_str()); setNames(); colorCamNode = pipeline->create(); @@ -42,26 +37,26 @@ void RGB::setNames() { } void RGB::setXinXout(std::shared_ptr pipeline) { + bool outputIsp = ph->getParam("i_output_isp"); + bool lowBandwidth = ph->getParam("i_low_bandwidth"); + std::function rgbLinkChoice; + if(outputIsp && !lowBandwidth) { + rgbLinkChoice = [&](auto input) { colorCamNode->isp.link(input); }; + } else { + rgbLinkChoice = [&](auto input) { colorCamNode->video.link(input); }; + } if(ph->getParam("i_publish_topic")) { - xoutColor = pipeline->create(); - xoutColor->setStreamName(ispQName); - if(ph->getParam("i_low_bandwidth")) { - videoEnc = sensor_helpers::createEncoder(pipeline, ph->getParam("i_low_bandwidth_quality")); - colorCamNode->video.link(videoEnc->input); - videoEnc->bitstream.link(xoutColor->input); - } else { - if(ph->getParam("i_output_isp")) - colorCamNode->isp.link(xoutColor->input); - else - colorCamNode->video.link(xoutColor->input); - } + utils::VideoEncoderConfig encConfig; + encConfig.profile = static_cast(ph->getParam("i_low_bandwidth_profile")); + encConfig.bitrate = ph->getParam("i_low_bandwidth_bitrate"); + encConfig.frameFreq = ph->getParam("i_low_bandwidth_frame_freq"); + encConfig.quality = ph->getParam("i_low_bandwidth_quality"); + encConfig.enabled = lowBandwidth; + + rgbPub = setupOutput(pipeline, ispQName, rgbLinkChoice, ph->getParam("i_synced"), encConfig); } if(ph->getParam("i_enable_preview")) { - xoutPreview = pipeline->create(); - xoutPreview->setStreamName(previewQName); - xoutPreview->input.setQueueSize(2); - xoutPreview->input.setBlocking(false); - colorCamNode->preview.link(xoutPreview->input); + previewPub = setupOutput(pipeline, previewQName, [&](auto input) { colorCamNode->preview.link(input); }); } xinControl = pipeline->create(); xinControl->setStreamName(controlQName); @@ -70,69 +65,71 @@ void RGB::setXinXout(std::shared_ptr pipeline) { void RGB::setupQueues(std::shared_ptr device) { if(ph->getParam("i_publish_topic")) { - auto tfPrefix = getTFPrefix(utils::getSocketName(static_cast(ph->getParam("i_board_socket_id")))); - infoManager = std::make_shared(ros::NodeHandle(getROSNode(), getName()), "/" + 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_low_bandwidth")) { - imageConverter->convertFromBitstream(dai::RawImgFrame::Type::BGR888i); - } - if(ph->getParam("i_add_exposure_offset")) { - auto offset = static_cast(ph->getParam("i_exposure_offset")); - imageConverter->addExposureOffset(offset); - } - if(ph->getParam("i_reverse_stereo_socket_order")) { - imageConverter->reverseStereoSocketOrder(); - } - if(ph->getParam("i_calibration_file").empty()) { - infoManager->setCameraInfo(sensor_helpers::getCalibInfo(*imageConverter, - device, - static_cast(ph->getParam("i_board_socket_id")), - ph->getParam("i_width"), - ph->getParam("i_height"))); - } else { - infoManager->loadCameraInfo(ph->getParam("i_calibration_file")); - } - rgbPubIT = it.advertiseCamera(getName() + "/image_raw", 1); - colorQ = device->getOutputQueue(ispQName, ph->getParam("i_max_q_size"), false); - colorQ->addCallback(std::bind(sensor_helpers::cameraPub, - std::placeholders::_1, - std::placeholders::_2, - *imageConverter, - rgbPubIT, - infoManager, - ph->getParam("i_enable_lazy_publisher"))); + auto tfPrefix = getOpticalTFPrefix(getSocketName(static_cast(ph->getParam("i_board_socket_id")))); + utils::ImgConverterConfig convConfig; + convConfig.tfPrefix = tfPrefix; + convConfig.getBaseDeviceTimestamp = ph->getParam("i_get_base_device_timestamp"); + convConfig.updateROSBaseTimeOnRosMsg = ph->getParam("i_update_ros_base_time_on_ros_msg"); + convConfig.lowBandwidth = ph->getParam("i_low_bandwidth"); + convConfig.encoding = dai::RawImgFrame::Type::BGR888i; + convConfig.addExposureOffset = ph->getParam("i_add_exposure_offset"); + convConfig.expOffset = static_cast(ph->getParam("i_exposure_offset")); + convConfig.reverseSocketOrder = ph->getParam("i_reverse_stereo_socket_order"); + + utils::ImgPublisherConfig pubConfig; + pubConfig.daiNodeName = getName(); + pubConfig.topicName = getName(); + pubConfig.lazyPub = ph->getParam("i_enable_lazy_publisher"); + pubConfig.socket = static_cast(ph->getParam("i_board_socket_id")); + pubConfig.calibrationFile = ph->getParam("i_calibration_file"); + pubConfig.rectified = false; + pubConfig.width = ph->getParam("i_width"); + pubConfig.height = ph->getParam("i_height"); + pubConfig.maxQSize = ph->getParam("i_max_q_size"); + pubConfig.publishCompressed = ph->getParam("i_publish_compressed"); + + rgbPub->setup(device, convConfig, pubConfig); } if(ph->getParam("i_enable_preview")) { - previewQ = device->getOutputQueue(previewQName, ph->getParam("i_max_q_size"), false); - previewPubIT = it.advertiseCamera(getName() + "/preview/image_raw", 1); - previewInfoManager = std::make_shared(ros::NodeHandle(getROSNode(), "/" + previewQName), previewQName); - auto tfPrefix = getTFPrefix(utils::getSocketName(static_cast(ph->getParam("i_board_socket_id")))); - imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); - if(ph->getParam("i_calibration_file").empty()) { - previewInfoManager->setCameraInfo(sensor_helpers::getCalibInfo(*imageConverter, - device, - static_cast(ph->getParam("i_board_socket_id")), - ph->getParam("i_preview_size"), - ph->getParam("i_preview_size"))); - } else { - infoManager->loadCameraInfo(ph->getParam("i_calibration_file")); - } - previewQ->addCallback( - std::bind(sensor_helpers::basicCameraPub, std::placeholders::_1, std::placeholders::_2, *imageConverter, previewPubIT, previewInfoManager)); + auto tfPrefix = getOpticalTFPrefix(getSocketName(static_cast(ph->getParam("i_board_socket_id")))); + utils::ImgConverterConfig convConfig; + convConfig.tfPrefix = tfPrefix; + convConfig.getBaseDeviceTimestamp = ph->getParam("i_get_base_device_timestamp"); + convConfig.updateROSBaseTimeOnRosMsg = ph->getParam("i_update_ros_base_time_on_ros_msg"); + + utils::ImgPublisherConfig pubConfig; + pubConfig.daiNodeName = getName(); + pubConfig.topicName = "~/" + getName(); + pubConfig.lazyPub = ph->getParam("i_enable_lazy_publisher"); + pubConfig.socket = static_cast(ph->getParam("i_board_socket_id")); + pubConfig.calibrationFile = ph->getParam("i_calibration_file"); + pubConfig.rectified = false; + pubConfig.width = ph->getParam("i_preview_width"); + pubConfig.height = ph->getParam("i_preview_height"); + pubConfig.maxQSize = ph->getParam("i_max_q_size"); + pubConfig.topicSuffix = "/preview/image_raw"; + + previewPub->setup(device, convConfig, pubConfig); }; controlQ = device->getInputQueue(controlQName); } void RGB::closeQueues() { if(ph->getParam("i_publish_topic")) { - colorQ->close(); + rgbPub->closeQueue(); if(ph->getParam("i_enable_preview")) { - previewQ->close(); + previewPub->closeQueue(); } } controlQ->close(); } +std::vector> RGB::getPublishers() { + std::vector> publishers; + if(ph->getParam("i_synced")) { + publishers.push_back(rgbPub); + } + return publishers; +} void RGB::link(dai::Node::Input in, int linkType) { if(linkType == static_cast(link_types::RGBLinkType::video)) { diff --git a/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp b/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp index 22f925c9..4c26755c 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp @@ -21,14 +21,6 @@ std::vector availableSensors = {{"IMX378", "1080P", {"12MP", "4K", {"AR0234", "1200P", {"1200P"}, true}, {"IMX582", "4K", {"48MP", "12MP", "4K"}, true}, {"LCM48", "4K", {"48MP", "12MP", "4K"}, true}}; -const std::unordered_map socketNameMap = { - {dai::CameraBoardSocket::AUTO, "rgb"}, - {dai::CameraBoardSocket::CAM_A, "rgb"}, - {dai::CameraBoardSocket::CAM_B, "left"}, - {dai::CameraBoardSocket::CAM_C, "right"}, - {dai::CameraBoardSocket::CAM_D, "left_back"}, - {dai::CameraBoardSocket::CAM_E, "right_back"}, -}; const std::unordered_map monoResolutionMap = { {"400P", dai::MonoCameraProperties::SensorResolution::THE_400_P}, {"480P", dai::MonoCameraProperties::SensorResolution::THE_480_P}, @@ -64,6 +56,59 @@ const std::unordered_map cameraImageOr {"VERTICAL_FLIP", dai::CameraImageOrientation::VERTICAL_FLIP}, }; +const std::unordered_map socketNameMap = { + {dai::CameraBoardSocket::AUTO, "rgb"}, + {dai::CameraBoardSocket::CAM_A, "rgb"}, + {dai::CameraBoardSocket::CAM_B, "left"}, + {dai::CameraBoardSocket::CAM_C, "right"}, + {dai::CameraBoardSocket::CAM_D, "left_back"}, + {dai::CameraBoardSocket::CAM_E, "right_back"}, +}; +const std::unordered_map rsSocketNameMap = { + {dai::CameraBoardSocket::AUTO, "color"}, + {dai::CameraBoardSocket::CAM_A, "color"}, + {dai::CameraBoardSocket::CAM_B, "infra2"}, + {dai::CameraBoardSocket::CAM_C, "infra1"}, + {dai::CameraBoardSocket::CAM_D, "infra4"}, + {dai::CameraBoardSocket::CAM_E, "infra3"}, +}; +const std::unordered_map rsNodeNameMap = { + {NodeNameEnum::RGB, "color"}, + {NodeNameEnum::Left, "infra2"}, + {NodeNameEnum::Right, "infra1"}, + {NodeNameEnum::Stereo, "depth"}, + {NodeNameEnum::IMU, "imu"}, + {NodeNameEnum::NN, "nn"}, +}; + +const std::unordered_map NodeNameMap = { + {NodeNameEnum::RGB, "rgb"}, + {NodeNameEnum::Left, "left"}, + {NodeNameEnum::Right, "right"}, + {NodeNameEnum::Stereo, "stereo"}, + {NodeNameEnum::IMU, "imu"}, + {NodeNameEnum::NN, "nn"}, +}; + +bool rsCompabilityMode(ros::NodeHandle node) { + bool compat = false; + node.getParam("camera_i_rs_compat", compat); + return compat; +} +std::string getNodeName(ros::NodeHandle node, NodeNameEnum name) { + if(rsCompabilityMode(node)) { + return rsNodeNameMap.at(name); + } + return NodeNameMap.at(name); +} + +std::string getSocketName(ros::NodeHandle node, dai::CameraBoardSocket socket) { + if(rsCompabilityMode(node)) { + return rsSocketNameMap.at(socket); + } + return socketNameMap.at(socket); +} + void basicCameraPub(const std::string& /*name*/, const std::shared_ptr& data, dai::ros::ImageConverter& converter, @@ -93,24 +138,18 @@ void cameraPub(const std::string& /*name*/, } } sensor_msgs::CameraInfo getCalibInfo( - dai::ros::ImageConverter& converter, std::shared_ptr device, dai::CameraBoardSocket socket, int width, int height) { + std::shared_ptr converter, std::shared_ptr device, dai::CameraBoardSocket socket, int width, int height) { sensor_msgs::CameraInfo info; auto calibHandler = device->readCalibration(); try { - info = converter.calibrationToCameraInfo(calibHandler, socket, width, height); + info = converter->calibrationToCameraInfo(calibHandler, socket, width, height); } catch(std::runtime_error& e) { ROS_ERROR("No calibration for socket %d! Publishing empty camera_info.", static_cast(socket)); } return info; } -std::shared_ptr createEncoder(std::shared_ptr pipeline, int quality, dai::VideoEncoderProperties::Profile profile) { - auto enc = pipeline->create(); - enc->setQuality(quality); - enc->setProfile(profile); - return enc; -} } // namespace sensor_helpers } // namespace dai_nodes -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver 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 0e352fcc..db82948c 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp @@ -131,6 +131,13 @@ void SensorWrapper::link(dai::Node::Input in, int linkType) { } } +std::vector> SensorWrapper::getPublishers() { + if(ph->getParam("i_disable_node")) { + return std::vector>(); + } + return sensorNode->getPublishers(); +} + void SensorWrapper::updateParams(parametersConfig& config) { sensorNode->updateParams(config); } diff --git a/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp b/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp new file mode 100644 index 00000000..d9e43bdf --- /dev/null +++ b/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp @@ -0,0 +1,327 @@ +#include "depthai_ros_driver/dai_nodes/sensors/stereo.hpp" + +#include "depthai/device/DataQueue.hpp" +#include "depthai/device/DeviceBase.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/datatype/ImgFrame.hpp" +#include "depthai/pipeline/node/StereoDepth.hpp" +#include "depthai_ros_driver/dai_nodes/nn/nn_helpers.hpp" +#include "depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/feature_tracker.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/img_pub.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/param_handlers/stereo_param_handler.hpp" +#include "depthai_ros_driver/utils.hpp" +#include "ros/node_handle.h" + +namespace depthai_ros_driver { +namespace dai_nodes { +Stereo::Stereo(const std::string& daiNodeName, + ros::NodeHandle node, + std::shared_ptr pipeline, + std::shared_ptr device, + dai::CameraBoardSocket leftSocket, + dai::CameraBoardSocket rightSocket) + : BaseNode(daiNodeName, node, pipeline) { + ROS_DEBUG("Creating node %s", daiNodeName.c_str()); + setNames(); + ph = std::make_unique(node, daiNodeName); + auto alignSocket = dai::CameraBoardSocket::CAM_A; + if(device->getDeviceName() == "OAK-D-SR") { + alignSocket = dai::CameraBoardSocket::CAM_C; + } + ph->updateSocketsFromParams(leftSocket, rightSocket, alignSocket); + auto features = device->getConnectedCameraFeatures(); + for(auto f : features) { + if(f.socket == leftSocket) { + leftSensInfo = f; + } else if(f.socket == rightSocket) { + rightSensInfo = f; + } else { + continue; + } + } + ROS_DEBUG("Creating stereo node with left sensor %s and right sensor %s", + getSocketName(leftSensInfo.socket).c_str(), + getSocketName(rightSensInfo.socket).c_str()); + left = std::make_unique(getSocketName(leftSensInfo.socket), node, pipeline, device, leftSensInfo.socket, false); + right = std::make_unique(getSocketName(rightSensInfo.socket), node, pipeline, device, rightSensInfo.socket, false); + stereoCamNode = pipeline->create(); + ph->declareParams(stereoCamNode); + setXinXout(pipeline); + left->link(stereoCamNode->left); + right->link(stereoCamNode->right); + if(ph->getParam("i_enable_spatial_nn")) { + if(ph->getParam("i_spatial_nn_source") == "left") { + nnNode = std::make_unique(getName() + "_spatial_nn", getROSNode(), pipeline, leftSensInfo.socket); + left->link(nnNode->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::input)), + static_cast(dai_nodes::link_types::RGBLinkType::preview)); + } else { + nnNode = std::make_unique(getName() + "_spatial_nn", getROSNode(), pipeline, rightSensInfo.socket); + right->link(nnNode->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::input)), + static_cast(dai_nodes::link_types::RGBLinkType::preview)); + } + stereoCamNode->depth.link(nnNode->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::inputDepth))); + } + ROS_DEBUG("Node %s created", daiNodeName.c_str()); +} +Stereo::~Stereo() = default; +void Stereo::setNames() { + stereoQName = getName() + "_stereo"; + leftRectQName = getName() + "_left_rect"; + rightRectQName = getName() + "_right_rect"; +} + +void Stereo::setXinXout(std::shared_ptr pipeline) { + bool outputDisparity = ph->getParam("i_output_disparity"); + std::function stereoLinkChoice; + if(outputDisparity) { + stereoLinkChoice = [&](auto input) { stereoCamNode->disparity.link(input); }; + } else { + stereoLinkChoice = [&](auto input) { stereoCamNode->depth.link(input); }; + } + if(ph->getParam("i_publish_topic")) { + utils::VideoEncoderConfig encConf; + encConf.profile = static_cast(ph->getParam("i_low_bandwidth_profile")); + encConf.bitrate = ph->getParam("i_low_bandwidth_bitrate"); + encConf.frameFreq = ph->getParam("i_low_bandwidth_frame_freq"); + encConf.quality = ph->getParam("i_low_bandwidth_quality"); + encConf.enabled = ph->getParam("i_low_bandwidth"); + + stereoPub = setupOutput(pipeline, stereoQName, stereoLinkChoice, ph->getParam("i_synced"), encConf); + } + + if(ph->getParam("i_left_rect_publish_topic") || ph->getParam("i_publish_synced_rect_pair")) { + utils::VideoEncoderConfig encConf; + encConf.profile = static_cast(ph->getParam("i_left_rect_low_bandwidth_profile")); + encConf.bitrate = ph->getParam("i_left_rect_low_bandwidth_bitrate"); + encConf.frameFreq = ph->getParam("i_left_rect_low_bandwidth_frame_freq"); + encConf.quality = ph->getParam("i_left_rect_low_bandwidth_quality"); + encConf.enabled = ph->getParam("i_left_rect_low_bandwidth"); + + leftRectPub = setupOutput( + pipeline, leftRectQName, [&](auto input) { stereoCamNode->rectifiedLeft.link(input); }, ph->getParam("i_left_rect_synced"), encConf); + } + + if(ph->getParam("i_right_rect_publish_topic") || ph->getParam("i_publish_synced_rect_pair")) { + utils::VideoEncoderConfig encConf; + encConf.profile = static_cast(ph->getParam("i_right_rect_low_bandwidth_profile")); + encConf.bitrate = ph->getParam("i_right_rect_low_bandwidth_bitrate"); + encConf.frameFreq = ph->getParam("i_right_rect_low_bandwidth_frame_freq"); + encConf.quality = ph->getParam("i_right_rect_low_bandwidth_quality"); + encConf.enabled = ph->getParam("i_right_rect_low_bandwidth"); + rightRectPub = setupOutput( + pipeline, rightRectQName, [&](auto input) { stereoCamNode->rectifiedRight.link(input); }, ph->getParam("i_right_rect_synced"), encConf); + } + + if(ph->getParam("i_left_rect_enable_feature_tracker")) { + featureTrackerLeftR = std::make_unique(leftSensInfo.name + std::string("_rect_feature_tracker"), getROSNode(), pipeline); + + stereoCamNode->rectifiedLeft.link(featureTrackerLeftR->getInput()); + } + + if(ph->getParam("i_right_rect_enable_feature_tracker")) { + featureTrackerRightR = std::make_unique(rightSensInfo.name + std::string("_rect_feature_tracker"), getROSNode(), pipeline); + stereoCamNode->rectifiedRight.link(featureTrackerRightR->getInput()); + } +} + +void Stereo::setupRectQueue(std::shared_ptr device, + dai::CameraFeatures& sensorInfo, + std::shared_ptr pub, + bool isLeft) { + auto sensorName = getSocketName(sensorInfo.socket); + auto tfPrefix = getOpticalTFPrefix(sensorName); + utils::ImgConverterConfig convConfig; + convConfig.tfPrefix = tfPrefix; + convConfig.interleaved = false; + convConfig.getBaseDeviceTimestamp = ph->getParam("i_get_base_device_timestamp"); + convConfig.updateROSBaseTimeOnRosMsg = ph->getParam("i_update_ros_base_time_on_ros_msg"); + convConfig.lowBandwidth = ph->getParam(isLeft ? "i_left_rect_low_bandwidth" : "i_right_rect_low_bandwidth"); + convConfig.encoding = dai::RawImgFrame::Type::GRAY8; + convConfig.addExposureOffset = ph->getParam(isLeft ? "i_left_rect_add_exposure_offset" : "i_right_rect_add_exposure_offset"); + convConfig.expOffset = static_cast(ph->getParam(isLeft ? "i_left_rect_exposure_offset" : "i_right_rect_exposure_offset")); + convConfig.reverseSocketOrder = ph->getParam("i_reverse_stereo_socket_order"); + + utils::ImgPublisherConfig pubConfig; + pubConfig.daiNodeName = sensorName; + pubConfig.rectified = true; + pubConfig.width = ph->getOtherNodeParam(sensorName, "i_width"); + pubConfig.height = ph->getOtherNodeParam(sensorName, "i_height"); + pubConfig.topicName = sensorName; + pubConfig.topicSuffix = rsCompabilityMode() ? "/image_rect_raw" : "/image_raw"; + pubConfig.maxQSize = ph->getOtherNodeParam(sensorName, "i_max_q_size"); + pubConfig.socket = sensorInfo.socket; + pubConfig.infoMgrSuffix = "rect"; + pubConfig.publishCompressed = ph->getParam(isLeft ? "i_left_rect_publish_compressed" : "i_right_rect_publish_compressed"); + + pub->setup(device, convConfig, pubConfig); +} + +void Stereo::setupLeftRectQueue(std::shared_ptr device) { + setupRectQueue(device, leftSensInfo, leftRectPub, true); +} + +void Stereo::setupRightRectQueue(std::shared_ptr device) { + setupRectQueue(device, rightSensInfo, rightRectPub, false); +} + +void Stereo::setupStereoQueue(std::shared_ptr device) { + std::string tfPrefix; + if(ph->getParam("i_align_depth")) { + tfPrefix = getOpticalTFPrefix(ph->getParam("i_socket_name")); + } else { + tfPrefix = getOpticalTFPrefix(getSocketName(rightSensInfo.socket).c_str()); + } + utils::ImgConverterConfig convConfig; + convConfig.getBaseDeviceTimestamp = ph->getParam("i_get_base_device_timestamp"); + convConfig.tfPrefix = tfPrefix; + convConfig.updateROSBaseTimeOnRosMsg = ph->getParam("i_update_ros_base_time_on_ros_msg"); + convConfig.lowBandwidth = ph->getParam("i_low_bandwidth"); + convConfig.encoding = dai::RawImgFrame::Type::RAW8; + convConfig.addExposureOffset = ph->getParam("i_add_exposure_offset"); + convConfig.expOffset = static_cast(ph->getParam("i_exposure_offset")); + convConfig.reverseSocketOrder = ph->getParam("i_reverse_stereo_socket_order"); + convConfig.alphaScalingEnabled = ph->getParam("i_enable_alpha_scaling"); + if(convConfig.alphaScalingEnabled) { + convConfig.alphaScaling = ph->getParam("i_alpha_scaling"); + } + convConfig.outputDisparity = ph->getParam("i_output_disparity"); + + utils::ImgPublisherConfig pubConf; + pubConf.daiNodeName = getName(); + pubConf.topicName = getName(); + pubConf.topicSuffix = rsCompabilityMode() ? "/image_rect_raw" : "/image_raw"; + pubConf.rectified = !convConfig.alphaScalingEnabled; + pubConf.width = ph->getParam("i_width"); + pubConf.height = ph->getParam("i_height"); + pubConf.socket = static_cast(ph->getParam("i_board_socket_id")); + pubConf.calibrationFile = ph->getParam("i_calibration_file"); + pubConf.leftSocket = leftSensInfo.socket; + pubConf.rightSocket = rightSensInfo.socket; + pubConf.lazyPub = ph->getParam("i_enable_lazy_publisher"); + pubConf.maxQSize = ph->getParam("i_max_q_size"); + pubConf.publishCompressed = ph->getParam("i_publish_compressed"); + + stereoPub->setup(device, convConfig, pubConf); +} + +void Stereo::syncTimerCB() { + auto left = leftRectQ->get(); + auto right = rightRectQ->get(); + if(left->getSequenceNum() != right->getSequenceNum()) { + ROS_WARN("Left and right rectified frames are not synchronized!"); + } else { + leftRectPub->publish(left); + rightRectPub->publish(right); + } +} + +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_left_rect_publish_topic") || ph->getParam("i_publish_synced_rect_pair")) { + setupLeftRectQueue(device); + } + if(ph->getParam("i_right_rect_publish_topic") || ph->getParam("i_publish_synced_rect_pair")) { + setupRightRectQueue(device); + } + if(ph->getParam("i_publish_synced_rect_pair")) { + int timerPeriod = 1000.0 / ph->getOtherNodeParam(leftSensInfo.name, "i_fps"); + ROS_INFO("Setting up stereo pair sync timer with period %d ms based on left sensor FPS.", timerPeriod); + leftRectQ = leftRectPub->getQueue(); + rightRectQ = rightRectPub->getQueue(); + syncTimer = std::make_shared(getROSNode().createTimer(ros::Duration(1.0 / timerPeriod), std::bind(&Stereo::syncTimerCB, this))); + } + if(ph->getParam("i_left_rect_enable_feature_tracker")) { + featureTrackerLeftR->setupQueues(device); + } + if(ph->getParam("i_right_rect_enable_feature_tracker")) { + featureTrackerRightR->setupQueues(device); + } + if(ph->getParam("i_enable_spatial_nn")) { + nnNode->setupQueues(device); + } +} +void Stereo::closeQueues() { + left->closeQueues(); + right->closeQueues(); + if(ph->getParam("i_publish_topic")) { + stereoPub->closeQueue(); + } + if(ph->getParam("i_left_rect_publish_topic")) { + leftRectPub->closeQueue(); + } + if(ph->getParam("i_right_rect_publish_topic")) { + rightRectPub->closeQueue(); + } + if(ph->getParam("i_publish_synced_rect_pair")) { + syncTimer.reset(); + leftRectPub->closeQueue(); + rightRectPub->closeQueue(); + } + if(ph->getParam("i_left_rect_enable_feature_tracker")) { + featureTrackerLeftR->closeQueues(); + } + if(ph->getParam("i_right_rect_enable_feature_tracker")) { + featureTrackerRightR->closeQueues(); + } + if(ph->getParam("i_enable_spatial_nn")) { + nnNode->closeQueues(); + } +} + +void Stereo::link(dai::Node::Input in, int linkType) { + if(linkType == static_cast(link_types::StereoLinkType::stereo)) { + stereoCamNode->depth.link(in); + } else if(linkType == static_cast(link_types::StereoLinkType::left)) { + stereoCamNode->rectifiedLeft.link(in); + } else if(linkType == static_cast(link_types::StereoLinkType::right)) { + stereoCamNode->rectifiedRight.link(in); + } else { + throw std::runtime_error("Wrong link type specified!"); + } +} + +std::vector> Stereo::getPublishers() { + std::vector> pubs; + if(ph->getParam("i_publish_topic") && ph->getParam("i_synced")) { + pubs.push_back(stereoPub); + } + if(ph->getParam("i_left_rect_publish_topic") && ph->getParam("i_left_rect_synced")) { + pubs.push_back(leftRectPub); + } + if(ph->getParam("i_right_rect_publish_topic") && ph->getParam("i_right_rect_synced")) { + pubs.push_back(rightRectPub); + } + auto pubsLeft = left->getPublishers(); + if(!pubsLeft.empty()) { + pubs.insert(pubs.end(), pubsLeft.begin(), pubsLeft.end()); + } + auto pubsRight = right->getPublishers(); + if(!pubsRight.empty()) { + pubs.insert(pubs.end(), pubsRight.begin(), pubsRight.end()); + } + return pubs; +} + +dai::Node::Input Stereo::getInput(int linkType) { + if(linkType == static_cast(link_types::StereoLinkType::left)) { + return stereoCamNode->left; + } else if(linkType == static_cast(link_types::StereoLinkType::right)) { + return stereoCamNode->right; + } else { + throw std::runtime_error("Wrong link type specified!"); + } +} + +void Stereo::updateParams(parametersConfig& config) { + ph->setRuntimeParams(config); +} + +} // namespace dai_nodes +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/dai_nodes/sensors/sync.cpp b/depthai_ros_driver/src/dai_nodes/sensors/sync.cpp new file mode 100644 index 00000000..ebeee6ef --- /dev/null +++ b/depthai_ros_driver/src/dai_nodes/sensors/sync.cpp @@ -0,0 +1,81 @@ +#include "depthai_ros_driver/dai_nodes/sensors/sync.hpp" + +#include + +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/datatype/MessageGroup.hpp" +#include "depthai/pipeline/node/Sync.hpp" +#include "depthai/pipeline/node/XLinkOut.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/img_pub.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" +#include "depthai_ros_driver/param_handlers/sync_param_handler.hpp" + +namespace depthai_ros_driver { +namespace dai_nodes { + +Sync::Sync(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline) : BaseNode(daiNodeName, node, pipeline) { + syncNode = pipeline->create(); + paramHandler = std::make_unique(node, daiNodeName); + paramHandler->declareParams(syncNode); + setNames(); + setXinXout(pipeline); +} + +Sync::~Sync() = default; + +void Sync::setNames() { + syncOutputName = getName() + "_out"; +} +void Sync::setXinXout(std::shared_ptr pipeline) { + xoutFrame = pipeline->create(); + xoutFrame->setStreamName(syncOutputName); + xoutFrame->input.setBlocking(false); + syncNode->out.link(xoutFrame->input); +} + +void Sync::setupQueues(std::shared_ptr device) { + outQueue = device->getOutputQueue(syncOutputName, 8, false); + outQueue->addCallback([this](const std::shared_ptr& in) { + auto group = std::dynamic_pointer_cast(in); + if(group) { + bool firstMsg = true; + ros::Time timestamp; + for(auto& msg : *group) { + // find publisher by message namespace + for(auto& pub : publishers) { + if(pub->getQueueName() == msg.first) { + auto data = pub->convertData(msg.second); + if(firstMsg) { + timestamp = data->info->header.stamp; + firstMsg = false; + } + pub->publish(std::move(data), timestamp); + } + } + } + } + }); +} + +void Sync::link(dai::Node::Input in, int /* linkType */) { + syncNode->out.link(in); +} + +dai::Node::Input Sync::getInputByName(const std::string& name) { + syncNode->inputs[name].setBlocking(false); + return syncNode->inputs[name]; +} + +void Sync::closeQueues() { + outQueue->close(); +} + +void Sync::addPublishers(const std::vector>& pubs) { + for(auto& pub : pubs) { + pub->link(getInputByName(pub->getQueueName())); + } + publishers.insert(publishers.end(), pubs.begin(), pubs.end()); +} + +} // namespace dai_nodes +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/dai_nodes/sensors/tof.cpp b/depthai_ros_driver/src/dai_nodes/sensors/tof.cpp new file mode 100644 index 00000000..38bc0193 --- /dev/null +++ b/depthai_ros_driver/src/dai_nodes/sensors/tof.cpp @@ -0,0 +1,107 @@ +#include "depthai_ros_driver/dai_nodes/sensors/tof.hpp" + +#include "depthai/device/Device.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/node/Camera.hpp" +#include "depthai/pipeline/node/ImageAlign.hpp" +#include "depthai/pipeline/node/ToF.hpp" +#include "depthai/pipeline/node/XLinkIn.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/img_pub.hpp" +#include "depthai_ros_driver/param_handlers/tof_param_handler.hpp" +#include "depthai_ros_driver/utils.hpp" +#include "ros/node_handle.h" + +namespace depthai_ros_driver { +namespace dai_nodes { +ToF::ToF(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline, dai::CameraBoardSocket socket) + : BaseNode(daiNodeName, node, pipeline) { + ROS_DEBUG("Creating node %s", daiNodeName.c_str()); + setNames(); + camNode = pipeline->create(); + tofNode = pipeline->create(); + boardSocket = socket; + ph = std::make_unique(node, daiNodeName); + ph->declareParams(camNode, tofNode); + setXinXout(pipeline); + ROS_DEBUG("Node %s created", daiNodeName.c_str()); +} +ToF::~ToF() = default; +void ToF::setNames() { + tofQName = getName() + "_tof"; +} + +void ToF::setXinXout(std::shared_ptr pipeline) { + if(ph->getParam("i_publish_topic")) { + camNode->raw.link(tofNode->input); + bool align = boardSocket == dai::CameraBoardSocket::CAM_A; + std::function tofLinkChoice; + if(align) { + tofLinkChoice = [&](auto input) { tofNode->depth.link(input); }; + } else { + alignNode = pipeline->create(); + tofNode->depth.link(alignNode->input); + tofLinkChoice = [&](auto input) { alignNode->outputAligned.link(input); }; + } + utils::VideoEncoderConfig encConfig; + encConfig.profile = static_cast(ph->getParam("i_low_bandwidth_profile")); + encConfig.bitrate = ph->getParam("i_low_bandwidth_bitrate"); + encConfig.frameFreq = ph->getParam("i_low_bandwidth_frame_freq"); + encConfig.quality = ph->getParam("i_low_bandwidth_quality"); + encConfig.enabled = ph->getParam("i_low_bandwidth"); + + tofPub = setupOutput(pipeline, tofQName, tofLinkChoice, ph->getParam("i_synced"), encConfig); + } +} + +void ToF::setupQueues(std::shared_ptr device) { + if(ph->getParam("i_publish_topic")) { + auto tfPrefix = getOpticalTFPrefix(getSocketName(boardSocket)); + + utils::ImgConverterConfig convConfig; + convConfig.tfPrefix = tfPrefix; + convConfig.getBaseDeviceTimestamp = ph->getParam("i_get_base_device_timestamp"); + convConfig.updateROSBaseTimeOnRosMsg = ph->getParam("i_update_ros_base_time_on_ros_msg"); + convConfig.lowBandwidth = ph->getParam("i_low_bandwidth"); + convConfig.encoding = dai::RawImgFrame::Type::RAW8; + convConfig.addExposureOffset = ph->getParam("i_add_exposure_offset"); + convConfig.expOffset = static_cast(ph->getParam("i_exposure_offset")); + convConfig.reverseSocketOrder = ph->getParam("i_reverse_stereo_socket_order"); + + utils::ImgPublisherConfig pubConfig; + pubConfig.daiNodeName = getName(); + pubConfig.topicName = getName(); + pubConfig.lazyPub = ph->getParam("i_enable_lazy_publisher"); + pubConfig.socket = static_cast(ph->getParam("i_board_socket_id")); + pubConfig.calibrationFile = ph->getParam("i_calibration_file"); + pubConfig.rectified = false; + pubConfig.width = ph->getParam("i_width"); + pubConfig.height = ph->getParam("i_height"); + pubConfig.maxQSize = ph->getParam("i_max_q_size"); + + tofPub->setup(device, convConfig, pubConfig); + } +} +void ToF::closeQueues() { + if(ph->getParam("i_publish_topic")) { + tofPub->closeQueue(); + } +} + +dai::Node::Input ToF::getInput(int /*linkType*/) { + return alignNode->inputAlignTo; +} + +void ToF::link(dai::Node::Input in, int /*linkType*/) { + tofNode->depth.link(in); +} + +std::vector> ToF::getPublishers() { + std::vector> pubs; + if(ph->getParam("i_publish_topic") && ph->getParam("i_synced")) { + pubs.push_back(tofPub); + } + return pubs; +} + +} // namespace dai_nodes +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/dai_nodes/stereo.cpp b/depthai_ros_driver/src/dai_nodes/stereo.cpp deleted file mode 100644 index 005b05b2..00000000 --- a/depthai_ros_driver/src/dai_nodes/stereo.cpp +++ /dev/null @@ -1,348 +0,0 @@ -#include "depthai_ros_driver/dai_nodes/stereo.hpp" - -#include "camera_info_manager/camera_info_manager.h" -#include "cv_bridge/cv_bridge.h" -#include "depthai/device/DataQueue.hpp" -#include "depthai/device/DeviceBase.hpp" -#include "depthai/pipeline/Pipeline.hpp" -#include "depthai/pipeline/node/StereoDepth.hpp" -#include "depthai/pipeline/node/VideoEncoder.hpp" -#include "depthai/pipeline/node/XLinkOut.hpp" -#include "depthai_bridge/ImageConverter.hpp" -#include "depthai_ros_driver/dai_nodes/nn/nn_helpers.hpp" -#include "depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp" -#include "depthai_ros_driver/dai_nodes/sensors/feature_tracker.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/param_handlers/stereo_param_handler.hpp" -#include "depthai_ros_driver/utils.hpp" -#include "image_transport/camera_publisher.h" -#include "image_transport/image_transport.h" -#include "ros/node_handle.h" - -namespace depthai_ros_driver { -namespace dai_nodes { -Stereo::Stereo(const std::string& daiNodeName, - ros::NodeHandle node, - std::shared_ptr pipeline, - std::shared_ptr device, - dai::CameraBoardSocket leftSocket, - dai::CameraBoardSocket rightSocket) - : BaseNode(daiNodeName, node, pipeline), it(node) { - ROS_DEBUG("Creating node %s", daiNodeName.c_str()); - setNames(); - ph = std::make_unique(node, daiNodeName); - auto alignSocket = dai::CameraBoardSocket::CAM_A; - if(device->getDeviceName() == "OAK-D-SR") { - alignSocket = dai::CameraBoardSocket::CAM_C; - } - ph->updateSocketsFromParams(leftSocket, rightSocket, alignSocket); - auto features = device->getConnectedCameraFeatures(); - for(auto f : features) { - if(f.socket == leftSocket) { - leftSensInfo = f; - } else if(f.socket == rightSocket) { - rightSensInfo = f; - } else { - continue; - } - } - ROS_DEBUG("Creating stereo node with left sensor %s and right sensor %s", - utils::getSocketName(leftSensInfo.socket).c_str(), - utils::getSocketName(rightSensInfo.socket).c_str()); - left = std::make_unique(utils::getSocketName(leftSensInfo.socket), node, pipeline, device, leftSensInfo.socket, false); - right = std::make_unique(utils::getSocketName(rightSensInfo.socket), node, pipeline, device, rightSensInfo.socket, false); - stereoCamNode = pipeline->create(); - ph->declareParams(stereoCamNode); - setXinXout(pipeline); - left->link(stereoCamNode->left); - right->link(stereoCamNode->right); - if(ph->getParam("i_enable_spatial_nn")) { - if(ph->getParam("i_spatial_nn_source") == "left") { - nnNode = std::make_unique(getName() + "_spatial_nn", getROSNode(), pipeline, leftSensInfo.socket); - left->link(nnNode->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::input)), - static_cast(dai_nodes::link_types::RGBLinkType::preview)); - } else { - nnNode = std::make_unique(getName() + "_spatial_nn", getROSNode(), pipeline, rightSensInfo.socket); - right->link(nnNode->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::input)), - static_cast(dai_nodes::link_types::RGBLinkType::preview)); - } - stereoCamNode->depth.link(nnNode->getInput(static_cast(dai_nodes::nn_helpers::link_types::SpatialNNLinkType::inputDepth))); - } - ROS_DEBUG("Node %s created", daiNodeName.c_str()); -} -Stereo::~Stereo() = default; -void Stereo::setNames() { - stereoQName = getName() + "_stereo"; - leftRectQName = getName() + "_left_rect"; - rightRectQName = getName() + "_right_rect"; -} - -void Stereo::setXinXout(std::shared_ptr pipeline) { - if(ph->getParam("i_publish_topic")) { - xoutStereo = pipeline->create(); - xoutStereo->setStreamName(stereoQName); - xoutStereo->input.setBlocking(false); - 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") || ph->getParam("i_publish_synced_rect_pair")) { - 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->rectifiedLeft.link(xoutLeftRect->input); - } - } - - if(ph->getParam("i_publish_right_rect") || ph->getParam("i_publish_synced_rect_pair")) { - 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); - } - } - - if(ph->getParam("i_left_rect_enable_feature_tracker")) { - featureTrackerLeftR = std::make_unique(leftSensInfo.name + std::string("_rect_feature_tracker"), getROSNode(), pipeline); - - stereoCamNode->rectifiedLeft.link(featureTrackerLeftR->getInput()); - } - - if(ph->getParam("i_right_rect_enable_feature_tracker")) { - featureTrackerRightR = std::make_unique(rightSensInfo.name + std::string("_rect_feature_tracker"), getROSNode(), pipeline); - stereoCamNode->rectifiedRight.link(featureTrackerRightR->getInput()); - } -} - -void Stereo::setupRectQueue(std::shared_ptr device, - dai::CameraFeatures& sensorInfo, - const std::string& queueName, - std::unique_ptr& conv, - std::shared_ptr& im, - std::shared_ptr& q, - image_transport::CameraPublisher& pubIT, - bool isLeft) { - auto sensorName = utils::getSocketName(sensorInfo.socket); - auto tfPrefix = getTFPrefix(sensorName); - conv = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); - conv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); - bool lowBandwidth = ph->getParam(isLeft ? "i_left_rect_low_bandwidth" : "i_right_rect_low_bandwidth"); - if(lowBandwidth) { - conv->convertFromBitstream(dai::RawImgFrame::Type::GRAY8); - } - bool addOffset = ph->getParam(isLeft ? "i_left_rect_add_exposure_offset" : "i_right_rect_add_exposure_offset"); - if(addOffset) { - auto offset = static_cast(ph->getParam(isLeft ? "i_left_rect_exposure_offset" : "i_right_rect_exposure_offset")); - conv->addExposureOffset(offset); - } - im = std::make_shared(ros::NodeHandle(getROSNode(), sensorName), "/" + sensorName + "/rect"); - if(ph->getParam("i_reverse_stereo_socket_order")) { - conv->reverseStereoSocketOrder(); - } - auto info = sensor_helpers::getCalibInfo( - *conv, device, sensorInfo.socket, ph->getOtherNodeParam(sensorName, "i_width"), ph->getOtherNodeParam(sensorName, "i_height")); - for(auto& d : info.D) { - d = 0.0; - } - - for(auto& r : info.R) { - r = 0.0; - } - info.R[0] = info.R[4] = info.R[8] = 1.0; - - im->setCameraInfo(info); - - q = device->getOutputQueue(queueName, ph->getOtherNodeParam(sensorName, "i_max_q_size"), false); - - // if publish synced pair is set to true then we skip individual publishing of left and right rectified frames - bool addCallback = !ph->getParam("i_publish_synced_rect_pair"); - - pubIT = it.advertiseCamera(sensorName + "/image_rect", 1); - if(addCallback) { - q->addCallback(std::bind( - sensor_helpers::cameraPub, std::placeholders::_1, std::placeholders::_2, *conv, pubIT, im, ph->getParam("i_enable_lazy_publisher"))); - } -} - -void Stereo::setupLeftRectQueue(std::shared_ptr device) { - setupRectQueue(device, leftSensInfo, leftRectQName, leftRectConv, leftRectIM, leftRectQ, leftRectPubIT, true); -} - -void Stereo::setupRightRectQueue(std::shared_ptr device) { - setupRectQueue(device, rightSensInfo, rightRectQName, rightRectConv, rightRectIM, rightRectQ, rightRectPubIT, false); -} - -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(ph->getParam("i_socket_name")); - } else { - tfPrefix = getTFPrefix(utils::getSocketName(rightSensInfo.socket)); - } - stereoConv = std::make_unique(tfPrefix + "_camera_optical_frame", false); - stereoConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); - if(ph->getParam("i_low_bandwidth")) { - stereoConv->convertFromBitstream(dai::RawImgFrame::Type::RAW8); - } - - if(ph->getParam("i_add_exposure_offset")) { - auto offset = static_cast(ph->getParam("i_exposure_offset")); - stereoConv->addExposureOffset(offset); - } - - if(ph->getParam("i_reverse_stereo_socket_order")) { - stereoConv->reverseStereoSocketOrder(); - } - if(ph->getParam("i_enable_alpha_scaling")) { - stereoConv->setAlphaScaling(ph->getParam("i_alpha_scaling")); - } - stereoIM = std::make_shared(ros::NodeHandle(getROSNode(), getName()), "/" + getName()); - auto info = sensor_helpers::getCalibInfo(*stereoConv, - device, - static_cast(ph->getParam("i_board_socket_id")), - ph->getParam("i_width"), - ph->getParam("i_height")); - auto calibHandler = device->readCalibration(); - if(!ph->getParam("i_output_disparity")) { - if(ph->getParam("i_reverse_stereo_socket_order")) { - stereoConv->convertDispToDepth(calibHandler.getBaselineDistance(leftSensInfo.socket, rightSensInfo.socket, false)); - } else { - stereoConv->convertDispToDepth(calibHandler.getBaselineDistance(rightSensInfo.socket, leftSensInfo.socket, false)); - } - } - // remove distortion if alpha scaling is not enabled - if(!ph->getParam("i_enable_alpha_scaling")) { - for(auto& d : info.D) { - d = 0.0; - } - for(auto& r : info.R) { - r = 0.0; - } - info.R[0] = info.R[4] = info.R[8] = 1.0; - } - stereoIM->setCameraInfo(info); - - stereoPubIT = it.advertiseCamera(getName() + "/image_raw", 1); - stereoQ->addCallback(std::bind(sensor_helpers::cameraPub, - std::placeholders::_1, - std::placeholders::_2, - *stereoConv, - stereoPubIT, - stereoIM, - ph->getParam("i_enable_lazy_publisher"))); -} - -void Stereo::syncTimerCB() { - auto left = leftRectQ->get(); - auto right = rightRectQ->get(); - if(left->getSequenceNum() != right->getSequenceNum()) { - ROS_WARN("Left and right rectified frames are not synchronized!"); - } else { - bool lazyPub = ph->getParam("i_enable_lazy_publisher"); - if(ros::ok() && (!lazyPub || leftRectPubIT.getNumSubscribers() > 0 || rightRectPubIT.getNumSubscribers() > 0)) { - auto leftInfo = leftRectIM->getCameraInfo(); - auto leftRawMsg = leftRectConv->toRosMsgRawPtr(left); - leftInfo.header = leftRawMsg.header; - auto rightInfo = rightRectIM->getCameraInfo(); - auto rightRawMsg = rightRectConv->toRosMsgRawPtr(right); - rightRawMsg.header.stamp = leftRawMsg.header.stamp; - rightInfo.header = rightRawMsg.header; - leftRectPubIT.publish(leftRawMsg, leftInfo); - rightRectPubIT.publish(rightRawMsg, rightInfo); - } - } -} - -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") || ph->getParam("i_publish_synced_rect_pair")) { - setupLeftRectQueue(device); - } - if(ph->getParam("i_publish_right_rect") || ph->getParam("i_publish_synced_rect_pair")) { - setupRightRectQueue(device); - } - if(ph->getParam("i_publish_synced_rect_pair")) { - int timerPeriod = 1000.0 / ph->getOtherNodeParam(leftSensInfo.name, "i_fps"); - ROS_INFO("Setting up stereo pair sync timer with period %d ms based on left sensor FPS.", timerPeriod); - syncTimer = std::make_shared(getROSNode().createTimer(ros::Duration(1.0 / timerPeriod), std::bind(&Stereo::syncTimerCB, this))); - } - if(ph->getParam("i_left_rect_enable_feature_tracker")) { - featureTrackerLeftR->setupQueues(device); - } - if(ph->getParam("i_right_rect_enable_feature_tracker")) { - featureTrackerRightR->setupQueues(device); - } - if(ph->getParam("i_enable_spatial_nn")) { - nnNode->setupQueues(device); - } -} -void Stereo::closeQueues() { - left->closeQueues(); - right->closeQueues(); - 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(); - } - if(ph->getParam("i_publish_synced_rect_pair")) { - syncTimer.reset(); - leftRectQ->close(); - rightRectQ->close(); - } - if(ph->getParam("i_left_rect_enable_feature_tracker")) { - featureTrackerLeftR->closeQueues(); - } - if(ph->getParam("i_right_rect_enable_feature_tracker")) { - featureTrackerRightR->closeQueues(); - } - if(ph->getParam("i_enable_spatial_nn")) { - nnNode->closeQueues(); - } -} - -void Stereo::link(dai::Node::Input in, int /*linkType*/) { - stereoCamNode->depth.link(in); -} - -dai::Node::Input Stereo::getInput(int linkType) { - if(linkType == static_cast(link_types::StereoLinkType::left)) { - return stereoCamNode->left; - } else if(linkType == static_cast(link_types::StereoLinkType::right)) { - return stereoCamNode->right; - } else { - throw std::runtime_error("Wrong link type specified!"); - } -} - -void Stereo::updateParams(parametersConfig& config) { - ph->setRuntimeParams(config); -} - -} // namespace dai_nodes -} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp b/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp index 6b1a60b6..27a80c9d 100644 --- a/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp @@ -24,7 +24,6 @@ dai::UsbSpeed CameraParamHandler::getUSBSpeed() { void CameraParamHandler::declareParams() { declareAndLogParam("i_pipeline_type", "RGBD"); declareAndLogParam("i_nn_type", "spatial"); - declareAndLogParam("i_enable_imu", true); declareAndLogParam("i_enable_ir", true); declareAndLogParam("i_usb_speed", "SUPER_PLUS"); declareAndLogParam("i_mx_id", ""); @@ -36,6 +35,8 @@ void CameraParamHandler::declareParams() { declareAndLogParam("i_laser_dot_brightness", 800, getRangedIntDescriptor(0, 1200)); declareAndLogParam("i_floodlight_brightness", 0, getRangedIntDescriptor(0, 1500)); declareAndLogParam("i_restart_on_diagnostics_error", false); + declareAndLogParam("i_rs_compat", false); + declareAndLogParam("i_restart_on_diagnostics_error", false); declareAndLogParam("i_publish_tf_from_calibration", false); declareAndLogParam("i_tf_camera_name", "oak"); declareAndLogParam("i_tf_camera_model", ""); @@ -56,4 +57,4 @@ dai::CameraControl CameraParamHandler::setRuntimeParams(parametersConfig& /*conf return ctrl; } } // namespace param_handlers -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/param_handlers/pipeline_gen_param_handler.cpp b/depthai_ros_driver/src/param_handlers/pipeline_gen_param_handler.cpp new file mode 100644 index 00000000..5baf9567 --- /dev/null +++ b/depthai_ros_driver/src/param_handlers/pipeline_gen_param_handler.cpp @@ -0,0 +1,21 @@ + +#include "depthai_ros_driver/param_handlers/pipeline_gen_param_handler.hpp" + +#include "ros/node_handle.h" + +namespace depthai_ros_driver { +namespace param_handlers { +PipelineGenParamHandler::PipelineGenParamHandler(ros::NodeHandle node, const std::string& name) : BaseParamHandler(node, name) {} +PipelineGenParamHandler::~PipelineGenParamHandler() = default; + +void PipelineGenParamHandler::declareParams() { + declareAndLogParam("i_enable_imu", true); + declareAndLogParam("i_enable_diagnostics", true); + declareAndLogParam("i_enable_sync", false); +} +dai::CameraControl PipelineGenParamHandler::setRuntimeParams(parametersConfig& config) { + dai::CameraControl ctrl; + return ctrl; +} +} // namespace param_handlers +} // namespace depthai_ros_driver 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 f0cd52b0..6c395281 100644 --- a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp @@ -18,7 +18,11 @@ SensorParamHandler::~SensorParamHandler() = default; void SensorParamHandler::declareCommonParams(dai::CameraBoardSocket socket) { declareAndLogParam("i_max_q_size", 30); declareAndLogParam("i_low_bandwidth", false); + declareAndLogParam("i_low_bandwidth_profile", 4); + declareAndLogParam("i_low_bandwidth_frame_freq", 30); + declareAndLogParam("i_low_bandwidth_bitrate", 0); declareAndLogParam("i_low_bandwidth_quality", 50); + declareAndLogParam("i_low_bandwidth_ffmpeg_encoder", "libx264"); declareAndLogParam("i_calibration_file", ""); declareAndLogParam("i_simulate_from_topic", false); declareAndLogParam("i_simulated_topic_name", ""); @@ -32,6 +36,8 @@ void SensorParamHandler::declareCommonParams(dai::CameraBoardSocket socket) { declareAndLogParam("i_add_exposure_offset", false); declareAndLogParam("i_exposure_offset", 0); declareAndLogParam("i_reverse_stereo_socket_order", false); + declareAndLogParam("i_synced", false); + declareAndLogParam("i_publish_compressed", false); } void SensorParamHandler::declareParams(std::shared_ptr monoCam, dai_nodes::sensor_helpers::ImageSensor sensor, bool publish) { @@ -221,4 +227,4 @@ dai::CameraControl SensorParamHandler::setRuntimeParams(parametersConfig& config } } // namespace param_handlers -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver 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 59bdb326..b03fbd29 100644 --- a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp @@ -35,8 +35,8 @@ StereoParamHandler::StereoParamHandler(ros::NodeHandle node, const std::string& } void StereoParamHandler::updateSocketsFromParams(dai::CameraBoardSocket& left, dai::CameraBoardSocket& right, dai::CameraBoardSocket& align) { - int newLeftS = getParam("i_left_socket_id", static_cast(left)); - int newRightS = getParam("i_right_socket_id", static_cast(right)); + int newLeftS = declareAndLogParam("i_left_socket_id", static_cast(left)); + int newRightS = declareAndLogParam("i_right_socket_id", static_cast(right)); alignSocket = static_cast(declareAndLogParam("i_board_socket_id", static_cast(align))); if(newLeftS != static_cast(left) || newRightS != static_cast(right)) { ROS_WARN("Left or right socket changed, updating stereo node"); @@ -52,6 +52,10 @@ void StereoParamHandler::declareParams(std::shared_ptr s declareAndLogParam("i_max_q_size", 30); declareAndLogParam("i_low_bandwidth", false); declareAndLogParam("i_low_bandwidth_quality", 50); + declareAndLogParam("i_low_bandwidth_profile", 4); + declareAndLogParam("i_low_bandwidth_frame_freq", 30); + declareAndLogParam("i_low_bandwidth_bitrate", 0); + declareAndLogParam("i_low_bandwidth_ffmpeg_encoder", "libx264"); declareAndLogParam("i_output_disparity", false); declareAndLogParam("i_get_base_device_timestamp", false); declareAndLogParam("i_update_ros_base_time_on_ros_msg", false); @@ -60,31 +64,46 @@ void StereoParamHandler::declareParams(std::shared_ptr s declareAndLogParam("i_exposure_offset", 0); declareAndLogParam("i_enable_lazy_publisher", true); declareAndLogParam("i_reverse_stereo_socket_order", false); + declareAndLogParam("i_publish_compressed", false); + declareAndLogParam("i_calibration_file", ""); declareAndLogParam("i_publish_synced_rect_pair", false); - declareAndLogParam("i_publish_left_rect", false); + declareAndLogParam("i_left_rect_publish_topic", false); declareAndLogParam("i_left_rect_low_bandwidth", false); + declareAndLogParam("i_left_rect_low_bandwidth_profile", 4); + declareAndLogParam("i_left_rect_low_bandwidth_frame_freq", 30); + declareAndLogParam("i_left_rect_low_bandwidth_bitrate", 0); declareAndLogParam("i_left_rect_low_bandwidth_quality", 50); + declareAndLogParam("i_left_rect_low_bandwidth_ffmpeg_encoder", "libx264"); declareAndLogParam("i_left_rect_add_exposure_offset", false); declareAndLogParam("i_left_rect_exposure_offset", 0); declareAndLogParam("i_left_rect_enable_feature_tracker", false); + declareAndLogParam("i_left_rect_synced", true); + declareAndLogParam("i_left_rect_publish_compressed", false); - declareAndLogParam("i_publish_right_rect", false); + declareAndLogParam("i_right_rect_publish_topic", false); declareAndLogParam("i_right_rect_low_bandwidth", false); declareAndLogParam("i_right_rect_low_bandwidth_quality", 50); + declareAndLogParam("i_right_rect_low_bandwidth_profile", 4); + declareAndLogParam("i_right_rect_low_bandwidth_frame_freq", 30); + declareAndLogParam("i_right_rect_low_bandwidth_bitrate", 0); + declareAndLogParam("i_right_rect_low_bandwidth_ffmpeg_encoder", "libx264"); declareAndLogParam("i_right_rect_enable_feature_tracker", false); declareAndLogParam("i_right_rect_add_exposure_offset", false); declareAndLogParam("i_right_rect_exposure_offset", 0); + declareAndLogParam("i_right_rect_synced", true); + declareAndLogParam("i_right_rect_publish_compressed", false); declareAndLogParam("i_enable_spatial_nn", false); declareAndLogParam("i_spatial_nn_source", "right"); + declareAndLogParam("i_synced", false); stereo->setLeftRightCheck(declareAndLogParam("i_lr_check", true)); int width = 1280; int height = 720; std::string socketName; if(declareAndLogParam("i_align_depth", true)) { - socketName = utils::getSocketName(alignSocket); + socketName = getSocketName(alignSocket); width = getOtherNodeParam(socketName, "i_width"); height = getOtherNodeParam(socketName, "i_height"); declareAndLogParam("i_socket_name", socketName); @@ -109,7 +128,7 @@ void StereoParamHandler::declareParams(std::shared_ptr s stereo->initialConfig.setLeftRightCheckThreshold(declareAndLogParam("i_lrc_threshold", 10)); stereo->initialConfig.setMedianFilter(static_cast(declareAndLogParam("i_depth_filter_size", 5))); stereo->initialConfig.setConfidenceThreshold(declareAndLogParam("i_stereo_conf_threshold", 240)); - if(declareAndLogParam("i_subpixel", false)) { + if(declareAndLogParam("i_subpixel", true)) { stereo->initialConfig.setSubpixel(true); stereo->initialConfig.setSubpixelFractionalBits(declareAndLogParam("i_subpixel_fractional_bits", 3)); } @@ -173,4 +192,4 @@ dai::CameraControl StereoParamHandler::setRuntimeParams(parametersConfig& /*conf return ctrl; } } // namespace param_handlers -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/param_handlers/sync_param_handler.cpp b/depthai_ros_driver/src/param_handlers/sync_param_handler.cpp new file mode 100644 index 00000000..32ef04df --- /dev/null +++ b/depthai_ros_driver/src/param_handlers/sync_param_handler.cpp @@ -0,0 +1,20 @@ +#include "depthai_ros_driver/param_handlers/sync_param_handler.hpp" + +#include "depthai/pipeline/node/Sync.hpp" +#include "ros/node_handle.h" + +namespace depthai_ros_driver { +namespace param_handlers { +SyncParamHandler::SyncParamHandler(ros::NodeHandle node, const std::string& name) : BaseParamHandler(node, name) {} +SyncParamHandler::~SyncParamHandler() = default; +void SyncParamHandler::declareParams(std::shared_ptr sync) { + sync->setSyncThreshold(std::chrono::milliseconds(declareAndLogParam("sync_threshold", 10))); + sync->setSyncAttempts(declareAndLogParam("sync_attempts", 10)); +} + +dai::CameraControl SyncParamHandler::setRuntimeParams(parametersConfig& config) { + dai::CameraControl ctrl; + return ctrl; +} +} // namespace param_handlers +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/param_handlers/tof_param_handler.cpp b/depthai_ros_driver/src/param_handlers/tof_param_handler.cpp new file mode 100644 index 00000000..fb73563a --- /dev/null +++ b/depthai_ros_driver/src/param_handlers/tof_param_handler.cpp @@ -0,0 +1,72 @@ +#include "depthai_ros_driver/param_handlers/tof_param_handler.hpp" + +#include "depthai-shared/common/CameraBoardSocket.hpp" +#include "depthai/pipeline/datatype/ToFConfig.hpp" +#include "depthai/pipeline/node/Camera.hpp" +#include "depthai/pipeline/node/ToF.hpp" +#include "depthai_ros_driver/utils.hpp" +#include "ros/node_handle.h" + +namespace depthai_ros_driver { +namespace param_handlers { +ToFParamHandler::ToFParamHandler(ros::NodeHandle node, const std::string& name) : BaseParamHandler(node, name) { + medianFilterMap = {{"MEDIAN_OFF", dai::MedianFilter::MEDIAN_OFF}, + {"KERNEL_3x3", dai::MedianFilter::KERNEL_3x3}, + {"KERNEL_5x5", dai::MedianFilter::KERNEL_5x5}, + {"KERNEL_7x7", dai::MedianFilter::KERNEL_7x7}}; +} +ToFParamHandler::~ToFParamHandler() = default; +void ToFParamHandler::declareParams(std::shared_ptr cam, std::shared_ptr tof) { + declareAndLogParam("i_publish_topic", true); + declareAndLogParam("i_synced", false); + declareAndLogParam("i_low_bandwidth", false); + declareAndLogParam("i_low_bandwidth_profile", 4); + declareAndLogParam("i_low_bandwidth_bitrate", 0); + declareAndLogParam("i_low_bandwidth_frame_freq", 30); + declareAndLogParam("i_low_bandwidth_quality", 80); + declareAndLogParam("i_get_base_device_timestamp", false); + declareAndLogParam("i_update_ros_base_time_on_ros_msg", false); + declareAndLogParam("i_add_exposure_offset", false); + declareAndLogParam("i_exposure_offset", 0); + declareAndLogParam("i_enable_lazy_publisher", false); + declareAndLogParam("i_reverse_stereo_socket_order", false); + declareAndLogParam("i_calibration_file", ""); + declareAndLogParam("i_max_q_size", 8); + + int socket = declareAndLogParam("i_board_socket_id", static_cast(dai::CameraBoardSocket::CAM_A)); + cam->setBoardSocket(static_cast(socket)); + cam->setSize(declareAndLogParam("i_width", 640), declareAndLogParam("i_height", 480)); + cam->setFps(declareAndLogParam("i_fps", 30)); + auto tofConf = tof->initialConfig.get(); + if(declareAndLogParam("i_enable_optical_correction", false)) { + tofConf.enableOpticalCorrection = true; + } + if(declareAndLogParam("i_enable_fppn_correction", false)) { + tofConf.enableFPPNCorrection = true; + } + if(declareAndLogParam("i_enable_temperature_correction", false)) { + tofConf.enableTemperatureCorrection = true; + } + if(declareAndLogParam("i_enable_wiggle_correction", false)) { + tofConf.enableWiggleCorrection = true; + } + if(declareAndLogParam("i_enable_phase_unwrapping", false)) { + tofConf.enablePhaseUnwrapping = true; + } + + tofConf.enablePhaseShuffleTemporalFilter = declareAndLogParam("i_enable_phase_shuffle_temporal_filter", true); + tofConf.phaseUnwrappingLevel = declareAndLogParam("i_phase_unwrapping_level", 4); + tofConf.phaseUnwrapErrorThreshold = declareAndLogParam("i_phase_unwrap_error_threshold", 100); + std::vector medianSettings = { + dai::MedianFilter::MEDIAN_OFF, dai::MedianFilter::KERNEL_3x3, dai::MedianFilter::KERNEL_5x5, dai::MedianFilter::KERNEL_7x7}; + tofConf.median = utils::getValFromMap(declareAndLogParam("i_median_filter", "MEDIAN_OFF"), medianFilterMap); + + tof->initialConfig.set(tofConf); +} + +dai::CameraControl ToFParamHandler::setRuntimeParams(parametersConfig& /*config*/) { + dai::CameraControl ctrl; + return ctrl; +} +} // namespace param_handlers +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/pipeline/base_types.cpp b/depthai_ros_driver/src/pipeline/base_types.cpp index 0e6c3b62..37cc1e23 100644 --- a/depthai_ros_driver/src/pipeline/base_types.cpp +++ b/depthai_ros_driver/src/pipeline/base_types.cpp @@ -9,23 +9,24 @@ #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/dai_nodes/sensors/stereo.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/tof.hpp" #include "depthai_ros_driver/pipeline/base_pipeline.hpp" #include "depthai_ros_driver/utils.hpp" #include "ros/node_handle.h" namespace depthai_ros_driver { namespace pipeline_gen { - std::vector> RGB::createPipeline(ros::NodeHandle node, std::shared_ptr device, std::shared_ptr pipeline, const std::string& nnType) { + using namespace dai_nodes::sensor_helpers; 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::CAM_A); + auto rgb = std::make_unique(getNodeName(node, NodeNameEnum::RGB), node, pipeline, device, dai::CameraBoardSocket::CAM_A); switch(nType) { case NNType::None: break; @@ -35,7 +36,7 @@ std::vector> RGB::createPipeline(ros::NodeH break; } case NNType::Spatial: { - ROS_WARN("Spatial NN selected, but configuration is RGB. Please change camera_i_nn_type parameter to RGB."); + ROS_WARN("Spatial NN selected, but configuration is RGB. Please change camera.i_nn_type parameter to RGB."); } default: break; @@ -47,12 +48,13 @@ std::vector> RGBD::createPipeline(ros::Node std::shared_ptr device, std::shared_ptr pipeline, const std::string& nnType) { + using namespace dai_nodes::sensor_helpers; 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::CAM_A); - auto stereo = std::make_unique("stereo", node, pipeline, device); + auto rgb = std::make_unique(getNodeName(node, NodeNameEnum::RGB), node, pipeline, device, dai::CameraBoardSocket::CAM_A); + auto stereo = std::make_unique(getNodeName(node, NodeNameEnum::Stereo), node, pipeline, device); switch(nType) { case NNType::None: break; @@ -77,13 +79,14 @@ std::vector> RGBStereo::createPipeline(ros: std::shared_ptr device, std::shared_ptr pipeline, const std::string& nnType) { + using namespace dai_nodes::sensor_helpers; 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::CAM_A); - auto left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::CAM_B); - auto right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::CAM_C); + auto rgb = std::make_unique(getNodeName(node, NodeNameEnum::RGB), node, pipeline, device, dai::CameraBoardSocket::CAM_A); + auto left = std::make_unique(getNodeName(node, NodeNameEnum::Left), node, pipeline, device, dai::CameraBoardSocket::CAM_B); + auto right = std::make_unique(getNodeName(node, NodeNameEnum::Right), node, pipeline, device, dai::CameraBoardSocket::CAM_C); switch(nType) { case NNType::None: break; @@ -93,7 +96,7 @@ std::vector> RGBStereo::createPipeline(ros: break; } case NNType::Spatial: { - ROS_WARN("Spatial NN selected, but configuration is RGBStereo. Please change camera_i_nn_type parameter to RGB."); + ROS_WARN("Spatial NN selected, but configuration is RGBStereo. Please change camera.i_nn_type parameter to RGB."); } default: break; @@ -107,9 +110,10 @@ std::vector> Stereo::createPipeline(ros::No std::shared_ptr device, std::shared_ptr pipeline, const std::string& /*nnType*/) { + using namespace dai_nodes::sensor_helpers; std::vector> daiNodes; - auto left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::CAM_B); - auto right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::CAM_C); + auto left = std::make_unique(getNodeName(node, NodeNameEnum::Left), node, pipeline, device, dai::CameraBoardSocket::CAM_B); + auto right = std::make_unique(getNodeName(node, NodeNameEnum::Right), node, pipeline, device, dai::CameraBoardSocket::CAM_C); daiNodes.push_back(std::move(left)); daiNodes.push_back(std::move(right)); return daiNodes; @@ -118,8 +122,9 @@ std::vector> Depth::createPipeline(ros::Nod std::shared_ptr device, std::shared_ptr pipeline, const std::string& /*nnType*/) { + using namespace dai_nodes::sensor_helpers; std::vector> daiNodes; - auto stereo = std::make_unique("stereo", node, pipeline, device); + auto stereo = std::make_unique(getNodeName(node, NodeNameEnum::Stereo), node, pipeline, device); daiNodes.push_back(std::move(stereo)); return daiNodes; } @@ -127,14 +132,81 @@ std::vector> CamArray::createPipeline(ros:: std::shared_ptr device, std::shared_ptr pipeline, const std::string& /*nnType*/) { + using namespace dai_nodes::sensor_helpers; std::vector> daiNodes; + for(auto& feature : device->getConnectedCameraFeatures()) { - auto name = utils::getSocketName(feature.socket); + auto name = getSocketName(node, feature.socket); auto daiNode = std::make_unique(name, node, pipeline, device, feature.socket); daiNodes.push_back(std::move(daiNode)); }; return daiNodes; } + +std::vector> DepthToF::createPipeline(ros::NodeHandle node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& /*nnType*/) { + std::vector> daiNodes; + auto tof = std::make_unique("tof", node, pipeline); + auto stereo = std::make_unique("stereo", node, pipeline, device); + daiNodes.push_back(std::move(tof)); + daiNodes.push_back(std::move(stereo)); + return daiNodes; +} +std::vector> StereoToF::createPipeline(ros::NodeHandle node, + std::shared_ptr device, + std::shared_ptr pipeline, + const std::string& /*nnType*/) { + std::vector> daiNodes; + auto tof = std::make_unique("tof", node, pipeline, dai::CameraBoardSocket::CAM_C); + auto left = std::make_unique("left", node, pipeline, device, dai::CameraBoardSocket::CAM_B); + auto right = std::make_unique("right", node, pipeline, device, dai::CameraBoardSocket::CAM_C); + right->link(tof->getInput()); + daiNodes.push_back(std::move(left)); + daiNodes.push_back(std::move(right)); + daiNodes.push_back(std::move(tof)); + return daiNodes; +} + +std::vector> ToF::createPipeline(ros::NodeHandle node, + std::shared_ptr /*device*/, + std::shared_ptr pipeline, + const std::string& /*nnType*/) { + std::vector> daiNodes; + auto tof = std::make_unique("tof", node, pipeline); + daiNodes.push_back(std::move(tof)); + return daiNodes; +} +std::vector> RGBToF::createPipeline(ros::NodeHandle 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("right", node, pipeline, device, dai::CameraBoardSocket::CAM_C); + auto tof = std::make_unique("tof", node, pipeline, dai::CameraBoardSocket::CAM_C); + rgb->link(tof->getInput()); + 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: { + ROS_WARN("Spatial NN selected, but configuration is RGBToF. Please change camera.i_nn_type parameter to RGB."); + } + default: + break; + } + daiNodes.push_back(std::move(rgb)); + daiNodes.push_back(std::move(tof)); + return daiNodes; +} } // namespace pipeline_gen } // namespace depthai_ros_driver @@ -145,4 +217,8 @@ PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::RGBD, depthai_ros_drive 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 +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::CamArray, depthai_ros_driver::pipeline_gen::BasePipeline) +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::StereoToF, depthai_ros_driver::pipeline_gen::BasePipeline) +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::DepthToF, depthai_ros_driver::pipeline_gen::BasePipeline) +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::ToF, depthai_ros_driver::pipeline_gen::BasePipeline) +PLUGINLIB_EXPORT_CLASS(depthai_ros_driver::pipeline_gen::RGBToF, depthai_ros_driver::pipeline_gen::BasePipeline) diff --git a/depthai_ros_driver/src/pipeline/pipeline_generator.cpp b/depthai_ros_driver/src/pipeline/pipeline_generator.cpp index a32e3089..92a2c907 100644 --- a/depthai_ros_driver/src/pipeline/pipeline_generator.cpp +++ b/depthai_ros_driver/src/pipeline/pipeline_generator.cpp @@ -3,7 +3,9 @@ #include "depthai/device/Device.hpp" #include "depthai/pipeline/Pipeline.hpp" #include "depthai_ros_driver/dai_nodes/sensors/imu.hpp" +#include "depthai_ros_driver/dai_nodes/sensors/sync.hpp" #include "depthai_ros_driver/dai_nodes/sys_logger.hpp" +#include "depthai_ros_driver/param_handlers/pipeline_gen_param_handler.hpp" #include "depthai_ros_driver/pipeline/base_pipeline.hpp" #include "depthai_ros_driver/utils.hpp" #include "pluginlib/class_loader.hpp" @@ -11,19 +13,44 @@ namespace depthai_ros_driver { namespace pipeline_gen { +PipelineGenerator::PipelineGenerator() { + 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"}, + {"DEPTHTOF", "depthai_ros_driver::pipeline_gen::DepthToF"}, + {"STEREOTOF", "depthai_ros_driver::pipeline_gen::StereoToF"}, + {"TOF", "depthai_ros_driver::pipeline_gen::ToF"}, + {"RGBTOF", "depthai_ros_driver::pipeline_gen::RGBToF"}}; + pipelineTypeMap = {{"RGB", PipelineType::RGB}, + {"RGBD", PipelineType::RGBD}, + {"RGBSTEREO", PipelineType::RGBStereo}, + {"STEREO", PipelineType::Stereo}, + {"DEPTH", PipelineType::Depth}, + {"CAMARRAY", PipelineType::CamArray}, + {"DEPTHTOF", PipelineType::DepthToF}, + {"STEREOTOF", PipelineType::StereoToF}, + {"TOF", PipelineType::ToF}, + {"RGBTOF", PipelineType::RGBToF}}; +} + +PipelineGenerator::~PipelineGenerator() = default; std::vector> PipelineGenerator::createPipeline(ros::NodeHandle node, std::shared_ptr device, std::shared_ptr pipeline, const std::string& pipelineType, - const std::string& nnType, - bool enableImu) { + const std::string& nnType) { + ph = std::make_unique(node, "pipeline_gen"); + ph->declareParams(); ROS_INFO("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 pTypeValidated = validatePipeline(pTypeUpCase, device->getCameraSensorNames().size()); + auto pTypeValidated = validatePipeline(pTypeUpCase, device->getCameraSensorNames().size(), device->getDeviceName()); pluginType = utils::getValFromMap(pTypeValidated, pluginTypeMap); } catch(std::out_of_range& e) { ROS_DEBUG("Pipeline type [%s] not found in base types, trying to load as a plugin.", pipelineType.c_str()); @@ -37,7 +64,7 @@ std::vector> PipelineGenerator::createPipel ROS_ERROR("The plugin failed to load for some reason. Error: %s\n", ex.what()); } - if(enableImu) { + if(ph->getParam("i_enable_imu")) { if(device->getConnectedIMU() == "NONE" || device->getConnectedIMU().empty()) { ROS_WARN("IMU enabled but not available!"); } else { @@ -45,14 +72,30 @@ std::vector> PipelineGenerator::createPipel daiNodes.push_back(std::move(imu)); } } - auto sysLogger = std::make_unique("sys_logger", node, pipeline); - daiNodes.push_back(std::move(sysLogger)); + if(ph->getParam("i_enable_sync")) { + auto sync = std::make_unique("sync", node, pipeline); + for(auto& daiNode : daiNodes) { + auto pubs = daiNode->getPublishers(); + ROS_DEBUG("Number of synced publishers found for %s: %zu", daiNode->getName().c_str(), pubs.size()); + if(!pubs.empty()) { + sync->addPublishers(pubs); + } + } + daiNodes.push_back(std::move(sync)); + } + if(ph->getParam("i_enable_diagnostics")) { + auto sysLogger = std::make_unique("sys_logger", node, pipeline); + daiNodes.push_back(std::move(sysLogger)); + } ROS_INFO("Finished setting up pipeline."); return daiNodes; } -std::string PipelineGenerator::validatePipeline(const std::string& typeStr, int sensorNum) { +std::string PipelineGenerator::validatePipeline(const std::string& typeStr, int sensorNum, const std::string& deviceName) { auto pType = utils::getValFromMap(typeStr, pipelineTypeMap); + if(deviceName == "OAK-D-SR-POE") { + ROS_WARN("OAK-D-SR-POE device detected. Pipeline types other than StereoToF/ToF/RGBToF might not work without reconfiguration."); + } if(sensorNum == 1) { if(pType != PipelineType::RGB) { ROS_ERROR("Invalid pipeline chosen for camera as it has only one sensor. Switching to RGB."); @@ -68,4 +111,4 @@ std::string PipelineGenerator::validatePipeline(const std::string& typeStr, int } } // namespace pipeline_gen -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver diff --git a/depthai_ros_driver/src/utils.cpp b/depthai_ros_driver/src/utils.cpp index d4813e55..e8a151b3 100644 --- a/depthai_ros_driver/src/utils.cpp +++ b/depthai_ros_driver/src/utils.cpp @@ -1,8 +1,8 @@ #include "depthai_ros_driver/utils.hpp" -#include "depthai-shared/common/CameraBoardSocket.hpp" -#include "depthai-shared/common/CameraFeatures.hpp" -#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/node/XLinkOut.hpp" +#include "depthai_ros_driver/utils.hpp" namespace depthai_ros_driver { namespace utils { std::string getUpperCaseStr(const std::string& string) { @@ -10,8 +10,13 @@ std::string getUpperCaseStr(const std::string& string) { for(auto& c : upper) c = toupper(c); return upper; } -std::string getSocketName(dai::CameraBoardSocket socket) { - return dai_nodes::sensor_helpers::socketNameMap.at(socket); -} +std::shared_ptr setupXout(std::shared_ptr pipeline, const std::string& name) { + auto xout = pipeline->create(); + xout->setStreamName(name); + xout->input.setBlocking(false); + xout->input.setWaitForMessage(false); + xout->input.setQueueSize(1); + return xout; +}; } // namespace utils -} // namespace depthai_ros_driver \ No newline at end of file +} // namespace depthai_ros_driver diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index 8631c783..b70af73a 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.9.0) +project (depthai_ros_msgs VERSION 2.10.1) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) @@ -24,6 +24,7 @@ find_package (catkin REQUIRED COMPONENTS add_message_files ( FILES AutoFocusCtrl.msg + FFMPEGPacket.msg HandLandmark.msg HandLandmarkArray.msg # ImageMarker.msg @@ -31,6 +32,8 @@ add_message_files ( SpatialDetection.msg SpatialDetectionArray.msg ImuWithMagneticField.msg + TrackDetection2D.msg + TrackDetection2DArray.msg TrackedFeature.msg TrackedFeatures.msg ) diff --git a/depthai_ros_msgs/msg/FFMPEGPacket.msg b/depthai_ros_msgs/msg/FFMPEGPacket.msg new file mode 100644 index 00000000..ae4323ec --- /dev/null +++ b/depthai_ros_msgs/msg/FFMPEGPacket.msg @@ -0,0 +1,8 @@ +std_msgs/Header header +int32 width # original image width +int32 height # original image height +string encoding # encoding used +uint64 pts # packet pts +uint8 flags # packet flags +bool is_bigendian # true if machine stores in big endian format +uint8[] data # ffmpeg compressed payload diff --git a/depthai_ros_msgs/msg/TrackDetection2D.msg b/depthai_ros_msgs/msg/TrackDetection2D.msg new file mode 100644 index 00000000..c300c602 --- /dev/null +++ b/depthai_ros_msgs/msg/TrackDetection2D.msg @@ -0,0 +1,25 @@ + +# Class probabilities +vision_msgs/ObjectHypothesisWithPose[] results + +# 2D bounding box surrounding the object. +vision_msgs/BoundingBox2D bbox + +# If true, this message contains object tracking information. +bool is_tracking + +# ID used for consistency across multiple detection messages. This value will +# likely differ from the id field set in each individual ObjectHypothesis. +# If you set this field, be sure to also set is_tracking to True. +string tracking_id + +# Age: number of frames the object is being tracked +int32 tracking_age + +# Status of the tracking: +# 0 = NEW -> the object is newly added. +# 1 = TRACKED -> the object is being tracked. +# 2 = LOST -> the object gets lost now. The object can be tracked again automatically (long term tracking) +# or by specifying detected object manually (short term and zero term tracking). +# 3 = REMOVED -> the object is removed. +int32 tracking_status diff --git a/depthai_ros_msgs/msg/TrackDetection2DArray.msg b/depthai_ros_msgs/msg/TrackDetection2DArray.msg new file mode 100644 index 00000000..f084d33b --- /dev/null +++ b/depthai_ros_msgs/msg/TrackDetection2DArray.msg @@ -0,0 +1,7 @@ +# A list of 2D tracklets, for a multi-object 2D tracker. + +std_msgs/Header header +# A list of the tracking proposals. +TrackDetection2D[] detections + +## These messages are created as close as possible to the official vision_msgs(http://wiki.ros.org/vision_msgs) diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml index 574cb0b2..ccccce4f 100644 --- a/depthai_ros_msgs/package.xml +++ b/depthai_ros_msgs/package.xml @@ -1,10 +1,10 @@ depthai_ros_msgs - 2.9.0 + 2.10.1 Package to keep interface independent of the driver - Sachin Guruswamy + Adam Serafin Sachin Guruswamy MIT