improved the cache behaviour in the Zed Node and updated dependencies.

.dockerignore

@@ -12,7 +12,6 @@ target/
 **/build/
 **/install/
 **/log/
-
 # IDE
 .vscode/
 .idea/

.github/workflows/headless-simulation.yml

@@ -2,7 +2,7 @@ name: Headless Simulation Test
 
 on:
   push:
-    branches: [ main, docker ]
+    branches: [ main, docker, zed_node_update ]
 
   workflow_dispatch:
 

.gitmodules

@@ -4,6 +4,6 @@
 [submodule "vendor/stonefish_ros2"]
 	path = vendor/stonefish_ros2
 	url = https://github.com/JuanDelPueblo/stonefish_ros2.git
-[submodule "vendor/zed-ros2-examples"]
-	path = vendor/zed-ros2-examples
-	url = https://github.com/stereolabs/zed-ros2-examples.git
+[submodule "vendor/zed-ros-interfaces"]
+	path = vendor/zed-ros-interfaces
+	url = https://github.com/stereolabs/zed-ros2-interfaces.git

README.md

@@ -3,8 +3,8 @@
 
 ### Clone and go in repo
 ```sh
-git clone --recursive git@github.com:joseburgosguntin/rumarino-ros2-jazzy.git
-cd ./rumarino-ros2-jazzy
+git clone --recursive https://github.com/Rumarino-Team/autonomy-stack.git
+cd ./autonomy-stack
 ```
 
 ## Quick Start with Docker (Recommended for CI/CD)
@@ -107,18 +107,12 @@ cd ../../../../../
 cd ~/ros2_ws/rumarino-ros2-jazzy
 
 # Source ROS 2 environment
-# Fedora:
-source /usr/lib64/ros2-jazzy/setup.zsh
-# Ubuntu:
 source /opt/ros/jazzy/setup.bash
 
 # Build packages
 colcon build --packages-select interfaces bringup Stonefish stonefish_ros2 controller_stonefish mission_executor
 
 # Source the workspace
-# Fedora
-source install/setup.sh
-# Ubuntu
 source install/setup.bash
 
 # Run with GUI
@@ -135,5 +129,5 @@ ros2 launch bringup test_mission_executor.launch.py mission_name:=prequalify env
 
 ### Run ZED Custom Wrapper
 ```sh
-colcon build --packages-select zed_custom_wrapper && source ./install/setup.bash && ros2 launch zed_custom_wrapper zed_custom.launch.py onnx_model_path:=./src/zed_custom_wrapper/yolov8n.onnx
+colcon build --packages-select zed_msg zed_custom_wrapper && source ./install/setup.bash && ros2 launch zed_custom_wrapper zed_custom.launch.py onnx_model_path:=./src/zed_custom_wrapper/yolov8n.onnx
 ```

src/zed_custom_wrapper/include/zed_custom_wrapper/zed_custom_node.hpp

@@ -20,7 +20,7 @@
 #include <chrono>
 #include <atomic>
 #include <memory>
-#include <map>
+#include <cstdint>
 #include <vector>
 
 namespace zed_custom_wrapper
@@ -126,16 +126,14 @@ class ZedCustomNode : public rclcpp::Node
     rclcpp::Publisher<interfaces::msg::Map>::SharedPtr pub_map_;
     rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_markers_;
 
-    // TF Broadcaster
     std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
 
-    // Object tracking state
     // Vector of tracked objects (index represents unique object position in map)
     std::vector<interfaces::msg::MapObject> map_objects_;
 
     // Tracking parameters
     double matching_distance_threshold_;
-    std::map<int, int> expected_object_counts_;  // class_id -> expected count
+    std::vector<uint8_t> expected_object_counts_;
 };
 
 }  // namespace zed_custom_wrapper

src/zed_custom_wrapper/package.xml

@@ -15,10 +15,9 @@
   <depend>nav_msgs</depend>
   <depend>image_transport</depend>
   <exec_depend>rviz2</exec_depend>
-  <exec_depend>rviz_plugin_zed_od</exec_depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
-  <depend>zed_msgs</depend>
+    <depend>zed_msgs</depend>
   <depend>interfaces</depend>
   <depend>vision_msgs</depend>
   <depend>visualization_msgs</depend>

src/zed_custom_wrapper/src/zed_custom_node.cpp

@@ -3,6 +3,7 @@
 #include <cmath>
 #include <set>
 #include <fstream>
+#include <limits>
 
 using namespace std::chrono_literals;
 
@@ -27,12 +28,32 @@ void ZedCustomNode::initialize() {
 
     // Load expected object counts
     auto counts_param = this->get_parameter("expected_object_counts").as_integer_array();
-    if (counts_param.size() % 2 == 0) {
+    if (counts_param.size() % 2 == 0 && !counts_param.empty()) {
+        int max_class_id = -1;
+        for (size_t i = 0; i < counts_param.size(); i += 2) {
+            int class_id = static_cast<int>(counts_param[i]);
+            if (class_id > max_class_id) {
+                max_class_id = class_id;
+            }
+        }
+
+        if (max_class_id >= 0) {
+            expected_object_counts_.assign(static_cast<size_t>(max_class_id + 1), 0);
+        }
+
         for (size_t i = 0; i < counts_param.size(); i += 2) {
             int class_id = static_cast<int>(counts_param[i]);
             int count = static_cast<int>(counts_param[i + 1]);
-            expected_object_counts_[class_id] = count;
-            RCLCPP_INFO(this->get_logger(), "Expected %d object(s) of class %d", count, class_id);
+            if (class_id >= 0 && static_cast<size_t>(class_id) < expected_object_counts_.size()) {
+                if (count > std::numeric_limits<uint8_t>::max()) {
+                    count = std::numeric_limits<uint8_t>::max();
+                    RCLCPP_WARN(this->get_logger(),
+                        "Expected count for class %d exceeds 255; clamping to 255",
+                        class_id);
+                }
+                expected_object_counts_[class_id] = static_cast<uint8_t>(count);
+                RCLCPP_INFO(this->get_logger(), "Expected %d object(s) of class %d", count, class_id);
+            }
         }
     }
 
@@ -391,8 +412,9 @@ void ZedCustomNode::publishMap(sl::Objects& sl_objects, rclcpp::Time timestamp)
         int class_id = sl_obj.raw_label;  // YOLO class ID
 
         // Skip if this class is not in expected_object_counts (filter unwanted classes)
-        if (!expected_object_counts_.empty() && 
-            expected_object_counts_.find(class_id) == expected_object_counts_.end()) {
+        if (!expected_object_counts_.empty() &&
+            (class_id < 0 || static_cast<size_t>(class_id) >= expected_object_counts_.size() ||
+             expected_object_counts_[class_id] == 0)) {
             RCLCPP_DEBUG(this->get_logger(), 
                 "Ignoring detected object of class %d (not in expected_object_counts)", class_id);
             continue;
@@ -437,8 +459,10 @@ void ZedCustomNode::publishMap(sl::Objects& sl_objects, rclcpp::Time timestamp)
             }
 
             // Get expected count for this class
-            auto it = expected_object_counts_.find(class_id);
-            int expected_count = (it != expected_object_counts_.end()) ? it->second : 0;
+            int expected_count = 0;
+            if (class_id >= 0 && static_cast<size_t>(class_id) < expected_object_counts_.size()) {
+                expected_count = expected_object_counts_[class_id];
+            }
 
             // Only add if we haven't reached the expected count
             if (current_count < expected_count) {