diff --git a/depthai_descriptions/launch/urdf.launch b/depthai_descriptions/launch/urdf.launch index 2d9dc6af..91ebd924 100644 --- a/depthai_descriptions/launch/urdf.launch +++ b/depthai_descriptions/launch/urdf.launch @@ -15,7 +15,7 @@ - - + + + diff --git a/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro b/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro index 919f64ba..9416e4e5 100644 --- a/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro +++ b/depthai_descriptions/urdf/include/depthai_macro.urdf.xacro @@ -12,35 +12,82 @@ + + + + + - + + + + + + + + + + + + + + + + + + + + + - + - - + + - + - + - - + + - + + + + + + + + + + + + + + + + + + + + + + + - + @@ -51,14 +98,13 @@ - - + - + @@ -68,8 +114,7 @@ - - + \ No newline at end of file diff --git a/depthai_ros_driver/config/sr_rgbd.yaml b/depthai_ros_driver/config/sr_rgbd.yaml new file mode 100644 index 00000000..3c8339a7 --- /dev/null +++ b/depthai_ros_driver/config/sr_rgbd.yaml @@ -0,0 +1,5 @@ +/oak: + camera_i_nn_type: none + camera_i_pipeline_type: 'Depth' + right_i_publish_topic: true + stereo_i_extended_disp: true \ No newline at end of file diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp index dd627dfb..bf7492ed 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp @@ -5,6 +5,7 @@ #include #include "camera_info_manager/camera_info_manager.h" +#include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai/device/DataQueue.hpp" #include "depthai/device/Device.hpp" #include "depthai/pipeline/Pipeline.hpp" @@ -17,6 +18,7 @@ #include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" #include "depthai_ros_driver/param_handlers/nn_param_handler.hpp" #include "depthai_ros_driver/parametersConfig.h" +#include "depthai_ros_driver/utils.hpp" #include "image_transport/camera_publisher.h" #include "image_transport/image_transport.h" #include "ros/node_handle.h" @@ -36,12 +38,16 @@ class Detection : public BaseNode { * @param node The node * @param pipeline The pipeline */ - Detection(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline) : BaseNode(daiNodeName, node, pipeline), it(node) { + Detection(const std::string& daiNodeName, + ros::NodeHandle node, + std::shared_ptr pipeline, + const dai::CameraBoardSocket& socket = dai::CameraBoardSocket::CAM_A) + : BaseNode(daiNodeName, node, pipeline), it(node) { ROS_DEBUG("Creating node %s", daiNodeName.c_str()); setNames(); detectionNode = pipeline->create(); imageManip = pipeline->create(); - ph = std::make_unique(node, daiNodeName); + ph = std::make_unique(node, daiNodeName, socket); ph->declareParams(detectionNode, imageManip); ROS_DEBUG("Node %s created", daiNodeName.c_str()); imageManip->out.link(detectionNode->input); @@ -56,12 +62,13 @@ class Detection : public BaseNode { */ void setupQueues(std::shared_ptr device) override { nnQ = device->getOutputQueue(nnQName, ph->getParam("i_max_q_size"), false); - auto tfPrefix = getTFPrefix("rgb"); + std::string socketName = utils::getSocketName(static_cast(ph->getParam("i_board_socket_id"))); + auto tfPrefix = getTFPrefix(socketName); int width; int height; if(ph->getParam("i_disable_resize")) { - width = ph->getOtherNodeParam("rgb", "i_preview_size"); - height = ph->getOtherNodeParam("rgb", "i_preview_size"); + width = ph->getOtherNodeParam(socketName, "i_preview_width"); + height = ph->getOtherNodeParam(socketName, "i_preview_height"); } else { width = imageManip->initialConfig.getResizeConfig().width; height = imageManip->initialConfig.getResizeConfig().height; diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/nn_wrapper.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/nn_wrapper.hpp index 4f3929ff..6d616f7e 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/nn_wrapper.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/nn_wrapper.hpp @@ -4,6 +4,7 @@ #include #include +#include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai_ros_driver/dai_nodes/base_node.hpp" #include "depthai_ros_driver/parametersConfig.h" @@ -25,7 +26,10 @@ namespace dai_nodes { class NNWrapper : public BaseNode { public: - explicit NNWrapper(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline); + explicit NNWrapper(const std::string& daiNodeName, + ros::NodeHandle node, + std::shared_ptr pipeline, + const dai::CameraBoardSocket& socket = dai::CameraBoardSocket::CAM_A); ~NNWrapper(); void updateParams(parametersConfig& config) override; void setupQueues(std::shared_ptr device) override; diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/segmentation.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/segmentation.hpp index e1d54bac..d336a059 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/segmentation.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/segmentation.hpp @@ -5,6 +5,7 @@ #include #include "cv_bridge/cv_bridge.h" +#include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai_ros_driver/dai_nodes/base_node.hpp" #include "depthai_ros_driver/parametersConfig.h" #include "image_transport/camera_publisher.h" @@ -39,7 +40,10 @@ namespace dai_nodes { namespace nn { class Segmentation : public BaseNode { public: - Segmentation(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline); + Segmentation(const std::string& daiNodeName, + ros::NodeHandle node, + std::shared_ptr pipeline, + const dai::CameraBoardSocket& socket = dai::CameraBoardSocket::CAM_A); ~Segmentation(); void updateParams(parametersConfig& config) override; void setupQueues(std::shared_ptr device) override; diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp index 9b249a8b..baff3ff6 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp @@ -5,6 +5,7 @@ #include #include "camera_info_manager/camera_info_manager.h" +#include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai/device/DataQueue.hpp" #include "depthai/device/Device.hpp" #include "depthai/pipeline/Pipeline.hpp" @@ -18,6 +19,7 @@ #include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" #include "depthai_ros_driver/param_handlers/nn_param_handler.hpp" #include "depthai_ros_driver/parametersConfig.h" +#include "depthai_ros_driver/utils.hpp" #include "image_transport/camera_publisher.h" #include "image_transport/image_transport.h" #include "ros/node_handle.h" @@ -28,13 +30,16 @@ namespace nn { template class SpatialDetection : public BaseNode { public: - SpatialDetection(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline) + SpatialDetection(const std::string& daiNodeName, + ros::NodeHandle node, + std::shared_ptr pipeline, + const dai::CameraBoardSocket& socket = dai::CameraBoardSocket::CAM_A) : BaseNode(daiNodeName, node, pipeline), it(node) { ROS_DEBUG("Creating node %s", daiNodeName.c_str()); setNames(); spatialNode = pipeline->create(); imageManip = pipeline->create(); - ph = std::make_unique(node, daiNodeName); + ph = std::make_unique(node, daiNodeName, socket); ph->declareParams(spatialNode, imageManip); ROS_DEBUG("Node %s created", daiNodeName.c_str()); imageManip->out.link(spatialNode->input); @@ -46,12 +51,14 @@ class SpatialDetection : public BaseNode { }; void setupQueues(std::shared_ptr device) override { nnQ = device->getOutputQueue(nnQName, ph->getParam("i_max_q_size"), false); - auto tfPrefix = getTFPrefix("rgb"); + std::string socketName = utils::getSocketName(static_cast(ph->getParam("i_board_socket_id"))); + + auto tfPrefix = getTFPrefix(socketName); int width; int height; if(ph->getParam("i_disable_resize")) { - width = ph->getOtherNodeParam("rgb", "i_preview_size"); - height = ph->getOtherNodeParam("rgb", "i_preview_size"); + width = ph->getOtherNodeParam(socketName, "i_preview_width"); + height = ph->getOtherNodeParam(socketName, "i_preview_height"); } else { width = imageManip->initialConfig.getResizeConfig().width; height = imageManip->initialConfig.getResizeConfig().height; @@ -74,10 +81,8 @@ class SpatialDetection : public BaseNode { } if(ph->getParam("i_enable_passthrough_depth")) { - dai::CameraBoardSocket socket = dai::CameraBoardSocket::CAM_A; - bool align; - getROSNode().getParam("stereo_i_align_depth", align); - if(!align) { + dai::CameraBoardSocket socket = static_cast(ph->getOtherNodeParam("stereo", "i_board_socket_id")); + if(!ph->getOtherNodeParam("stereo", "i_align_depth")) { tfPrefix = getTFPrefix("right"); socket = dai::CameraBoardSocket::CAM_C; }; diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp index 3eeeb150..c8371c8a 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp @@ -4,6 +4,7 @@ #include #include +#include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai_ros_driver/dai_nodes/base_node.hpp" #include "depthai_ros_driver/parametersConfig.h" @@ -26,7 +27,10 @@ namespace dai_nodes { class SpatialNNWrapper : public BaseNode { public: - explicit SpatialNNWrapper(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline); + explicit SpatialNNWrapper(const std::string& daiNodeName, + ros::NodeHandle node, + std::shared_ptr pipeline, + const dai::CameraBoardSocket& socket = dai::CameraBoardSocket::CAM_A); ~SpatialNNWrapper(); void updateParams(parametersConfig& config) override; void setupQueues(std::shared_ptr device) override; diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp index 02b04f09..6c264791 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_wrapper.hpp @@ -52,7 +52,7 @@ class SensorWrapper : public BaseNode { private: void subCB(const sensor_msgs::Image::ConstPtr& img); - std::unique_ptr sensorNode, featureTrackerNode; + std::unique_ptr sensorNode, featureTrackerNode, nnNode; std::unique_ptr ph; std::unique_ptr converter; ros::Subscriber sub; diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp index 893627c7..15519c41 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/stereo.hpp @@ -84,7 +84,7 @@ class Stereo : public BaseNode { std::shared_ptr stereoEnc, leftRectEnc, rightRectEnc; std::unique_ptr left; std::unique_ptr right; - std::unique_ptr featureTrackerLeftR, featureTrackerRightR; + std::unique_ptr featureTrackerLeftR, featureTrackerRightR, nnNode; std::unique_ptr ph; std::shared_ptr stereoQ, leftRectQ, rightRectQ; std::shared_ptr xoutStereo, xoutLeftRect, xoutRightRect; diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp index b13df172..4d91c053 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/nn_param_handler.hpp @@ -6,6 +6,7 @@ #include #include +#include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai/pipeline/datatype/CameraControl.hpp" #include "depthai_ros_driver/param_handlers/base_param_handler.hpp" #include "nlohmann/json.hpp" @@ -32,7 +33,7 @@ enum class NNFamily { Segmentation, Mobilenet, Yolo }; } class NNParamHandler : public BaseParamHandler { public: - explicit NNParamHandler(ros::NodeHandle node, const std::string& name); + explicit NNParamHandler(ros::NodeHandle node, const std::string& name, const dai::CameraBoardSocket& socket = dai::CameraBoardSocket::CAM_A); ~NNParamHandler(); nn::NNFamily getNNFamily(); std::string getConfigPath(); diff --git a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/stereo_param_handler.hpp b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/stereo_param_handler.hpp index fbd94eb8..6cd5b663 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/param_handlers/stereo_param_handler.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/param_handlers/stereo_param_handler.hpp @@ -25,13 +25,14 @@ class StereoParamHandler : public BaseParamHandler { ~StereoParamHandler(); void declareParams(std::shared_ptr stereo); dai::CameraControl setRuntimeParams(parametersConfig& config) override; - void updateSocketsFromParams(dai::CameraBoardSocket& left, dai::CameraBoardSocket& right); + void updateSocketsFromParams(dai::CameraBoardSocket& left, dai::CameraBoardSocket& right, dai::CameraBoardSocket& align); private: std::unordered_map depthPresetMap; std::unordered_map disparityWidthMap; std::unordered_map decimationModeMap; std::unordered_map temporalPersistencyMap; + dai::CameraBoardSocket alignSocket; }; } // namespace param_handlers } // namespace depthai_ros_driver \ No newline at end of file diff --git a/depthai_ros_driver/launch/camera.launch b/depthai_ros_driver/launch/camera.launch index 1457f51b..092aca1a 100644 --- a/depthai_ros_driver/launch/camera.launch +++ b/depthai_ros_driver/launch/camera.launch @@ -1,7 +1,7 @@ - + @@ -35,27 +35,27 @@ - + - - - - - - - - - - - - - - + + + + + + + + + + + + + + diff --git a/depthai_ros_driver/launch/sr_rgbd_pcl.launch b/depthai_ros_driver/launch/sr_rgbd_pcl.launch new file mode 100644 index 00000000..88d6df3b --- /dev/null +++ b/depthai_ros_driver/launch/sr_rgbd_pcl.launch @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/depthai_ros_driver/src/camera.cpp b/depthai_ros_driver/src/camera.cpp index f30a723b..03421efc 100644 --- a/depthai_ros_driver/src/camera.cpp +++ b/depthai_ros_driver/src/camera.cpp @@ -59,7 +59,9 @@ void Camera::diagCB(const diagnostic_msgs::DiagnosticArray::ConstPtr& msg) { if(status.name == nodeletName + std::string(": sys_logger")) { if(status.level == diagnostic_msgs::DiagnosticStatus::ERROR) { ROS_ERROR("Camera diagnostics error: %s", status.message.c_str()); - restart(); + if(ph->getParam("i_restart_on_diagnostics_error")) { + restart(); + } } } } diff --git a/depthai_ros_driver/src/dai_nodes/nn/nn_wrapper.cpp b/depthai_ros_driver/src/dai_nodes/nn/nn_wrapper.cpp index 9044bf54..3c2754cc 100644 --- a/depthai_ros_driver/src/dai_nodes/nn/nn_wrapper.cpp +++ b/depthai_ros_driver/src/dai_nodes/nn/nn_wrapper.cpp @@ -10,9 +10,10 @@ namespace depthai_ros_driver { namespace dai_nodes { -NNWrapper::NNWrapper(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline) : BaseNode(daiNodeName, node, pipeline) { +NNWrapper::NNWrapper(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline, const dai::CameraBoardSocket& socket) + : BaseNode(daiNodeName, node, pipeline) { ROS_DEBUG("Creating node %s base", daiNodeName.c_str()); - ph = std::make_unique(node, daiNodeName); + ph = std::make_unique(node, daiNodeName, socket); auto family = ph->getNNFamily(); switch(family) { case param_handlers::nn::NNFamily::Yolo: { diff --git a/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp b/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp index 2e9b975c..8f75dadc 100644 --- a/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp +++ b/depthai_ros_driver/src/dai_nodes/nn/segmentation.cpp @@ -12,6 +12,7 @@ #include "depthai_bridge/ImageConverter.hpp" #include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp" #include "depthai_ros_driver/param_handlers/nn_param_handler.hpp" +#include "depthai_ros_driver/utils.hpp" #include "image_transport/camera_publisher.h" #include "image_transport/image_transport.h" #include "ros/node_handle.h" @@ -22,13 +23,13 @@ namespace depthai_ros_driver { namespace dai_nodes { namespace nn { -Segmentation::Segmentation(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline) +Segmentation::Segmentation(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline, const dai::CameraBoardSocket& socket) : BaseNode(daiNodeName, node, pipeline), it(node) { ROS_DEBUG("Creating node %s", daiNodeName.c_str()); setNames(); segNode = pipeline->create(); imageManip = pipeline->create(); - ph = std::make_unique(node, daiNodeName); + ph = std::make_unique(node, daiNodeName, socket); ph->declareParams(segNode, imageManip); imageManip->out.link(segNode->input); setXinXout(pipeline); @@ -57,7 +58,7 @@ void Segmentation::setupQueues(std::shared_ptr device) { nnPub = it.advertiseCamera(getName() + "/image_raw", 1); nnQ->addCallback(std::bind(&Segmentation::segmentationCB, this, std::placeholders::_1, std::placeholders::_2)); if(ph->getParam("i_enable_passthrough")) { - auto tfPrefix = getTFPrefix("rgb"); + auto tfPrefix = getTFPrefix(utils::getSocketName(static_cast(ph->getParam("i_board_socket_id")))); ptQ = device->getOutputQueue(ptQName, ph->getParam("i_max_q_size"), false); imageConverter = std::make_unique(tfPrefix + "_camera_optical_frame", false); diff --git a/depthai_ros_driver/src/dai_nodes/nn/spatial_nn_wrapper.cpp b/depthai_ros_driver/src/dai_nodes/nn/spatial_nn_wrapper.cpp index d96a9948..83513d04 100644 --- a/depthai_ros_driver/src/dai_nodes/nn/spatial_nn_wrapper.cpp +++ b/depthai_ros_driver/src/dai_nodes/nn/spatial_nn_wrapper.cpp @@ -9,10 +9,13 @@ namespace depthai_ros_driver { namespace dai_nodes { -SpatialNNWrapper::SpatialNNWrapper(const std::string& daiNodeName, ros::NodeHandle node, std::shared_ptr pipeline) +SpatialNNWrapper::SpatialNNWrapper(const std::string& daiNodeName, + ros::NodeHandle node, + std::shared_ptr pipeline, + const dai::CameraBoardSocket& socket) : BaseNode(daiNodeName, node, pipeline) { ROS_DEBUG("Creating node %s base", daiNodeName.c_str()); - ph = std::make_unique(node, daiNodeName); + ph = std::make_unique(node, daiNodeName, socket); auto family = ph->getNNFamily(); switch(family) { case param_handlers::nn::NNFamily::Yolo: { diff --git a/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp b/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp index 6288abb6..22f925c9 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp @@ -100,7 +100,7 @@ sensor_msgs::CameraInfo getCalibInfo( try { info = converter.calibrationToCameraInfo(calibHandler, socket, width, height); } catch(std::runtime_error& e) { - ROS_ERROR("No calibration! Publishing empty camera_info."); + ROS_ERROR("No calibration for socket %d! Publishing empty camera_info.", static_cast(socket)); } return info; } diff --git a/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp b/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp index a4a5e02e..0e352fcc 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp @@ -4,6 +4,7 @@ #include "depthai/pipeline/Pipeline.hpp" #include "depthai/pipeline/node/XLinkIn.hpp" #include "depthai_bridge/ImageConverter.hpp" +#include "depthai_ros_driver/dai_nodes/nn/nn_wrapper.hpp" #include "depthai_ros_driver/dai_nodes/sensors/feature_tracker.hpp" #include "depthai_ros_driver/dai_nodes/sensors/mono.hpp" #include "depthai_ros_driver/dai_nodes/sensors/rgb.hpp" @@ -52,6 +53,9 @@ SensorWrapper::SensorWrapper(const std::string& daiNodeName, } ROS_DEBUG("Node %s has sensor %s", daiNodeName.c_str(), sensorName.c_str()); sensorData = *sensorIt; + if(device->getDeviceName() == "OAK-D-SR") { + (*sensorIt).color = true; // ov9282 is color sensor in this case + } if((*sensorIt).color) { sensorNode = std::make_unique(daiNodeName, node, pipeline, socket, (*sensorIt), publish); } else { @@ -62,6 +66,10 @@ SensorWrapper::SensorWrapper(const std::string& daiNodeName, featureTrackerNode = std::make_unique(daiNodeName + std::string("_feature_tracker"), node, pipeline); sensorNode->link(featureTrackerNode->getInput()); } + if(ph->getParam("i_enable_nn")) { + nnNode = std::make_unique(daiNodeName + std::string("_nn"), node, pipeline, static_cast(socketID)); + sensorNode->link(nnNode->getInput(), static_cast(link_types::RGBLinkType::preview)); + } ROS_DEBUG("Base node %s created", daiNodeName.c_str()); } SensorWrapper::~SensorWrapper() = default; @@ -96,6 +104,9 @@ void SensorWrapper::setupQueues(std::shared_ptr device) { if(ph->getParam("i_enable_feature_tracker")) { featureTrackerNode->setupQueues(device); } + if(ph->getParam("i_enable_nn")) { + nnNode->setupQueues(device); + } } void SensorWrapper::closeQueues() { if(ph->getParam("i_simulate_from_topic")) { @@ -107,6 +118,9 @@ void SensorWrapper::closeQueues() { if(ph->getParam("i_enable_feature_tracker")) { featureTrackerNode->closeQueues(); } + if(ph->getParam("i_enable_nn")) { + nnNode->closeQueues(); + } } void SensorWrapper::link(dai::Node::Input in, int linkType) { diff --git a/depthai_ros_driver/src/dai_nodes/stereo.cpp b/depthai_ros_driver/src/dai_nodes/stereo.cpp index 1e3ff4b5..005b05b2 100644 --- a/depthai_ros_driver/src/dai_nodes/stereo.cpp +++ b/depthai_ros_driver/src/dai_nodes/stereo.cpp @@ -9,6 +9,8 @@ #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" @@ -30,7 +32,11 @@ Stereo::Stereo(const std::string& daiNodeName, ROS_DEBUG("Creating node %s", daiNodeName.c_str()); setNames(); ph = std::make_unique(node, daiNodeName); - ph->updateSocketsFromParams(leftSocket, rightSocket); + 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) { @@ -51,6 +57,18 @@ Stereo::Stereo(const std::string& daiNodeName, 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))); + } ROS_DEBUG("Node %s created", daiNodeName.c_str()); } Stereo::~Stereo() = default; @@ -276,6 +294,9 @@ void Stereo::setupQueues(std::shared_ptr 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(); @@ -300,6 +321,9 @@ void Stereo::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*/) { diff --git a/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp b/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp index 3490c12a..6b1a60b6 100644 --- a/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/camera_param_handler.cpp @@ -35,6 +35,7 @@ void CameraParamHandler::declareParams() { declareAndLogParam("i_external_calibration_path", ""); declareAndLogParam("i_laser_dot_brightness", 800, getRangedIntDescriptor(0, 1200)); declareAndLogParam("i_floodlight_brightness", 0, getRangedIntDescriptor(0, 1500)); + declareAndLogParam("i_restart_on_diagnostics_error", false); declareAndLogParam("i_publish_tf_from_calibration", false); declareAndLogParam("i_tf_camera_name", "oak"); declareAndLogParam("i_tf_camera_model", ""); diff --git a/depthai_ros_driver/src/param_handlers/nn_param_handler.cpp b/depthai_ros_driver/src/param_handlers/nn_param_handler.cpp index a7da1a5b..1abe31e8 100644 --- a/depthai_ros_driver/src/param_handlers/nn_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/nn_param_handler.cpp @@ -2,6 +2,7 @@ #include +#include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai/pipeline/node/DetectionNetwork.hpp" #include "depthai/pipeline/node/ImageManip.hpp" #include "depthai/pipeline/node/NeuralNetwork.hpp" @@ -14,12 +15,13 @@ namespace depthai_ros_driver { namespace param_handlers { -NNParamHandler::NNParamHandler(ros::NodeHandle node, const std::string& name) : BaseParamHandler(node, name) { +NNParamHandler::NNParamHandler(ros::NodeHandle node, const std::string& name, const dai::CameraBoardSocket& socket) : BaseParamHandler(node, name) { nnFamilyMap = { {"segmentation", nn::NNFamily::Segmentation}, {"mobilenet", nn::NNFamily::Mobilenet}, {"YOLO", nn::NNFamily::Yolo}, }; + declareAndLogParam("i_board_socket_id", static_cast(socket)); } NNParamHandler::~NNParamHandler() = default; std::string NNParamHandler::getConfigPath() { 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 544ce0e7..f0cd52b0 100644 --- a/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp @@ -27,6 +27,7 @@ void SensorParamHandler::declareCommonParams(dai::CameraBoardSocket socket) { socketID = static_cast(declareAndLogParam("i_board_socket_id", static_cast(socket), 0)); declareAndLogParam("i_update_ros_base_time_on_ros_msg", false); declareAndLogParam("i_enable_feature_tracker", false); + declareAndLogParam("i_enable_nn", false); declareAndLogParam("i_enable_lazy_publisher", true); declareAndLogParam("i_add_exposure_offset", false); declareAndLogParam("i_exposure_offset", 0); @@ -94,9 +95,20 @@ void SensorParamHandler::declareParams(std::shared_ptr c int height = colorCam->getResolutionHeight(); colorCam->setInterleaved(declareAndLogParam("i_interleaved", false)); - if(declareAndLogParam("i_set_isp_scale", true)) { - int num = declareAndLogParam("i_isp_num", 2); - int den = declareAndLogParam("i_isp_den", 3); + + bool setIspScale = true; + if(sensor.defaultResolution != "1080P" + && sensor.defaultResolution != "1200P") { // default disable ISP scaling since default resolution is not 1080P or 1200P + setIspScale = false; + } + if(declareAndLogParam("i_set_isp_scale", setIspScale)) { + int num = 2; + int den = 3; + if(sensor.defaultResolution == "1200P") { + den = 5; // for improved performance + } + num = declareAndLogParam("i_isp_num", num); + den = declareAndLogParam("i_isp_den", den); width = (width * num + den - 1) / den; height = (height * num + den - 1) / den; colorCam->setIspScale(num, den); 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 f0deb522..59bdb326 100644 --- a/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp @@ -34,9 +34,10 @@ StereoParamHandler::StereoParamHandler(ros::NodeHandle node, const std::string& }; } -void StereoParamHandler::updateSocketsFromParams(dai::CameraBoardSocket& left, dai::CameraBoardSocket& right) { +void StereoParamHandler::updateSocketsFromParams(dai::CameraBoardSocket& left, dai::CameraBoardSocket& right, dai::CameraBoardSocket& align) { int newLeftS = getParam("i_left_socket_id", static_cast(left)); int newRightS = getParam("i_right_socket_id", static_cast(right)); + alignSocket = static_cast(declareAndLogParam("i_board_socket_id", static_cast(align))); if(newLeftS != static_cast(left) || newRightS != static_cast(right)) { ROS_WARN("Left or right socket changed, updating stereo node"); ROS_WARN("Old left socket: %d, new left socket: %d", static_cast(left), newLeftS); @@ -75,17 +76,19 @@ void StereoParamHandler::declareParams(std::shared_ptr s declareAndLogParam("i_right_rect_add_exposure_offset", false); declareAndLogParam("i_right_rect_exposure_offset", 0); + declareAndLogParam("i_enable_spatial_nn", false); + declareAndLogParam("i_spatial_nn_source", "right"); + stereo->setLeftRightCheck(declareAndLogParam("i_lr_check", true)); int width = 1280; int height = 720; - auto socket = static_cast(declareAndLogParam("i_board_socket_id", static_cast(dai::CameraBoardSocket::CAM_A))); std::string socketName; if(declareAndLogParam("i_align_depth", true)) { - socketName = utils::getSocketName(socket); + socketName = utils::getSocketName(alignSocket); width = getOtherNodeParam(socketName, "i_width"); height = getOtherNodeParam(socketName, "i_height"); declareAndLogParam("i_socket_name", socketName); - stereo->setDepthAlign(socket); + stereo->setDepthAlign(alignSocket); } if(declareAndLogParam("i_set_input_size", false)) { diff --git a/depthai_ros_driver/src/pipeline/pipeline_generator.cpp b/depthai_ros_driver/src/pipeline/pipeline_generator.cpp index 10b550fb..03827ee3 100644 --- a/depthai_ros_driver/src/pipeline/pipeline_generator.cpp +++ b/depthai_ros_driver/src/pipeline/pipeline_generator.cpp @@ -55,12 +55,12 @@ std::string PipelineGenerator::validatePipeline(const std::string& typeStr, int auto pType = utils::getValFromMap(typeStr, pipelineTypeMap); if(sensorNum == 1) { if(pType != PipelineType::RGB) { - ROS_ERROR("Wrong pipeline chosen for camera as it has only one sensor. Switching to RGB."); + ROS_ERROR("Invalid pipeline chosen for camera as it has only one sensor. Switching to RGB."); return "RGB"; } } else if(sensorNum == 2) { - if(pType != PipelineType::Stereo || pType != PipelineType::Depth) { - ROS_ERROR("Wrong pipeline chosen for camera as it has only stereo pair. Switching to DEPTH."); + if(pType != PipelineType::Stereo && pType != PipelineType::Depth) { + ROS_ERROR("Invalid pipeline chosen for camera as it has only stereo pair. Switching to DEPTH."); return "DEPTH"; } }