From ecf772b321435845966f8652adf3df3b7ce71e7a Mon Sep 17 00:00:00 2001 From: Adam Serafin Date: Mon, 26 Jun 2023 11:04:10 +0200 Subject: [PATCH] Ros time update humble (#340) --- .../depthai_bridge/DisparityConverter.hpp | 24 ++++++++++++-- .../include/depthai_bridge/ImageConverter.hpp | 22 +++++++++++++ .../depthai_bridge/ImgDetectionConverter.hpp | 22 +++++++++++++ .../include/depthai_bridge/ImuConverter.hpp | 22 +++++++++++++ .../SpatialDetectionConverter.hpp | 22 +++++++++++++ .../include/depthai_bridge/depthaiUtility.hpp | 20 ++++++++++++ depthai_bridge/src/DisparityConverter.cpp | 7 +++++ depthai_bridge/src/ImageConverter.cpp | 10 ++++++ depthai_bridge/src/ImgDetectionConverter.cpp | 8 ++++- depthai_bridge/src/ImuConverter.cpp | 10 ++++++ .../src/SpatialDetectionConverter.cpp | 10 ++++++ .../launch/stereo_inertial_node.launch.py | 9 +++++- .../src/stereo_inertial_publisher.cpp | 15 +++++++++ .../depthai_filters/detection2d_overlay.hpp | 4 +-- .../depthai_filters/segmentation_overlay.hpp | 4 +-- .../include/depthai_filters/spatial_bb.hpp | 11 +++---- depthai_filters/src/detection2d_overlay.cpp | 4 +-- depthai_filters/src/spatial_bb.cpp | 31 +++++++++---------- depthai_filters/src/utils.cpp | 3 +- .../dai_nodes/nn/detection.hpp | 12 +++---- .../dai_nodes/nn/spatial_detection.hpp | 13 ++++---- .../param_handlers/nn_param_handler.hpp | 1 + .../src/dai_nodes/sensors/imu.cpp | 1 + .../src/dai_nodes/sensors/mono.cpp | 1 + .../src/dai_nodes/sensors/rgb.cpp | 2 ++ .../src/dai_nodes/sensors/sensor_wrapper.cpp | 2 +- depthai_ros_driver/src/dai_nodes/stereo.cpp | 26 ++++++---------- .../src/param_handlers/imu_param_handler.cpp | 1 + .../param_handlers/sensor_param_handler.cpp | 9 +++--- .../param_handlers/stereo_param_handler.cpp | 11 +++---- .../src/pipeline/pipeline_generator.cpp | 2 +- 31 files changed, 261 insertions(+), 78 deletions(-) diff --git a/depthai_bridge/include/depthai_bridge/DisparityConverter.hpp b/depthai_bridge/include/depthai_bridge/DisparityConverter.hpp index 6432520e..568844b6 100644 --- a/depthai_bridge/include/depthai_bridge/DisparityConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/DisparityConverter.hpp @@ -25,17 +25,37 @@ class DisparityConverter { DisparityConverter( const std::string frameName, float focalLength, float baseline = 7.5, float minDepth = 80, float maxDepth = 1100, bool getBaseDeviceTimestamp = false); ~DisparityConverter(); + + /** + * @brief Handles cases in which the ROS time shifts forward or backward + * Should be called at regular intervals or on-change of ROS time, depending + * on monitoring. + * + */ + void updateRosBaseTime(); + + /** + * @brief Commands the converter to automatically update the ROS base time on message conversion based on variable + * + * @param update: bool whether to automatically update the ROS base time on message conversion + */ + void setUpdateRosBaseTimeOnToRosMsg(bool update = true) { + _updateRosBaseTimeOnToRosMsg = update; + } + void toRosMsg(std::shared_ptr inData, std::deque& outImageMsg); DisparityImagePtr toRosMsgPtr(std::shared_ptr inData); - // void toDaiMsg(const DisparityMsgs::DisparityImage& inMsg, dai::ImgFrame& outData); - private: const std::string _frameName = ""; const float _focalLength = 882.2, _baseline = 7.5, _minDepth = 80, _maxDepth; std::chrono::time_point _steadyBaseTime; rclcpp::Time _rosBaseTime; bool _getBaseDeviceTimestamp; + // For handling ROS time shifts and debugging + int64_t _totalNsChange{0}; + // Whether to update the ROS base time on each message conversion + bool _updateRosBaseTimeOnToRosMsg{false}; }; } // namespace ros diff --git a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp index 9dc09523..834f4e05 100644 --- a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp @@ -37,6 +37,24 @@ class ImageConverter { ImageConverter(const std::string frameName, bool interleaved, bool getBaseDeviceTimestamp = false); ~ImageConverter(); ImageConverter(bool interleaved, bool getBaseDeviceTimestamp = false); + + /** + * @brief Handles cases in which the ROS time shifts forward or backward + * Should be called at regular intervals or on-change of ROS time, depending + * on monitoring. + * + */ + void updateRosBaseTime(); + + /** + * @brief Commands the converter to automatically update the ROS base time on message conversion based on variable + * + * @param update: bool whether to automatically update the ROS base time on message conversion + */ + void setUpdateRosBaseTimeOnToRosMsg(bool update = true) { + _updateRosBaseTimeOnToRosMsg = update; + } + void toRosMsgFromBitStream(std::shared_ptr inData, std::deque& outImageMsgs, dai::RawImgFrame::Type type, @@ -73,6 +91,10 @@ class ImageConverter { rclcpp::Time _rosBaseTime; bool _getBaseDeviceTimestamp; + // For handling ROS time shifts and debugging + int64_t _totalNsChange{0}; + // Whether to update the ROS base time on each message conversion + bool _updateRosBaseTimeOnToRosMsg{false}; }; } // namespace ros diff --git a/depthai_bridge/include/depthai_bridge/ImgDetectionConverter.hpp b/depthai_bridge/include/depthai_bridge/ImgDetectionConverter.hpp index ce74a758..e6fe15b3 100644 --- a/depthai_bridge/include/depthai_bridge/ImgDetectionConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImgDetectionConverter.hpp @@ -20,6 +20,24 @@ class ImgDetectionConverter { // DetectionConverter() = default; ImgDetectionConverter(std::string frameName, int width, int height, bool normalized = false, bool getBaseDeviceTimestamp = false); ~ImgDetectionConverter(); + + /** + * @brief Handles cases in which the ROS time shifts forward or backward + * Should be called at regular intervals or on-change of ROS time, depending + * on monitoring. + * + */ + void updateRosBaseTime(); + + /** + * @brief Commands the converter to automatically update the ROS base time on message conversion based on variable + * + * @param update: bool whether to automatically update the ROS base time on message conversion + */ + void setUpdateRosBaseTimeOnToRosMsg(bool update = true) { + _updateRosBaseTimeOnToRosMsg = update; + } + void toRosMsg(std::shared_ptr inNetData, std::deque& opDetectionMsgs); Detection2DArrayPtr toRosMsgPtr(std::shared_ptr inNetData); @@ -31,6 +49,10 @@ class ImgDetectionConverter { std::chrono::time_point _steadyBaseTime; rclcpp::Time _rosBaseTime; bool _getBaseDeviceTimestamp; + // For handling ROS time shifts and debugging + int64_t _totalNsChange{0}; + // Whether to update the ROS base time on each message conversion + bool _updateRosBaseTimeOnToRosMsg{false}; }; /** TODO(sachin): Do we need to have ros msg -> dai bounding box ? diff --git a/depthai_bridge/include/depthai_bridge/ImuConverter.hpp b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp index 74f8a471..cc2b1546 100644 --- a/depthai_bridge/include/depthai_bridge/ImuConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp @@ -35,6 +35,24 @@ class ImuConverter { bool enable_rotation = false, bool getBaseDeviceTimestamp = false); ~ImuConverter(); + + /** + * @brief Handles cases in which the ROS time shifts forward or backward + * Should be called at regular intervals or on-change of ROS time, depending + * on monitoring. + * + */ + void updateRosBaseTime(); + + /** + * @brief Commands the converter to automatically update the ROS base time on message conversion based on variable + * + * @param update: bool whether to automatically update the ROS base time on message conversion + */ + void setUpdateRosBaseTimeOnToRosMsg(bool update = true) { + _updateRosBaseTimeOnToRosMsg = update; + } + void toRosMsg(std::shared_ptr inData, std::deque& outImuMsgs); void toRosDaiMsg(std::shared_ptr inData, std::deque& outImuMsgs); @@ -114,6 +132,10 @@ class ImuConverter { std::chrono::time_point _steadyBaseTime; rclcpp::Time _rosBaseTime; bool _getBaseDeviceTimestamp; + // For handling ROS time shifts and debugging + int64_t _totalNsChange{0}; + // Whether to update the ROS base time on each message conversion + bool _updateRosBaseTimeOnToRosMsg{false}; void fillImuMsg(dai::IMUReportAccelerometer report, ImuMsgs::Imu& msg); void fillImuMsg(dai::IMUReportGyroscope report, ImuMsgs::Imu& msg); diff --git a/depthai_bridge/include/depthai_bridge/SpatialDetectionConverter.hpp b/depthai_bridge/include/depthai_bridge/SpatialDetectionConverter.hpp index 76285987..f9eab50e 100644 --- a/depthai_bridge/include/depthai_bridge/SpatialDetectionConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/SpatialDetectionConverter.hpp @@ -20,6 +20,24 @@ class SpatialDetectionConverter { public: SpatialDetectionConverter(std::string frameName, int width, int height, bool normalized = false, bool getBaseDeviceTimestamp = false); ~SpatialDetectionConverter(); + + /** + * @brief Handles cases in which the ROS time shifts forward or backward + * Should be called at regular intervals or on-change of ROS time, depending + * on monitoring. + * + */ + void updateRosBaseTime(); + + /** + * @brief Commands the converter to automatically update the ROS base time on message conversion based on variable + * + * @param update: bool whether to automatically update the ROS base time on message conversion + */ + void setUpdateRosBaseTimeOnToRosMsg(bool update = true) { + _updateRosBaseTimeOnToRosMsg = update; + } + void toRosMsg(std::shared_ptr inNetData, std::deque& opDetectionMsg); void toRosVisionMsg(std::shared_ptr inNetData, std::deque& opDetectionMsg); @@ -33,6 +51,10 @@ class SpatialDetectionConverter { rclcpp::Time _rosBaseTime; bool _getBaseDeviceTimestamp; + // For handling ROS time shifts and debugging + int64_t _totalNsChange{0}; + // Whether to update the ROS base time on each message conversion + bool _updateRosBaseTimeOnToRosMsg{false}; }; /** TODO(sachin): Do we need to have ros msg -> dai bounding box ? diff --git a/depthai_bridge/include/depthai_bridge/depthaiUtility.hpp b/depthai_bridge/include/depthai_bridge/depthaiUtility.hpp index cda9132c..c2d315a8 100644 --- a/depthai_bridge/include/depthai_bridge/depthaiUtility.hpp +++ b/depthai_bridge/include/depthai_bridge/depthaiUtility.hpp @@ -74,6 +74,8 @@ enum LogLevel { DEBUG, INFO, WARN, ERROR, FATAL }; #define DEPTHAI_ROS_FATAL_STREAM_ONCE(loggerName, args) DEPTHAI_ROS_LOG_STREAM(loggerName, dai::ros::LogLevel::FATAL, true, args) +static const int64_t ZERO_TIME_DELTA_NS{100}; + inline rclcpp::Time getFrameTime(rclcpp::Time rclBaseTime, std::chrono::time_point steadyBaseTime, std::chrono::time_point currTimePoint) { @@ -84,5 +86,23 @@ inline rclcpp::Time getFrameTime(rclcpp::Time rclBaseTime, return rclStamp; } +inline void updateBaseTime(std::chrono::time_point steadyBaseTime, rclcpp::Time rclBaseTime, int64_t& totalNsChange) { + rclcpp::Time currentRosTime = rclcpp::Clock().now(); + std::chrono::time_point currentSteadyTime = std::chrono::steady_clock::now(); + // In nanoseconds + auto expectedOffset = std::chrono::duration_cast(currentSteadyTime - steadyBaseTime).count(); + uint64_t previousBaseTimeNs = rclBaseTime.nanoseconds(); + rclBaseTime = rclcpp::Time(currentRosTime.nanoseconds() - expectedOffset); + uint64_t newBaseTimeNs = rclBaseTime.nanoseconds(); + int64_t diff = static_cast(newBaseTimeNs - previousBaseTimeNs); + totalNsChange += diff; + if(::abs(diff) > ZERO_TIME_DELTA_NS) { + // Has been updated + DEPTHAI_ROS_DEBUG_STREAM("ROS BASE TIME CHANGE: ", + "ROS base time changed by " << std::to_string(diff) << " ns. Total change: " << std::to_string(totalNsChange) + << " ns. New time: " << std::to_string(rclBaseTime.nanoseconds()) << " ns."); + } +} + } // namespace ros } // namespace dai \ No newline at end of file diff --git a/depthai_bridge/src/DisparityConverter.cpp b/depthai_bridge/src/DisparityConverter.cpp index 90367591..107e4e1d 100644 --- a/depthai_bridge/src/DisparityConverter.cpp +++ b/depthai_bridge/src/DisparityConverter.cpp @@ -21,7 +21,14 @@ DisparityConverter::DisparityConverter( DisparityConverter::~DisparityConverter() = default; +void DisparityConverter::updateRosBaseTime() { + updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange); +} + void DisparityConverter::toRosMsg(std::shared_ptr inData, std::deque& outDispImageMsgs) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) tstamp = inData->getTimestampDevice(); diff --git a/depthai_bridge/src/ImageConverter.cpp b/depthai_bridge/src/ImageConverter.cpp index 6025663c..3ab623b8 100644 --- a/depthai_bridge/src/ImageConverter.cpp +++ b/depthai_bridge/src/ImageConverter.cpp @@ -36,10 +36,17 @@ ImageConverter::ImageConverter(const std::string frameName, bool interleaved, bo ImageConverter::~ImageConverter() = default; +void ImageConverter::updateRosBaseTime() { + updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange); +} + void ImageConverter::toRosMsgFromBitStream(std::shared_ptr inData, std::deque& outImageMsgs, dai::RawImgFrame::Type type, const sensor_msgs::msg::CameraInfo& info) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) tstamp = inData->getTimestampDevice(); @@ -95,6 +102,9 @@ void ImageConverter::toRosMsgFromBitStream(std::shared_ptr inData } void ImageConverter::toRosMsg(std::shared_ptr inData, std::deque& outImageMsgs) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) tstamp = inData->getTimestampDevice(); diff --git a/depthai_bridge/src/ImgDetectionConverter.cpp b/depthai_bridge/src/ImgDetectionConverter.cpp index 4e89eb48..d631c88e 100644 --- a/depthai_bridge/src/ImgDetectionConverter.cpp +++ b/depthai_bridge/src/ImgDetectionConverter.cpp @@ -18,8 +18,14 @@ ImgDetectionConverter::ImgDetectionConverter(std::string frameName, int width, i ImgDetectionConverter::~ImgDetectionConverter() = default; +void ImgDetectionConverter::updateRosBaseTime() { + updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange); +} + void ImgDetectionConverter::toRosMsg(std::shared_ptr inNetData, std::deque& opDetectionMsgs) { - // setting the header + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) tstamp = inNetData->getTimestampDevice(); diff --git a/depthai_bridge/src/ImuConverter.cpp b/depthai_bridge/src/ImuConverter.cpp index 54eea829..84771132 100644 --- a/depthai_bridge/src/ImuConverter.cpp +++ b/depthai_bridge/src/ImuConverter.cpp @@ -30,6 +30,10 @@ ImuConverter::ImuConverter(const std::string& frameName, ImuConverter::~ImuConverter() = default; +void ImuConverter::updateRosBaseTime() { + updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange); +} + void ImuConverter::fillImuMsg(dai::IMUReportAccelerometer report, ImuMsgs::Imu& msg) { msg.linear_acceleration.x = report.x; msg.linear_acceleration.y = report.y; @@ -84,6 +88,9 @@ void ImuConverter::fillImuMsg(dai::IMUReportMagneticField report, depthai_ros_ms } void ImuConverter::toRosMsg(std::shared_ptr inData, std::deque& outImuMsgs) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } if(_syncMode != ImuSyncMethod::COPY) { FillImuData_LinearInterpolation(inData->packets, outImuMsgs); } else { @@ -106,6 +113,9 @@ void ImuConverter::toRosMsg(std::shared_ptr inData, std::deque inData, std::deque& outImuMsgs) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } if(_syncMode != ImuSyncMethod::COPY) { FillImuData_LinearInterpolation(inData->packets, outImuMsgs); } else { diff --git a/depthai_bridge/src/SpatialDetectionConverter.cpp b/depthai_bridge/src/SpatialDetectionConverter.cpp index 5ebd1391..bf5ca47d 100644 --- a/depthai_bridge/src/SpatialDetectionConverter.cpp +++ b/depthai_bridge/src/SpatialDetectionConverter.cpp @@ -17,8 +17,15 @@ SpatialDetectionConverter::SpatialDetectionConverter(std::string frameName, int SpatialDetectionConverter::~SpatialDetectionConverter() = default; +void SpatialDetectionConverter::updateRosBaseTime() { + updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange); +} + void SpatialDetectionConverter::toRosMsg(std::shared_ptr inNetData, std::deque& opDetectionMsgs) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) tstamp = inNetData->getTimestampDevice(); @@ -87,6 +94,9 @@ SpatialDetectionArrayPtr SpatialDetectionConverter::toRosMsgPtr(std::shared_ptr< void SpatialDetectionConverter::toRosVisionMsg(std::shared_ptr inNetData, std::deque& opDetectionMsgs) { + if(_updateRosBaseTimeOnToRosMsg) { + updateRosBaseTime(); + } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) tstamp = inNetData->getTimestampDevice(); diff --git a/depthai_examples/launch/stereo_inertial_node.launch.py b/depthai_examples/launch/stereo_inertial_node.launch.py index ee5f24c7..e6febeeb 100644 --- a/depthai_examples/launch/stereo_inertial_node.launch.py +++ b/depthai_examples/launch/stereo_inertial_node.launch.py @@ -75,6 +75,7 @@ def generate_launch_description(): enableFloodLight = LaunchConfiguration('enableFloodLight', default = False) dotProjectormA = LaunchConfiguration('dotProjectormA', default = 200.0) floodLightmA = LaunchConfiguration('floodLightmA', default = 200.0) + enableRosBaseTimeUpdate = LaunchConfiguration('enableRosBaseTimeUpdate', default = False) enableRviz = LaunchConfiguration('enableRviz', default = True) @@ -294,6 +295,11 @@ def generate_launch_description(): 'floodLightmA', default_value=floodLightmA, description='Set the mA at which you intend to drive the FloodLight. Default is set to 200mA.') + declare_enableRosBaseTimeUpdate_cmd = DeclareLaunchArgument( + 'enableRosBaseTimeUpdate', + default_value=enableRosBaseTimeUpdate, + description='Whether to update ROS time on each message.') + declare_enableRviz_cmd = DeclareLaunchArgument( 'enableRviz', @@ -359,7 +365,8 @@ def generate_launch_description(): {'enableDotProjector': enableDotProjector}, {'enableFloodLight': enableFloodLight}, {'dotProjectormA': dotProjectormA}, - {'floodLightmA': floodLightmA} + {'floodLightmA': floodLightmA}, + {'enableRosBaseTimeUpdate': enableRosBaseTimeUpdate} ]) depth_metric_converter = launch_ros.descriptions.ComposableNode( diff --git a/depthai_examples/src/stereo_inertial_publisher.cpp b/depthai_examples/src/stereo_inertial_publisher.cpp index 7a0cb155..08a60e55 100644 --- a/depthai_examples/src/stereo_inertial_publisher.cpp +++ b/depthai_examples/src/stereo_inertial_publisher.cpp @@ -296,6 +296,7 @@ int main(int argc, char** argv) { bool usb2Mode, poeMode, syncNN; double angularVelCovariance, linearAccelCovariance; double dotProjectormA, floodLightmA; + bool enableRosBaseTimeUpdate; std::string nnName(BLOB_NAME); // Set your blob name for the model here node->declare_parameter("mxId", ""); @@ -338,6 +339,7 @@ int main(int argc, char** argv) { node->declare_parameter("enableFloodLight", false); node->declare_parameter("dotProjectormA", 200.0); node->declare_parameter("floodLightmA", 200.0); + node->declare_parameter("enableRosBaseTimeUpdate", false); // updating parameters if defined in launch file. @@ -380,6 +382,7 @@ int main(int argc, char** argv) { node->get_parameter("enableFloodLight", enableFloodLight); node->get_parameter("dotProjectormA", dotProjectormA); node->get_parameter("floodLightmA", floodLightmA); + node->get_parameter("enableRosBaseTimeUpdate", enableRosBaseTimeUpdate); if(resourceBaseFolder.empty()) { throw std::runtime_error("Send the path to the resouce folder containing NNBlob in \'resourceBaseFolder\' "); @@ -491,11 +494,20 @@ int main(int argc, char** argv) { } dai::rosBridge::ImageConverter converter(tfPrefix + "_left_camera_optical_frame", true); + if(enableRosBaseTimeUpdate) { + converter.setUpdateRosBaseTimeOnToRosMsg(); + } dai::rosBridge::ImageConverter rightconverter(tfPrefix + "_right_camera_optical_frame", true); + if(enableRosBaseTimeUpdate) { + rightconverter.setUpdateRosBaseTimeOnToRosMsg(); + } const std::string leftPubName = rectify ? std::string("left/image_rect") : std::string("left/image_raw"); const std::string rightPubName = rectify ? std::string("right/image_rect") : std::string("right/image_raw"); dai::rosBridge::ImuConverter imuConverter(tfPrefix + "_imu_frame", imuMode, linearAccelCovariance, angularVelCovariance); + if(enableRosBaseTimeUpdate) { + imuConverter.setUpdateRosBaseTimeOnToRosMsg(); + } dai::rosBridge::BridgePublisher imuPublish( imuQueue, node, @@ -513,6 +525,9 @@ int main(int argc, char** argv) { // const std::string rightPubName = rectify ? std::string("right/image_rect") : std::string("right/image_raw"); dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false); + if(enableRosBaseTimeUpdate) { + rgbConverter.setUpdateRosBaseTimeOnToRosMsg(); + } if(enableDepth) { auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height); auto depthCameraInfo = diff --git a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp index de080f7a..2b79ef75 100644 --- a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp +++ b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp @@ -23,8 +23,8 @@ class Detection2DOverlay : public rclcpp::Node { std::unique_ptr> sync; rclcpp::Publisher::SharedPtr overlayPub; std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", - "car", "cat", "chair", "cow", "diningtable", "dog", "horse", - "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; }; } // namespace depthai_filters \ No newline at end of file diff --git a/depthai_filters/include/depthai_filters/segmentation_overlay.hpp b/depthai_filters/include/depthai_filters/segmentation_overlay.hpp index e8dbe781..3ddc7a91 100644 --- a/depthai_filters/include/depthai_filters/segmentation_overlay.hpp +++ b/depthai_filters/include/depthai_filters/segmentation_overlay.hpp @@ -21,8 +21,8 @@ class SegmentationOverlay : public rclcpp::Node { std::unique_ptr> sync; rclcpp::Publisher::SharedPtr overlayPub; std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", - "car", "cat", "chair", "cow", "diningtable", "dog", "horse", - "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; }; } // namespace depthai_filters \ No newline at end of file diff --git a/depthai_filters/include/depthai_filters/spatial_bb.hpp b/depthai_filters/include/depthai_filters/spatial_bb.hpp index 8b2385d4..2eaf92d3 100644 --- a/depthai_filters/include/depthai_filters/spatial_bb.hpp +++ b/depthai_filters/include/depthai_filters/spatial_bb.hpp @@ -1,6 +1,5 @@ #pragma once -#include "visualization_msgs/msg/marker_array.hpp" #include "message_filters/subscriber.h" #include "message_filters/sync_policies/approximate_time.h" #include "message_filters/synchronizer.h" @@ -8,6 +7,7 @@ #include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/image.hpp" #include "vision_msgs/msg/detection3_d_array.hpp" +#include "visualization_msgs/msg/marker_array.hpp" namespace depthai_filters { class SpatialBB : public rclcpp::Node { @@ -23,15 +23,14 @@ class SpatialBB : public rclcpp::Node { message_filters::Subscriber detSub; message_filters::Subscriber infoSub; - typedef message_filters::sync_policies:: - ApproximateTime - syncPolicy; + typedef message_filters::sync_policies::ApproximateTime + syncPolicy; std::unique_ptr> sync; rclcpp::Publisher::SharedPtr markerPub; rclcpp::Publisher::SharedPtr overlayPub; std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", - "car", "cat", "chair", "cow", "diningtable", "dog", "horse", - "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; bool desqueeze = false; }; diff --git a/depthai_filters/src/detection2d_overlay.cpp b/depthai_filters/src/detection2d_overlay.cpp index 0f2c5ac0..8ad65b36 100644 --- a/depthai_filters/src/detection2d_overlay.cpp +++ b/depthai_filters/src/detection2d_overlay.cpp @@ -30,10 +30,10 @@ void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr auto y2 = detection.bbox.center.position.y + detections->detections[0].bbox.size_y / 2.0; auto labelStr = labelMap[stoi(detection.results[0].hypothesis.class_id)]; auto confidence = detection.results[0].hypothesis.score; - utils::addTextToFrame(previewMat, labelStr, x1+10, y1+20); + utils::addTextToFrame(previewMat, labelStr, x1 + 10, y1 + 20); std::stringstream confStr; confStr << std::fixed << std::setprecision(2) << confidence * 100; - utils::addTextToFrame(previewMat, confStr.str(), x1+10, y1+40); + utils::addTextToFrame(previewMat, confStr.str(), x1 + 10, y1 + 40); cv::rectangle(previewMat, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), blue); } sensor_msgs::msg::Image outMsg; diff --git a/depthai_filters/src/spatial_bb.cpp b/depthai_filters/src/spatial_bb.cpp index f2c41d8d..5a3a7f0f 100644 --- a/depthai_filters/src/spatial_bb.cpp +++ b/depthai_filters/src/spatial_bb.cpp @@ -15,8 +15,7 @@ void SpatialBB::onInit() { infoSub.subscribe(this, "stereo/camera_info"); detSub.subscribe(this, "nn/spatial_detections"); sync = std::make_unique>(syncPolicy(10), previewSub, infoSub, detSub); - sync->registerCallback( - std::bind(&SpatialBB::overlayCB, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + sync->registerCallback(std::bind(&SpatialBB::overlayCB, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); markerPub = this->create_publisher("spatial_bb", 10); overlayPub = this->create_publisher("overlay", 10); desqueeze = this->declare_parameter("desqueeze", false); @@ -24,19 +23,18 @@ void SpatialBB::onInit() { } void SpatialBB::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, - const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info, - const vision_msgs::msg::Detection3DArray::ConstSharedPtr& detections) { + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info, + const vision_msgs::msg::Detection3DArray::ConstSharedPtr& detections) { cv::Mat previewMat = utils::msgToMat(this->get_logger(), preview, sensor_msgs::image_encodings::BGR8); auto blue = cv::Scalar(255, 0, 0); double ratioY = double(info->height) / double(previewMat.rows); double ratioX; int offsetX; - if(desqueeze){ + if(desqueeze) { ratioX = double(info->width) / double(previewMat.cols); - offsetX=0; - } - else{ + offsetX = 0; + } else { ratioX = ratioY; offsetX = (info->width - info->height) / 2.0; } @@ -55,29 +53,28 @@ void SpatialBB::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview cv::rectangle(previewMat, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), blue); auto labelStr = labelMap[stoi(detection.results[0].hypothesis.class_id)]; - utils::addTextToFrame(previewMat, labelStr, x1+10, y1+10); + utils::addTextToFrame(previewMat, labelStr, x1 + 10, y1 + 10); auto confidence = detection.results[0].hypothesis.score; std::stringstream confStr; confStr << std::fixed << std::setprecision(2) << confidence * 100; - utils::addTextToFrame(previewMat, confStr.str(), x1+10, y1+40); + utils::addTextToFrame(previewMat, confStr.str(), x1 + 10, y1 + 40); std::stringstream depthX; depthX << "X: " << detection.results[0].pose.pose.position.x << " mm"; - utils::addTextToFrame(previewMat, depthX.str(), x1+10, y1+60); - + utils::addTextToFrame(previewMat, depthX.str(), x1 + 10, y1 + 60); + std::stringstream depthY; - depthY << "Y: " << detection.results[0].pose.pose.position.y << " mm"; - utils::addTextToFrame(previewMat, depthY.str(), x1+10, y1+75); + depthY << "Y: " << detection.results[0].pose.pose.position.y << " mm"; + utils::addTextToFrame(previewMat, depthY.str(), x1 + 10, y1 + 75); std::stringstream depthZ; - depthZ << "Z: " << detection.results[0].pose.pose.position.z << " mm"; - utils::addTextToFrame(previewMat, depthZ.str(), x1+10, y1+90); + depthZ << "Z: " << detection.results[0].pose.pose.position.z << " mm"; + utils::addTextToFrame(previewMat, depthZ.str(), x1 + 10, y1 + 90); // Marker publishing const auto& bbox = detection.bbox; auto bbox_size_x = bbox.size.x * ratioX; auto bbox_size_y = bbox.size.y * ratioY; - auto bbox_center_x = bbox.center.position.x * ratioX + offsetX; auto bbox_center_y = bbox.center.position.y * ratioY + offsetY; visualization_msgs::msg::Marker box_marker; diff --git a/depthai_filters/src/utils.cpp b/depthai_filters/src/utils.cpp index 4fa097b5..1e6b9cea 100644 --- a/depthai_filters/src/utils.cpp +++ b/depthai_filters/src/utils.cpp @@ -14,13 +14,12 @@ cv::Mat msgToMat(const rclcpp::Logger& logger, const sensor_msgs::msg::Image::Co } return mat; } -void addTextToFrame(cv::Mat &frame, const std::string &text, int x, int y){ +void addTextToFrame(cv::Mat& frame, const std::string& text, int x, int y) { auto white = cv::Scalar(255, 255, 255); auto black = cv::Scalar(0, 0, 0); cv::putText(frame, text, cv::Point(x, y), cv::FONT_HERSHEY_TRIPLEX, 0.5, white, 3); cv::putText(frame, text, cv::Point(x, y), cv::FONT_HERSHEY_TRIPLEX, 0.5, black); - } } // namespace utils } // namespace depthai_filters \ No newline at end of file diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp index fbaeb3cc..26728ad9 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp @@ -48,16 +48,13 @@ class Detection : public BaseNode { if(ph->getParam("i_disable_resize")) { width = getROSNode()->get_parameter("rgb.i_preview_size").as_int(); height = getROSNode()->get_parameter("rgb.i_preview_size").as_int(); - } - else{ + } else { width = imageManip->initialConfig.getResizeConfig().width; height = imageManip->initialConfig.getResizeConfig().height; } - detConverter = std::make_unique(tfPrefix + "_camera_optical_frame", - width, - height, - false, - ph->getParam("i_get_base_device_timestamp")); + detConverter = std::make_unique( + tfPrefix + "_camera_optical_frame", width, height, false, ph->getParam("i_get_base_device_timestamp")); + detConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); rclcpp::PublisherOptions options; options.qos_overriding_options = rclcpp::QosOverridingOptions(); detPub = getROSNode()->template create_publisher("~/" + getName() + "/detections", 10, options); @@ -66,6 +63,7 @@ class Detection : public BaseNode { if(ph->getParam("i_enable_passthrough")) { ptQ = device->getOutputQueue(ptQName, ph->getParam("i_max_q_size"), false); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); + imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); infoManager = std::make_shared( getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); infoManager->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), 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 4cf8e620..5cba6324 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp @@ -50,16 +50,13 @@ class SpatialDetection : public BaseNode { if(ph->getParam("i_disable_resize")) { width = getROSNode()->get_parameter("rgb.i_preview_size").as_int(); height = getROSNode()->get_parameter("rgb.i_preview_size").as_int(); - } - else{ + } else { width = imageManip->initialConfig.getResizeConfig().width; height = imageManip->initialConfig.getResizeConfig().height; } - detConverter = std::make_unique(tfPrefix + "_camera_optical_frame", - width, - height, - false, - ph->getParam("i_get_base_device_timestamp")); + detConverter = std::make_unique( + tfPrefix + "_camera_optical_frame", width, height, false, ph->getParam("i_get_base_device_timestamp")); + detConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); nnQ->addCallback(std::bind(&SpatialDetection::spatialCB, this, std::placeholders::_1, std::placeholders::_2)); rclcpp::PublisherOptions options; options.qos_overriding_options = rclcpp::QosOverridingOptions(); @@ -68,6 +65,7 @@ class SpatialDetection : public BaseNode { if(ph->getParam("i_enable_passthrough")) { ptQ = device->getOutputQueue(ptQName, ph->getParam("i_max_q_size"), false); ptImageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); + ptImageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); ptInfoMan = std::make_shared( getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); ptInfoMan->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), @@ -88,6 +86,7 @@ class SpatialDetection : public BaseNode { }; ptDepthQ = device->getOutputQueue(ptDepthQName, ph->getParam("i_max_q_size"), false); ptDepthImageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); + ptDepthImageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); ptDepthInfoMan = std::make_shared( getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); ptDepthInfoMan->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp index 3dec9c08..2fc65c79 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp @@ -42,6 +42,7 @@ class NNParamHandler : public BaseParamHandler { declareAndLogParam("i_enable_passthrough", false); declareAndLogParam("i_enable_passthrough_depth", false); declareAndLogParam("i_get_base_device_timestamp", false); + declareAndLogParam("i_update_ros_base_time_on_ros_msg", false); auto nn_path = getParam("i_nn_config_path"); using json = nlohmann::json; std::ifstream f(nn_path); diff --git a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp index cee348cd..78f6cc7a 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp @@ -48,6 +48,7 @@ void Imu::setupQueues(std::shared_ptr device) { ph->getParam("i_mag_cov"), ph->getParam("i_enable_rotation"), ph->getParam("i_get_base_device_timestamp")); + imuConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); param_handlers::imu::ImuMsgType msgType = ph->getMsgType(); switch(msgType) { case param_handlers::imu::ImuMsgType::IMU: { diff --git a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp index 66de75a1..7266a08b 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/mono.cpp @@ -60,6 +60,7 @@ void Mono::setupQueues(std::shared_ptr device) { auto tfPrefix = getTFPrefix(getName()); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); + imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); monoPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw"); infoManager = std::make_shared( getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); diff --git a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp index 2ac136a5..52425001 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp @@ -73,6 +73,7 @@ void RGB::setupQueues(std::shared_ptr device) { getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); + imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); if(ph->getParam("i_calibration_file").empty()) { infoManager->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), *imageConverter, @@ -104,6 +105,7 @@ void RGB::setupQueues(std::shared_ptr device) { getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + previewQName).get(), previewQName); auto tfPrefix = getTFPrefix(getName()); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); + imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); if(ph->getParam("i_calibration_file").empty()) { previewInfoManager->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(), *imageConverter, diff --git a/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp b/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp index 93b68385..06566462 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp @@ -63,7 +63,7 @@ SensorWrapper::SensorWrapper(const std::string& daiNodeName, } SensorWrapper::~SensorWrapper() = default; -sensor_helpers::ImageSensor SensorWrapper::getSensorData(){ +sensor_helpers::ImageSensor SensorWrapper::getSensorData() { return sensorData; } diff --git a/depthai_ros_driver/src/dai_nodes/stereo.cpp b/depthai_ros_driver/src/dai_nodes/stereo.cpp index b8457ba4..6e087ccf 100644 --- a/depthai_ros_driver/src/dai_nodes/stereo.cpp +++ b/depthai_ros_driver/src/dai_nodes/stereo.cpp @@ -90,6 +90,7 @@ void Stereo::setupLeftRectQueue(std::shared_ptr device) { leftRectQ = device->getOutputQueue(leftRectQName, ph->getOtherNodeParam(leftSensInfo.name, "i_max_q_size"), false); auto tfPrefix = getTFPrefix(leftSensInfo.name); leftRectConv = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); + leftRectConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); leftRectIM = std::make_shared( getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + leftSensInfo.name).get(), "/rect"); auto info = sensor_helpers::getCalibInfo(getROSNode()->get_logger(), @@ -101,17 +102,12 @@ void Stereo::setupLeftRectQueue(std::shared_ptr device) { leftRectIM->setCameraInfo(info); leftRectPub = image_transport::create_camera_publisher(getROSNode(), "~/" + leftSensInfo.name + "/image_rect"); dai::RawImgFrame::Type encType = dai::RawImgFrame::Type::GRAY8; - if(left->getSensorData().color){ + if(left->getSensorData().color) { encType = dai::RawImgFrame::Type::BGR888i; } if(ph->getParam("i_left_rect_low_bandwidth")) { - leftRectQ->addCallback(std::bind(sensor_helpers::compressedImgCB, - std::placeholders::_1, - std::placeholders::_2, - *leftRectConv, - leftRectPub, - leftRectIM, - encType)); + leftRectQ->addCallback( + std::bind(sensor_helpers::compressedImgCB, std::placeholders::_1, std::placeholders::_2, *leftRectConv, leftRectPub, leftRectIM, encType)); } else { leftRectQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *leftRectConv, leftRectPub, leftRectIM)); } @@ -121,6 +117,7 @@ void Stereo::setupRightRectQueue(std::shared_ptr device) { rightRectQ = device->getOutputQueue(rightRectQName, ph->getOtherNodeParam(rightSensInfo.name, "i_max_q_size"), false); auto tfPrefix = getTFPrefix(rightSensInfo.name); rightRectConv = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); + rightRectConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); rightRectIM = std::make_shared( getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + rightSensInfo.name).get(), "/rect"); auto info = sensor_helpers::getCalibInfo(getROSNode()->get_logger(), @@ -132,17 +129,12 @@ void Stereo::setupRightRectQueue(std::shared_ptr device) { rightRectIM->setCameraInfo(info); rightRectPub = image_transport::create_camera_publisher(getROSNode(), "~/" + rightSensInfo.name + "/image_rect"); dai::RawImgFrame::Type encType = dai::RawImgFrame::Type::GRAY8; - if(right->getSensorData().color){ + if(right->getSensorData().color) { encType = dai::RawImgFrame::Type::BGR888i; } if(ph->getParam("i_right_rect_low_bandwidth")) { - rightRectQ->addCallback(std::bind(sensor_helpers::compressedImgCB, - std::placeholders::_1, - std::placeholders::_2, - *rightRectConv, - rightRectPub, - rightRectIM, - encType)); + rightRectQ->addCallback( + std::bind(sensor_helpers::compressedImgCB, std::placeholders::_1, std::placeholders::_2, *rightRectConv, rightRectPub, rightRectIM, encType)); } else { rightRectQ->addCallback(std::bind(sensor_helpers::imgCB, std::placeholders::_1, std::placeholders::_2, *rightRectConv, rightRectPub, rightRectIM)); } @@ -157,7 +149,7 @@ void Stereo::setupStereoQueue(std::shared_ptr device) { tfPrefix = getTFPrefix(rightSensInfo.name); } stereoConv = std::make_unique(tfPrefix + "_camera_optical_frame", false, ph->getParam("i_get_base_device_timestamp")); - + stereoConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam("i_update_ros_base_time_on_ros_msg")); stereoPub = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw"); stereoIM = std::make_shared( getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); diff --git a/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp b/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp index 879817e9..7dcdb3b1 100644 --- a/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/imu_param_handler.cpp @@ -25,6 +25,7 @@ void ImuParamHandler::declareParams(std::shared_ptr imu, const s declareAndLogParam("i_gyro_cov", 0.0); declareAndLogParam("i_rot_cov", -1.0); declareAndLogParam("i_mag_cov", 0.0); + declareAndLogParam("i_update_ros_base_time_on_ros_msg", false); bool rotationAvailable = imuType == "BNO086"; if(declareAndLogParam("i_enable_rotation", false)) { if(rotationAvailable) { diff --git a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp index c05a4b50..18a75206 100644 --- a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp @@ -26,6 +26,7 @@ void SensorParamHandler::declareCommonParams() { declareAndLogParam("i_disable_node", false); declareAndLogParam("i_get_base_device_timestamp", false); declareAndLogParam("i_board_socket_id", 0); + declareAndLogParam("i_update_ros_base_time_on_ros_msg", false); fSyncModeMap = { {"OFF", dai::CameraControl::FrameSyncMode::OFF}, {"OUTPUT", dai::CameraControl::FrameSyncMode::OUTPUT}, @@ -57,10 +58,10 @@ void SensorParamHandler::declareParams(std::shared_ptr mo if(declareAndLogParam("r_set_man_exposure", false)) { monoCam->initialControl.setManualExposure(exposure, iso); } - if(declareAndLogParam("i_fsync_continuous", false)){ + if(declareAndLogParam("i_fsync_continuous", false)) { monoCam->initialControl.setFrameSyncMode(utils::getValFromMap(declareAndLogParam("i_fsync_mode", "INPUT"), fSyncModeMap)); } - if(declareAndLogParam("i_fsync_trigger", false)){ + if(declareAndLogParam("i_fsync_trigger", false)) { monoCam->initialControl.setExternalTrigger(declareAndLogParam("i_num_frames_burst", 1), declareAndLogParam("i_num_frames_discard", 0)); } } @@ -124,10 +125,10 @@ void SensorParamHandler::declareParams(std::shared_ptr c if(declareAndLogParam("r_set_man_whitebalance", false)) { colorCam->initialControl.setManualWhiteBalance(whitebalance); } - if(declareAndLogParam("i_fsync_continuous", false)){ + if(declareAndLogParam("i_fsync_continuous", false)) { colorCam->initialControl.setFrameSyncMode(utils::getValFromMap(declareAndLogParam("i_fsync_mode", "INPUT"), fSyncModeMap)); } - if(declareAndLogParam("i_fsync_trigger", false)){ + if(declareAndLogParam("i_fsync_trigger", false)) { colorCam->initialControl.setExternalTrigger(declareAndLogParam("i_num_frames_burst", 1), declareAndLogParam("i_num_frames_discard", 0)); } } 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 f334264f..7400a46d 100644 --- a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp @@ -43,8 +43,9 @@ void StereoParamHandler::declareParams(std::shared_ptr s declareAndLogParam("i_low_bandwidth_quality", 50); declareAndLogParam("i_output_disparity", false); declareAndLogParam("i_get_base_device_timestamp", false); + declareAndLogParam("i_update_ros_base_time_on_ros_msg", false); declareAndLogParam("i_publish_topic", true); - + declareAndLogParam("i_publish_left_rect", false); declareAndLogParam("i_left_rect_low_bandwidth", false); declareAndLogParam("i_left_rect_low_bandwidth_quality", 50); @@ -77,7 +78,7 @@ void StereoParamHandler::declareParams(std::shared_ptr s } declareAndLogParam("i_board_socket_id", static_cast(socket)); stereo->setDepthAlign(socket); - + if(declareAndLogParam("i_set_input_size", false)) { stereo->setInputResolution(declareAndLogParam("i_input_width", 1280), declareAndLogParam("i_input_height", 720)); } @@ -93,13 +94,11 @@ void StereoParamHandler::declareParams(std::shared_ptr s stereo->initialConfig.setSubpixel(true); stereo->initialConfig.setSubpixelFractionalBits(declareAndLogParam("i_subpixel_fractional_bits", 3)); } - stereo->setExtendedDisparity(declareAndLogParam("i_extended_disp", false)); stereo->setRectifyEdgeFillColor(declareAndLogParam("i_rectify_edge_fill_color", 0)); auto config = stereo->initialConfig.get(); config.costMatching.disparityWidth = utils::getValFromMap(declareAndLogParam("i_disparity_width", "DISPARITY_96"), disparityWidthMap); - if(!config.algorithmControl.enableExtended) { - config.costMatching.enableCompanding = declareAndLogParam("i_enable_companding", false); - } + stereo->setExtendedDisparity(declareAndLogParam("i_extended_disp", false)); + config.costMatching.enableCompanding = declareAndLogParam("i_enable_companding", false); config.postProcessing.temporalFilter.enable = declareAndLogParam("i_enable_temporal_filter", false); if(config.postProcessing.temporalFilter.enable) { config.postProcessing.temporalFilter.alpha = declareAndLogParam("i_temporal_filter_alpha", 0.4); diff --git a/depthai_ros_driver/src/pipeline/pipeline_generator.cpp b/depthai_ros_driver/src/pipeline/pipeline_generator.cpp index bf0b6747..d9a6be32 100644 --- a/depthai_ros_driver/src/pipeline/pipeline_generator.cpp +++ b/depthai_ros_driver/src/pipeline/pipeline_generator.cpp @@ -38,7 +38,7 @@ std::vector> PipelineGenerator::createPipel } if(enableImu) { - if(device->getConnectedIMU()== "NONE") { + if(device->getConnectedIMU() == "NONE") { RCLCPP_WARN(node->get_logger(), "IMU enabled but not available!"); } else { auto imu = std::make_unique("imu", node, pipeline, device);