unmanaged instance, bump core, enable tests

Author: Serafadam

Dockerfile

@@ -14,7 +14,7 @@ RUN sh -c "$(wget https://raw.github.com/ohmyzsh/ohmyzsh/master/tools/install.sh
 ENV WS=/ws
 RUN mkdir -p $WS/src
 COPY ./ $WS/src/depthai-ros
-RUN cd .${WS}/src && git clone --branch ros-v3.0.7 https://github.com/luxonis/depthai-core.git && cd depthai-core && git submodule update --init --recursive
+RUN cd .${WS}/src && git clone --branch ros-v3.1.0 https://github.com/luxonis/depthai-core.git && cd depthai-core && git submodule update --init --recursive
 RUN cd $WS/ && rosdep install --from-paths src --ignore-src  -y
 
 RUN cd $WS/ && . /opt/ros/${ROS_DISTRO}/setup.sh && ./src/depthai-ros/build.sh -s $BUILD_SEQUENTIAL -r 1 -m 1 -t $BUILD_TESTS

build.sh

@@ -50,7 +50,7 @@ build_testing_flag="-DBUILD_TESTING=OFF"
 test_ros_driver_flag="-DTEST_DEPTHAI_ROS_DRIVER=OFF"
 if [ "$tests" == 1 ]; then
   build_testing_flag="-DBUILD_TESTING=ON"
-  test_ros_driver_flag="-DTEST_DEPTHAI_ROS_DRIVER=OFF"
+  test_ros_driver_flag="-DTEST_DEPTHAI_ROS_DRIVER=ON"
 fi
 
 

depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp

@@ -53,15 +53,16 @@ class PipelineGenerator {
      */
     void createPipeline(std::shared_ptr<rclcpp::Node> node, std::shared_ptr<dai::Device> device, std::shared_ptr<dai::Pipeline> pipeline, bool rsCompat);
 
+    void updateParams(const std::vector<rclcpp::Parameter>& params);
+
    protected:
     std::unordered_map<std::string, std::string> pluginTypeMap;
     std::unordered_map<std::string, PipelineType> pipelineTypeMap;
 
    private:
     std::shared_ptr<param_handlers::PipelineGenParamHandler> ph;
-    std::vector<std::unique_ptr<dai_nodes::BaseNode>> daiNodes;
     std::shared_ptr<pluginlib::ClassLoader<BasePipeline>> pipelineLoader;
-    class_loader::ClassLoader::UniquePtr<BasePipeline> pipelinePlugin;
+    std::vector<std::unique_ptr<dai_nodes::BaseNode>> daiNodes;
 };
 }  // namespace pipeline_gen
 }  // namespace depthai_ros_driver

depthai_ros_driver/src/driver.cpp

@@ -109,7 +109,9 @@ void Driver::stop() {
             RCLCPP_INFO(get_logger(), "Driver stopped!");
         }
     } else {
-        RCLCPP_INFO(get_logger(), "Driver already stopped!");
+        if(rclcpp::ok()) {
+            RCLCPP_INFO(get_logger(), "Driver already stopped!");
+        }
     }
 }
 
@@ -307,9 +309,7 @@ rcl_interfaces::msg::SetParametersResult Driver::parameterCB(const std::vector<r
             }
         }
     }
-    // for(const auto& node : daiNodes) {
-    //     node->updateParams(params);
-    // }
+    generator->updateParams(params);
     rcl_interfaces::msg::SetParametersResult res;
     res.successful = true;
     return res;

depthai_ros_driver/src/pipeline/pipeline_generator.cpp

@@ -39,11 +39,7 @@ PipelineGenerator::PipelineGenerator()
                        {"THERMAL", PipelineType::Thermal}};
 }
 
-PipelineGenerator::~PipelineGenerator() {
-    pipelinePlugin.reset();
-    daiNodes.clear();
-    pipelineLoader.reset();
-}
+PipelineGenerator::~PipelineGenerator() = default;
 void PipelineGenerator::createPipeline(std::shared_ptr<rclcpp::Node> node,
                                        std::shared_ptr<dai::Device> device,
                                        std::shared_ptr<dai::Pipeline> pipeline,
@@ -66,8 +62,11 @@ void PipelineGenerator::createPipeline(std::shared_ptr<rclcpp::Node> node,
     }
 
     try {
-        pipelinePlugin = pipelineLoader->createUniqueInstance(pluginType);
+        // other types of instancing (shared/unique ptr) seem to cause memory issues when shutting down the driver
+        auto pipelinePlugin = pipelineLoader->createUnmanagedInstance(pluginType);
         daiNodes = pipelinePlugin->createPipeline(node, device, pipeline, ph, deviceName, rsCompat, nnType);
+        pipelineLoader->unloadLibraryForClass(pluginType);
+        delete pipelinePlugin;
     } catch(pluginlib::PluginlibException& ex) {
         RCLCPP_ERROR(node->get_logger(), "The plugin failed to load for some reason. Error: %s\n", ex.what());
         throw std::runtime_error("Plugin loading failed.");
@@ -106,6 +105,11 @@ void PipelineGenerator::createPipeline(std::shared_ptr<rclcpp::Node> node,
     RCLCPP_INFO(node->get_logger(), "Finished setting up pipeline.");
 }
 
+void PipelineGenerator::updateParams(const std::vector<rclcpp::Parameter>& params) {
+    for(const auto& node : daiNodes) {
+        node->updateParams(params);
+    }
+}
 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-D-SR-POE") {