From 7935cb0b599640d6a1b6acccf5e143601c90476e Mon Sep 17 00:00:00 2001 From: Adam Serafin Date: Wed, 13 Nov 2024 15:41:16 +0100 Subject: [PATCH] Iron 2.10.5 (#626) --- depthai-ros/CHANGELOG.rst | 10 + depthai-ros/CMakeLists.txt | 2 +- depthai-ros/package.xml | 2 +- depthai_bridge/CMakeLists.txt | 2 +- depthai_bridge/package.xml | 2 +- depthai_descriptions/CMakeLists.txt | 2 +- depthai_descriptions/package.xml | 2 +- depthai_examples/CMakeLists.txt | 2 +- depthai_examples/package.xml | 2 +- depthai_filters/CMakeLists.txt | 2 +- depthai_filters/package.xml | 2 +- depthai_ros_driver/CMakeLists.txt | 2 +- depthai_ros_driver/config/pcl.yaml | 2 +- depthai_ros_driver/launch/camera.launch.py | 4 +- depthai_ros_driver/package.xml | 2 +- .../src/dai_nodes/sensors/img_pub.cpp | 2 +- .../src/dai_nodes/sensors/stereo.cpp | 7 +- depthai_ros_driver/src/dai_nodes/stereo.cpp | 403 ------------------ .../param_handlers/sensor_param_handler.cpp | 64 +++ .../param_handlers/stereo_param_handler.cpp | 4 +- depthai_ros_msgs/CMakeLists.txt | 2 +- depthai_ros_msgs/package.xml | 2 +- 22 files changed, 98 insertions(+), 426 deletions(-) delete mode 100644 depthai_ros_driver/src/dai_nodes/stereo.cpp diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst index 710a9c3d..d7fb88f4 100644 --- a/depthai-ros/CHANGELOG.rst +++ b/depthai-ros/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package depthai-ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.5 (2024-11-12) +------------------- +* Fix low bandwidth stereo publishing + +2.10.4 (2024-11-07) +------------------- +* Fix rectified topic names +* Fix pointcloud launch +* Add sensor parameters for max autoexposure, sharpness, luma and chroma denoise + 2.10.3 (2024-10-14) ------------------- * Allow setting USB speed without specifying device information diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt index 3da4853a..7c913b18 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.10.3 LANGUAGES CXX C) +project(depthai-ros VERSION 2.10.5 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml index 5b9d07ee..4fcf6ffb 100644 --- a/depthai-ros/package.xml +++ b/depthai-ros/package.xml @@ -1,7 +1,7 @@ depthai-ros - 2.10.3 + 2.10.5 The depthai-ros package diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index 82f2ee9f..253f6c38 100644 --- a/depthai_bridge/CMakeLists.txt +++ b/depthai_bridge/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.10.2) # CMake version in Ubuntu 18.04 LTS set(CMAKE_POSITION_INDEPENDENT_CODE ON) -project(depthai_bridge VERSION 2.10.3 LANGUAGES CXX C) +project(depthai_bridge VERSION 2.10.5 LANGUAGES CXX C) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/depthai_bridge/package.xml b/depthai_bridge/package.xml index 124296c3..13bab0c1 100644 --- a/depthai_bridge/package.xml +++ b/depthai_bridge/package.xml @@ -1,7 +1,7 @@ depthai_bridge - 2.10.3 + 2.10.5 The depthai_bridge package Adam Serafin diff --git a/depthai_descriptions/CMakeLists.txt b/depthai_descriptions/CMakeLists.txt index 2d751133..bc03ec82 100644 --- a/depthai_descriptions/CMakeLists.txt +++ b/depthai_descriptions/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(depthai_descriptions VERSION 2.10.3) +project(depthai_descriptions VERSION 2.10.5) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/depthai_descriptions/package.xml b/depthai_descriptions/package.xml index 27f85f05..7128de43 100644 --- a/depthai_descriptions/package.xml +++ b/depthai_descriptions/package.xml @@ -1,7 +1,7 @@ depthai_descriptions - 2.10.3 + 2.10.5 The depthai_descriptions package Adam Serafin diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index 48d76b9a..d4655d93 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.10.3 LANGUAGES CXX C) +project(depthai_examples VERSION 2.10.5 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 879d4861..8d3bf94e 100644 --- a/depthai_examples/package.xml +++ b/depthai_examples/package.xml @@ -1,7 +1,7 @@ depthai_examples - 2.10.3 + 2.10.5 The depthai_examples package diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt index cdac3a5d..e70aa597 100644 --- a/depthai_filters/CMakeLists.txt +++ b/depthai_filters/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(depthai_filters VERSION 2.10.3 LANGUAGES CXX C) +project(depthai_filters VERSION 2.10.5 LANGUAGES CXX C) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/depthai_filters/package.xml b/depthai_filters/package.xml index 1cc2c127..418d56be 100644 --- a/depthai_filters/package.xml +++ b/depthai_filters/package.xml @@ -2,7 +2,7 @@ depthai_filters - 2.10.3 + 2.10.5 Depthai filters package Adam Serafin MIT diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index d17b8a9c..eb993ea6 100644 --- a/depthai_ros_driver/CMakeLists.txt +++ b/depthai_ros_driver/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.22) -project(depthai_ros_driver VERSION 2.10.3) +project(depthai_ros_driver VERSION 2.10.5) set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_BUILD_SHARED_LIBS ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) diff --git a/depthai_ros_driver/config/pcl.yaml b/depthai_ros_driver/config/pcl.yaml index 1ea1fbb1..f51ede76 100644 --- a/depthai_ros_driver/config/pcl.yaml +++ b/depthai_ros_driver/config/pcl.yaml @@ -6,4 +6,4 @@ i_align_depth: true i_board_socket_id: 2 i_subpixel: true - i_publish_right_rect: true \ No newline at end of file + i_right_rect_publish_topic: true diff --git a/depthai_ros_driver/launch/camera.launch.py b/depthai_ros_driver/launch/camera.launch.py index 5f3a8d87..83bb4ca8 100644 --- a/depthai_ros_driver/launch/camera.launch.py +++ b/depthai_ros_driver/launch/camera.launch.py @@ -143,8 +143,8 @@ def launch_setup(context, *args, **kwargs): }, } parameter_overrides["depth"] = { - "i_publish_left_rect": True, - "i_publish_right_rect": True, + "i_left_rect_publish_topic": True, + "i_right_rect_publish_topic": True, } tf_params = {} diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index 7d7f8ee6..372b0b56 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -2,7 +2,7 @@ depthai_ros_driver - 2.10.3 + 2.10.5 Depthai ROS Monolithic node. Adam Serafin diff --git a/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp index 2d861911..7d42ddab 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp @@ -99,7 +99,7 @@ void ImagePublisher::createImageConverter(std::shared_ptr device) { if(convConfig.alphaScalingEnabled) { converter->setAlphaScaling(convConfig.alphaScaling); } - if(convConfig.outputDisparity) { + if(!convConfig.outputDisparity) { auto calHandler = device->readCalibration(); double baseline = calHandler.getBaselineDistance(pubConfig.leftSocket, pubConfig.rightSocket, false); if(convConfig.reverseSocketOrder) { diff --git a/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp b/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp index 1775f9f6..423d7ce8 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp @@ -80,8 +80,9 @@ void Stereo::setNames() { void Stereo::setXinXout(std::shared_ptr pipeline) { bool outputDisparity = ph->getParam("i_output_disparity"); + bool lowBandwidth = ph->getParam("i_low_bandwidth"); std::function stereoLinkChoice; - if(outputDisparity) { + if(outputDisparity || lowBandwidth) { stereoLinkChoice = [&](auto input) { stereoCamNode->disparity.link(input); }; } else { stereoLinkChoice = [&](auto input) { stereoCamNode->depth.link(input); }; @@ -92,7 +93,7 @@ void Stereo::setXinXout(std::shared_ptr pipeline) { 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"); + encConf.enabled = lowBandwidth; stereoPub = setupOutput(pipeline, stereoQName, stereoLinkChoice, ph->getParam("i_synced"), encConf); } @@ -155,7 +156,7 @@ void Stereo::setupRectQueue(std::shared_ptr device, 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.topicSuffix = rsCompabilityMode() ? "/image_rect_raw" : "/image_rect"; pubConfig.maxQSize = ph->getOtherNodeParam(sensorName, "i_max_q_size"); pubConfig.socket = sensorInfo.socket; pubConfig.infoMgrSuffix = "rect"; 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 47b4fe66..00000000 --- a/depthai_ros_driver/src/dai_nodes/stereo.cpp +++ /dev/null @@ -1,403 +0,0 @@ -#include "depthai_ros_driver/dai_nodes/stereo.hpp" - -#include "camera_info_manager/camera_info_manager.hpp" -#include "cv_bridge/cv_bridge.hpp" -#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.hpp" -#include "image_transport/image_transport.hpp" -#include "rclcpp/node.hpp" - -namespace depthai_ros_driver { -namespace dai_nodes { -Stereo::Stereo(const std::string& daiNodeName, - rclcpp::Node* node, - std::shared_ptr pipeline, - std::shared_ptr device, - dai::CameraBoardSocket leftSocket, - dai::CameraBoardSocket rightSocket) - : BaseNode(daiNodeName, node, pipeline) { - RCLCPP_DEBUG(node->get_logger(), "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; - } - } - RCLCPP_DEBUG(node->get_logger(), - "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))); - } - - RCLCPP_DEBUG(node->get_logger(), "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); - 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, - rclcpp::Publisher::SharedPtr pub, - rclcpp::Publisher::SharedPtr infoPub, - 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(getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + sensorName).get(), - "/rect"); - if(ph->getParam("i_reverse_stereo_socket_order")) { - conv->reverseStereoSocketOrder(); - } - auto info = sensor_helpers::getCalibInfo(getROSNode()->get_logger(), - *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"); - - if(ipcEnabled()) { - pub = getROSNode()->create_publisher("~/" + sensorName + "/image_rect", 10); - infoPub = getROSNode()->create_publisher("~/" + getName() + "/camera_info", 10); - if(addCallback) { - q->addCallback(std::bind(sensor_helpers::splitPub, - std::placeholders::_1, - std::placeholders::_2, - *conv, - pub, - infoPub, - im, - ph->getParam("i_enable_lazy_publisher"))); - } - } else { - pubIT = image_transport::create_camera_publisher(getROSNode(), "~/" + sensorName + "/image_rect"); - 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, leftRectPub, leftRectInfoPub, leftRectPubIT, true); -} - -void Stereo::setupRightRectQueue(std::shared_ptr device) { - setupRectQueue(device, rightSensInfo, rightRectQName, rightRectConv, rightRectIM, rightRectQ, rightRectPub, rightRectInfoPub, rightRectPubIT, false); -} - -void Stereo::setupStereoQueue(std::shared_ptr device) { - std::string tfPrefix; - if(ph->getParam("i_align_depth")) { - tfPrefix = getTFPrefix(ph->getParam("i_socket_name")); - } else { - tfPrefix = getTFPrefix(utils::getSocketName(rightSensInfo.socket).c_str()); - } - 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")); - 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( - getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName()); - auto info = sensor_helpers::getCalibInfo(getROSNode()->get_logger(), - *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); - stereoQ = device->getOutputQueue(stereoQName, ph->getParam("i_max_q_size"), false); - if(ipcEnabled()) { - stereoPub = getROSNode()->create_publisher("~/" + getName() + "/image_raw", 10); - stereoInfoPub = getROSNode()->create_publisher("~/" + getName() + "/camera_info", 10); - stereoQ->addCallback(std::bind(sensor_helpers::splitPub, - std::placeholders::_1, - std::placeholders::_2, - *stereoConv, - stereoPub, - stereoInfoPub, - stereoIM, - ph->getParam("i_enable_lazy_publisher"))); - } else { - stereoPubIT = image_transport::create_camera_publisher(getROSNode(), "~/" + getName() + "/image_raw"); - 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()) { - RCLCPP_WARN(getROSNode()->get_logger(), "Left and right rectified frames are not synchronized!"); - } else { - bool lazyPub = ph->getParam("i_enable_lazy_publisher"); - if(ipcEnabled() && rclcpp::ok() - && (!lazyPub || sensor_helpers::detectSubscription(leftRectPub, leftRectInfoPub) - || sensor_helpers::detectSubscription(rightRectPub, rightRectInfoPub))) { - 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; - sensor_msgs::msg::CameraInfo::UniquePtr leftInfoMsg = std::make_unique(leftInfo); - sensor_msgs::msg::Image::UniquePtr leftMsg = std::make_unique(leftRawMsg); - sensor_msgs::msg::CameraInfo::UniquePtr rightInfoMsg = std::make_unique(rightInfo); - sensor_msgs::msg::Image::UniquePtr rightMsg = std::make_unique(rightRawMsg); - leftRectPub->publish(std::move(leftMsg)); - leftRectInfoPub->publish(std::move(leftInfoMsg)); - rightRectPub->publish(std::move(rightMsg)); - rightRectInfoPub->publish(std::move(rightInfoMsg)); - } else if(!ipcEnabled() && rclcpp::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"); - RCLCPP_INFO(getROSNode()->get_logger(), "Setting up stereo pair sync timer with period %d ms based on left sensor FPS.", timerPeriod); - syncTimer = getROSNode()->create_wall_timer(std::chrono::milliseconds(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->cancel(); - 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(const std::vector& params) { - ph->setRuntimeParams(params); -} - -} // namespace dai_nodes -} // 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 0b9fe0dd..cb4bec17 100644 --- a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp @@ -79,6 +79,22 @@ void SensorParamHandler::declareParams(std::shared_ptr mo } monoCam->setImageOrientation( utils::getValFromMap(declareAndLogParam("i_sensor_img_orientation", "AUTO"), dai_nodes::sensor_helpers::cameraImageOrientationMap)); + int expLimit = declareAndLogParam("r_auto_exposure_limit", 1000); + if(declareAndLogParam("r_set_auto_exposure_limit", false)) { + monoCam->initialControl.setAutoExposureLimit(expLimit); + } + int sharpness = declareAndLogParam("r_sharpness", 1); + if(declareAndLogParam("r_set_sharpness", false)) { + monoCam->initialControl.setSharpness(sharpness); + } + int chromaDenoise = declareAndLogParam("r_chroma_denoise", 1); + if(declareAndLogParam("r_set_chroma_denoise", false)) { + monoCam->initialControl.setChromaDenoise(chromaDenoise); + } + int lumaDenoise = declareAndLogParam("r_luma_denoise", 1); + if(declareAndLogParam("r_set_luma_denoise", false)) { + monoCam->initialControl.setLumaDenoise(lumaDenoise); + } } void SensorParamHandler::declareParams(std::shared_ptr colorCam, dai_nodes::sensor_helpers::ImageSensor sensor, bool publish) { declareAndLogParam("i_publish_topic", publish); @@ -178,6 +194,22 @@ void SensorParamHandler::declareParams(std::shared_ptr c } colorCam->setImageOrientation( utils::getValFromMap(declareAndLogParam("i_sensor_img_orientation", "AUTO"), dai_nodes::sensor_helpers::cameraImageOrientationMap)); + int expLimit = declareAndLogParam("r_auto_exposure_limit", 1000); + if(declareAndLogParam("r_set_auto_exposure_limit", false)) { + colorCam->initialControl.setAutoExposureLimit(expLimit); + } + int sharpness = declareAndLogParam("r_sharpness", 1); + if(declareAndLogParam("r_set_sharpness", false)) { + colorCam->initialControl.setSharpness(sharpness); + } + int chromaDenoise = declareAndLogParam("r_chroma_denoise", 1); + if(declareAndLogParam("r_set_chroma_denoise", false)) { + colorCam->initialControl.setChromaDenoise(chromaDenoise); + } + int lumaDenoise = declareAndLogParam("r_luma_denoise", 1); + if(declareAndLogParam("r_set_luma_denoise", false)) { + colorCam->initialControl.setLumaDenoise(lumaDenoise); + } } dai::CameraControl SensorParamHandler::setRuntimeParams(const std::vector& params) { dai::CameraControl ctrl; @@ -216,6 +248,38 @@ dai::CameraControl SensorParamHandler::setRuntimeParams(const std::vector("r_set_man_whitebalance")) { ctrl.setManualWhiteBalance(p.get_value()); } + } else if(p.get_name() == getFullParamName("r_set_auto_exposure_limit")) { + if(p.get_value()) { + ctrl.setAutoExposureLimit(getParam("r_auto_exposure_limit")); + } + } else if(p.get_name() == getFullParamName("r_auto_exposure_limit")) { + if(getParam("r_set_auto_exposure_limit")) { + ctrl.setAutoExposureLimit(p.get_value()); + } + } else if(p.get_name() == getFullParamName("r_set_sharpness")) { + if(p.get_value()) { + ctrl.setSharpness(getParam("r_sharpness")); + } + } else if(p.get_name() == getFullParamName("r_sharpness")) { + if(getParam("r_set_sharpness")) { + ctrl.setSharpness(p.get_value()); + } + } else if(p.get_name() == getFullParamName("r_set_chroma_denoise")) { + if(p.get_value()) { + ctrl.setChromaDenoise(getParam("r_chroma_denoise")); + } + } else if(p.get_name() == getFullParamName("r_chroma_denoise")) { + if(getParam("r_set_chroma_denoise")) { + ctrl.setChromaDenoise(p.get_value()); + } + } else if(p.get_name() == getFullParamName("r_set_luma_denoise")) { + if(p.get_value()) { + ctrl.setLumaDenoise(getParam("r_luma_denoise")); + } + } else if(p.get_name() == getFullParamName("r_luma_denoise")) { + if(getParam("r_set_luma_denoise")) { + ctrl.setLumaDenoise(p.get_value()); + } } } return ctrl; diff --git a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp index e2bede44..75b67ae1 100644 --- a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp @@ -82,7 +82,7 @@ void StereoParamHandler::declareParams(std::shared_ptr s 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_synced", false); declareAndLogParam("i_left_rect_publish_compressed", false); declareAndLogParam("i_right_rect_publish_topic", false); @@ -95,7 +95,7 @@ void StereoParamHandler::declareParams(std::shared_ptr s 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_synced", false); declareAndLogParam("i_right_rect_publish_compressed", false); declareAndLogParam("i_enable_spatial_nn", false); diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index b1b99c90..d027a765 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.10.3) +project(depthai_ros_msgs VERSION 2.10.5) if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) diff --git a/depthai_ros_msgs/package.xml b/depthai_ros_msgs/package.xml index f7efee68..f455901d 100644 --- a/depthai_ros_msgs/package.xml +++ b/depthai_ros_msgs/package.xml @@ -1,7 +1,7 @@ depthai_ros_msgs - 2.10.3 + 2.10.5 Package to keep interface independent of the driver Adam Serafin