Refactor and enhance YOLO launch files and camera info publishing

.gitignore

@@ -1,5 +1,5 @@
 # VSCode
-.vscode/*
+.vscode/
 !.vscode/settings.json
 !.vscode/tasks.json
 !.vscode/launch.json
@@ -260,3 +260,5 @@ CATKIN_IGNORE
 
 # Models should not be pushed to git
 perception_setup/models/
+
+.claude/

README.md

@@ -4,11 +4,6 @@ Parent repository including launch files for Perception related packages.
 External package dependencies are managed using a `dependencies.repos` file and the ROS-standard `vcs` tool.
 
 ## Launch
-Main launch command to launch all Perception AUV related packages.
-```bash
-# launch with --show-args to print out all available launch arguments
-ros2 launch perception_setup perception.launch.py
-```
 See [perception_setup/README.md](perception_setup/README.md) for more information.
 
 ## Isaac ROS Developer Environment

config/fastdds_multi_interface.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<dds xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
+    <profiles>
+        <transport_descriptors>
+            <transport_descriptor>
+                <transport_id>multi_iface_udp</transport_id>
+                <type>UDPv4</type>
+                <interfaceWhiteList>
+                    <!-- eno1: topside / robot network (Pi, other ROS2 hosts) -->
+                    <address>10.0.0.67</address>
+                    <!-- enP5p1s0f3: RealSense camera NIC -->
+                    <address>192.168.11.70</address>
+                    <!-- loopback: intra-process comms -->
+                    <address>127.0.0.1</address>
+                </interfaceWhiteList>
+            </transport_descriptor>
+        </transport_descriptors>
+        <participant profile_name="default_profile" is_default_profile="true">
+            <rtps>
+                <userTransports>
+                    <transport_id>multi_iface_udp</transport_id>
+                </userTransports>
+                <useBuiltinTransports>false</useBuiltinTransports>
+            </rtps>
+        </participant>
+    </profiles>
+</dds>

perception_setup/CMakeLists.txt

@@ -1,7 +1,62 @@
 cmake_minimum_required(VERSION 3.8)
 project(perception_setup)
 
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(CMAKE_CXX_STANDARD 17)
+
 find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(yaml-cpp REQUIRED)
+
+# image_undistort composable node
+add_library(image_undistort_component SHARED
+  src/image_undistort.cpp
+)
+
+target_include_directories(image_undistort_component PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(image_undistort_component
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  cv_bridge
+  OpenCV
+)
+
+target_link_libraries(image_undistort_component yaml-cpp)
+
+rclcpp_components_register_node(
+  image_undistort_component
+  PLUGIN "perception_setup::ImageUndistort"
+  EXECUTABLE image_undistort_node
+)
+
+install(TARGETS image_undistort_component
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+# Python scripts (kept for reference)
+install(PROGRAMS
+  scripts/camera_info_publisher.py
+  scripts/image_crop.py
+  DESTINATION lib/${PROJECT_NAME}
+)
 
 install(DIRECTORY
   launch

perception_setup/README.md

@@ -1,104 +1,150 @@
-# Perception AUV Launch Instructions
+# perception_setup
 
-This repository contains a launch file for running various nodes related to image processing, ArUco detection, and camera drivers in a ROS 2 environment. The launch file supports both composable nodes and regular nodes based on user-specified configurations.
+Launch and configuration package for the perception pipeline. It contains camera drivers, image preprocessing, YOLO inference pipelines, and the mission launch files that tie everything together.
 
-## Prerequisites
+The main idea is:
+- Root launch files are the mission-level entry points we actually use during operations.
+- `launch/yolo/` contains the individual Isaac ROS YOLO launch files for standalone inference pipelines.
+- `launch/ultralytics/` contains equivalent launch files built with Ultralytics nodes instead of Isaac ROS nodes.
+- `launch/isaac_ros/` contains Isaac ROS variants similar to the Ultralytics ones, but they do not launch cameras themselves; they expect the camera topics to already exist.
 
-Ensure that you have the following packages installed and properly set up in your ROS 2 workspace:
-- `perception_setup`
-- `image_filtering`
-- `aruco_detector`
-- `spinnaker_camera_driver`
+Config YAMLs hold model and algorithm tuning only: thresholds, tensor names, model paths, class names, and similar parameters. Launch files own the wiring and topic names.
 
-## Launch File Description
+## Package structure
 
-The main launch file provided is `perception.launch.py`, which includes several arguments to control the behavior of the nodes being launched.
-
-### Launch Arguments
-
-- `enable_filtering` (default: `True`): Enable or disable the image filtering node.
-- `enable_aruco` (default: `True`): Enable or disable the ArUco detection node.
-- `enable_gripper_camera` (default: `True`): Enable or disable the gripper camera driver node.
-- `enable_front_camera` (default: `True`): Enable or disable the front camera driver node.
-- `enable_composable_nodes` (default: `True`): Enable or disable the use of composable nodes.
+```
+perception_setup/
+  config/
+    cameras/
+      color_realsense_d555_calib.yaml     # RealSense D555 color camera calibration (K, D)
+      blackfly_s_calib.yaml               # Blackfly S camera calibration
+      blackfly_s_params.yaml              # Blackfly S ROS parameters
+      blackfly_s_driver_params.yaml       # Spinnaker SDK node mapping
+    yolo/
+      yolo_obb.yaml                       # YOLO OBB tuning (model paths, thresholds, tensor names)
+      yolo_detect.yaml                    # YOLO detection tuning
+      yolo_seg.yaml                       # YOLO segmentation tuning
+      yolo_cls.yaml                       # YOLO classification tuning
+  launch/
+    cameras/
+      realsense_d555.launch.py            # RealSense D555 + image_undistort
+      blackfly_s.launch.py                # Blackfly S camera driver
+    isaac_ros/
+      isaac_ros_valve_intervention.launch.py  # Simulator-driven Isaac ROS valve pipeline
+    yolo/
+      yolo_obb.launch.py                  # Standalone YOLO OBB inference
+      yolo_detect.launch.py               # Standalone YOLO detection inference
+      yolo_seg.launch.py                  # Standalone YOLO segmentation inference
+      yolo_cls.launch.py                  # Standalone YOLO classification inference
+    ultralytics/
+      ultralytics_pipeline_line_fitting.launch.py  # Ultralytics-based pipeline
+      ultralytics_valve_detection.launch.py         # Ultralytics valve pipeline
+    valve_intervention.launch.py          # Full valve detection pipeline
+    visual_inspection.launch.py           # ArUco marker detection pipeline
+  models/                                 # ONNX and TensorRT engine files
+  src/
+    image_undistort.cpp                   # C++ composable node (lens undistortion)
+  include/
+    perception_setup/
+      image_undistort.hpp
+  scripts/
+    image_undistort.py                    # Python equivalent (legacy, kept for reference)
+    image_crop.py                         # Image cropping utility node
+    camera_info_publisher.py              # Standalone camera_info publisher
+```
 
-### Configuration Files
+## Helper nodes
 
-The following configuration files are used by the nodes:
-- `image_filtering_params.yaml` Ros2 parameters for the image_filtering_node
-- `aruco_detector_params.yaml` Ros2 parameters for the aruco_detector_node
-- `gripper_camera_params.yaml` Ros2 parameters for the gripper_camera_node
-- `gripper_camera_calib.yaml` Camera calibration file for the gripper_camera
-- `front_camera_params.yaml` Ros2 parameters for the front_camera_node
-- `front_camera_calib.yaml` Camera calibration file for the front_camera
-- `blackfly_s_params.yaml` This file maps the ros parameters to the corresponding Spinnaker "nodes" in the camera.
+This package provides one C++ composable node:
 
-These files should be located in the `config` directory of the `perception_setup` package.
+| Plugin name | Description |
+|---|---|
+| `perception_setup::ImageUndistort` | Undistorts a raw camera image using a calibration YAML, or passes through unchanged. Publishes a rectified image and zero-distortion `camera_info`. |
 
-## Usage
+### Python scripts (not currently used in any launch file)
 
-To use the launch file, follow these steps:
+| Script | Description |
+|---|---|
+| `camera_info_publisher.py` | Publishes a `sensor_msgs/CameraInfo` message from a calibration YAML file on a given topic. Useful for cameras whose drivers do not publish camera_info. |
+| `image_crop.py` | Crops an image and updates the corresponding camera_info. Was previously used for depth image cropping but removed because it interfered with valve detection. |
 
-1. **Navigate to your ROS 2 workspace**:
-    ```sh
-    cd ~/<ros2_ws>
-    ```
+### ImageUndistort parameters
 
-2. **Source your workspace**:
-    ```sh
-    source install/setup.bash
-    ```
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `image_topic` | string | *(required)* | Input raw image topic |
+| `camera_info_topic` | string | `""` | Input camera_info topic (used if `camera_info_file` is empty) |
+| `camera_info_file` | string | `""` | Path to calibration YAML (takes priority over topic) |
+| `raw_camera_info_topic` | string | *(required)* | Raw camera_info topic (used in passthrough mode) |
+| `output_image_topic` | string | *(required)* | Output rectified image topic |
+| `output_camera_info_topic` | string | *(required)* | Output camera_info topic |
+| `enable_undistort` | bool | *(required)* | `true` = undistort, `false` = passthrough |
+| `image_qos` | string | `"sensor_data"` | QoS for image publisher: `"reliable"` or `"sensor_data"` (best effort) |
 
-3. **Run the launch file with default arguments**:
-    ```sh
-    ros2 launch perception_setup perception.launch.py
-    ```
+## Launch files
 
-4. **Run the launch file with custom arguments**:
-    You can customize the launch arguments directly from the command line. For example, to disable the front camera and ArUco detection, use:
-    ```sh
-    ros2 launch perception_setup perception.launch.py enable_front_camera:=False enable_aruco:=False
-    ```
-5. **Launch with --show-args to print out all available launch arguments**
-   ```sh
-    ros2 launch perception_setup perception.launch.py --show-args
-    ```
-## Nodes and Composable Nodes
+### valve_intervention.launch.py
 
-The launch file can run nodes as either composable nodes or as separate nodes based on the `enable_composable_nodes` argument.
+Full valve detection pipeline used in missions: RealSense D555 -> image undistortion -> YOLO OBB -> valve pose estimation.
 
-### Composable Nodes
+All C++ nodes run in composable containers for zero-copy intra-process transport:
+- **obb_tensor_rt_container**: RealSense driver, image_undistort, image format converter, DNN image encoder, TensorRT, YOLO OBB decoder
+- **valve_detection_container**: valve pose estimator
 
-When `enable_composable_nodes` is set to `True`, the enabled nodes are launched as composable nodes within a container. To allow for intra-process-communication between the different composable nodes within the same container set the `use_intra_process_comms` argument to true for the individual nodes.
+```sh
+ros2 launch perception_setup valve_intervention.launch.py
+```
 
-For understanding of how to achieve a zero-copy transport of messages when publishing and subscribing in ros2 see the [Setting up efficient intra-process communication](https://docs.ros.org/en/humble/Tutorials/Demos/Intra-Process-Communication.html) tutorial.
+| Argument | Default | Description |
+|---|---|---|
+| `enable_undistort` | `true` | Undistort color image before YOLO inference |
+| `drone` | `nautilus` | Robot name, prepended to TF frame IDs |
+| `undistort_detections` | `false` | Undistort YOLO detections using lens distortion coefficients (mutually exclusive with `enable_undistort`) |
+| `debug_visualize` | `true` | Enable debug visualization topics |
 
-When launching as composable nodes in the same container if one node crashes it will cause the other nodes in the same container to crash as well. To avoid this and achieve better fault isolation one can set the `enable_composable_nodes` to `False`.
+### visual_inspection.launch.py
 
-### Standalone Nodes
+ArUco marker detection pipeline: RealSense D555 -> image undistortion -> image filtering -> ArUco detector.
 
-When `enable_composable_nodes` is set to `False`, the enabled nodes will be launched as standalone nodes to allow for better fault isolation. The config files included in this repository will be still be used to create node instances directly in this launch file to provide a simplistic overview. This configuration also makes this launch file independent from the launch files in the other packages.
+All nodes run in a single composable container (`visual_inspection_container`).
 
-## Calibration Files
+```sh
+ros2 launch perception_setup visual_inspection.launch.py
+```
 
-The camera calibration files are located in the `config` directory of the `perception_setup` package:
-- `gripper_camera_calib.yaml`
-- `front_camera_calib.yaml`
+| Argument | Default | Description |
+|---|---|---|
+| `enable_undistort` | `true` | Undistort color image before processing |
 
-These files are referenced in the launch file to provide calibration data for the cameras.
+### cameras/realsense_d555.launch.py
 
-## Example Commands
+Standalone RealSense D555 camera driver with image undistortion. Publishes both color (undistorted) and depth streams.
 
-**Launch with all nodes enabled (default):**
 ```sh
-ros2 launch perception_setup perception.launch.py
+ros2 launch perception_setup realsense_d555.launch.py
 ```
-**Launch with only the gripper camera and image filtering enabled:**
+
+| Argument | Default | Description |
+|---|---|---|
+| `enable_undistort` | `true` | Undistort color image before publishing |
+
+## Configuration
+
+### YOLO config files
+
+Each YOLO variant (`yolo_obb.yaml`, `yolo_detect.yaml`, etc.) holds model tuning only: model/engine paths, network input dimensions, tensor I/O bindings, confidence thresholds, class names, encoding, and visualizer enable. No topics.
+
+### Launch layout summary
+
+- Root launch files are the mission entry points.
+- `yolo/` launch files are the standalone Isaac ROS inference pipelines.
+- `ultralytics/` launch files use Ultralytics nodes for the same kinds of inference tasks.
+- `isaac_ros/` launch files use Isaac ROS inference nodes without launching cameras.
+
+## Building
+
 ```sh
-ros2 launch perception_setup perception.launch.py enable_front_camera:=False enable_aruco:=False
+colcon build --packages-up-to perception_setup
 ```
-**Launch without using composable nodes:**
-```sh
-ros2 launch perception_setup perception.launch.py enable_composable_nodes:=False
 
-```
+The C++ composable node requires: `rclcpp`, `rclcpp_components`, `sensor_msgs`, `cv_bridge`, `OpenCV`, `yaml-cpp`.