Add ToF intensity output as topic

depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/tof.hpp

@@ -53,12 +53,14 @@ class ToF : public BaseNode {
 
    private:
     std::shared_ptr<sensor_helpers::ImagePublisher> tofPub;
+    std::shared_ptr<sensor_helpers::ImagePublisher> intensityPub;
     std::shared_ptr<dai::node::ToF> tofNode;
     std::shared_ptr<dai::node::ImageAlign> alignNode;
     std::unique_ptr<param_handlers::ToFParamHandler> ph;
     dai::CameraBoardSocket boardSocket;
     dai::CameraBoardSocket alignedSocket;
     std::string tofQName;
+    std::string intensityQName;
     bool aligned;
 };
 

depthai_ros_driver/src/dai_nodes/sensors/tof.cpp

@@ -42,6 +42,7 @@ ToF::ToF(const std::string& daiNodeName,
 ToF::~ToF() = default;
 void ToF::setNames() {
     tofQName = getName() + "_tof";
+    intensityQName = getName() + "_intensity";
 }
 std::shared_ptr<dai::node::ToF> ToF::getUnderlyingNode() {
     return tofNode;
@@ -59,6 +60,18 @@ void ToF::setInOut(std::shared_ptr<dai::Pipeline> pipeline) {
 
         tofPub = setupOutput(pipeline, tofQName, &tofNode->depth, ph->getParam<bool>(ParamNames::SYNCED), encConfig);
     }
+
+    if(ph->getParam<bool>("i_publish_intensity_topic")) {
+        // Use the same encoder settings as ToF output
+        utils::VideoEncoderConfig intensityEncConfig;
+        intensityEncConfig.profile = static_cast<dai::VideoEncoderProperties::Profile>(ph->getParam<int>(ParamNames::LOW_BANDWIDTH_PROFILE));
+        intensityEncConfig.bitrate = ph->getParam<int>(ParamNames::LOW_BANDWIDTH_BITRATE);
+        intensityEncConfig.frameFreq = ph->getParam<int>(ParamNames::LOW_BANDWIDTH_FRAME_FREQ);
+        intensityEncConfig.quality = ph->getParam<int>(ParamNames::LOW_BANDWIDTH_QUALITY);
+        intensityEncConfig.enabled = ph->getParam<bool>(ParamNames::LOW_BANDWIDTH);
+
+        intensityPub = setupOutput(pipeline, intensityQName, &tofNode->intensity, ph->getParam<bool>(ParamNames::SYNCED), intensityEncConfig);
+    }
 }
 
 void ToF::setupQueues(std::shared_ptr<dai::Device> device) {
@@ -89,11 +102,42 @@ void ToF::setupQueues(std::shared_ptr<dai::Device> device) {
 
         tofPub->setup(device, convConfig, pubConfig);
     }
+
+    if(ph->getParam<bool>("i_publish_intensity_topic")) {
+        auto tfPrefix = getOpticalFrameName(getSocketName(boardSocket));
+
+        utils::ImgConverterConfig intensityConvConfig;
+        intensityConvConfig.tfPrefix = tfPrefix;
+        intensityConvConfig.getBaseDeviceTimestamp = ph->getParam<bool>(ParamNames::GET_BASE_DEVICE_TIMESTAMP);
+        intensityConvConfig.updateROSBaseTimeOnRosMsg = ph->getParam<bool>(ParamNames::UPDATE_ROS_BASE_TIME_ON_ROS_MSG);
+        intensityConvConfig.lowBandwidth = ph->getParam<bool>(ParamNames::LOW_BANDWIDTH);
+        intensityConvConfig.encoding = dai::ImgFrame::Type::RAW8;
+        intensityConvConfig.addExposureOffset = ph->getParam<bool>(ParamNames::ADD_EXPOSURE_OFFSET);
+        intensityConvConfig.expOffset = static_cast<dai::CameraExposureOffset>(ph->getParam<int>(ParamNames::EXPOSURE_OFFSET));
+        intensityConvConfig.reverseSocketOrder = ph->getParam<bool>(ParamNames::REVERSE_STEREO_SOCKET_ORDER);
+
+        utils::ImgPublisherConfig intensityPubConfig;
+        intensityPubConfig.daiNodeName = getName();
+        intensityPubConfig.topicName = "~/" + getName() + "/intensity";
+        intensityPubConfig.lazyPub = ph->getParam<bool>(ParamNames::ENABLE_LAZY_PUBLISHER);
+        intensityPubConfig.socket = ph->getSocketID();
+        intensityPubConfig.calibrationFile = ph->getParam<std::string>(ParamNames::CALIBRATION_FILE);
+        intensityPubConfig.rectified = false;
+        intensityPubConfig.width = ph->getParam<int>(ParamNames::WIDTH);
+        intensityPubConfig.height = ph->getParam<int>(ParamNames::HEIGHT);
+        intensityPubConfig.maxQSize = ph->getParam<int>(ParamNames::MAX_Q_SIZE);
+
+        intensityPub->setup(device, intensityConvConfig, intensityPubConfig);
+    }
 }
 void ToF::closeQueues() {
     if(ph->getParam<bool>(param_handlers::ParamNames::PUBLISH_TOPIC)) {
         tofPub->closeQueue();
     }
+
+    if(ph->getParam<bool>("i_publish_intensity_topic")) {
+        intensityPub->closeQueue();
+    }
 }
 
 void ToF::link(dai::Node::Input& in, int /*linkType*/) {
@@ -130,6 +174,9 @@ std::vector<std::shared_ptr<sensor_helpers::ImagePublisher>> ToF::getPublishers(
     if(ph->getParam<bool>(ParamNames::PUBLISH_TOPIC) && ph->getParam<bool>(ParamNames::SYNCED)) {
         pubs.push_back(tofPub);
     }
+    if(ph->getParam<bool>("i_publish_intensity_topic") && ph->getParam<bool>(ParamNames::SYNCED)) {
+        pubs.push_back(tofPub);
+    }
     return pubs;
 }
 

depthai_ros_driver/src/param_handlers/tof_param_handler.cpp

@@ -22,6 +22,7 @@ ToFParamHandler::ToFParamHandler(std::shared_ptr<rclcpp::Node> node, const std::
 ToFParamHandler::~ToFParamHandler() = default;
 void ToFParamHandler::declareParams(std::shared_ptr<dai::node::ToF> tof, dai::CameraBoardSocket socket) {
     declareAndLogParam<bool>(ParamNames::PUBLISH_TOPIC, true);
+    declareAndLogParam<bool>("i_publish_intensity_topic", false);
     declareAndLogParam<bool>(ParamNames::SYNCED, false);
     declareAndLogParam<bool>(ParamNames::LOW_BANDWIDTH, false);
     declareAndLogParam<int>(ParamNames::LOW_BANDWIDTH_PROFILE, 4);

depthai_ros_driver/src/pipeline/pipeline_generator.cpp

@@ -103,6 +103,10 @@ std::vector<std::unique_ptr<dai_nodes::BaseNode>> PipelineGenerator::createPipel
 
 std::string PipelineGenerator::validatePipeline(std::shared_ptr<rclcpp::Node> node, const std::string& typeStr, int sensorNum, const std::string& deviceName) {
     auto pType = utils::getValFromMap(typeStr, pipelineTypeMap);
+    if (deviceName == "OAK-FFC-4P" || deviceName == "OAK-FFC-3P") {
+        RCLCPP_WARN(node->get_logger(), "OAK-FFC-4P/3P device detected. Default pipelines may not work without reconfiguration.");
+        return typeStr;
+    }
     if(deviceName == "OAK-D-SR-POE") {
         RCLCPP_WARN(node->get_logger(), "OAK-D-SR-POE device detected. Pipeline types other than StereoToF/ToF/RGBToF might not work without reconfiguration.");
     }