From 64b5d9395c0464970b07b07f3112ddcd1a4cd144 Mon Sep 17 00:00:00 2001 From: Cruiz102 Date: Sat, 7 Feb 2026 11:41:29 -0400 Subject: [PATCH 1/3] improve the cache behaviour in the Zed Node and updated dependencies. --- .dockerignore | 2 +- .gitmodules | 6 +-- README.md | 12 ++---- .../zed_custom_wrapper/zed_custom_node.hpp | 6 +-- src/zed_custom_wrapper/package.xml | 3 +- .../src/zed_custom_node.cpp | 38 +++++++++++++++---- vendor/zed-ros2-examples | 1 - 7 files changed, 41 insertions(+), 27 deletions(-) delete mode 160000 vendor/zed-ros2-examples diff --git a/.dockerignore b/.dockerignore index 284a092..c805cce 100644 --- a/.dockerignore +++ b/.dockerignore @@ -12,7 +12,7 @@ target/ **/build/ **/install/ **/log/ - +vendor/stonefish # IDE .vscode/ .idea/ diff --git a/.gitmodules b/.gitmodules index 3af3478..6a05355 100644 --- a/.gitmodules +++ b/.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 diff --git a/README.md b/README.md index 40a7e29..16ba6e1 100644 --- a/README.md +++ b/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 ``` diff --git a/src/zed_custom_wrapper/include/zed_custom_wrapper/zed_custom_node.hpp b/src/zed_custom_wrapper/include/zed_custom_wrapper/zed_custom_node.hpp index 3589702..a5d497a 100644 --- a/src/zed_custom_wrapper/include/zed_custom_wrapper/zed_custom_node.hpp +++ b/src/zed_custom_wrapper/include/zed_custom_wrapper/zed_custom_node.hpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include namespace zed_custom_wrapper @@ -126,16 +126,14 @@ class ZedCustomNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_map_; rclcpp::Publisher::SharedPtr pub_markers_; - // TF Broadcaster std::unique_ptr tf_broadcaster_; - // Object tracking state // Vector of tracked objects (index represents unique object position in map) std::vector map_objects_; // Tracking parameters double matching_distance_threshold_; - std::map expected_object_counts_; // class_id -> expected count + std::vector expected_object_counts_; }; } // namespace zed_custom_wrapper diff --git a/src/zed_custom_wrapper/package.xml b/src/zed_custom_wrapper/package.xml index f7f2395..81dbaba 100644 --- a/src/zed_custom_wrapper/package.xml +++ b/src/zed_custom_wrapper/package.xml @@ -15,10 +15,9 @@ nav_msgs image_transport rviz2 - rviz_plugin_zed_od tf2_ros tf2_geometry_msgs - zed_msgs + zed_msgs interfaces vision_msgs visualization_msgs diff --git a/src/zed_custom_wrapper/src/zed_custom_node.cpp b/src/zed_custom_wrapper/src/zed_custom_node.cpp index e2decab..8cf2325 100644 --- a/src/zed_custom_wrapper/src/zed_custom_node.cpp +++ b/src/zed_custom_wrapper/src/zed_custom_node.cpp @@ -3,6 +3,7 @@ #include #include #include +#include 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(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(max_class_id + 1), 0); + } + for (size_t i = 0; i < counts_param.size(); i += 2) { int class_id = static_cast(counts_param[i]); int count = static_cast(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(class_id) < expected_object_counts_.size()) { + if (count > std::numeric_limits::max()) { + count = std::numeric_limits::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(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(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(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) { diff --git a/vendor/zed-ros2-examples b/vendor/zed-ros2-examples deleted file mode 160000 index 3ee3f69..0000000 --- a/vendor/zed-ros2-examples +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 3ee3f69449b93413cb8f201e97fbe0bb19554709 From 1dcb0c28606e3db351f87929dd92922a6148e5d8 Mon Sep 17 00:00:00 2001 From: Cruiz102 Date: Sat, 7 Feb 2026 11:49:25 -0400 Subject: [PATCH 2/3] run ci-cd in the branch --- .github/workflows/headless-simulation.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/headless-simulation.yml b/.github/workflows/headless-simulation.yml index 63346eb..485ea59 100644 --- a/.github/workflows/headless-simulation.yml +++ b/.github/workflows/headless-simulation.yml @@ -2,7 +2,7 @@ name: Headless Simulation Test on: push: - branches: [ main, docker ] + branches: [ main, zed_node_update ] workflow_dispatch: From 5a562adaee0b58f89bad74d5ca39d2e11ccdd912 Mon Sep 17 00:00:00 2001 From: Cruiz102 Date: Sat, 7 Feb 2026 11:51:11 -0400 Subject: [PATCH 3/3] fix ci-cd --- .dockerignore | 1 - .github/workflows/headless-simulation.yml | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/.dockerignore b/.dockerignore index c805cce..b5d44fd 100644 --- a/.dockerignore +++ b/.dockerignore @@ -12,7 +12,6 @@ target/ **/build/ **/install/ **/log/ -vendor/stonefish # IDE .vscode/ .idea/ diff --git a/.github/workflows/headless-simulation.yml b/.github/workflows/headless-simulation.yml index 485ea59..4d04e73 100644 --- a/.github/workflows/headless-simulation.yml +++ b/.github/workflows/headless-simulation.yml @@ -2,7 +2,7 @@ name: Headless Simulation Test on: push: - branches: [ main, zed_node_update ] + branches: [ main, docker, zed_node_update ] workflow_dispatch: