From 1b78691aa8ff4efdf4247611a005cdd593f12e15 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 20 Apr 2026 14:06:29 +0000 Subject: [PATCH 1/6] feat: Add an option to use still output in RGB node --- depthai_ros_driver/CMakeLists.txt | 1 + .../dai_nodes/sensors/rgb.hpp | 9 ++++- .../src/dai_nodes/sensors/rgb.cpp | 40 +++++++++++++++++++ .../param_handlers/sensor_param_handler.cpp | 5 +++ 4 files changed, 53 insertions(+), 2 deletions(-) diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index 45f003b9c..132a932e8 100644 --- a/depthai_ros_driver/CMakeLists.txt +++ b/depthai_ros_driver/CMakeLists.txt @@ -40,6 +40,7 @@ depthai depthai_bridge rclcpp std_msgs +std_srvs sensor_msgs image_transport) 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 b3f8e4aa4..b3b7139db 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 @@ -1,6 +1,8 @@ #pragma once #include "depthai_ros_driver/dai_nodes/base_node.hpp" +#include "rclcpp/service.hpp" +#include "std_srvs/srv/trigger.hpp" namespace dai { class Pipeline; @@ -48,12 +50,15 @@ class RGB : public BaseNode { std::vector> getPublishers() override; private: - std::shared_ptr rgbPub, previewPub; + void triggerStillCB(std_srvs::srv::Trigger::Request::ConstSharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr res); + + std::shared_ptr rgbPub, previewPub, stillPub; + rclcpp::Service::SharedPtr triggerStillService; std::shared_ptr colorCamNode; std::unique_ptr ph; std::shared_ptr controlQ; std::shared_ptr xinControl; - std::string ispQName, previewQName, controlQName; + std::string ispQName, previewQName, controlQName, stillQName; }; } // namespace dai_nodes diff --git a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp index b274d0ad1..88d078fa1 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp @@ -35,6 +35,7 @@ void RGB::setNames() { ispQName = getName() + "_isp"; previewQName = getName() + "_preview"; controlQName = getName() + "_control"; + stillQName = getName() + "_still"; } void RGB::setXinXout(std::shared_ptr pipeline) { @@ -59,6 +60,9 @@ void RGB::setXinXout(std::shared_ptr pipeline) { if(ph->getParam("i_enable_preview")) { previewPub = setupOutput(pipeline, previewQName, [&](auto input) { colorCamNode->preview.link(input); }); } + if(ph->getParam("i_enable_still")) { + stillPub = setupOutput(pipeline, stillQName, [&](auto input) { colorCamNode->still.link(input); }); + } xinControl = pipeline->create(); xinControl->setStreamName(controlQName); xinControl->out.link(colorCamNode->inputControl); @@ -118,6 +122,31 @@ void RGB::setupQueues(std::shared_ptr device) { previewPub->setup(device, convConfig, pubConfig); }; + if(ph->getParam("i_enable_still")) { + 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_still_width"); + pubConfig.height = ph->getParam("i_still_height"); + pubConfig.maxQSize = ph->getParam("i_max_q_size"); + pubConfig.topicSuffix = "/still/image_raw"; + pubConfig.flipImage = ph->getParam("i_flip_published_image"); + + stillPub->setup(device, convConfig, pubConfig); + + triggerStillService = getROSNode()->create_service( + "~/" + getName() + "/trigger_still", std::bind(&RGB::triggerStillCB, this, std::placeholders::_1, std::placeholders::_2)); + }; controlQ = device->getInputQueue(controlQName); } @@ -128,6 +157,10 @@ void RGB::closeQueues() { previewPub->closeQueue(); } } + if(ph->getParam("i_enable_still")) { + triggerStillService.reset(); + stillPub->closeQueue(); + } controlQ->close(); } @@ -156,5 +189,12 @@ void RGB::updateParams(const std::vector& params) { controlQ->send(ctrl); } +void RGB::triggerStillCB(std_srvs::srv::Trigger::Request::ConstSharedPtr /*req*/, std_srvs::srv::Trigger::Response::SharedPtr res) { + dai::CameraControl ctrl; + ctrl.setCaptureStill(true); + controlQ->send(ctrl); + res->success = true; +} + } // 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 eb0f4f697..c6fae813a 100644 --- a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp @@ -42,6 +42,7 @@ void SensorParamHandler::declareCommonParams(dai::CameraBoardSocket socket) { declareAndLogParam("i_reverse_stereo_socket_order", false); declareAndLogParam("i_synced", false); declareAndLogParam("i_publish_compressed", false); + declareAndLogParam("i_enable_still", false); } void SensorParamHandler::declareParams(std::shared_ptr cam, dai::CameraFeatures features, bool publish) { @@ -125,6 +126,7 @@ void SensorParamHandler::declareParams(std::shared_ptr c colorCam->setBoardSocket(socketID); declareAndLogParam("i_output_isp", true); declareAndLogParam("i_enable_preview", false); + declareAndLogParam("i_enable_still", false); declareAndLogParam("i_flip_published_image", false); colorCam->setFps(declareAndLogParam("i_fps", 30.0)); int preview_size = declareAndLogParam("i_preview_size", 300); @@ -194,6 +196,9 @@ void SensorParamHandler::declareParams(std::shared_ptr c videoHeight = maxVideoHeight; } colorCam->setVideoSize(videoWidth, videoHeight); + int still_width = declareAndLogParam("i_still_width", width); + int still_height = declareAndLogParam("i_still_height", height); + colorCam->setStillSize(still_width, still_height); colorCam->setPreviewKeepAspectRatio(declareAndLogParam("i_keep_preview_aspect_ratio", true)); size_t iso = declareAndLogParam("r_iso", 800, getRangedIntDescriptor(100, 1600)); size_t exposure = declareAndLogParam("r_exposure", 20000, getRangedIntDescriptor(1, 33000)); From 4ce950e81658488a0e5aa011e77afe4c007a7d14 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 20 Apr 2026 14:19:35 +0000 Subject: [PATCH 2/6] Initialize controlQ before creating the service --- depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp index 88d078fa1..f6c76b3cf 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp @@ -122,6 +122,7 @@ void RGB::setupQueues(std::shared_ptr device) { previewPub->setup(device, convConfig, pubConfig); }; + controlQ = device->getInputQueue(controlQName); if(ph->getParam("i_enable_still")) { auto tfPrefix = getOpticalTFPrefix(getSocketName(static_cast(ph->getParam("i_board_socket_id")))); utils::ImgConverterConfig convConfig; @@ -147,7 +148,6 @@ void RGB::setupQueues(std::shared_ptr device) { triggerStillService = getROSNode()->create_service( "~/" + getName() + "/trigger_still", std::bind(&RGB::triggerStillCB, this, std::placeholders::_1, std::placeholders::_2)); }; - controlQ = device->getInputQueue(controlQName); } void RGB::closeQueues() { From 83ca622fc3373f0cae5aa215df3e6277f9f19c88 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 20 Apr 2026 14:21:17 +0000 Subject: [PATCH 3/6] Add response message --- depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp index f6c76b3cf..ee8da9ac6 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp @@ -194,6 +194,7 @@ void RGB::triggerStillCB(std_srvs::srv::Trigger::Request::ConstSharedPtr /*req*/ ctrl.setCaptureStill(true); controlQ->send(ctrl); res->success = true; + res->message = "Still capture request sent"; } } // namespace dai_nodes From 15fb9e81065cebd852846c0bd0dc114fc5b374d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 20 Apr 2026 14:21:50 +0000 Subject: [PATCH 4/6] Don't redeclare i_enable_still --- depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp | 1 - 1 file changed, 1 deletion(-) 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 c6fae813a..c0eeaa953 100644 --- a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp @@ -42,7 +42,6 @@ void SensorParamHandler::declareCommonParams(dai::CameraBoardSocket socket) { declareAndLogParam("i_reverse_stereo_socket_order", false); declareAndLogParam("i_synced", false); declareAndLogParam("i_publish_compressed", false); - declareAndLogParam("i_enable_still", false); } void SensorParamHandler::declareParams(std::shared_ptr cam, dai::CameraFeatures features, bool publish) { From cae627b38ba902bb912f0491b13d81085fb6e186 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 20 Apr 2026 14:30:29 +0000 Subject: [PATCH 5/6] Fix naming convention --- .../src/param_handlers/sensor_param_handler.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 c0eeaa953..4a752b15e 100644 --- a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp @@ -176,6 +176,9 @@ void SensorParamHandler::declareParams(std::shared_ptr c RCLCPP_ERROR(getROSNode()->get_logger(), "%s", err_stream.str().c_str()); } } + int stillWidth = declareAndLogParam("i_still_width", width); + int stillHeight = declareAndLogParam("i_still_height", height); + colorCam->setStillSize(stillWidth, stillHeight); int maxVideoWidth = 3840; int maxVideoHeight = 2160; int videoWidth = declareAndLogParam("i_width", width); @@ -195,9 +198,6 @@ void SensorParamHandler::declareParams(std::shared_ptr c videoHeight = maxVideoHeight; } colorCam->setVideoSize(videoWidth, videoHeight); - int still_width = declareAndLogParam("i_still_width", width); - int still_height = declareAndLogParam("i_still_height", height); - colorCam->setStillSize(still_width, still_height); colorCam->setPreviewKeepAspectRatio(declareAndLogParam("i_keep_preview_aspect_ratio", true)); size_t iso = declareAndLogParam("r_iso", 800, getRangedIntDescriptor(100, 1600)); size_t exposure = declareAndLogParam("r_exposure", 20000, getRangedIntDescriptor(1, 33000)); From 6229dcf62e09b973fb6c0c1ddcf060e894fa5e2a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Fri, 14 Aug 2026 10:20:32 +0000 Subject: [PATCH 6/6] Apply exposure offset to still output --- depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp index ee8da9ac6..eab2f6364 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp @@ -129,6 +129,8 @@ void RGB::setupQueues(std::shared_ptr device) { 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.addExposureOffset = ph->getParam("i_add_exposure_offset"); + convConfig.expOffset = static_cast(ph->getParam("i_exposure_offset")); utils::ImgPublisherConfig pubConfig; pubConfig.daiNodeName = getName();