diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index 45f003b9..132a932e 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 b3f8e4aa..b3b7139d 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 b274d0ad..eab2f636 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); @@ -119,6 +123,33 @@ 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; + 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(); + 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)); + }; } void RGB::closeQueues() { @@ -128,6 +159,10 @@ void RGB::closeQueues() { previewPub->closeQueue(); } } + if(ph->getParam("i_enable_still")) { + triggerStillService.reset(); + stillPub->closeQueue(); + } controlQ->close(); } @@ -156,5 +191,13 @@ 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; + res->message = "Still capture request sent"; +} + } // 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 eb0f4f69..4a752b15 100644 --- a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp @@ -125,6 +125,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); @@ -175,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);