Merge branch 'main' into UpdateCamSync

Author: lucasreljic

.github/actions/build-and-test/action.yml

@@ -34,6 +34,7 @@ runs:
     - name: ✅ Run tests
       shell: bash
       run: |
+        source install/setup.bash
         source /opt/ros/${{ inputs.ros-distro }}/setup.bash
         IS_CI=1 colcon test --merge-install --event-handlers console_cohesion+
         colcon test-result --verbose

deep_sample/test/launch_tests/test_sample_gpu_backend.py

@@ -36,6 +36,18 @@
 import numpy as np
 
 
+def _is_gpu_available():
+    """Check if GPU and CUDA libraries are available."""
+    try:
+        import ctypes
+
+        # Try to load CUDA runtime library
+        ctypes.CDLL("libcuda.so.1")
+        return True
+    except (OSError, AttributeError):
+        return False
+
+
 @pytest.mark.launch_test
 def generate_test_description():
     """Generate launch description for GPU backend test."""
@@ -77,6 +89,9 @@ def generate_test_description():
     )
 
 
+@unittest.skipUnless(
+    _is_gpu_available(), "GPU/CUDA not available - skipping GPU backend tests"
+)
 class TestGPUBackend(unittest.TestCase):
     """Test GPU backend functionality."""
 

deep_sample/test/launch_tests/test_sample_tensorrt_backend.py

@@ -36,6 +36,18 @@
 import numpy as np
 
 
+def _is_gpu_available():
+    """Check if GPU and CUDA libraries are available."""
+    try:
+        import ctypes
+
+        # Try to load CUDA runtime library
+        ctypes.CDLL("libcuda.so.1")
+        return True
+    except (OSError, AttributeError):
+        return False
+
+
 @pytest.mark.launch_test
 def generate_test_description():
     """Generate launch description for TensorRT backend test."""
@@ -77,6 +89,9 @@ def generate_test_description():
     )
 
 
+@unittest.skipUnless(
+    _is_gpu_available(), "GPU/CUDA not available - skipping TensorRT backend tests"
+)
 class TestTensorRTBackend(unittest.TestCase):
     """Test TensorRT backend functionality."""
 

deep_test/include/test_fixtures/test_executor_fixture.hpp

@@ -14,6 +14,7 @@
 
 #pragma once
 
+#include <atomic>
 #include <memory>
 #include <thread>
 #include <type_traits>
@@ -62,6 +63,12 @@ class TestExecutorFixture : private ROS2Initializer
    */
   void start_spinning();
 
+  /**
+   * @brief Stop spinning the executor
+   * Can be called to cleanly stop the executor before fixture destruction
+   */
+  void stop_spinning();
+
   /**
    * @brief Add a templated test node to be spun by this executor
    *
@@ -83,9 +90,22 @@ class TestExecutorFixture : private ROS2Initializer
     }
   }
 
+  /**
+   * @brief Remove a node from the executor
+   *
+   * @tparam T The test node type
+   * @param node The test node to remove
+   */
+  template <typename T>
+  void remove_node(std::shared_ptr<T> node)
+  {
+    executor_.remove_node(node->get_node_base_interface());
+  }
+
 protected:
   rclcpp::executors::SingleThreadedExecutor executor_;
   std::thread spin_thread_;
+  std::atomic<bool> should_spin_{true};
   std::vector<std::shared_ptr<rclcpp_lifecycle::LifecycleNode>> lifecycle_nodes_;
 };
 

deep_test/src/test_executor_fixture.cpp

@@ -14,19 +14,38 @@
 
 #include "test_fixtures/test_executor_fixture.hpp"
 
+#include <atomic>
+#include <mutex>
+
 #include <lifecycle_msgs/msg/state.hpp>
 
 namespace deep_ros::test
 {
 
+// Static members for safe one-time initialization
+static std::mutex ros_init_mutex;
+static std::atomic<int> ros_init_count{0};
+static std::atomic<bool> ros_initialized{false};
+
 ROS2Initializer::ROS2Initializer()
 {
-  rclcpp::init(0, nullptr);
+  std::lock_guard<std::mutex> lock(ros_init_mutex);
+  if (!ros_initialized.load()) {
+    rclcpp::init(0, nullptr);
+    ros_initialized.store(true);
+  }
+  ros_init_count++;
 }
 
 ROS2Initializer::~ROS2Initializer()
 {
-  rclcpp::shutdown();
+  std::lock_guard<std::mutex> lock(ros_init_mutex);
+  ros_init_count--;
+  // Only shutdown when the last instance is destroyed
+  if (ros_init_count.load() == 0 && ros_initialized.load()) {
+    rclcpp::shutdown();
+    ros_initialized.store(false);
+  }
 }
 
 TestExecutorFixture::TestExecutorFixture()
@@ -35,31 +54,48 @@ TestExecutorFixture::TestExecutorFixture()
 
 TestExecutorFixture::~TestExecutorFixture()
 {
-  // Deactivate all lifecycle nodes to ensure proper cleanup
+  stop_spinning();
+
+  // Now that spinning has stopped, we can safely deactivate lifecycle nodes
   for (auto & node : lifecycle_nodes_) {
-    if (node && node->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
-      try {
-        node->deactivate();
-      } catch (const std::exception & e) {
-        // Log but don't throw during destruction
-        RCLCPP_WARN(
-          rclcpp::get_logger("TestExecutorFixture"), "Failed to deactivate node during cleanup: %s", e.what());
-      }
+    if (!node) {
+      continue;
     }
-  }
-
-  // Cancel executor and wait for thread to finish
-  executor_.cancel();
 
-  if (spin_thread_.joinable()) {
-    spin_thread_.join();
+    auto state = node->get_current_state().id();
+    if (state == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
+      node->deactivate();
+    }
   }
+
+  lifecycle_nodes_.clear();
 }
 
 void TestExecutorFixture::start_spinning()
 {
   if (!spin_thread_.joinable()) {
-    spin_thread_ = std::thread([this]() { executor_.spin(); });
+    should_spin_ = true;
+    spin_thread_ = std::thread([this]() {
+      // Use spin_once with timeout instead of blocking spin()
+      // This allows the thread to exit when should_spin_ is set to false
+      while (should_spin_.load()) {
+        executor_.spin_once(std::chrono::milliseconds(100));
+      }
+    });
+  }
+}
+
+void TestExecutorFixture::stop_spinning()
+{
+  should_spin_ = false;
+  executor_.cancel();
+
+  if (spin_thread_.joinable()) {
+    // Give it a moment to exit the loop
+    if (!spin_thread_.joinable()) {
+      return;  // Already joined
+    }
+    spin_thread_.join();
   }
 }