Merge pull request #783 from bjsowa/feat/add-still-output

feat: Add an option to use still output in RGB node

Author: asahtik

depthai_ros_driver/CMakeLists.txt

@@ -40,6 +40,7 @@ depthai
 depthai_bridge 
 rclcpp  
 std_msgs 
+std_srvs 
 sensor_msgs 
 image_transport)
 

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<std::shared_ptr<sensor_helpers::ImagePublisher>> getPublishers() override;
 
    private:
-    std::shared_ptr<sensor_helpers::ImagePublisher> rgbPub, previewPub;
+    void triggerStillCB(std_srvs::srv::Trigger::Request::ConstSharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr res);
+
+    std::shared_ptr<sensor_helpers::ImagePublisher> rgbPub, previewPub, stillPub;
+    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr triggerStillService;
     std::shared_ptr<dai::node::ColorCamera> colorCamNode;
     std::unique_ptr<param_handlers::SensorParamHandler> ph;
     std::shared_ptr<dai::DataInputQueue> controlQ;
     std::shared_ptr<dai::node::XLinkIn> xinControl;
-    std::string ispQName, previewQName, controlQName;
+    std::string ispQName, previewQName, controlQName, stillQName;
 };
 
 }  // namespace dai_nodes

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<dai::Pipeline> pipeline) {
@@ -59,6 +60,9 @@ void RGB::setXinXout(std::shared_ptr<dai::Pipeline> pipeline) {
     if(ph->getParam<bool>("i_enable_preview")) {
         previewPub = setupOutput(pipeline, previewQName, [&](auto input) { colorCamNode->preview.link(input); });
     }
+    if(ph->getParam<bool>("i_enable_still")) {
+        stillPub = setupOutput(pipeline, stillQName, [&](auto input) { colorCamNode->still.link(input); });
+    }
     xinControl = pipeline->create<dai::node::XLinkIn>();
     xinControl->setStreamName(controlQName);
     xinControl->out.link(colorCamNode->inputControl);
@@ -119,6 +123,33 @@ void RGB::setupQueues(std::shared_ptr<dai::Device> device) {
         previewPub->setup(device, convConfig, pubConfig);
     };
     controlQ = device->getInputQueue(controlQName);
+    if(ph->getParam<bool>("i_enable_still")) {
+        auto tfPrefix = getOpticalTFPrefix(getSocketName(static_cast<dai::CameraBoardSocket>(ph->getParam<int>("i_board_socket_id"))));
+        utils::ImgConverterConfig convConfig;
+        convConfig.tfPrefix = tfPrefix;
+        convConfig.getBaseDeviceTimestamp = ph->getParam<bool>("i_get_base_device_timestamp");
+        convConfig.updateROSBaseTimeOnRosMsg = ph->getParam<bool>("i_update_ros_base_time_on_ros_msg");
+        convConfig.addExposureOffset = ph->getParam<bool>("i_add_exposure_offset");
+        convConfig.expOffset = static_cast<dai::CameraExposureOffset>(ph->getParam<int>("i_exposure_offset"));
+
+        utils::ImgPublisherConfig pubConfig;
+        pubConfig.daiNodeName = getName();
+        pubConfig.topicName = "~/" + getName();
+        pubConfig.lazyPub = ph->getParam<bool>("i_enable_lazy_publisher");
+        pubConfig.socket = static_cast<dai::CameraBoardSocket>(ph->getParam<int>("i_board_socket_id"));
+        pubConfig.calibrationFile = ph->getParam<std::string>("i_calibration_file");
+        pubConfig.rectified = false;
+        pubConfig.width = ph->getParam<int>("i_still_width");
+        pubConfig.height = ph->getParam<int>("i_still_height");
+        pubConfig.maxQSize = ph->getParam<int>("i_max_q_size");
+        pubConfig.topicSuffix = "/still/image_raw";
+        pubConfig.flipImage = ph->getParam<bool>("i_flip_published_image");
+
+        stillPub->setup(device, convConfig, pubConfig);
+
+        triggerStillService = getROSNode()->create_service<std_srvs::srv::Trigger>(
+            "~/" + getName() + "/trigger_still", std::bind(&RGB::triggerStillCB, this, std::placeholders::_1, std::placeholders::_2));
+    };
 }
 
 void RGB::closeQueues() {
@@ -128,6 +159,10 @@ void RGB::closeQueues() {
             previewPub->closeQueue();
         }
     }
+    if(ph->getParam<bool>("i_enable_still")) {
+        triggerStillService.reset();
+        stillPub->closeQueue();
+    }
     controlQ->close();
 }
 
@@ -156,5 +191,13 @@ void RGB::updateParams(const std::vector<rclcpp::Parameter>& 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;
+    res->message = "Still capture request sent";
+}
+
 }  // namespace dai_nodes
 }  // namespace depthai_ros_driver

depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp

@@ -125,6 +125,7 @@ void SensorParamHandler::declareParams(std::shared_ptr<dai::node::ColorCamera> c
     colorCam->setBoardSocket(socketID);
     declareAndLogParam<bool>("i_output_isp", true);
     declareAndLogParam<bool>("i_enable_preview", false);
+    declareAndLogParam<bool>("i_enable_still", false);
     declareAndLogParam<bool>("i_flip_published_image", false);
     colorCam->setFps(declareAndLogParam<double>("i_fps", 30.0));
     int preview_size = declareAndLogParam<int>("i_preview_size", 300);
@@ -175,6 +176,9 @@ void SensorParamHandler::declareParams(std::shared_ptr<dai::node::ColorCamera> c
             RCLCPP_ERROR(getROSNode()->get_logger(), "%s", err_stream.str().c_str());
         }
     }
+    int stillWidth = declareAndLogParam<int>("i_still_width", width);
+    int stillHeight = declareAndLogParam<int>("i_still_height", height);
+    colorCam->setStillSize(stillWidth, stillHeight);
     int maxVideoWidth = 3840;
     int maxVideoHeight = 2160;
     int videoWidth = declareAndLogParam<int>("i_width", width);