[BUG] Segfault with custom pipeline plugin and new node

BOOTCFG commented 8 months ago

Hi, I'm working on a pipeline plugin, but I'm getting a segfault when I try to create a custom daiNode.

This is my plugin:

namespace depthai_test_plugin{
namespace pipeline_gen{
        std::vector<std::unique_ptr<depthai_ros_driver::dai_nodes::BaseNode>> RGB_test_pipeline::createPipeline(rclcpp::Node *node,
                                                                               std::shared_ptr<dai::Device> device,
                                                                               std::shared_ptr<dai::Pipeline> pipeline,
                                                                               const std::string &nnType)
        { 
            [....]
            auto rgb = std::make_unique<depthai_test_plugin::dai_nodes::RGB_test>("RGB_test", node, pipeline, dai::CameraBoardSocket::CAM_A, (*sensorIt), true);
            // auto rgb = std::make_unique<depthai_ros_driver::dai_nodes::RGB>("RGB_test", node, pipeline, dai::CameraBoardSocket::CAM_A, (*sensorIt), true);

            daiNodes.push_back(std::move(rgb));
            return daiNodes;
        }
}}

RGB_test is a copy of the RGB class with a changed namespace.

namespace depthai_test_plugin {
namespace dai_nodes {

class RGB_test : public depthai_ros_driver::dai_nodes::BaseNode {
   public:
    explicit RGB_test(const std::string& daiNodeName,
                 rclcpp::Node* node,
                 std::shared_ptr<dai::Pipeline> pipeline,
                 dai::CameraBoardSocket socket,
                 depthai_ros_driver::dai_nodes::sensor_helpers::ImageSensor sensor,
                 bool publish);
    ~RGB_test();
    void updateParams(const std::vector<rclcpp::Parameter>& params) override;
    void setupQueues(std::shared_ptr<dai::Device> device) override;
    void link(dai::Node::Input in, int linkType = 0) override;
    void setNames() override;
    void setXinXout(std::shared_ptr<dai::Pipeline> pipeline) override;
    void closeQueues() override;

   private:
    std::unique_ptr<dai::ros::ImageConverter> imageConverter;
    image_transport::CameraPublisher rgbPubIT, previewPubIT;
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub, previewPub;
    rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rgbInfoPub, previewInfoPub;
    std::shared_ptr<camera_info_manager::CameraInfoManager> infoManager, previewInfoManager;
    std::shared_ptr<dai::node::ColorCamera> colorCamNode;
    std::shared_ptr<dai::node::VideoEncoder> videoEnc;
    std::unique_ptr<depthai_ros_driver::param_handlers::SensorParamHandler> ph;
    std::shared_ptr<dai::DataOutputQueue> colorQ, previewQ;
    std::shared_ptr<dai::DataInputQueue> controlQ;
    std::shared_ptr<dai::node::XLinkOut> xoutColor, xoutPreview;
    std::shared_ptr<dai::node::XLinkIn> xinControl;
    std::string ispQName, previewQName, controlQName;
};
} 
}

While using depthai_test_plugin::dai_nodes::RGB_test I get a segfault here.

node->setupQueues(device);

Executing any method on this object results in a crash.

The issue doesn't happen when rgb is std::make_unique<depthai_ros_driver::dai_nodes::RGB> instead of RGB_test.

The method works while in PipelineGenerator::createPipeline(), but once the main program returns to Camera::createPipeline(), using this method results in a segfault.

Here is a repo with the MRE: https://github.com/BOOTCFG/depthai_test_plugin It can be launched with ros2 launch depthai_test_plugin load.launch.py.

I'm wondering it's even possible to get my class RGB_test working in the main driver with pluginlib.

Extra info: Ubuntu 22 ROS 2 Humble depthai-ros built from source log_system_information.json

Serafadam commented 8 months ago

Hi, since by default depthai_ros_driver is loaded as a Component(shared library), to use custom nodes you also need to "load" them from a shared library so that they retain their identity (without that, they are loaded as a BaseNode class and that slices off overriden virtual functions, which causes segfault when the driver tries to call them). A quick test example to mitigate that:

In rgb_test.cpp, add following line:

extern "C" {
std::unique_ptr<depthai_ros_driver::dai_nodes::BaseNode> create(const std::string& daiNodeName,
             rclcpp::Node* node,
             std::shared_ptr<dai::Pipeline> pipeline,
             dai::CameraBoardSocket socket,
             depthai_ros_driver::dai_nodes::sensor_helpers::ImageSensor sensor,
             bool publish) {
return  std::make_unique<RGB_test>(daiNodeName, node, pipeline, socket, sensor, publish);
}
}

Then modify the test_pipeline.cpp to load the library via dlopen

#include "depthai_test_plugin/pipeline/test_base_types.hpp"
#include <dlfcn.h>

#include "depthai/device/Device.hpp"
#include "depthai/pipeline/Pipeline.hpp"
#include "depthai_ros_driver/dai_nodes/base_node.hpp"
#include "depthai_ros_driver/dai_nodes/nn/nn_helpers.hpp"
#include "depthai_ros_driver/dai_nodes/nn/nn_wrapper.hpp"
#include "depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp"
#include "depthai_ros_driver/dai_nodes/sensors/imu.hpp"
#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp"
#include "depthai_test_plugin/dai_nodes/sensors/rgb_test.hpp"
#include "depthai_ros_driver/dai_nodes/sensors/rgb.hpp"
#include "depthai_ros_driver/dai_nodes/stereo.hpp"
#include "depthai_ros_driver/pipeline/base_pipeline.hpp"
#include "depthai_ros_driver/utils.hpp"
#include "rclcpp/node.hpp"

using RGB_test_creator_t = std::function<std::unique_ptr<depthai_ros_driver::dai_nodes::BaseNode>(const std::string &,
                                                                                                  rclcpp::Node *,
                                                                                                  std::shared_ptr<dai::Pipeline>,
                                                                                                  dai::CameraBoardSocket,
                                                                                                  depthai_ros_driver::dai_nodes::sensor_helpers::ImageSensor,
                                                                                                  bool)>;
using RGB_test_raw_creator_t = std::unique_ptr<depthai_ros_driver::dai_nodes::BaseNode> (*)(const std::string &,
                                                                                            rclcpp::Node *,
                                                                                            std::shared_ptr<dai::Pipeline>,
                                                                                            dai::CameraBoardSocket,
                                                                                            depthai_ros_driver::dai_nodes::sensor_helpers::ImageSensor,
                                                                                            bool);
namespace depthai_test_plugin
{
    namespace pipeline_gen
    {
        std::vector<std::unique_ptr<depthai_ros_driver::dai_nodes::BaseNode>> RGB_test_pipeline::createPipeline(rclcpp::Node *node,
                                                                                                                std::shared_ptr<dai::Device> device,
                                                                                                                std::shared_ptr<dai::Pipeline> pipeline,
                                                                                                                const std::string &nnType)
        {
            auto sensorName = device->getCameraSensorNames().at(dai::CameraBoardSocket::CAM_A);
            std::vector<std::unique_ptr<depthai_ros_driver::dai_nodes::BaseNode>> daiNodes;
            std::vector<depthai_ros_driver::dai_nodes::sensor_helpers::ImageSensor>::iterator sensorIt =
                std::find_if(depthai_ros_driver::dai_nodes::sensor_helpers::availableSensors.begin(), depthai_ros_driver::dai_nodes::sensor_helpers::availableSensors.end(), [&sensorName](const depthai_ros_driver::dai_nodes::sensor_helpers::ImageSensor &s)
                             { return s.name == sensorName; });
            if (sensorIt == depthai_ros_driver::dai_nodes::sensor_helpers::availableSensors.end())
            {
                RCLCPP_ERROR(node->get_logger(), "Sensor %s not supported!", sensorName.c_str());
                throw std::runtime_error("Sensor not supported!");
            }

            void *handle = dlopen("libdepthai_test_plugin.so", RTLD_LAZY);
            if (!handle)
            {
                throw std::runtime_error(dlerror());
            }

            dlerror(); //
            RGB_test_raw_creator_t raw_creator = reinterpret_cast<RGB_test_raw_creator_t>(dlsym(handle, "create"));
            RGB_test_creator_t creator = raw_creator;
            const char *dlsym_error = dlerror();
            if (dlsym_error)
            {
                std::cerr << "Cannot load symbol 'create': " << dlsym_error << '\n';
                dlclose(handle);
                throw std::runtime_error(dlsym_error);
            }

            auto rgb = creator("rgb", node, pipeline, dai::CameraBoardSocket::CAM_A, (*sensorIt), true);
            dlopen("libdepthai_test_plugin.so", RTLD_LAZY);
            daiNodes.push_back(std::move(rgb));
            return daiNodes;
        }
    }
}

#include <pluginlib/class_list_macros.hpp>

PLUGINLIB_EXPORT_CLASS(depthai_test_plugin::pipeline_gen::RGB_test_pipeline, depthai_ros_driver::pipeline_gen::BasePipeline)

BOOTCFG commented 8 months ago

That fixed it for me, thank you.

luxonis / depthai-ros

[BUG] Segfault with custom pipeline plugin and new node #430