diff --git a/.gitignore b/.gitignore
index 584b7b2..194edec 100644
--- a/.gitignore
+++ b/.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/
diff --git a/README.md b/README.md
index 98e46e7..742bd9b 100644
--- a/README.md
+++ b/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
diff --git a/config/fastdds_multi_interface.xml b/config/fastdds_multi_interface.xml
new file mode 100644
index 0000000..8741d04
--- /dev/null
+++ b/config/fastdds_multi_interface.xml
@@ -0,0 +1,27 @@
+
+
+
+
+
+ multi_iface_udp
+ UDPv4
+
+
+ 10.0.0.67
+
+ 192.168.11.70
+
+ 127.0.0.1
+
+
+
+
+
+
+ multi_iface_udp
+
+ false
+
+
+
+
diff --git a/perception_setup/CMakeLists.txt b/perception_setup/CMakeLists.txt
index eb12bfb..824a750 100644
--- a/perception_setup/CMakeLists.txt
+++ b/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
+ $
+ $
+)
+
+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
diff --git a/perception_setup/README.md b/perception_setup/README.md
index 59046ed..d13c4cf 100644
--- a/perception_setup/README.md
+++ b/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 ~/
- ```
+### 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`.
diff --git a/perception_setup/config/aruco_detector_params.yaml b/perception_setup/config/aruco_detector_params.yaml
deleted file mode 100644
index 7aa1e3c..0000000
--- a/perception_setup/config/aruco_detector_params.yaml
+++ /dev/null
@@ -1,33 +0,0 @@
-/**:
- ros__parameters:
- camera_frame: "camera_link"
- # Flir camera
- subs:
- image_topic: "/filtered_image"
- # camera_info_topic: "/flir_camera/camera_info"
- camera_info_topic: "/gripper_camera/camera_info"
- # Flir camera
- camera:
- intrinsic: [1050.0, 1050.0, 960.0, 540.0] # fx, fy, cx, cy
- distortion: [-0.356890438215317, 0.168613819658546, 0.0, 0.0, -0.0441848241077346] # k1, k2, p1, p2, k3
- detect_board: true # Set to true to detect aruco boards
- visualize: true # Set to true to visualize the detected markers and board
- # TAC ARUCO PARAMETERS
- aruco:
- marker_size: 0.150
- dictionary: "DICT_ARUCO_ORIGINAL"
- #TAC ARUCO BOARD PARAMETERS
- # board:
- # xDist: 0.430
- # yDist: 0.830
- # ids: [28, 7, 96, 19]
- # marker_size: 0.150
-
- # Vortex docking plate
- board:
- xDist: 0.462
- yDist: 0.862
- ids: [28, 7, 96, 19]
- models:
- dynmod_stddev: 0.01
- sendmod_stddev: 0.01
diff --git a/perception_setup/config/cameras/blackfly_s_calib.yaml b/perception_setup/config/cameras/blackfly_s_calib.yaml
new file mode 100644
index 0000000..3670dd7
--- /dev/null
+++ b/perception_setup/config/cameras/blackfly_s_calib.yaml
@@ -0,0 +1,31 @@
+image_width: 1440
+image_height: 1080
+camera_name: "blackfly_s"
+
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1121.47396, 0.0, 756.53693,
+ 0.0, 1120.84539, 607.43005,
+ 0.0, 0.0, 1.0]
+
+distortion_model: plumb_bob
+
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [-0.365699, 0.203235, -0.002605, -0.001334, -0.075892]
+
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1.0, 0.0, 0.0,
+ 0.0, 1.0, 0.0,
+ 0.0, 0.0, 1.0]
+
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [1121.47396, 0.0, 756.53693, 0.0,
+ 0.0, 1120.84539, 607.43005, 0.0,
+ 0.0, 0.0, 1.0, 0.0]
diff --git a/perception_setup/config/front_camera_params.yaml b/perception_setup/config/cameras/blackfly_s_driver_params.yaml
similarity index 96%
rename from perception_setup/config/front_camera_params.yaml
rename to perception_setup/config/cameras/blackfly_s_driver_params.yaml
index cf8cccf..4f2f522 100644
--- a/perception_setup/config/front_camera_params.yaml
+++ b/perception_setup/config/cameras/blackfly_s_driver_params.yaml
@@ -6,7 +6,7 @@
'dump_node_map': False
# set parameters defined in blackfly_s.yaml
'gain_auto': 'Continuous'
- # 'pixel_format': 'BayerRG8'
+ 'pixel_format': 'BGR8'
'exposure_auto': 'Continuous'
# These are useful for GigE cameras
# 'device_link_throughput_limit': 380000000
diff --git a/perception_setup/config/blackfly_s_params.yaml b/perception_setup/config/cameras/blackfly_s_params.yaml
similarity index 100%
rename from perception_setup/config/blackfly_s_params.yaml
rename to perception_setup/config/cameras/blackfly_s_params.yaml
diff --git a/perception_setup/config/cameras/color_realsense_d555_calib.yaml b/perception_setup/config/cameras/color_realsense_d555_calib.yaml
new file mode 100644
index 0000000..e141ccb
--- /dev/null
+++ b/perception_setup/config/cameras/color_realsense_d555_calib.yaml
@@ -0,0 +1,31 @@
+image_width: 896
+image_height: 504
+camera_name: color_camera
+
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [687.47, 0.0, 410.31,
+ 0.0, 572.95, 252.74,
+ 0.0, 0.0, 1.0]
+
+distortion_model: plumb_bob
+
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.268120, 0.659922, 0.004430, -0.024228, -0.353035]
+
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1.0, 0.0, 0.0,
+ 0.0, 1.0, 0.0,
+ 0.0, 0.0, 1.0]
+
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [687.47, 0.0, 410.31, 0.0,
+ 0.0, 572.95, 252.74, 0.0,
+ 0.0, 0.0, 1.0, 0.0]
diff --git a/perception_setup/config/downwards_cam_calib.yaml b/perception_setup/config/downwards_cam_calib.yaml
deleted file mode 100644
index 41037c9..0000000
--- a/perception_setup/config/downwards_cam_calib.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 720
-image_height: 540
-camera_name: downwards_cam
-camera_matrix:
- rows: 3
- cols: 3
- data: [530.6475899435, 0, 361.752285027748, 0, 530.585845717265, 292.6327059549775, 0, 0, 1]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [-0.356890438215317, 0.168613819658546, 0.0, 0.0, -0.0441848241077346]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
- rows: 3
- cols: 4
- data: [530.6475899435, 0, 361.752285027748, 0, 0, 530.585845717265, 292.6327059549775, 0, 0, 0, 1, 0]
diff --git a/perception_setup/config/downwards_cam_params.yaml b/perception_setup/config/downwards_cam_params.yaml
deleted file mode 100644
index cf8cccf..0000000
--- a/perception_setup/config/downwards_cam_params.yaml
+++ /dev/null
@@ -1,35 +0,0 @@
-/**:
- ros__parameters:
- 'debug': False
- 'compute_brightness': False
- 'adjust_timestamp': True
- 'dump_node_map': False
- # set parameters defined in blackfly_s.yaml
- 'gain_auto': 'Continuous'
- # 'pixel_format': 'BayerRG8'
- 'exposure_auto': 'Continuous'
- # These are useful for GigE cameras
- # 'device_link_throughput_limit': 380000000
- # 'gev_scps_packet_size': 9000
- # ---- to reduce the sensor width and shift the crop
- 'image_width': 720
- 'image_height': 540
- # 'offset_x': 16
- # 'offset_y': 0
- 'binning_x': 2
- 'binning_y': 2
- # 'connect_while_subscribed': True
- 'frame_rate_auto': 'Off'
- 'frame_rate': 40.0
- 'frame_rate_enable': True
- 'buffer_queue_size': 10
- 'trigger_mode': 'Off'
- 'chunk_mode_active': True
- 'chunk_selector_frame_id': 'FrameID'
- 'chunk_enable_frame_id': True
- 'chunk_selector_exposure_time': 'ExposureTime'
- 'chunk_enable_exposure_time': True
- 'chunk_selector_gain': 'Gain'
- 'chunk_enable_gain': True
- 'chunk_selector_timestamp': 'Timestamp'
- 'chunk_enable_timestamp': True
diff --git a/perception_setup/config/front_camera_calib.yaml b/perception_setup/config/front_camera_calib.yaml
deleted file mode 100644
index 9f4adb2..0000000
--- a/perception_setup/config/front_camera_calib.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 1440
-image_height: 1080
-camera_name: front_camera
-camera_matrix:
- rows: 3
- cols: 3
- data: [1061.29517988700, 0, 723.504570055496, 0, 1061.17169143453, 585.265411909955, 0, 0, 1]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 3
- data: [-0.356890438215317, 0.168613819658546, 0.0, 0.0, -0.0441848241077346]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
- rows: 3
- cols: 4
- data: [1061.29517988700, 0, 723.504570055496, 0, 0, 1061.17169143453, 585.265411909955, 0, 0, 0, 1, 0]
diff --git a/perception_setup/config/front_camera_calib_downscale.yaml b/perception_setup/config/front_camera_calib_downscale.yaml
deleted file mode 100644
index 1bf5ad0..0000000
--- a/perception_setup/config/front_camera_calib_downscale.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 720
-image_height: 540
-camera_name: front_camera
-camera_matrix:
- rows: 3
- cols: 3
- data: [530.6475899435, 0, 361.752285027748, 0, 530.585845717265, 292.6327059549775, 0, 0, 1]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [-0.356890438215317, 0.168613819658546, 0.0, 0.0, -0.0441848241077346]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
- rows: 3
- cols: 4
- data: [530.6475899435, 0, 361.752285027748, 0, 0, 530.585845717265, 292.6327059549775, 0, 0, 0, 1, 0]
diff --git a/perception_setup/config/gripper_camera_calib.yaml b/perception_setup/config/gripper_camera_calib.yaml
deleted file mode 100644
index 8390b31..0000000
--- a/perception_setup/config/gripper_camera_calib.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 1440
-image_height: 1080
-camera_name: gripper_camera
-camera_matrix:
- rows: 3
- cols: 3
- data: [1061.29517988700, 0, 723.504570055496, 0, 1061.17169143453, 585.265411909955, 0, 0, 1]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 3
- data: [-0.356890438215317, 0.168613819658546, 0.0, 0.0, -0.0441848241077346]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
- rows: 3
- cols: 4
- data: [1061.29517988700, 0, 723.504570055496, 0, 0, 1061.17169143453, 585.265411909955, 0, 0, 0, 1, 0]
diff --git a/perception_setup/config/gripper_camera_calib_downscale.yaml b/perception_setup/config/gripper_camera_calib_downscale.yaml
deleted file mode 100644
index 8b5460b..0000000
--- a/perception_setup/config/gripper_camera_calib_downscale.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 720
-image_height: 540
-camera_name: gripper_camera
-camera_matrix:
- rows: 3
- cols: 3
- data: [530.6475899435, 0, 361.752285027748, 0, 530.585845717265, 292.6327059549775, 0, 0, 1]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [-0.356890438215317, 0.168613819658546, 0.0, 0.0, -0.0441848241077346]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
- rows: 3
- cols: 4
- data: [530.6475899435, 0, 361.752285027748, 0, 0, 530.585845717265, 292.6327059549775, 0, 0, 0, 1, 0]
diff --git a/perception_setup/config/gripper_camera_params.yaml b/perception_setup/config/gripper_camera_params.yaml
deleted file mode 100644
index cf8cccf..0000000
--- a/perception_setup/config/gripper_camera_params.yaml
+++ /dev/null
@@ -1,35 +0,0 @@
-/**:
- ros__parameters:
- 'debug': False
- 'compute_brightness': False
- 'adjust_timestamp': True
- 'dump_node_map': False
- # set parameters defined in blackfly_s.yaml
- 'gain_auto': 'Continuous'
- # 'pixel_format': 'BayerRG8'
- 'exposure_auto': 'Continuous'
- # These are useful for GigE cameras
- # 'device_link_throughput_limit': 380000000
- # 'gev_scps_packet_size': 9000
- # ---- to reduce the sensor width and shift the crop
- 'image_width': 720
- 'image_height': 540
- # 'offset_x': 16
- # 'offset_y': 0
- 'binning_x': 2
- 'binning_y': 2
- # 'connect_while_subscribed': True
- 'frame_rate_auto': 'Off'
- 'frame_rate': 40.0
- 'frame_rate_enable': True
- 'buffer_queue_size': 10
- 'trigger_mode': 'Off'
- 'chunk_mode_active': True
- 'chunk_selector_frame_id': 'FrameID'
- 'chunk_enable_frame_id': True
- 'chunk_selector_exposure_time': 'ExposureTime'
- 'chunk_enable_exposure_time': True
- 'chunk_selector_gain': 'Gain'
- 'chunk_enable_gain': True
- 'chunk_selector_timestamp': 'Timestamp'
- 'chunk_enable_timestamp': True
diff --git a/perception_setup/config/image_filtering_params.yaml b/perception_setup/config/image_filtering_params.yaml
deleted file mode 100644
index 86237ad..0000000
--- a/perception_setup/config/image_filtering_params.yaml
+++ /dev/null
@@ -1,24 +0,0 @@
-/**:
- ros__parameters:
- # sub_topic: "/flir_camera/image_raw"
- sub_topic: "/gripper_camera/image_raw"
- pub_topic: "/filtered_image"
- filter_params:
- filter_type: "ebus"
- flip:
- flip_code: 1
- unsharpening:
- blur_size: 8
- erosion:
- size: 1
- dilation:
- size: 1
- white_balancing:
- contrast_percentage: 0.8
- ebus:
- erosion_size: 2
- blur_size: 30
- mask_weight: 5
-
-# Filter params should reflect the FilterParams struct
-# defined in /include/image_filters/image_processing.hpp
diff --git a/perception_setup/config/pipeline_filtering_params.yaml b/perception_setup/config/pipeline_filtering_params.yaml
deleted file mode 100644
index 35fc9a1..0000000
--- a/perception_setup/config/pipeline_filtering_params.yaml
+++ /dev/null
@@ -1,16 +0,0 @@
-/**:
- ros__parameters:
- sub_topic: "/downwards_cam/image_raw"
- pub_topic: "/filtered_image"
- filter_params:
- filter_type: "otsu"
- otsu:
- gsc_weight_r: 1.0 # Grayscale red weight
- gsc_weight_g: 1.0 # Grayscale green weight
- gsc_weight_b: 1.0 # Grayscale blue weight
- gamma_auto_correction: true
- gamma_auto_correction_weight: 2.0
- otsu_segmentation: true
-
-# Filter params should reflect the FilterParams struct
-# defined in /include/pipeline_filters/pipeline_processing.hpp
diff --git a/perception_setup/config/pipeline_line_fitting_params.yaml b/perception_setup/config/pipeline_line_fitting_params.yaml
deleted file mode 100644
index cd5d28c..0000000
--- a/perception_setup/config/pipeline_line_fitting_params.yaml
+++ /dev/null
@@ -1,17 +0,0 @@
-/**:
- ros__parameters:
- image_sub_topic: "/filtered_image"
- image_visualization_pub_topic: "/line_fitting/visualization"
- pose_array_pub_topic: "/line/pose_array"
- publish_visualization: false
- morph_close_size: 20
- dist_thresh: 0.2
- #randsac__parameters:
- n: 5 # How many random points to choose to start each iteration
- k: 500 # Number of iterations to run
- t: 50.0 # Distance threshold to consider a point as an inlier
- fracOfPoints: 0.001 # Fraction of points to be considered as inliers
- removeT: 1000.0 # Threshold to remove points after a line is detected
- finalScorethresh: 65.0 # Final score threshold to accept a line
- minTurnAngle: 1.5 # Minimum turning angle to consider a line
- size: 200 # Size of the sample points
diff --git a/perception_setup/config/valve_detection_params.yaml b/perception_setup/config/valve_detection_params.yaml
deleted file mode 100644
index d259f6a..0000000
--- a/perception_setup/config/valve_detection_params.yaml
+++ /dev/null
@@ -1,6 +0,0 @@
-/**:
- ros__parameters:
- depth_image_sub_topic: "/zed_node/depth/depth_registered"
- color_image_sub_topic: "/image_rect"
- detections_sub_topic: "/detections_output"
- line_detection_area: 70
diff --git a/perception_setup/config/yolo/yolo_cls.yaml b/perception_setup/config/yolo/yolo_cls.yaml
new file mode 100644
index 0000000..3a0047d
--- /dev/null
+++ b/perception_setup/config/yolo/yolo_cls.yaml
@@ -0,0 +1,20 @@
+# Model architecture contract — only change these if you change the model architecture
+# or its input/output tensor names. Deployment-specific values (model weights,
+# class names, visualizer, encoding, verbose) live in the launch file.
+
+# Tensor I/O bindings
+# Classification model output shape: [1, num_classes]
+input_tensor_names: ["input_tensor"]
+input_binding_names: ["images"]
+output_tensor_names: ["output_tensor"]
+output_binding_names: ["output0"]
+
+# Network input resolution (must match the model's expected input)
+network_image_width: 224
+network_image_height: 224
+image_mean: [0.0, 0.0, 0.0]
+image_stddev: [1.0, 1.0, 1.0]
+
+# Classification Decoder
+confidence_threshold: 0.25
+num_classes: 2
diff --git a/perception_setup/config/yolo/yolo_detect.yaml b/perception_setup/config/yolo/yolo_detect.yaml
new file mode 100644
index 0000000..b6671d9
--- /dev/null
+++ b/perception_setup/config/yolo/yolo_detect.yaml
@@ -0,0 +1,20 @@
+# Model architecture contract — only change these if you change the model architecture
+# or its input/output tensor names. Deployment-specific values (model weights,
+# class names, visualizer, encoding, verbose) live in the launch file.
+
+# Tensor I/O bindings
+input_tensor_names: ["input_tensor"]
+input_binding_names: ["images"]
+output_tensor_names: ["output_tensor"]
+output_binding_names: ["output0"]
+
+# Network
+network_image_width: 640
+network_image_height: 640
+image_mean: [0.0, 0.0, 0.0]
+image_stddev: [1.0, 1.0, 1.0]
+
+# Decoder
+confidence_threshold: 0.25
+nms_threshold: 0.45
+num_classes: 1
diff --git a/perception_setup/config/yolo/yolo_obb.yaml b/perception_setup/config/yolo/yolo_obb.yaml
new file mode 100644
index 0000000..50a3bc9
--- /dev/null
+++ b/perception_setup/config/yolo/yolo_obb.yaml
@@ -0,0 +1,22 @@
+# Model architecture contract — only change these if you change the model architecture
+# or its input/output tensor names. Deployment-specific values (model weights,
+# class names, visualizer, encoding, verbose) live in the launch file.
+
+# Tensor I/O bindings
+# OBB model exported with NMS; output shape: [1, 300, 7]
+input_tensor_names: ["input_tensor"]
+input_binding_names: ["images"]
+output_tensor_names: ["output_tensor"]
+output_binding_names: ["output0"]
+
+# Network input resolution (must match the model's expected input)
+network_image_width: 640
+network_image_height: 640
+image_mean: [0.0, 0.0, 0.0]
+image_stddev: [1.0, 1.0, 1.0]
+
+# OBB Decoder
+# NMS is embedded in the model export - only confidence filtering is applied here.
+confidence_threshold: 0.25
+# Must match the inner-batch dim of the output tensor [1, num_detections, 7]
+num_detections: 300
diff --git a/perception_setup/config/yolo/yolo_seg.yaml b/perception_setup/config/yolo/yolo_seg.yaml
new file mode 100644
index 0000000..c2aacba
--- /dev/null
+++ b/perception_setup/config/yolo/yolo_seg.yaml
@@ -0,0 +1,29 @@
+# Model architecture contract — only change these if you change the model architecture
+# or its input/output tensor names. Deployment-specific values (model weights,
+# class names, visualizer, encoding, output mask resolution, verbose) live in the
+# launch file.
+
+# Tensor I/O bindings
+# Segmentation model with embedded NMS; two outputs:
+# output0: [1, 300, 38] detections with 32 mask coefficients
+# output1: [1, 32, 160, 160] prototype masks
+input_tensor_names: ["input_tensor"]
+input_binding_names: ["images"]
+output_tensor_names: ["output_tensor", "mask_tensor"]
+output_binding_names: ["output0", "output1"]
+
+# Network input resolution (must match the model's expected input size)
+network_image_width: 640
+network_image_height: 640
+image_mean: [0.0, 0.0, 0.0]
+image_stddev: [1.0, 1.0, 1.0]
+
+# Segmentation Decoder
+# NMS is embedded in the model export - only confidence filtering is applied.
+confidence_threshold: 0.25
+# Must match the inner-batch dim of output0 [1, num_detections, 38]
+num_detections: 300
+# Prototype mask dimensions from output1 [1, num_protos, mask_height, mask_width]
+mask_width: 160
+mask_height: 160
+num_protos: 32
diff --git a/perception_setup/config/zed_driver_params.yaml b/perception_setup/config/zed_driver_params.yaml
deleted file mode 100644
index 831ccfb..0000000
--- a/perception_setup/config/zed_driver_params.yaml
+++ /dev/null
@@ -1,170 +0,0 @@
-/**:
- ros__parameters:
- general:
- camera_model: "zed2i"
- camera_name: "zed" # usually overwritten by launch file
- grab_resolution: 'HD720' # The native camera grab resolution. 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO'
- grab_frame_rate: 60 # ZED SDK internal grabbing rate
- svo_file: "live" # usually overwritten by launch file
- svo_loop: false # Enable loop mode when using an SVO as input source
- svo_realtime: true # if true the SVO will be played trying to respect the original framerate eventually skipping frames, otherwise every frame will be processed respecting the `pub_frame_rate` setting
- camera_timeout_sec: 5
- camera_max_reconnect: 5
- camera_flip: false
- zed_id: 0 # IMPORTANT: to be used in simulation to distinguish individual cameras im multi-camera configurations - usually overwritten by launch file
- serial_number: 0 # usually overwritten by launch file
- pub_resolution: 'NATIVE' # The resolution used for output. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission
- pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM'
- pub_frame_rate: 30.0 # frequency of publishing of visual images and depth images
- gpu_id: -1
- region_of_interest: '[]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent.
- #region_of_interest: '[[0.25,0.33],[0.75,0.33],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent.
- #region_of_interest: '[[0.25,0.25],[0.75,0.25],[0.75,0.75],[0.25,0.75]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent.
- #region_of_interest: '[[0.5,0.25],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent.
- sdk_verbose: 1
- video:
- brightness: 4 # [DYNAMIC] Not available for ZED X/ZED X Mini
- contrast: 4 # [DYNAMIC] Not available for ZED X/ZED X Mini
- hue: 0 # [DYNAMIC] Not available for ZED X/ZED X Mini
- saturation: 4 # [DYNAMIC]
- sharpness: 4 # [DYNAMIC]
- gamma: 8 # [DYNAMIC]
- auto_exposure_gain: true # [DYNAMIC]
- exposure: 80 # [DYNAMIC]
- gain: 80 # [DYNAMIC]
- auto_whitebalance: true # [DYNAMIC]
- whitebalance_temperature: 42 # [DYNAMIC] - [28,65] works only if `auto_whitebalance` is false
- qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL
- qos_depth: 1 # Queue size if using KEEP_LAST
- qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT -
- qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE
- depth:
- depth_mode: 'NEURAL' # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...)
- depth_stabilization: 1 # Forces positional tracking to start if major than 0 - Range: [0,100]
- openni_depth_mode: false # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters]
- point_cloud_freq: 10.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value)
- depth_confidence: 10 # [DYNAMIC]
- depth_texture_conf: 100 # [DYNAMIC]
- remove_saturated_areas: true # [DYNAMIC]
- min_depth: 0.2 # Min: 0.2, Max: 3.0
- max_depth: 5.0 # Max: 40.0
- qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL
- qos_depth: 1 # Queue size if using KEEP_LAST
- qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT -
- qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE
- pos_tracking:
- pos_tracking_enabled: true # True to enable positional tracking from start
- pos_tracking_mode: "GEN_2" # Matches the ZED SDK setting: 'GEN_1', 'GEN_2'
- imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used.
- publish_tf: true # [usually overwritten by launch file] publish `odom -> base_link` TF
- publish_map_tf: true # [usually overwritten by launch file] publish `map -> odom` TF
- base_frame: "base_link" # usually overwritten by launch file
- map_frame: "map"
- odometry_frame: "odom"
- area_memory_db_path: ""
- area_memory: true # Enable to detect loop closure
- depth_min_range: 0.0 # Set this value for removing fixed zones of the robot in the FoV of the camerafrom the visual odometry evaluation
- set_as_static: false # If 'true' the camera will be static and not move in the environment
- set_gravity_as_origin: true # If 'true' align the positional tracking world to imu gravity measurement. Keep the yaw from the user initial pose.
- floor_alignment: false # Enable to automatically calculate camera/floor offset
- initial_base_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Initial position of the `base_frame` in the map -> [X, Y, Z, R, P, Y]
- init_odom_with_first_valid_pose: true # Enable to initialize the odometry with the first valid pose
- path_pub_rate: 2.0 # [DYNAMIC] - Camera trajectory publishing frequency
- path_max_count: -1 # use '-1' for unlimited path size
- two_d_mode: false # Force navigation on a plane. If true the Z value will be fixed to "fixed_z_value", roll and pitch to zero
- fixed_z_value: 0.00 # Value to be used for Z coordinate if `two_d_mode` is true
- transform_time_offset: 0.0 # The value added to the timestamp of `map->odom` and `odom->base_link`` transform being generated
- qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL
- qos_depth: 1 # Queue size if using KEEP_LAST
- qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT
- qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE
- gnss_fusion:
- gnss_fusion_enabled: false # fuse 'sensor_msg/NavSatFix' message information into pose data
- gnss_fix_topic: "/gps/fix" # Name of the GNSS topic of type NavSatFix to subscribe [Default: "/gps/fix"]
- gnss_init_distance: 5.0 # The minimum distance traveled by the robot required to initialize the GNSS fusion [SDK <= v4.0.5]
- gnss_zero_altitude: false # Set to `true` to ignore GNSS altitude information
- gnss_frame: "gnss_link" # [usually overwritten by launch file] The TF frame of the GNSS sensor
- publish_utm_tf: true # Publish `utm` -> `map` TF
- broadcast_utm_transform_as_parent_frame: false # if 'true' publish `utm` -> `map` TF, otherwise `map` -> `utm`
- enable_reinitialization: true # determines whether reinitialization should be performed between GNSS and VIO fusion when a significant disparity is detected between GNSS data and the current fusion data. It becomes particularly crucial during prolonged GNSS signal loss scenarios.
- enable_rolling_calibration: true # If this parameter is set to true, the fusion algorithm will used a rough VIO / GNSS calibration at first and then refine it. This allow you to quickly get a fused position.
- enable_translation_uncertainty_target: false # When this parameter is enabled (set to true), the calibration process between GNSS and VIO accounts for the uncertainty in the determined translation, thereby facilitating the calibration termination. The maximum allowable uncertainty is controlled by the 'target_translation_uncertainty' parameter.
- gnss_vio_reinit_threshold: 5 # determines the threshold for GNSS/VIO reinitialization. If the fused position deviates beyond out of the region defined by the product of the GNSS covariance and the gnss_vio_reinit_threshold, a reinitialization will be triggered.
- target_translation_uncertainty: 10e-2 # defines the target translation uncertainty at which the calibration process between GNSS and VIO concludes. By default, the threshold is set at 10 centimeters.
- target_yaw_uncertainty: 0.1 # defines the target yaw uncertainty at which the calibration process between GNSS and VIO concludes. The unit of this parameter is in radian. By default, the threshold is set at 0.1 radians.
- mapping:
- mapping_enabled: false # True to enable mapping and fused point cloud pubblication
- resolution: 0.05 # maps resolution in meters [min: 0.01f - max: 0.2f]
- max_mapping_range: 5.0 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0]
- fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud
- clicked_point_topic: "/clicked_point" # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection
- pd_max_distance_threshold: 0.15 # Plane detection: controls the spread of plane by checking the position difference.
- pd_normal_similarity_threshold: 15.0 # Plane detection: controls the spread of plane by checking the angle difference.
- qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL
- qos_depth: 1 # Queue size if using KEEP_LAST
- qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT -
- qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE
- sensors:
- publish_imu_tf: false # [usually overwritten by launch file] enable/disable the IMU TF broadcasting
- sensors_image_sync: false # Synchronize Sensors messages with latest published video/depth message
- sensors_pub_rate: 200. # frequency of publishing of sensors data. MAX: 400. - MIN: grab rate
- qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL
- qos_depth: 1 # Queue size if using KEEP_LAST
- qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT -
- qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE
- object_detection:
- od_enabled: false # True to enable Object Detection
- model: 'MULTI_CLASS_BOX_MEDIUM' # 'MULTI_CLASS_BOX_FAST', 'MULTI_CLASS_BOX_MEDIUM', 'MULTI_CLASS_BOX_ACCURATE', 'PERSON_HEAD_BOX_FAST', 'PERSON_HEAD_BOX_ACCURATE'
- allow_reduced_precision_inference: true # Allow inference to run at a lower precision to improve runtime and memory usage
- max_range: 20.0 # [m] Defines a upper depth range for detections
- confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of an object [0,99]
- prediction_timeout: 0.5 # During this time [sec], the object will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions
- filtering_mode: 1 # '0': NONE - '1': NMS3D - '2': NMS3D_PER_CLASS
- mc_people: true # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_X' models
- mc_vehicle: true # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_X' models
- mc_bag: true # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_X' models
- mc_animal: true # [DYNAMIC] - Enable/disable the detection of animals for 'MULTI_CLASS_X' models
- mc_electronics: true # [DYNAMIC] - Enable/disable the detection of electronic devices for 'MULTI_CLASS_X' models
- mc_fruit_vegetable: true # [DYNAMIC] - Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_X' models
- mc_sport: true # [DYNAMIC] - Enable/disable the detection of sport-related objects for 'MULTI_CLASS_X' models
- qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL
- qos_depth: 1 # Queue size if using KEEP_LAST
- qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT
- qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE
- body_tracking:
- bt_enabled: false # True to enable Body Tracking
- model: 'HUMAN_BODY_MEDIUM' # 'HUMAN_BODY_FAST', 'HUMAN_BODY_MEDIUM', 'HUMAN_BODY_ACCURATE'
- body_format: 'BODY_38' # 'BODY_18','BODY_34','BODY_38','BODY_70'
- allow_reduced_precision_inference: false # Allow inference to run at a lower precision to improve runtime and memory usage
- max_range: 20.0 # [m] Defines a upper depth range for detections
- body_kp_selection: 'FULL' # 'FULL', 'UPPER_BODY'
- enable_body_fitting: false # Defines if the body fitting will be applied
- enable_tracking: true # Defines if the object detection will track objects across images flow
- prediction_timeout_s: 0.5 # During this time [sec], the skeleton will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions
- confidence_threshold: 50.0 # [DYNAMIC] - Minimum value of the detection confidence of skeleton key points [0,99]
- minimum_keypoints_threshold: 5 # [DYNAMIC] - Minimum number of skeleton key points to be detected for a valid skeleton
- qos_history: 1 # '1': KEEP_LAST - '2': KEEP_ALL
- qos_depth: 1 # Queue size if using KEEP_LAST
- qos_reliability: 1 # '1': RELIABLE - '2': BEST_EFFORT
- qos_durability: 2 # '1': TRANSIENT_LOCAL - '2': VOLATILE
- use_sim_time: false # EXPERIMENTAL (only for development) - Set to true to enable SIMULATION mode #
- sim_address: '192.168.1.90' # EXPERIMENTAL (only for development) - The local address of the machine running the simulator
- advanced: # WARNING: do not modify unless you are confident of what you are doing
- # Reference documentation: https://man7.org/linux/man-pages/man7/sched.7.html
- thread_sched_policy: 'SCHED_BATCH' # 'SCHED_OTHER', 'SCHED_BATCH', 'SCHED_FIFO', 'SCHED_RR' - NOTE: 'SCHED_FIFO' and 'SCHED_RR' require 'sudo'
- thread_grab_priority: 50 # ONLY with 'SCHED_FIFO' and 'SCHED_RR' - [1 (LOW) z-> 99 (HIGH)] - NOTE: 'sudo' required
- thread_sensor_priority: 70 # ONLY with 'SCHED_FIFO' and 'SCHED_RR' - [1 (LOW) z-> 99 (HIGH)] - NOTE: 'sudo' required
- thread_pointcloud_priority: 60 # ONLY with 'SCHED_FIFO' and 'SCHED_RR' - [1 (LOW) z-> 99 (HIGH)] - NOTE: 'sudo' required
- debug:
- debug_common: false
- debug_video_depth: false
- debug_camera_controls: false
- debug_point_cloud: false
- debug_positional_tracking: false
- debug_gnss: false
- debug_sensors: false
- debug_mapping: false
- debug_terrain_mapping: false
- debug_object_detection: false
- debug_body_tracking: false
- debug_advanced: false
diff --git a/perception_setup/include/perception_setup/image_undistort.hpp b/perception_setup/include/perception_setup/image_undistort.hpp
new file mode 100644
index 0000000..98beaa9
--- /dev/null
+++ b/perception_setup/include/perception_setup/image_undistort.hpp
@@ -0,0 +1,34 @@
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+
+namespace perception_setup {
+
+class ImageUndistort : public rclcpp::Node {
+ public:
+ explicit ImageUndistort(const rclcpp::NodeOptions& options);
+
+ private:
+ void init_maps_from_file(const std::string& path);
+ void build_maps(const cv::Mat& k, const cv::Mat& d, int w, int h);
+
+ void info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg);
+ void image_callback(const sensor_msgs::msg::Image::SharedPtr msg);
+ void relay_image(const sensor_msgs::msg::Image::SharedPtr msg);
+ void relay_camera_info(const sensor_msgs::msg::CameraInfo::SharedPtr msg);
+
+ rclcpp::Publisher::SharedPtr image_pub_;
+ rclcpp::Publisher::SharedPtr info_pub_;
+ rclcpp::Subscription::SharedPtr image_sub_;
+ rclcpp::Subscription::SharedPtr info_sub_;
+
+ cv::Mat map1_, map2_;
+ sensor_msgs::msg::CameraInfo rectified_info_;
+ bool maps_ready_{false};
+};
+
+} // namespace perception_setup
diff --git a/perception_setup/launch/cameras/blackfly_s.launch.py b/perception_setup/launch/cameras/blackfly_s.launch.py
new file mode 100644
index 0000000..a515e0f
--- /dev/null
+++ b/perception_setup/launch/cameras/blackfly_s.launch.py
@@ -0,0 +1,101 @@
+"""FLIR Blackfly S downwards camera launch file.
+
+Starts:
+ - Spinnaker camera driver component in a dedicated container
+ - image_to_gstreamer to stream the raw image over RTP/H.265
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+ pkg_dir = get_package_share_directory('perception_setup')
+
+ driver_params = os.path.join(
+ pkg_dir, 'config', 'cameras', 'blackfly_s_driver_params.yaml'
+ )
+ spinnaker_map = os.path.join(pkg_dir, 'config', 'cameras', 'blackfly_s_params.yaml')
+ calib_path = os.path.join(pkg_dir, 'config', 'cameras', 'blackfly_s_calib.yaml')
+
+ drone = LaunchConfiguration('drone')
+
+ flir_container = ComposableNodeContainer(
+ name='blackfly_s_container',
+ namespace='',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ ComposableNode(
+ package='spinnaker_camera_driver',
+ plugin='spinnaker_camera_driver::CameraDriver',
+ name='blackfly_s',
+ parameters=[
+ driver_params,
+ {
+ 'parameter_file': spinnaker_map,
+ 'serial_number': '23494258',
+ 'camerainfo_url': f'file://{calib_path}',
+ },
+ ],
+ remappings=[
+ ('~/control', '/exposure_control/control'),
+ ('/blackfly_s/image_raw', ['/', drone, '/down_camera/image_color']),
+ (
+ '/blackfly_s/camera_info',
+ ['/', drone, '/down_camera/camera_info'],
+ ),
+ ],
+ extra_arguments=[{'use_intra_process_comms': True}],
+ condition=IfCondition(LaunchConfiguration('enable_camera')),
+ )
+ ],
+ output='screen',
+ )
+
+ image_to_gstreamer_node = Node(
+ package='image_to_gstreamer',
+ executable='image_to_gstreamer_node',
+ name='image_to_gstreamer_node',
+ additional_env={'EGL_PLATFORM': 'surfaceless'},
+ parameters=[
+ {
+ 'input_topic': ['/', drone, '/down_camera/image_color'],
+ 'host': '10.0.0.68',
+ 'port': 5001,
+ 'bitrate': 500000,
+ 'framerate': 15,
+ 'preset_level': 1,
+ 'iframe_interval': 15,
+ 'control_rate': 1,
+ 'pt': 96,
+ 'config_interval': 1,
+ 'format': 'RGB',
+ }
+ ],
+ output='screen',
+ )
+
+ return LaunchDescription(
+ [
+ DeclareLaunchArgument(
+ 'drone',
+ default_value='nautilus',
+ description='Drone name, prepended to all published topics (e.g. /nautilus/down_camera/image_color)',
+ ),
+ DeclareLaunchArgument(
+ 'enable_camera',
+ default_value='true',
+ description='Enable FLIR Blackfly S camera component',
+ ),
+ flir_container,
+ image_to_gstreamer_node,
+ ]
+ )
diff --git a/perception_setup/launch/cameras/realsense_d555.launch.py b/perception_setup/launch/cameras/realsense_d555.launch.py
new file mode 100644
index 0000000..88e0305
--- /dev/null
+++ b/perception_setup/launch/cameras/realsense_d555.launch.py
@@ -0,0 +1,138 @@
+"""RealSense D555 camera launch file.
+
+Starts:
+ - RealSense camera driver node (raw color + depth)
+ - image_undistort (undistorts color image using color_realsense_d555_calib.yaml)
+ - image_to_gstreamer to stream the undistorted color image
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+ pkg_dir = get_package_share_directory('perception_setup')
+
+ calib_file = os.path.join(
+ pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml'
+ )
+
+ drone = LaunchConfiguration('drone')
+
+ realsense_node = Node(
+ package='realsense2_camera',
+ executable='realsense2_camera_node',
+ name='camera',
+ namespace='camera',
+ parameters=[
+ {
+ 'enable_color': True,
+ # NB: When updating the image size, also update the calibration file
+ 'rgb_camera.color_profile': '896,504,15',
+ 'rgb_camera.color_format': 'BGR8',
+ 'rgb_camera.enable_auto_exposure': True,
+ 'enable_depth': True,
+ 'depth_module.depth_profile': '896,504,15',
+ 'depth_module.depth_format': 'Z16',
+ 'depth_module.enable_auto_exposure': True,
+ 'depth_module.emitter_enabled': False,
+ 'enable_infra1': False,
+ 'enable_infra2': False,
+ 'enable_gyro': False,
+ 'enable_accel': False,
+ 'enable_motion': False,
+ 'publish_tf': False,
+ 'enable_sync': False,
+ }
+ ],
+ remappings=[
+ (
+ '/camera/camera/depth/image_rect_raw',
+ ['/', drone, '/depth_camera/image_depth'],
+ ),
+ (
+ '/camera/camera/depth/camera_info',
+ ['/', drone, '/depth_camera/camera_info'],
+ ),
+ ],
+ output='screen',
+ arguments=['--ros-args', '--log-level', 'info'],
+ emulate_tty=True,
+ )
+
+ image_undistort_container = ComposableNodeContainer(
+ name='image_undistort_container',
+ namespace='',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ ComposableNode(
+ package='perception_setup',
+ plugin='perception_setup::ImageUndistort',
+ name='color_image_undistort',
+ parameters=[
+ {
+ 'image_topic': '/camera/camera/color/image_raw',
+ 'camera_info_file': calib_file,
+ 'raw_camera_info_topic': '/camera/camera/color/camera_info',
+ 'output_image_topic': ['/', drone, '/front_camera/image_color'],
+ 'output_camera_info_topic': [
+ '/',
+ drone,
+ '/front_camera/camera_info',
+ ],
+ 'enable_undistort': LaunchConfiguration('enable_undistort'),
+ 'image_qos': 'reliable',
+ }
+ ],
+ ),
+ ],
+ output='screen',
+ )
+
+ image_to_gstreamer_node = Node(
+ package='image_to_gstreamer',
+ executable='image_to_gstreamer_node',
+ name='image_to_gstreamer_node',
+ additional_env={'EGL_PLATFORM': 'surfaceless'},
+ parameters=[
+ {
+ 'input_topic': ['/', drone, '/front_camera/image_color'],
+ 'host': '10.0.0.169',
+ 'port': 5000,
+ 'bitrate': 500000,
+ 'framerate': 15,
+ 'preset_level': 1,
+ 'iframe_interval': 15,
+ 'control_rate': 1,
+ 'pt': 96,
+ 'config_interval': 1,
+ 'format': 'RGB',
+ }
+ ],
+ output='screen',
+ )
+
+ return LaunchDescription(
+ [
+ DeclareLaunchArgument(
+ 'drone',
+ default_value='nautilus',
+ description='Drone name, prepended to all published topics (e.g. /nautilus/front_camera/image_color)',
+ ),
+ DeclareLaunchArgument(
+ 'enable_undistort',
+ default_value='true',
+ description='Undistort color image before publishing to image_topic',
+ ),
+ realsense_node,
+ image_undistort_container,
+ image_to_gstreamer_node,
+ ]
+ )
diff --git a/perception_setup/launch/cameras/sonar.launch.py b/perception_setup/launch/cameras/sonar.launch.py
new file mode 100644
index 0000000..f82cdca
--- /dev/null
+++ b/perception_setup/launch/cameras/sonar.launch.py
@@ -0,0 +1,71 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ config = os.path.join(
+ get_package_share_directory('norbit_fls_ros_interface'),
+ 'config',
+ 'norbit_fls_ros_interface_params.yaml',
+ )
+
+ namespace = LaunchConfiguration('namespace')
+
+ norbit_fls_ros_interface_node = Node(
+ package='norbit_fls_ros_interface',
+ executable='norbit_fls_ros_interface_node',
+ name='norbit_fls_ros_interface_node',
+ namespace=namespace,
+ parameters=[config],
+ output='screen',
+ )
+
+ sonar_overlay_node = Node(
+ package='norbit_fls_ros_interface',
+ executable='sonar_overlay_node',
+ name='sonar_overlay_node',
+ namespace=namespace,
+ parameters=[config],
+ output='screen',
+ )
+
+ image_to_gstreamer_node = Node(
+ package='image_to_gstreamer',
+ executable='image_to_gstreamer_node',
+ name='image_to_gstreamer_node',
+ additional_env={'EGL_PLATFORM': 'surfaceless'},
+ parameters=[
+ {
+ 'input_topic': '/fls_image/display_mono',
+ 'host': '10.0.0.68',
+ 'port': 5002,
+ 'bitrate': 500000,
+ 'framerate': 15,
+ 'preset_level': 1,
+ 'iframe_interval': 15,
+ 'control_rate': 1,
+ 'pt': 96,
+ 'config_interval': 1,
+ 'format': 'GRAY8',
+ }
+ ],
+ output='screen',
+ )
+
+ return LaunchDescription(
+ [
+ DeclareLaunchArgument(
+ 'namespace',
+ default_value='',
+ description='Namespace for the norbit_fls_ros_interface node',
+ ),
+ norbit_fls_ros_interface_node,
+ sonar_overlay_node,
+ image_to_gstreamer_node,
+ ]
+ )
diff --git a/perception_setup/launch/docking_blackfly_s.launch.py b/perception_setup/launch/docking_blackfly_s.launch.py
new file mode 100644
index 0000000..975b4bc
--- /dev/null
+++ b/perception_setup/launch/docking_blackfly_s.launch.py
@@ -0,0 +1,125 @@
+"""Blackfly S -> ArUco Detection -> GStreamer stream.
+
+Pipeline:
+1. Blackfly S camera driver publishes raw BGR image
+2. aruco_detector runs on the raw image, publishes detections and visualization
+3. image_to_gstreamer streams the ArUco visualization image over RTP/H.265
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+ pkg_dir = get_package_share_directory('perception_setup')
+
+ driver_params = os.path.join(
+ pkg_dir, 'config', 'cameras', 'blackfly_s_driver_params.yaml'
+ )
+ spinnaker_map = os.path.join(pkg_dir, 'config', 'cameras', 'blackfly_s_params.yaml')
+ calib_path = os.path.join(pkg_dir, 'config', 'cameras', 'blackfly_s_calib.yaml')
+
+ drone = LaunchConfiguration('drone')
+
+ docking_container = ComposableNodeContainer(
+ name='docking_blackfly_s_container',
+ namespace='',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ ComposableNode(
+ package='spinnaker_camera_driver',
+ plugin='spinnaker_camera_driver::CameraDriver',
+ name='blackfly_s',
+ parameters=[
+ driver_params,
+ {
+ 'parameter_file': spinnaker_map,
+ 'serial_number': '23494258',
+ 'camerainfo_url': f'file://{calib_path}',
+ },
+ ],
+ remappings=[
+ ('~/control', '/exposure_control/control'),
+ ('/blackfly_s/image_raw', ['/', drone, '/down_camera/image_color']),
+ (
+ '/blackfly_s/camera_info',
+ ['/', drone, '/down_camera/camera_info'],
+ ),
+ ],
+ extra_arguments=[{'use_intra_process_comms': True}],
+ condition=IfCondition(LaunchConfiguration('enable_camera')),
+ ),
+ ComposableNode(
+ package='aruco_detector',
+ plugin='ArucoDetectorNode',
+ name='aruco_detector_node',
+ parameters=[
+ os.path.join(
+ get_package_share_directory('aruco_detector'),
+ 'config',
+ 'aruco_detector_params.yaml',
+ ),
+ {
+ 'subs.image_topic': ['/', drone, '/down_camera/image_color'],
+ 'subs.camera_info_topic': [
+ '/',
+ drone,
+ '/down_camera/camera_info',
+ ],
+ 'pubs.aruco_image': '/down_cam/aruco_detector/image',
+ 'out_tf_frame': 'nautilus/downwards_camera_optical',
+ },
+ ],
+ ),
+ ],
+ output='screen',
+ arguments=['--ros-args', '--log-level', 'info'],
+ )
+
+ image_to_gstreamer_node = Node(
+ package='image_to_gstreamer',
+ executable='image_to_gstreamer_node',
+ name='image_to_gstreamer_node',
+ additional_env={'EGL_PLATFORM': 'surfaceless'},
+ parameters=[
+ {
+ 'input_topic': '/down_cam/aruco_detector/image',
+ 'host': '10.0.0.169',
+ 'port': 5001,
+ 'bitrate': 500000,
+ 'framerate': 15,
+ 'preset_level': 1,
+ 'iframe_interval': 15,
+ 'control_rate': 1,
+ 'pt': 96,
+ 'config_interval': 1,
+ 'format': 'BGR',
+ }
+ ],
+ output='screen',
+ )
+
+ return LaunchDescription(
+ [
+ DeclareLaunchArgument(
+ 'drone',
+ default_value='nautilus',
+ description='Drone name, prepended to all published topics (e.g. /nautilus/down_camera/image_color)',
+ ),
+ DeclareLaunchArgument(
+ 'enable_camera',
+ default_value='true',
+ description='Enable FLIR Blackfly S camera component',
+ ),
+ docking_container,
+ image_to_gstreamer_node,
+ ]
+ )
diff --git a/perception_setup/launch/docking_realsense_d555.launch.py b/perception_setup/launch/docking_realsense_d555.launch.py
new file mode 100644
index 0000000..8e97d94
--- /dev/null
+++ b/perception_setup/launch/docking_realsense_d555.launch.py
@@ -0,0 +1,142 @@
+"""RealSense D555 -> ArUco Detection -> GStreamer stream.
+
+Pipeline:
+1. RealSense D555 camera driver publishes raw RGB color image
+2. image_undistort undistorts the raw color image
+3. aruco_detector runs on the undistorted image, publishes detections and visualization
+4. image_to_gstreamer streams the ArUco visualization image over RTP/H.265
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+ pkg_dir = get_package_share_directory('perception_setup')
+
+ calib_file = os.path.join(
+ pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml'
+ )
+
+ drone = LaunchConfiguration('drone')
+
+ docking_container = ComposableNodeContainer(
+ name='docking_realsense_d555_container',
+ namespace='',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ ComposableNode(
+ package='realsense2_camera',
+ plugin='realsense2_camera::RealSenseNodeFactory',
+ name='camera',
+ namespace='camera',
+ parameters=[
+ {
+ 'enable_color': True,
+ 'rgb_camera.color_profile': '896,504,15',
+ 'rgb_camera.color_format': 'RGB8',
+ 'rgb_camera.enable_auto_exposure': True,
+ 'enable_depth': False,
+ 'enable_infra1': False,
+ 'enable_infra2': False,
+ 'enable_gyro': False,
+ 'enable_accel': False,
+ 'enable_motion': False,
+ 'publish_tf': False,
+ 'enable_sync': False,
+ }
+ ],
+ ),
+ ComposableNode(
+ package='perception_setup',
+ plugin='perception_setup::ImageUndistort',
+ name='color_image_undistort',
+ parameters=[
+ {
+ 'image_topic': '/camera/camera/color/image_raw',
+ 'camera_info_file': calib_file,
+ 'raw_camera_info_topic': '/camera/camera/color/camera_info',
+ 'output_image_topic': ['/', drone, '/front_camera/image_color'],
+ 'output_camera_info_topic': [
+ '/',
+ drone,
+ '/front_camera/camera_info',
+ ],
+ 'enable_undistort': LaunchConfiguration('enable_undistort'),
+ 'image_qos': 'sensor_data',
+ }
+ ],
+ ),
+ ComposableNode(
+ package='aruco_detector',
+ plugin='ArucoDetectorNode',
+ name='aruco_detector_node',
+ parameters=[
+ os.path.join(
+ get_package_share_directory('aruco_detector'),
+ 'config',
+ 'aruco_detector_params.yaml',
+ ),
+ {
+ 'subs.image_topic': ['/', drone, '/front_camera/image_color'],
+ 'subs.camera_info_topic': [
+ '/',
+ drone,
+ '/front_camera/camera_info',
+ ],
+ 'pubs.aruco_image': '/front_camera/aruco_detector/image',
+ 'out_tf_frame': ['/', drone, '/front_camera_optical'],
+ },
+ ],
+ ),
+ ],
+ output='screen',
+ arguments=['--ros-args', '--log-level', 'info'],
+ )
+
+ image_to_gstreamer_node = Node(
+ package='image_to_gstreamer',
+ executable='image_to_gstreamer_node',
+ name='image_to_gstreamer_node',
+ additional_env={'EGL_PLATFORM': 'surfaceless'},
+ parameters=[
+ {
+ 'input_topic': '/front_camera/aruco_detector/image',
+ 'host': '10.0.0.169',
+ 'port': 5001,
+ 'bitrate': 500000,
+ 'framerate': 15,
+ 'preset_level': 1,
+ 'iframe_interval': 15,
+ 'control_rate': 1,
+ 'pt': 96,
+ 'config_interval': 1,
+ 'format': 'RGB',
+ }
+ ],
+ output='screen',
+ )
+
+ return LaunchDescription(
+ [
+ DeclareLaunchArgument(
+ 'drone',
+ default_value='nautilus',
+ description='Drone name, prepended to all published topics (e.g. /nautilus/front_camera/image_color)',
+ ),
+ DeclareLaunchArgument(
+ 'enable_undistort',
+ default_value='true',
+ description='Undistort color image before ArUco detection',
+ ),
+ docking_container,
+ image_to_gstreamer_node,
+ ]
+ )
diff --git a/perception_setup/launch/isaac_ros/isaac_ros_valve_intervention.launch.py b/perception_setup/launch/isaac_ros/isaac_ros_valve_intervention.launch.py
new file mode 100644
index 0000000..4c73367
--- /dev/null
+++ b/perception_setup/launch/isaac_ros/isaac_ros_valve_intervention.launch.py
@@ -0,0 +1,219 @@
+"""Simulator -> YOLO OBB -> Valve Detection.
+
+Simulator variant of valve_intervention.launch.py. Instead of running a
+RealSense camera and image_undistort, this launch file accepts the color
+image topic, camera info topic, depth topics, and image dimensions directly
+as launch arguments.
+
+Pipeline:
+1. Simulator publishes color image, camera info, depth image and depth info
+2. YOLO OBB detects oriented bounding boxes on the color image
+3. Valve Detection uses oriented bounding boxes + depth image to compute valve pose
+"""
+
+import os
+
+import launch
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import (
+ IncludeLaunchDescription,
+ OpaqueFunction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def _launch_setup(context, *args, **kwargs):
+ pkg_dir = get_package_share_directory('perception_setup')
+ models_dir = os.path.join(pkg_dir, 'models')
+
+ with open(os.path.join(pkg_dir, 'config', 'yolo', 'yolo_obb.yaml')) as f:
+ yolo_cfg = yaml.safe_load(f)
+
+ model_file_path = os.path.join(models_dir, 'obb_best_simulator.onnx')
+ engine_file_path = os.path.join(models_dir, 'obb_best_simulator.engine')
+
+ image_format_converter = ComposableNode(
+ package='isaac_ros_image_proc',
+ plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
+ name='image_format_converter',
+ parameters=[
+ {
+ 'encoding_desired': 'rgb8',
+ 'image_width': 640,
+ 'image_height': 640,
+ 'input_qos': 'sensor_data',
+ }
+ ],
+ remappings=[
+ ('image_raw', '/nautilus/front_camera/image_color'),
+ ('image', '/yolo_obb/internal/converted_image'),
+ ],
+ )
+
+ tensor_rt_node = ComposableNode(
+ name='tensor_rt',
+ package='isaac_ros_tensor_rt',
+ plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode',
+ parameters=[
+ {
+ 'model_file_path': model_file_path,
+ 'engine_file_path': engine_file_path,
+ 'output_binding_names': yolo_cfg['output_binding_names'],
+ 'output_tensor_names': yolo_cfg['output_tensor_names'],
+ 'input_tensor_names': yolo_cfg['input_tensor_names'],
+ 'input_binding_names': yolo_cfg['input_binding_names'],
+ 'verbose': True,
+ 'force_engine_update': False,
+ 'tensor_output_topic': '/yolo_obb/tensor_pub',
+ }
+ ],
+ remappings=[
+ ('tensor_pub', '/yolo_obb/tensor_pub'),
+ ('tensor_sub', '/yolo_obb/tensor_sub'),
+ ],
+ )
+
+ yolo_obb_decoder_node = ComposableNode(
+ name='yolo_obb_decoder_node',
+ package='isaac_ros_yolov26_obb',
+ plugin='nvidia::isaac_ros::yolov26_obb::YoloV26OBBDecoderNode',
+ parameters=[
+ {
+ 'tensor_input_topic': '/yolo_obb/tensor_sub',
+ 'confidence_threshold': float(yolo_cfg['confidence_threshold']),
+ 'num_detections': int(yolo_cfg['num_detections']),
+ 'detections_topic': '/obb_detections_output',
+ }
+ ],
+ )
+
+ tensor_rt_container = ComposableNodeContainer(
+ name='obb_tensor_rt_container',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ image_format_converter,
+ tensor_rt_node,
+ yolo_obb_decoder_node,
+ ],
+ output='screen',
+ arguments=['--ros-args', '--log-level', 'INFO'],
+ namespace='',
+ )
+
+ encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder')
+ dnn_image_encoder_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py')
+ ),
+ launch_arguments={
+ 'input_image_width': str(640),
+ 'input_image_height': str(640),
+ 'network_image_width': str(yolo_cfg['network_image_width']),
+ 'network_image_height': str(yolo_cfg['network_image_height']),
+ 'image_mean': str(yolo_cfg['image_mean']),
+ 'image_stddev': str(yolo_cfg['image_stddev']),
+ 'attach_to_shared_component_container': 'True',
+ 'component_container_name': 'obb_tensor_rt_container',
+ 'dnn_image_encoder_namespace': 'yolo_obb_encoder/internal',
+ 'image_input_topic': '/yolo_obb/internal/converted_image',
+ 'camera_info_input_topic': '/nautilus/front_camera/camera_info',
+ 'tensor_output_topic': '/yolo_obb/tensor_pub',
+ }.items(),
+ )
+
+ yolo_obb_visualizer = Node(
+ package='isaac_ros_yolov26_obb',
+ executable='isaac_ros_yolov26_obb_visualizer.py',
+ name='yolo_obb_visualizer',
+ parameters=[
+ {
+ 'detections_topic': '/obb_detections_output',
+ 'image_topic': '/yolo_obb_encoder/internal/resize/image',
+ 'output_image_topic': '/yolo_obb_processed_image',
+ 'class_names_yaml': str({0: 'valve'}),
+ }
+ ],
+ )
+
+ valve_detection_tuning = os.path.join(
+ get_package_share_directory('valve_detection'),
+ 'config',
+ 'valve_detection_params.yaml',
+ )
+
+ valve_detection_container = ComposableNodeContainer(
+ name='valve_detection_container',
+ namespace='',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ ComposableNode(
+ package='valve_detection',
+ plugin='valve_detection::ValvePoseNode',
+ name='valve_pose_node',
+ parameters=[
+ valve_detection_tuning,
+ {
+ 'depth_image_sub_topic': '/nautilus/depth_camera/image_depth',
+ 'detections_sub_topic': '/obb_detections_output',
+ 'depth_image_info_topic': '/nautilus/depth_camera/camera_info',
+ 'depth_frame_id': 'front_camera_depth_optical',
+ 'color_frame_id': 'front_camera_color_optical',
+ 'landmarks_pub_topic': '/valve_landmarks',
+ 'output_frame_id': 'front_camera_depth_optical',
+ 'drone': 'nautilus',
+ 'undistort_detections': False,
+ 'debug_visualize': True,
+ 'clamp_rotation': True,
+ 'use_hardcoded_extrinsic': True,
+ 'extrinsic_tx': -0.059,
+ 'extrinsic_ty': 0.0,
+ 'extrinsic_tz': 0.0,
+ 'debug.depth_colormap_value_min': 0.1,
+ 'debug.depth_colormap_value_max': 2.0,
+ },
+ ],
+ )
+ ],
+ output='screen',
+ )
+
+ image_to_gstreamer_node = Node(
+ package='image_to_gstreamer',
+ executable='image_to_gstreamer_node',
+ name='image_to_gstreamer_node',
+ additional_env={'EGL_PLATFORM': 'surfaceless'},
+ parameters=[
+ {
+ 'input_topic': '/yolo_obb_processed_image',
+ 'host': '10.0.0.169',
+ 'port': 5000,
+ 'bitrate': 500000,
+ 'framerate': 15,
+ 'preset_level': 1,
+ 'iframe_interval': 15,
+ 'control_rate': 1,
+ 'pt': 96,
+ 'config_interval': 1,
+ 'format': 'RGB',
+ }
+ ],
+ output='screen',
+ )
+
+ actions = [
+ tensor_rt_container,
+ dnn_image_encoder_launch,
+ valve_detection_container,
+ image_to_gstreamer_node,
+ yolo_obb_visualizer,
+ ]
+ return actions
+
+
+def generate_launch_description():
+ return launch.LaunchDescription([OpaqueFunction(function=_launch_setup)])
diff --git a/perception_setup/launch/perception.launch.py b/perception_setup/launch/perception.launch.py
deleted file mode 100644
index 22d1d72..0000000
--- a/perception_setup/launch/perception.launch.py
+++ /dev/null
@@ -1,244 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, GroupAction
-from launch.conditions import IfCondition, UnlessCondition
-from launch.substitutions import (
- LaunchConfiguration,
- PathJoinSubstitution,
-)
-from launch_ros.actions import ComposableNodeContainer, Node
-from launch_ros.descriptions import ComposableNode
-from launch_ros.substitutions import FindPackageShare
-
-
-def generate_launch_description():
- enable_filtering = LaunchConfiguration('enable_filtering')
- enable_filtering_arg = DeclareLaunchArgument(
- 'enable_filtering',
- default_value='False',
- description='enable image filtering',
- )
-
- enable_aruco = LaunchConfiguration('enable_aruco')
- enable_aruco_arg = DeclareLaunchArgument(
- 'enable_aruco',
- default_value='False',
- description='enable Aruco detection',
- )
-
- enable_gripper_camera = LaunchConfiguration('enable_gripper_camera')
- enable_gripper_camera_arg = DeclareLaunchArgument(
- 'enable_gripper_camera',
- default_value='False',
- description='enable gripper camera',
- )
-
- enable_front_camera = LaunchConfiguration('enable_front_camera')
- enable_front_camera_arg = DeclareLaunchArgument(
- 'enable_front_camera',
- default_value='False',
- description='enable front camera',
- )
-
- enable_composable_nodes = LaunchConfiguration('enable_composable_nodes')
- enable_composable_nodes_arg = DeclareLaunchArgument(
- 'enable_composable_nodes',
- default_value='True',
- description='enable composable nodes',
- )
-
- # Note: Params file for composable node is different format from params file for standard node.
- # However, to make the files compatible for both we use /** as namespace.
- # See https://github.com/ros2/launch_ros/pull/259 for more info.
-
- filtering_params_file = os.path.join(
- get_package_share_directory('perception_setup'),
- 'config',
- 'image_filtering_params.yaml',
- )
-
- aruco_params_file = os.path.join(
- get_package_share_directory('perception_setup'),
- 'config',
- 'aruco_detector_params.yaml',
- )
-
- gripper_params_file = PathJoinSubstitution(
- [FindPackageShare('perception_setup'), 'config', 'gripper_camera_params.yaml']
- )
-
- front_camera_params_file = PathJoinSubstitution(
- [FindPackageShare('perception_setup'), 'config', 'front_camera_params.yaml']
- )
-
- blackfly_s_config_file = PathJoinSubstitution(
- [FindPackageShare('perception_setup'), 'config', 'blackfly_s_params.yaml']
- )
-
- package_share_directory = get_package_share_directory('perception_setup')
- gripper_camera_calib_path = os.path.join(
- package_share_directory, 'config', 'gripper_camera_calib_downscale.yaml'
- )
- # gripper_camera_calib_path = os.path.join(package_share_directory, 'config', 'gripper_camera_calib.yaml')
- front_camera_calib_path = os.path.join(
- package_share_directory, 'config', 'front_camera_calib_downscale.yaml'
- )
-
- # Add 'file://' prefix to the path required by the CameraInfoManager that sets the camera calibration in the spinnaker driver
- gripper_camera_calib_url = f'file://{gripper_camera_calib_path}'
- front_camera_calib_url = f'file://{front_camera_calib_path}'
-
- composable_node_container = ComposableNodeContainer(
- name='perception_image_proc_container',
- namespace='',
- package='rclcpp_components',
- executable='component_container_mt',
- composable_node_descriptions=[
- ComposableNode(
- package='image_filtering',
- plugin='vortex::image_processing::ImageFilteringNode',
- name='image_filtering',
- parameters=[filtering_params_file],
- extra_arguments=[{'use_intra_process_comms': False}],
- condition=IfCondition(enable_filtering),
- ),
- ComposableNode(
- package='aruco_detector',
- plugin='vortex::aruco_detector::ArucoDetectorNode',
- name='aruco_detector',
- parameters=[aruco_params_file],
- extra_arguments=[{'use_intra_process_comms': True}],
- condition=IfCondition(enable_aruco),
- ),
- ComposableNode(
- package='spinnaker_camera_driver',
- plugin='spinnaker_camera_driver::CameraDriver',
- name='gripper_camera',
- parameters=[
- gripper_params_file,
- {
- 'parameter_file': blackfly_s_config_file,
- 'serial_number': '23494258',
- 'camerainfo_url': gripper_camera_calib_url,
- },
- ],
- remappings=[
- ('~/control', '/exposure_control/control'),
- ('/flir_camera/image_raw', '/gripper_camera/image_raw'),
- ('/flir_camera/camera_info', '/gripper_camera/camera_info'),
- ],
- extra_arguments=[{'use_intra_process_comms': False}],
- condition=IfCondition(enable_gripper_camera),
- ),
- ComposableNode(
- package='spinnaker_camera_driver',
- plugin='spinnaker_camera_driver::CameraDriver',
- name='front_camera',
- parameters=[
- front_camera_params_file,
- {
- 'parameter_file': blackfly_s_config_file,
- 'serial_number': '23494259',
- 'camerainfo_url': front_camera_calib_url,
- },
- ],
- remappings=[
- ('~/control', '/exposure_control/control'),
- ('/flir_camera/image_raw', '/front_camera/image_raw'),
- ('/flir_camera/camera_info', '/front_camera/camera_info'),
- ],
- extra_arguments=[{'use_intra_process_comms': False}],
- condition=IfCondition(enable_front_camera),
- ),
- ],
- output='screen',
- condition=IfCondition(enable_composable_nodes),
- )
-
- # Create separate nodes for each component to launch them individually if composable nodes are disabled
- filtering_node = Node(
- package='image_filtering',
- executable='image_filtering_node',
- name='image_filtering',
- parameters=[filtering_params_file],
- output='screen',
- condition=IfCondition(enable_filtering),
- )
-
- aruco_node = Node(
- package='aruco_detector',
- executable='aruco_detector_node',
- name='aruco_detector',
- parameters=[aruco_params_file],
- output='screen',
- condition=IfCondition(enable_aruco),
- )
-
- gripper_camera_node = Node(
- package='spinnaker_camera_driver',
- executable='camera_driver_node',
- name='gripper_camera',
- parameters=[
- gripper_params_file,
- {
- 'parameter_file': blackfly_s_config_file,
- 'serial_number': '23494258',
- 'camerainfo_url': gripper_camera_calib_url,
- },
- ],
- remappings=[
- ('~/control', '/exposure_control/control'),
- ('/flir_camera/image_raw', '/gripper_camera/image_raw'),
- ('/flir_camera/camera_info', '/gripper_camera/camera_info'),
- ],
- output='screen',
- condition=IfCondition(enable_gripper_camera),
- )
-
- front_camera_node = Node(
- package='spinnaker_camera_driver',
- executable='camera_driver_node',
- name='front_camera',
- parameters=[
- front_camera_params_file,
- {
- 'parameter_file': blackfly_s_config_file,
- 'serial_number': '23494259',
- 'camerainfo_url': front_camera_calib_url,
- },
- ],
- remappings=[
- ('~/control', '/exposure_control/control'),
- ('/flir_camera/image_raw', '/front_camera/image_raw'),
- ('/flir_camera/camera_info', '/front_camera/camera_info'),
- ],
- output='screen',
- condition=IfCondition(enable_front_camera),
- )
-
- separate_nodes_group = GroupAction(
- actions=[
- filtering_node,
- aruco_node,
- gripper_camera_node,
- front_camera_node,
- ],
- condition=UnlessCondition(enable_composable_nodes),
- )
-
- return LaunchDescription(
- [
- enable_filtering_arg,
- enable_aruco_arg,
- enable_gripper_camera_arg,
- enable_front_camera_arg,
- enable_composable_nodes_arg,
- composable_node_container,
- separate_nodes_group,
- ]
- )
-
-
-generate_launch_description()
diff --git a/perception_setup/launch/pipeline_blackfly_s.launch.py b/perception_setup/launch/pipeline_blackfly_s.launch.py
new file mode 100644
index 0000000..14166cc
--- /dev/null
+++ b/perception_setup/launch/pipeline_blackfly_s.launch.py
@@ -0,0 +1,361 @@
+"""Blackfly S -> YOLO Segmentation -> YOLO Classification -> GStreamer stream.
+
+Pipeline:
+1. Blackfly S camera driver publishes raw BGR image
+2. YOLO segmentation detects pipeline instances and produces a segmentation mask
+ and visualization image
+3. YOLO classification runs on the segmentation mask to classify the pipeline state
+4. image_to_gstreamer streams the segmentation visualization image over RTP/H.265
+"""
+
+import os
+
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+ DeclareLaunchArgument,
+ IncludeLaunchDescription,
+ OpaqueFunction,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def _launch_setup(context, *args, **kwargs):
+ pkg_dir = get_package_share_directory('perception_setup')
+ models_dir = os.path.join(pkg_dir, 'models')
+ encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder')
+ drone = LaunchConfiguration('drone').perform(context)
+
+ with open(os.path.join(pkg_dir, 'config', 'yolo', 'yolo_seg.yaml')) as f:
+ seg_cfg = yaml.safe_load(f)
+ with open(os.path.join(pkg_dir, 'config', 'yolo', 'yolo_cls.yaml')) as f:
+ cls_cfg = yaml.safe_load(f)
+
+ # -------------------------------------------------------------------------
+ # Camera
+ # -------------------------------------------------------------------------
+ spinnaker_map = os.path.join(pkg_dir, 'config', 'cameras', 'blackfly_s_params.yaml')
+ driver_params = os.path.join(
+ pkg_dir, 'config', 'cameras', 'blackfly_s_driver_params.yaml'
+ )
+ calib_path = os.path.join(pkg_dir, 'config', 'cameras', 'blackfly_s_calib.yaml')
+
+ camera_container = ComposableNodeContainer(
+ name='down_cam_camera_container',
+ namespace='',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ ComposableNode(
+ package='spinnaker_camera_driver',
+ plugin='spinnaker_camera_driver::CameraDriver',
+ name='blackfly_s',
+ parameters=[
+ driver_params,
+ {
+ 'parameter_file': spinnaker_map,
+ 'serial_number': '23494258',
+ 'camerainfo_url': f'file://{calib_path}',
+ 'reliable_qos': True,
+ },
+ ],
+ remappings=[
+ ('~/control', '/exposure_control/control'),
+ ('/blackfly_s/image_raw', f'/{drone}/down_camera/image_color'),
+ ('/blackfly_s/camera_info', f'/{drone}/down_camera/camera_info'),
+ ],
+ extra_arguments=[{'use_intra_process_comms': True}],
+ condition=IfCondition(LaunchConfiguration('enable_camera')),
+ ),
+ ],
+ output='screen',
+ arguments=['--ros-args', '--log-level', 'info'],
+ )
+
+ # -------------------------------------------------------------------------
+ # YOLO Segmentation
+ # -------------------------------------------------------------------------
+ seg_image_format_converter = ComposableNode(
+ package='isaac_ros_image_proc',
+ plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
+ name='seg_image_format_converter',
+ parameters=[
+ {
+ 'encoding_desired': 'rgb8',
+ 'image_width': 720,
+ 'image_height': 540,
+ 'input_qos': 'sensor_data',
+ }
+ ],
+ remappings=[
+ ('image_raw', f'/{drone}/down_camera/image_color'),
+ ('image', '/down_cam/yolo_seg/internal/converted_image'),
+ ],
+ )
+
+ seg_tensor_rt_node = ComposableNode(
+ name='seg_tensor_rt',
+ package='isaac_ros_tensor_rt',
+ plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode',
+ parameters=[
+ {
+ 'model_file_path': os.path.join(models_dir, 'seg_pipe_real.onnx'),
+ 'engine_file_path': os.path.join(models_dir, 'seg_pipe_real.engine'),
+ 'output_binding_names': seg_cfg['output_binding_names'],
+ 'output_tensor_names': seg_cfg['output_tensor_names'],
+ 'input_tensor_names': seg_cfg['input_tensor_names'],
+ 'input_binding_names': seg_cfg['input_binding_names'],
+ 'verbose': True,
+ 'force_engine_update': False,
+ 'tensor_output_topic': '/down_cam/yolo_seg/tensor_pub',
+ }
+ ],
+ remappings=[
+ ('tensor_pub', '/down_cam/yolo_seg/tensor_pub'),
+ ('tensor_sub', '/down_cam/yolo_seg/tensor_sub'),
+ ],
+ )
+
+ seg_decoder_node = ComposableNode(
+ name='seg_decoder_node',
+ package='isaac_ros_yolov26_seg',
+ plugin='nvidia::isaac_ros::yolov26_seg::YoloV26SegDecoderNode',
+ parameters=[
+ {
+ 'tensor_input_topic': '/down_cam/yolo_seg/tensor_sub',
+ 'confidence_threshold': float(seg_cfg['confidence_threshold']),
+ 'num_detections': int(seg_cfg['num_detections']),
+ 'mask_width': int(seg_cfg['mask_width']),
+ 'mask_height': int(seg_cfg['mask_height']),
+ 'num_protos': int(seg_cfg['num_protos']),
+ 'network_image_width': int(seg_cfg['network_image_width']),
+ 'network_image_height': int(seg_cfg['network_image_height']),
+ 'output_mask_width': 640,
+ 'output_mask_height': 640,
+ 'detections_topic': '/down_cam/seg_detections_output',
+ 'mask_topic': '/down_cam/yolo_seg_mask',
+ }
+ ],
+ )
+
+ seg_container = ComposableNodeContainer(
+ name='down_cam_seg_container',
+ namespace='',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ seg_image_format_converter,
+ seg_tensor_rt_node,
+ seg_decoder_node,
+ ],
+ output='screen',
+ arguments=['--ros-args', '--log-level', 'INFO'],
+ )
+
+ seg_encoder_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py')
+ ),
+ launch_arguments={
+ 'input_image_width': str(720),
+ 'input_image_height': str(540),
+ 'network_image_width': str(seg_cfg['network_image_width']),
+ 'network_image_height': str(seg_cfg['network_image_height']),
+ 'image_mean': str(seg_cfg['image_mean']),
+ 'image_stddev': str(seg_cfg['image_stddev']),
+ 'attach_to_shared_component_container': 'True',
+ 'component_container_name': 'down_cam_seg_container',
+ 'dnn_image_encoder_namespace': 'down_cam/yolo_seg_encoder/internal',
+ 'image_input_topic': '/down_cam/yolo_seg/internal/converted_image',
+ 'camera_info_input_topic': f'/{drone}/down_camera/camera_info',
+ 'tensor_output_topic': '/down_cam/yolo_seg/tensor_pub',
+ }.items(),
+ )
+
+ seg_visualizer = Node(
+ package='isaac_ros_yolov26_seg',
+ executable='isaac_ros_yolov26_seg_visualizer.py',
+ name='seg_visualizer',
+ parameters=[
+ {
+ 'detections_topic': '/down_cam/seg_detections_output',
+ 'image_topic': '/down_cam/yolo_seg_encoder/internal/resize/image',
+ 'mask_topic': '/down_cam/yolo_seg_mask',
+ 'output_image_topic': '/down_cam/yolo_seg_processed_image',
+ 'class_names_yaml': str({0: 'pipeline'}),
+ }
+ ],
+ )
+
+ # -------------------------------------------------------------------------
+ # YOLO Classification
+ # -------------------------------------------------------------------------
+ cls_image_format_converter = ComposableNode(
+ package='isaac_ros_image_proc',
+ plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
+ name='cls_image_format_converter',
+ parameters=[
+ {
+ 'encoding_desired': 'rgb8',
+ 'image_width': 640,
+ 'image_height': 640,
+ 'input_qos': 'sensor_data',
+ }
+ ],
+ remappings=[
+ ('image_raw', '/down_cam/yolo_seg_mask'),
+ ('image', '/down_cam/yolo_cls/internal/converted_image'),
+ ],
+ )
+
+ cls_tensor_rt_node = ComposableNode(
+ name='cls_tensor_rt',
+ package='isaac_ros_tensor_rt',
+ plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode',
+ parameters=[
+ {
+ 'model_file_path': os.path.join(
+ models_dir, 'end-of-pipeline-yolov26.onnx'
+ ),
+ 'engine_file_path': os.path.join(
+ models_dir, 'end-of-pipeline-yolov26.engine'
+ ),
+ 'output_binding_names': cls_cfg['output_binding_names'],
+ 'output_tensor_names': cls_cfg['output_tensor_names'],
+ 'input_tensor_names': cls_cfg['input_tensor_names'],
+ 'input_binding_names': cls_cfg['input_binding_names'],
+ 'verbose': True,
+ 'force_engine_update': False,
+ 'tensor_output_topic': '/down_cam/yolo_cls/tensor_pub',
+ }
+ ],
+ remappings=[
+ ('tensor_pub', '/down_cam/yolo_cls/tensor_pub'),
+ ('tensor_sub', '/down_cam/yolo_cls/tensor_sub'),
+ ],
+ )
+
+ cls_decoder_node = ComposableNode(
+ name='cls_decoder_node',
+ package='isaac_ros_yolov26_cls',
+ plugin='nvidia::isaac_ros::yolov26_cls::YoloV26ClsDecoderNode',
+ parameters=[
+ {
+ 'tensor_input_topic': '/down_cam/yolo_cls/tensor_sub',
+ 'confidence_threshold': float(cls_cfg['confidence_threshold']),
+ 'num_classes': int(cls_cfg['num_classes']),
+ 'class_topic': '/down_cam/classification_output',
+ }
+ ],
+ )
+
+ cls_container = ComposableNodeContainer(
+ name='down_cam_cls_container',
+ namespace='',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ cls_image_format_converter,
+ cls_tensor_rt_node,
+ cls_decoder_node,
+ ],
+ output='screen',
+ arguments=['--ros-args', '--log-level', 'INFO'],
+ )
+
+ cls_encoder_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py')
+ ),
+ launch_arguments={
+ 'input_image_width': str(640),
+ 'input_image_height': str(640),
+ 'network_image_width': str(cls_cfg['network_image_width']),
+ 'network_image_height': str(cls_cfg['network_image_height']),
+ 'image_mean': str(cls_cfg['image_mean']),
+ 'image_stddev': str(cls_cfg['image_stddev']),
+ 'attach_to_shared_component_container': 'True',
+ 'component_container_name': 'down_cam_cls_container',
+ 'dnn_image_encoder_namespace': 'down_cam/yolo_cls_encoder/internal',
+ 'image_input_topic': '/down_cam/yolo_cls/internal/converted_image',
+ 'camera_info_input_topic': '/camera/camera/color/camera_info_rect',
+ 'tensor_output_topic': '/down_cam/yolo_cls/tensor_pub',
+ }.items(),
+ )
+
+ cls_visualizer = Node(
+ package='isaac_ros_yolov26_cls',
+ executable='isaac_ros_yolov26_cls_visualizer.py',
+ name='cls_visualizer',
+ parameters=[
+ {
+ 'class_topic': '/down_cam/classification_output',
+ 'image_topic': '/down_cam/yolo_cls_encoder/internal/resize/image',
+ 'output_image_topic': '/down_cam/yolo_cls_processed_image',
+ 'class_names_yaml': str({0: 'continue', 1: 'end'}),
+ }
+ ],
+ )
+
+ # -------------------------------------------------------------------------
+ # GStreamer — streams the segmentation visualization (bgr8)
+ # -------------------------------------------------------------------------
+ image_to_gstreamer_node = Node(
+ package='image_to_gstreamer',
+ executable='image_to_gstreamer_node',
+ name='image_to_gstreamer_node',
+ additional_env={'EGL_PLATFORM': 'surfaceless'},
+ parameters=[
+ {
+ 'input_topic': '/down_cam/yolo_seg_processed_image',
+ 'host': '10.0.0.68',
+ 'port': 5001,
+ 'bitrate': 500000,
+ 'framerate': 15,
+ 'preset_level': 1,
+ 'iframe_interval': 15,
+ 'control_rate': 1,
+ 'pt': 96,
+ 'config_interval': 1,
+ 'format': 'BGR',
+ }
+ ],
+ output='screen',
+ )
+
+ actions = [
+ camera_container,
+ seg_container,
+ seg_encoder_launch,
+ cls_container,
+ cls_encoder_launch,
+ image_to_gstreamer_node,
+ ]
+
+ actions.append(seg_visualizer)
+ actions.append(cls_visualizer)
+
+ return actions
+
+
+def generate_launch_description():
+ return LaunchDescription(
+ [
+ DeclareLaunchArgument(
+ 'drone',
+ default_value='nautilus',
+ description='Drone name, prepended to all published topics (e.g. /nautilus/down_camera/image_color)',
+ ),
+ DeclareLaunchArgument(
+ 'enable_camera',
+ default_value='true',
+ description='Enable FLIR Blackfly S camera component',
+ ),
+ OpaqueFunction(function=_launch_setup),
+ ]
+ )
diff --git a/perception_setup/launch/pipeline_detection.launch.py b/perception_setup/launch/pipeline_detection.launch.py
deleted file mode 100644
index b06daa1..0000000
--- a/perception_setup/launch/pipeline_detection.launch.py
+++ /dev/null
@@ -1,122 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.conditions import IfCondition
-from launch.substitutions import (
- LaunchConfiguration,
- PathJoinSubstitution,
-)
-from launch_ros.actions import ComposableNodeContainer
-from launch_ros.descriptions import ComposableNode
-from launch_ros.substitutions import FindPackageShare
-
-
-def generate_launch_description():
- enable_filtering = LaunchConfiguration('enable_filtering')
- enable_filtering_arg = DeclareLaunchArgument(
- 'enable_filtering',
- default_value='True',
- description='enable pipeline filtering',
- )
-
- enable_line_fitting = LaunchConfiguration('enable_line_fitting')
- enable_line_fitting_arg = DeclareLaunchArgument(
- 'enable_line_fitting',
- default_value='True',
- description='enable pipeline line fitting',
- )
-
- enable_downwards_camera = LaunchConfiguration('enable_downwards_camera')
- enable_downwards_camera_arg = DeclareLaunchArgument(
- 'enable_downwards_camera',
- default_value='True',
- description='enable flir camera',
- )
-
- filtering_params_file = os.path.join(
- get_package_share_directory('perception_setup'),
- 'config',
- 'pipeline_filtering_params.yaml',
- )
-
- filtering_line_fitting_params_file = os.path.join(
- get_package_share_directory('perception_setup'),
- 'config',
- 'pipeline_line_fitting_params.yaml',
- )
-
- downwards_cam_params_file = PathJoinSubstitution(
- [FindPackageShare('perception_setup'), 'config', 'downwards_cam_params.yaml']
- )
-
- blackfly_s_config_file = PathJoinSubstitution(
- [FindPackageShare('perception_setup'), 'config', 'blackfly_s_params.yaml']
- )
-
- package_share_directory = get_package_share_directory('perception_setup')
- downwards_cam_calib_path = os.path.join(
- package_share_directory, 'config', 'downwards_cam_calib.yaml'
- )
-
- # Add 'file://' prefix to the path required by the CameraInfoManager that sets the camera calibration in the spinnaker driver
- downwards_cam_calib_url = f'file://{downwards_cam_calib_path}'
-
- composable_node_container = ComposableNodeContainer(
- name='pipeline_detection_container',
- namespace='',
- package='rclcpp_components',
- executable='component_container_mt',
- composable_node_descriptions=[
- ComposableNode(
- package='pipeline_filtering',
- plugin='vortex::pipeline_processing::PipelineFilteringNode',
- name='pipeline_filtering_node',
- parameters=[filtering_params_file],
- extra_arguments=[{'use_intra_process_comms': True}],
- condition=IfCondition(enable_filtering),
- ),
- ComposableNode(
- package='pipeline_line_fitting',
- plugin='PipelineLineFittingNode',
- name='pipeline_line_fitting_node',
- parameters=[filtering_line_fitting_params_file],
- extra_arguments=[{'use_intra_process_comms': True}],
- condition=IfCondition(enable_line_fitting),
- ),
- ComposableNode(
- package='spinnaker_camera_driver',
- plugin='spinnaker_camera_driver::CameraDriver',
- name='downwards_cam',
- parameters=[
- downwards_cam_params_file,
- {
- 'parameter_file': blackfly_s_config_file,
- 'serial_number': '23494259',
- 'camerainfo_url': downwards_cam_calib_url,
- },
- ],
- remappings=[
- ('~/control', '/exposure_control/control'),
- ('/flir_camera/image_raw', '/downwards_cam/image_raw'),
- ('/flir_camera/camera_info', '/downwards_cam/camera_info'),
- ],
- extra_arguments=[{'use_intra_process_comms': True}],
- condition=IfCondition(enable_downwards_camera),
- ),
- ],
- output='screen',
- )
-
- return LaunchDescription(
- [
- enable_filtering_arg,
- enable_line_fitting_arg,
- enable_downwards_camera_arg,
- composable_node_container,
- ]
- )
-
-
-generate_launch_description()
diff --git a/perception_setup/launch/pipeline_realsense_d555.launch.py b/perception_setup/launch/pipeline_realsense_d555.launch.py
new file mode 100644
index 0000000..9bbe8a1
--- /dev/null
+++ b/perception_setup/launch/pipeline_realsense_d555.launch.py
@@ -0,0 +1,260 @@
+"""RealSense D555 -> YOLO Segmentation -> GStreamer stream.
+
+Pipeline:
+1. RealSense D555 camera driver publishes raw RGB color image
+2. image_undistort undistorts the raw color image
+3. YOLO segmentation detects pipeline instances and produces a segmentation mask
+ and visualization image
+4. image_to_gstreamer streams the segmentation visualization image over RTP/H.265
+"""
+
+import os
+
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+ DeclareLaunchArgument,
+ IncludeLaunchDescription,
+ OpaqueFunction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def _launch_setup(context, *args, **kwargs):
+ pkg_dir = get_package_share_directory('perception_setup')
+ models_dir = os.path.join(pkg_dir, 'models')
+ encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder')
+ drone = LaunchConfiguration('drone').perform(context)
+
+ with open(os.path.join(pkg_dir, 'config', 'yolo', 'yolo_seg.yaml')) as f:
+ seg_cfg = yaml.safe_load(f)
+
+ calib_file = os.path.join(
+ pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml'
+ )
+
+ # -------------------------------------------------------------------------
+ # Camera + Undistort
+ # -------------------------------------------------------------------------
+ camera_container = ComposableNodeContainer(
+ name='front_cam_camera_container',
+ namespace='',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ ComposableNode(
+ package='realsense2_camera',
+ plugin='realsense2_camera::RealSenseNodeFactory',
+ name='camera',
+ namespace='camera',
+ parameters=[
+ {
+ 'enable_color': True,
+ 'rgb_camera.color_profile': '896,504,15',
+ 'rgb_camera.color_format': 'RGB8',
+ 'rgb_camera.enable_auto_exposure': True,
+ 'enable_depth': False,
+ 'enable_infra1': False,
+ 'enable_infra2': False,
+ 'enable_gyro': False,
+ 'enable_accel': False,
+ 'enable_motion': False,
+ 'publish_tf': False,
+ 'enable_sync': False,
+ }
+ ],
+ ),
+ ComposableNode(
+ package='perception_setup',
+ plugin='perception_setup::ImageUndistort',
+ name='color_image_undistort',
+ parameters=[
+ {
+ 'image_topic': '/camera/camera/color/image_raw',
+ 'camera_info_file': calib_file,
+ 'raw_camera_info_topic': '/camera/camera/color/camera_info',
+ 'output_image_topic': f'/{drone}/front_camera/image_color',
+ 'output_camera_info_topic': f'/{drone}/front_camera/camera_info',
+ 'enable_undistort': LaunchConfiguration('enable_undistort'),
+ 'image_qos': 'sensor_data',
+ }
+ ],
+ ),
+ ],
+ output='screen',
+ arguments=['--ros-args', '--log-level', 'info'],
+ )
+
+ # -------------------------------------------------------------------------
+ # YOLO Segmentation
+ # -------------------------------------------------------------------------
+ seg_image_format_converter = ComposableNode(
+ package='isaac_ros_image_proc',
+ plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
+ name='seg_image_format_converter',
+ parameters=[
+ {
+ 'encoding_desired': 'rgb8',
+ 'image_width': 896,
+ 'image_height': 504,
+ 'input_qos': 'sensor_data',
+ }
+ ],
+ remappings=[
+ ('image_raw', f'/{drone}/front_camera/image_color'),
+ ('image', '/front_cam/yolo_seg/internal/converted_image'),
+ ],
+ )
+
+ seg_tensor_rt_node = ComposableNode(
+ name='seg_tensor_rt',
+ package='isaac_ros_tensor_rt',
+ plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode',
+ parameters=[
+ {
+ 'model_file_path': os.path.join(models_dir, 'seg_pipe_real.onnx'),
+ 'engine_file_path': os.path.join(models_dir, 'seg_pipe_real.engine'),
+ 'output_binding_names': seg_cfg['output_binding_names'],
+ 'output_tensor_names': seg_cfg['output_tensor_names'],
+ 'input_tensor_names': seg_cfg['input_tensor_names'],
+ 'input_binding_names': seg_cfg['input_binding_names'],
+ 'verbose': True,
+ 'force_engine_update': False,
+ 'tensor_output_topic': '/front_cam/yolo_seg/tensor_pub',
+ }
+ ],
+ remappings=[
+ ('tensor_pub', '/front_cam/yolo_seg/tensor_pub'),
+ ('tensor_sub', '/front_cam/yolo_seg/tensor_sub'),
+ ],
+ )
+
+ seg_decoder_node = ComposableNode(
+ name='seg_decoder_node',
+ package='isaac_ros_yolov26_seg',
+ plugin='nvidia::isaac_ros::yolov26_seg::YoloV26SegDecoderNode',
+ parameters=[
+ {
+ 'tensor_input_topic': '/front_cam/yolo_seg/tensor_sub',
+ 'confidence_threshold': float(seg_cfg['confidence_threshold']),
+ 'num_detections': int(seg_cfg['num_detections']),
+ 'mask_width': int(seg_cfg['mask_width']),
+ 'mask_height': int(seg_cfg['mask_height']),
+ 'num_protos': int(seg_cfg['num_protos']),
+ 'network_image_width': int(seg_cfg['network_image_width']),
+ 'network_image_height': int(seg_cfg['network_image_height']),
+ 'output_mask_width': 640,
+ 'output_mask_height': 640,
+ 'detections_topic': '/front_cam/seg_detections_output',
+ 'mask_topic': '/front_cam/yolo_seg_mask',
+ }
+ ],
+ )
+
+ seg_container = ComposableNodeContainer(
+ name='front_cam_seg_container',
+ namespace='',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ seg_image_format_converter,
+ seg_tensor_rt_node,
+ seg_decoder_node,
+ ],
+ output='screen',
+ arguments=['--ros-args', '--log-level', 'INFO'],
+ )
+
+ seg_encoder_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py')
+ ),
+ launch_arguments={
+ 'input_image_width': str(896),
+ 'input_image_height': str(504),
+ 'network_image_width': str(seg_cfg['network_image_width']),
+ 'network_image_height': str(seg_cfg['network_image_height']),
+ 'image_mean': str(seg_cfg['image_mean']),
+ 'image_stddev': str(seg_cfg['image_stddev']),
+ 'attach_to_shared_component_container': 'True',
+ 'component_container_name': 'front_cam_seg_container',
+ 'dnn_image_encoder_namespace': 'front_cam/yolo_seg_encoder/internal',
+ 'image_input_topic': '/front_cam/yolo_seg/internal/converted_image',
+ 'camera_info_input_topic': f'/{drone}/front_camera/camera_info',
+ 'tensor_output_topic': '/front_cam/yolo_seg/tensor_pub',
+ }.items(),
+ )
+
+ seg_visualizer = Node(
+ package='isaac_ros_yolov26_seg',
+ executable='isaac_ros_yolov26_seg_visualizer.py',
+ name='seg_visualizer',
+ parameters=[
+ {
+ 'detections_topic': '/front_cam/seg_detections_output',
+ 'image_topic': '/front_cam/yolo_seg_encoder/internal/resize/image',
+ 'mask_topic': '/front_cam/yolo_seg_mask',
+ 'output_image_topic': '/front_cam/yolo_seg_processed_image',
+ 'class_names_yaml': str({0: 'pipeline'}),
+ }
+ ],
+ )
+
+ # -------------------------------------------------------------------------
+ # GStreamer — streams the segmentation visualization (rgb8)
+ # -------------------------------------------------------------------------
+ image_to_gstreamer_node = Node(
+ package='image_to_gstreamer',
+ executable='image_to_gstreamer_node',
+ name='image_to_gstreamer_node',
+ additional_env={'EGL_PLATFORM': 'surfaceless'},
+ parameters=[
+ {
+ 'input_topic': '/front_cam/yolo_seg_processed_image',
+ 'host': '10.0.0.68',
+ 'port': 5000,
+ 'bitrate': 500000,
+ 'framerate': 15,
+ 'preset_level': 1,
+ 'iframe_interval': 15,
+ 'control_rate': 1,
+ 'pt': 96,
+ 'config_interval': 1,
+ 'format': 'BGR',
+ }
+ ],
+ output='screen',
+ )
+
+ actions = [
+ camera_container,
+ seg_container,
+ seg_encoder_launch,
+ image_to_gstreamer_node,
+ ]
+
+ actions.append(seg_visualizer)
+
+ return actions
+
+
+def generate_launch_description():
+ return LaunchDescription(
+ [
+ DeclareLaunchArgument(
+ 'drone',
+ default_value='nautilus',
+ description='Drone name, prepended to all published topics (e.g. /nautilus/front_camera/image_color)',
+ ),
+ DeclareLaunchArgument(
+ 'enable_undistort',
+ default_value='true',
+ description='Undistort color image before segmentation',
+ ),
+ OpaqueFunction(function=_launch_setup),
+ ]
+ )
diff --git a/perception_setup/launch/ultralytics/ultralytics_pipeline_line_fitting.launch.py b/perception_setup/launch/ultralytics/ultralytics_pipeline_line_fitting.launch.py
new file mode 100644
index 0000000..6105e35
--- /dev/null
+++ b/perception_setup/launch/ultralytics/ultralytics_pipeline_line_fitting.launch.py
@@ -0,0 +1,67 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ yolo_params = os.path.join(
+ get_package_share_directory('yolo_segmentation'),
+ 'params',
+ 'yolo_params.yaml',
+ )
+
+ irls_config = os.path.join(
+ get_package_share_directory('irls_line_fitter_2x'), 'config', 'irls_line.yaml'
+ )
+
+ return LaunchDescription(
+ [
+ # YOLO segmentation node
+ Node(
+ package='yolo_segmentation',
+ executable='yolo_seg_node',
+ name='yolo_segmentation_node',
+ output='screen',
+ parameters=[yolo_params],
+ ),
+ # YOLO classifier node
+ Node(
+ package='vortex_yolo_classifiy',
+ executable='classifier_node',
+ name='classifier_node',
+ output='screen',
+ parameters=[{'model_path': 'best.pt'}],
+ ),
+ # IRLS line fitter node
+ Node(
+ package='irls_line_fitter_2x',
+ executable='irls_line_node',
+ name='irls_line_node',
+ parameters=[irls_config],
+ ),
+ # Image to GStreamer node
+ Node(
+ package='image_to_gstreamer',
+ executable='image_to_gstreamer_node',
+ name='image_to_gstreamer_node',
+ output='screen',
+ parameters=[
+ {
+ 'input_topic': "/irls_line/image",
+ 'host': '10.0.0.169',
+ 'port': 5001,
+ 'bitrate': 500000,
+ 'framerate': 15,
+ 'preset_level': 1,
+ 'iframe_interval': 15,
+ 'control_rate': 1,
+ 'pt': 96,
+ 'config_interval': 1,
+ 'format': 'RGB',
+ }
+ ],
+ ),
+ ]
+ )
diff --git a/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py b/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py
new file mode 100644
index 0000000..50fe922
--- /dev/null
+++ b/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py
@@ -0,0 +1,150 @@
+#!/usr/bin/env python3
+"""Simulator -> Ultralytics YOLO OBB (Python) -> Valve Detection -> Subtype Resolver.
+
+All topics and TF frames are defined inline in this launch file.
+Node tuning defaults live in each node's own yaml under its package
+and are loaded once (no inline re-overriding).
+"""
+
+import os
+
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def _validate_device(device: str) -> None:
+ if device in ('cpu', 'cuda', 'mps'):
+ return
+ if device.isdigit():
+ return
+ if device.startswith('cuda:') and device.split(':')[1].isdigit():
+ return
+ raise RuntimeError(
+ f"Invalid device '{device}'. Use 'cpu', GPU index (0,1,...), "
+ "'cuda', 'cuda:N', or 'mps'."
+ )
+
+
+def _launch_setup(context, *args, **kwargs):
+ pkg_dir = get_package_share_directory('perception_setup')
+ models_dir = os.path.join(pkg_dir, 'models')
+
+ model_file_path = os.path.join(models_dir, 'best_valve_handle_real_and_sim.pt')
+ device = LaunchConfiguration('device').perform(context)
+ _validate_device(device)
+
+ drone = LaunchConfiguration('drone')
+ clamp_yaw = LaunchConfiguration('clamp_yaw')
+
+ detections_topic = '/ultralytics_valve_detection/detections'
+ raw_landmarks_topic = '/nautilus/landmarks'
+ typed_landmarks_topic = '/nautilus/landmarks_test'
+
+ yolo_node = Node(
+ package='yolo_obb_object_detection',
+ executable='yolo_obb_object_detection_node',
+ name='yolo_obb_object_detection',
+ output='screen',
+ parameters=[
+ {
+ 'device': device,
+ 'model_path': model_file_path,
+ 'confidence_threshold': 0.7,
+ 'input_topic': '/camera/camera/color/image_raw',
+ 'output_detections_topic': detections_topic,
+ 'output_annotated_topic': '/ultralytics_valve_detection/annotated_image',
+ },
+ ],
+ )
+
+ valve_container = ComposableNodeContainer(
+ name='valve_detection_container',
+ namespace='',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ ComposableNode(
+ package='valve_detection',
+ plugin='valve_detection::ValvePoseNode',
+ name='valve_pose_node',
+ parameters=[
+ os.path.join(
+ get_package_share_directory('valve_detection'),
+ 'config',
+ 'valve_detection_params.yaml',
+ ),
+ {
+ 'detections_sub_topic': detections_topic,
+ 'depth_image_sub_topic': '/camera/camera/depth/image_rect_raw',
+ 'depth_image_info_topic': '/camera/camera/depth/camera_info',
+ 'color_image_info_topic': '/camera/camera/color/camera_info',
+ 'depth_frame_id': 'front_camera_depth_optical',
+ 'color_frame_id': 'front_camera_color_optical',
+ 'landmarks_pub_topic': raw_landmarks_topic,
+ 'output_frame_id': 'front_camera_depth_optical',
+ 'drone': drone,
+ 'undistort_detections': True,
+ # yolo_obb_object_detection publishes detections in
+ # original image coordinates, not letterbox space.
+ 'detections_letterboxed': False,
+ 'debug_visualize': True,
+ 'use_hardcoded_extrinsic': True,
+ 'clamp_yaw': False,
+ },
+ ],
+ )
+ ],
+ output='screen',
+ )
+
+ # subtype_resolver_node = Node(
+ # package='valve_subtype_resolver',
+ # executable='valve_subtype_resolver_node',
+ # name='valve_subtype_resolver_node',
+ # output='screen',
+ # parameters=[
+ # os.path.join(
+ # get_package_share_directory('valve_subtype_resolver'),
+ # 'config',
+ # 'valve_subtype_resolver_params.yaml',
+ # ),
+ # {
+ # 'drone': drone,
+ # 'clamp_yaw': clamp_yaw,
+ # 'landmarks_sub_topic': raw_landmarks_topic,
+ # 'landmarks_pub_topic': typed_landmarks_topic,
+ # },
+ # ],
+ # )
+
+ return [yolo_node, valve_container]
+
+
+def generate_launch_description():
+ return LaunchDescription(
+ [
+ DeclareLaunchArgument(
+ 'device',
+ default_value='0',
+ description="YOLO device: 'cpu', GPU index, 'cuda', 'cuda:N', or 'mps'",
+ ),
+ DeclareLaunchArgument(
+ 'drone',
+ default_value='nautilus',
+ description='Robot name, prepended to TF frame IDs',
+ ),
+ DeclareLaunchArgument(
+ 'clamp_yaw',
+ default_value='true',
+ description=(
+ 'Fold landmark yaw into [0, 90°] in the world frame '
+ '(drone-roll invariant). Valve handle is 180°/90° symmetric.'
+ ),
+ ),
+ OpaqueFunction(function=_launch_setup),
+ ]
+ )
diff --git a/perception_setup/launch/valve_intervention.launch.py b/perception_setup/launch/valve_intervention.launch.py
new file mode 100644
index 0000000..7eaa2bc
--- /dev/null
+++ b/perception_setup/launch/valve_intervention.launch.py
@@ -0,0 +1,328 @@
+"""RealSense D555 -> YOLO OBB -> Valve Detection.
+
+Pipeline:
+1. RealSense D555 camera publishes raw color image and rectified depth image
+2. image_undistort undistorts the raw color image
+3. YOLO OBB detects oriented bounding boxes on the undistorted color image
+4. Valve Detection uses oriented bounding boxes and depth image detections to compute valve pose
+"""
+
+import os
+
+import launch
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import (
+ DeclareLaunchArgument,
+ IncludeLaunchDescription,
+ OpaqueFunction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def _launch_setup(context, *args, **kwargs):
+ pkg_dir = get_package_share_directory('perception_setup')
+ models_dir = os.path.join(pkg_dir, 'models')
+ drone = LaunchConfiguration('drone').perform(context)
+
+ with open(os.path.join(pkg_dir, 'config', 'yolo', 'yolo_obb.yaml')) as f:
+ yolo_cfg = yaml.safe_load(f)
+
+ calib_file = os.path.join(
+ pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml'
+ )
+ model_file_path = os.path.join(models_dir, 'obb_best.onnx')
+ engine_file_path = os.path.join(models_dir, 'obb_best.engine')
+
+ realsense_node = ComposableNode(
+ package='realsense2_camera',
+ plugin='realsense2_camera::RealSenseNodeFactory',
+ name='camera',
+ namespace='camera',
+ parameters=[
+ {
+ 'enable_color': True,
+ 'rgb_camera.color_profile': '896,504,15',
+ 'rgb_camera.color_format': 'RGB8',
+ 'rgb_camera.enable_auto_exposure': True,
+ 'enable_depth': True,
+ 'depth_module.depth_profile': '896,504,15',
+ 'depth_module.depth_format': 'Z16',
+ 'depth_module.enable_auto_exposure': True,
+ 'depth_module.emitter_enabled': False,
+ 'enable_infra1': False,
+ 'enable_infra2': False,
+ 'enable_gyro': False,
+ 'enable_accel': False,
+ 'enable_motion': False,
+ 'publish_tf': False,
+ 'enable_sync': False,
+ }
+ ],
+ remappings=[
+ (
+ '/camera/camera/depth/image_rect_raw',
+ f'/{drone}/depth_camera/image_depth',
+ ),
+ ('/camera/camera/depth/camera_info', f'/{drone}/depth_camera/camera_info'),
+ ],
+ )
+
+ image_undistort_node = ComposableNode(
+ package='perception_setup',
+ plugin='perception_setup::ImageUndistort',
+ name='color_image_undistort',
+ parameters=[
+ {
+ 'image_topic': '/camera/camera/color/image_raw',
+ 'camera_info_file': calib_file,
+ 'raw_camera_info_topic': '/camera/camera/color/camera_info',
+ 'output_image_topic': f'/{drone}/front_camera/image_color',
+ 'output_camera_info_topic': f'/{drone}/front_camera/camera_info',
+ 'enable_undistort': LaunchConfiguration('enable_undistort'),
+ 'image_qos': 'reliable', # Isaac ros only works with reliable QoS
+ }
+ ],
+ )
+
+ image_format_converter = ComposableNode(
+ package='isaac_ros_image_proc',
+ plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
+ name='image_format_converter',
+ parameters=[
+ {
+ 'encoding_desired': 'rgb8',
+ 'image_width': 896,
+ 'image_height': 504,
+ 'input_qos': 'sensor_data',
+ }
+ ],
+ remappings=[
+ ('image_raw', f'/{drone}/front_camera/image_color'),
+ ('image', '/yolo_obb/internal/converted_image'),
+ ],
+ )
+
+ tensor_rt_node = ComposableNode(
+ name='tensor_rt',
+ package='isaac_ros_tensor_rt',
+ plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode',
+ parameters=[
+ {
+ 'model_file_path': model_file_path,
+ 'engine_file_path': engine_file_path,
+ 'output_binding_names': yolo_cfg['output_binding_names'],
+ 'output_tensor_names': yolo_cfg['output_tensor_names'],
+ 'input_tensor_names': yolo_cfg['input_tensor_names'],
+ 'input_binding_names': yolo_cfg['input_binding_names'],
+ 'verbose': True,
+ 'force_engine_update': False,
+ 'tensor_output_topic': '/yolo_obb/tensor_pub',
+ }
+ ],
+ remappings=[
+ ('tensor_pub', '/yolo_obb/tensor_pub'),
+ ('tensor_sub', '/yolo_obb/tensor_sub'),
+ ],
+ )
+
+ yolo_obb_decoder_node = ComposableNode(
+ name='yolo_obb_decoder_node',
+ package='isaac_ros_yolov26_obb',
+ plugin='nvidia::isaac_ros::yolov26_obb::YoloV26OBBDecoderNode',
+ parameters=[
+ {
+ 'tensor_input_topic': '/yolo_obb/tensor_sub',
+ 'confidence_threshold': float(yolo_cfg['confidence_threshold']),
+ 'num_detections': int(yolo_cfg['num_detections']),
+ 'detections_topic': '/obb_detections_output',
+ }
+ ],
+ )
+
+ tensor_rt_container = ComposableNodeContainer(
+ name='obb_tensor_rt_container',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ realsense_node,
+ image_undistort_node,
+ image_format_converter,
+ tensor_rt_node,
+ yolo_obb_decoder_node,
+ ],
+ output='screen',
+ arguments=['--ros-args', '--log-level', 'INFO'],
+ namespace='',
+ )
+
+ encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder')
+ dnn_image_encoder_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py')
+ ),
+ launch_arguments={
+ 'input_image_width': str(896),
+ 'input_image_height': str(504),
+ 'network_image_width': str(yolo_cfg['network_image_width']),
+ 'network_image_height': str(yolo_cfg['network_image_height']),
+ 'image_mean': str(yolo_cfg['image_mean']),
+ 'image_stddev': str(yolo_cfg['image_stddev']),
+ 'attach_to_shared_component_container': 'True',
+ 'component_container_name': 'obb_tensor_rt_container',
+ 'dnn_image_encoder_namespace': 'yolo_obb_encoder/internal',
+ 'image_input_topic': '/yolo_obb/internal/converted_image',
+ 'camera_info_input_topic': f'/{drone}/front_camera/camera_info',
+ 'tensor_output_topic': '/yolo_obb/tensor_pub',
+ }.items(),
+ )
+
+ yolo_obb_visualizer = Node(
+ package='isaac_ros_yolov26_obb',
+ executable='isaac_ros_yolov26_obb_visualizer.py',
+ name='yolo_obb_visualizer',
+ parameters=[
+ {
+ 'detections_topic': '/obb_detections_output',
+ 'image_topic': '/yolo_obb_encoder/internal/resize/image',
+ 'output_image_topic': '/yolo_obb_processed_image',
+ 'class_names_yaml': str({0: 'valve'}),
+ }
+ ],
+ )
+
+ valve_detection_tuning = os.path.join(
+ get_package_share_directory('valve_detection'),
+ 'config',
+ 'valve_detection_params.yaml',
+ )
+
+ valve_detection_container = ComposableNodeContainer(
+ name='valve_detection_container',
+ namespace='',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ ComposableNode(
+ package='valve_detection',
+ plugin='valve_detection::ValvePoseNode',
+ name='valve_pose_node',
+ parameters=[
+ valve_detection_tuning,
+ {
+ 'depth_image_sub_topic': f'/{drone}/depth_camera/image_depth',
+ 'detections_sub_topic': '/obb_detections_output',
+ 'depth_image_info_topic': f'/{drone}/depth_camera/camera_info',
+ 'depth_frame_id': 'front_camera_depth_optical',
+ 'color_frame_id': 'front_camera_color_optical',
+ 'landmarks_pub_topic': '/valve_landmarks',
+ 'output_frame_id': 'front_camera_depth_optical',
+ 'drone': LaunchConfiguration('drone'),
+ 'undistort_detections': LaunchConfiguration(
+ 'undistort_detections'
+ ),
+ 'debug_visualize': LaunchConfiguration('debug_visualize'),
+ 'clamp_rotation': LaunchConfiguration('clamp_rotation'),
+ 'use_hardcoded_extrinsic': LaunchConfiguration(
+ 'use_hardcoded_extrinsic'
+ ),
+ 'extrinsic_tx': LaunchConfiguration('extrinsic_tx'),
+ 'extrinsic_ty': LaunchConfiguration('extrinsic_ty'),
+ 'extrinsic_tz': LaunchConfiguration('extrinsic_tz'),
+ },
+ ],
+ )
+ ],
+ output='screen',
+ )
+
+ image_to_gstreamer_node = Node(
+ package='image_to_gstreamer',
+ executable='image_to_gstreamer_node',
+ name='image_to_gstreamer_node',
+ additional_env={'EGL_PLATFORM': 'surfaceless'},
+ parameters=[
+ {
+ 'input_topic': '/yolo_obb_processed_image',
+ 'host': '10.0.0.169',
+ 'port': 5000,
+ 'bitrate': 500000,
+ 'framerate': 15,
+ 'preset_level': 1,
+ 'iframe_interval': 15,
+ 'control_rate': 1,
+ 'pt': 96,
+ 'config_interval': 1,
+ 'format': 'RGB',
+ }
+ ],
+ output='screen',
+ )
+
+ actions = [
+ tensor_rt_container,
+ dnn_image_encoder_launch,
+ valve_detection_container,
+ image_to_gstreamer_node,
+ ]
+
+ actions.append(yolo_obb_visualizer)
+
+ return actions
+
+
+def generate_launch_description():
+ return launch.LaunchDescription(
+ [
+ DeclareLaunchArgument(
+ 'enable_undistort',
+ default_value='true',
+ description='Undistort color image before passing to YOLO',
+ ),
+ DeclareLaunchArgument(
+ 'drone',
+ default_value='nautilus',
+ description='Robot name, prepended to TF frame IDs',
+ ),
+ DeclareLaunchArgument(
+ 'undistort_detections',
+ # If enabled, disable enable_undistort to avoid double-undistortion.
+ default_value='false',
+ description='Undistort detections using color camera distortion',
+ ),
+ DeclareLaunchArgument(
+ 'debug_visualize',
+ default_value='true',
+ description='Enable debug visualization topics',
+ ),
+ DeclareLaunchArgument(
+ 'clamp_rotation',
+ default_value='true',
+ description='Clamp valve handle angle to 0-90 deg (0=vertical, 90=horizontal)',
+ ),
+ DeclareLaunchArgument(
+ 'use_hardcoded_extrinsic',
+ default_value='true',
+ description='Use hardcoded depth-to-color extrinsic instead of TF lookup',
+ ),
+ DeclareLaunchArgument(
+ 'extrinsic_tx',
+ default_value='-0.059',
+ description='Hardcoded extrinsic translation X (metres)',
+ ),
+ DeclareLaunchArgument(
+ 'extrinsic_ty',
+ default_value='0.0',
+ description='Hardcoded extrinsic translation Y (metres)',
+ ),
+ DeclareLaunchArgument(
+ 'extrinsic_tz',
+ default_value='0.0',
+ description='Hardcoded extrinsic translation Z (metres)',
+ ),
+ OpaqueFunction(function=_launch_setup),
+ ]
+ )
diff --git a/perception_setup/launch/visual_inspection.launch.py b/perception_setup/launch/visual_inspection.launch.py
new file mode 100644
index 0000000..6954f0d
--- /dev/null
+++ b/perception_setup/launch/visual_inspection.launch.py
@@ -0,0 +1,166 @@
+"""RealSense D555 -> Image Filtering -> ArUco Detection.
+
+Pipeline:
+1. RealSense D555 camera publishes raw color image
+2. image_undistort undistorts the raw color image
+3. image_filtering to filter the undistorted image
+4. aruco_detector runs on the filtered image, publishes detections, visualization and writes logs
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+ pkg_dir = get_package_share_directory('perception_setup')
+
+ calib_file = os.path.join(
+ pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml'
+ )
+
+ drone = LaunchConfiguration('drone')
+
+ visual_inspection_container = ComposableNodeContainer(
+ name='visual_inspection_container',
+ namespace='',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ ComposableNode(
+ package='realsense2_camera',
+ plugin='realsense2_camera::RealSenseNodeFactory',
+ name='camera',
+ namespace='camera',
+ parameters=[
+ {
+ 'enable_color': True,
+ 'rgb_camera.color_profile': '896,504,15',
+ 'rgb_camera.color_format': 'RGB8',
+ 'rgb_camera.enable_auto_exposure': True,
+ 'enable_depth': False,
+ 'enable_infra1': False,
+ 'enable_infra2': False,
+ 'enable_gyro': False,
+ 'enable_accel': False,
+ 'enable_motion': False,
+ 'publish_tf': False,
+ 'enable_sync': False,
+ }
+ ],
+ ),
+ ComposableNode(
+ package='perception_setup',
+ plugin='perception_setup::ImageUndistort',
+ name='color_image_undistort',
+ parameters=[
+ {
+ 'image_topic': '/camera/camera/color/image_raw',
+ 'camera_info_file': calib_file,
+ 'raw_camera_info_topic': '/camera/camera/color/camera_info',
+ 'output_image_topic': ['/', drone, '/front_camera/image_color'],
+ 'output_camera_info_topic': [
+ '/',
+ drone,
+ '/front_camera/camera_info',
+ ],
+ 'enable_undistort': LaunchConfiguration('enable_undistort'),
+ 'image_qos': 'sensor_data',
+ }
+ ],
+ ),
+ ComposableNode(
+ package='image_filtering',
+ plugin='ImageFilteringNode',
+ name='image_filtering_node',
+ parameters=[
+ os.path.join(
+ get_package_share_directory('image_filtering'),
+ 'config',
+ 'image_filtering_params.yaml',
+ ),
+ {
+ 'sub_topic': ['/', drone, '/front_camera/image_color'],
+ 'pub_topic': '/visual_inspection/filtered_image',
+ 'input_encoding': 'rgb8',
+ 'output_encoding': 'rgb8',
+ 'filter_params.filter_type': LaunchConfiguration('filter_type'),
+ },
+ ],
+ ),
+ ComposableNode(
+ package='aruco_detector',
+ plugin='ArucoDetectorNode',
+ name='aruco_detector_node',
+ parameters=[
+ os.path.join(
+ get_package_share_directory('aruco_detector'),
+ 'config',
+ 'aruco_detector_params.yaml',
+ ),
+ {
+ 'subs.image_topic': '/visual_inspection/filtered_image',
+ 'subs.camera_info_topic': [
+ '/',
+ drone,
+ '/front_camera/camera_info',
+ ],
+ 'detect_board': False,
+ 'publish_landmarks': False,
+ },
+ ],
+ ),
+ ],
+ output='screen',
+ arguments=['--ros-args', '--log-level', 'info'],
+ )
+
+ image_to_gstreamer_node = Node(
+ package='image_to_gstreamer',
+ executable='image_to_gstreamer_node',
+ name='image_to_gstreamer_node',
+ additional_env={'EGL_PLATFORM': 'surfaceless'},
+ parameters=[
+ {
+ 'input_topic': '/aruco_detector/image',
+ 'host': '10.0.0.154',
+ 'port': 5000,
+ 'bitrate': 500000,
+ 'framerate': 15,
+ 'preset_level': 1,
+ 'iframe_interval': 15,
+ 'control_rate': 1,
+ 'pt': 96,
+ 'config_interval': 1,
+ 'format': 'RGB',
+ }
+ ],
+ output='screen',
+ )
+
+ return LaunchDescription(
+ [
+ DeclareLaunchArgument(
+ 'drone',
+ default_value='nautilus',
+ description='Drone name, prepended to all published topics (e.g. /nautilus/front_camera/image_color)',
+ ),
+ DeclareLaunchArgument(
+ 'enable_undistort',
+ default_value='true',
+ description='Undistort color image before publishing to image_topic',
+ ),
+ DeclareLaunchArgument(
+ 'filter_type',
+ default_value='no_filter',
+ description='Type of filter to apply to the image',
+ ),
+ visual_inspection_container,
+ image_to_gstreamer_node,
+ ]
+ )
diff --git a/perception_setup/launch/yolo/yolo_cls.launch.py b/perception_setup/launch/yolo/yolo_cls.launch.py
new file mode 100644
index 0000000..6c8c93d
--- /dev/null
+++ b/perception_setup/launch/yolo/yolo_cls.launch.py
@@ -0,0 +1,160 @@
+# SPDX-License-Identifier: MIT
+
+"""Isaac ROS YOLO classification TensorRT inference pipeline.
+
+1. ImageFormatConverterNode
+ Input: IMAGE_INPUT_TOPIC (launch arg)
+ Output: CONVERTED_IMAGE_TOPIC
+
+2. DNNImageEncoderNode
+ Input: CONVERTED_IMAGE_TOPIC
+ Output: TENSOR_OUTPUT_TOPIC
+
+3. TensorRTNode
+ Input: TENSOR_OUTPUT_TOPIC
+ Output: TENSOR_INPUT_TOPIC
+ Expected output tensor shape: [1, num_classes]
+
+4. YoloV26ClsDecoderNode
+ Input: TENSOR_INPUT_TOPIC
+ Output: CLASS_TOPIC (UInt8 with predicted class index)
+"""
+
+import os
+
+import launch
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import (
+ IncludeLaunchDescription,
+ OpaqueFunction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def _launch_setup(context, *args, **kwargs):
+ pkg_dir = get_package_share_directory('perception_setup')
+ models_dir = os.path.join(pkg_dir, 'models')
+
+ model_file_path = os.path.join(models_dir, 'end-of-pipeline-yolov26.onnx')
+ engine_file_path = os.path.join(models_dir, 'end-of-pipeline-yolov26.engine')
+
+ config_path = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_cls.yaml')
+ with open(config_path) as f:
+ cfg = yaml.safe_load(f)
+
+ image_format_converter = ComposableNode(
+ package='isaac_ros_image_proc',
+ plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
+ name='image_format_converter',
+ parameters=[
+ {
+ 'encoding_desired': 'rgb8',
+ 'image_width': 640,
+ 'image_height': 640,
+ 'input_qos': 'sensor_data',
+ }
+ ],
+ remappings=[
+ ('image_raw', '/yolo_seg_mask'),
+ ('image', '/yolo_cls/internal/converted_image'),
+ ],
+ )
+
+ tensor_rt_node = ComposableNode(
+ name='tensor_rt',
+ package='isaac_ros_tensor_rt',
+ plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode',
+ parameters=[
+ {
+ 'model_file_path': model_file_path,
+ 'engine_file_path': engine_file_path,
+ 'output_binding_names': cfg['output_binding_names'],
+ 'output_tensor_names': cfg['output_tensor_names'],
+ 'input_tensor_names': cfg['input_tensor_names'],
+ 'input_binding_names': cfg['input_binding_names'],
+ 'verbose': True,
+ 'force_engine_update': False,
+ 'tensor_output_topic': '/yolo_cls/tensor_pub',
+ }
+ ],
+ remappings=[
+ ('tensor_pub', '/yolo_cls/tensor_pub'),
+ ('tensor_sub', '/yolo_cls/tensor_sub'),
+ ],
+ )
+
+ yolo_cls_decoder_node = ComposableNode(
+ name='yolo_cls_decoder_node',
+ package='isaac_ros_yolov26_cls',
+ plugin='nvidia::isaac_ros::yolov26_cls::YoloV26ClsDecoderNode',
+ parameters=[
+ {
+ 'tensor_input_topic': '/yolo_cls/tensor_sub',
+ 'confidence_threshold': float(cfg['confidence_threshold']),
+ 'num_classes': int(cfg['num_classes']),
+ 'class_topic': '/classification_output',
+ }
+ ],
+ )
+
+ tensor_rt_container = ComposableNodeContainer(
+ name='cls_tensor_rt_container',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ image_format_converter,
+ tensor_rt_node,
+ yolo_cls_decoder_node,
+ ],
+ output='screen',
+ arguments=['--ros-args', '--log-level', 'INFO'],
+ namespace='',
+ )
+
+ encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder')
+ yolo_cls_encoder_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py')
+ ),
+ launch_arguments={
+ 'input_image_width': str(640),
+ 'input_image_height': str(640),
+ 'network_image_width': str(cfg['network_image_width']),
+ 'network_image_height': str(cfg['network_image_height']),
+ 'image_mean': str(cfg['image_mean']),
+ 'image_stddev': str(cfg['image_stddev']),
+ 'attach_to_shared_component_container': 'True',
+ 'component_container_name': 'cls_tensor_rt_container',
+ 'dnn_image_encoder_namespace': 'yolo_cls_encoder/internal',
+ 'image_input_topic': '/yolo_cls/internal/converted_image',
+ 'camera_info_input_topic': '/camera/camera/color/camera_info_rect',
+ 'tensor_output_topic': '/yolo_cls/tensor_pub',
+ }.items(),
+ )
+
+ actions = [tensor_rt_container, yolo_cls_encoder_launch]
+
+ actions.append(
+ Node(
+ package='isaac_ros_yolov26_cls',
+ executable='isaac_ros_yolov26_cls_visualizer.py',
+ name='yolo_cls_visualizer',
+ parameters=[
+ {
+ 'class_topic': '/classification_output',
+ 'image_topic': '/dnn_image_encoder/resize/image',
+ 'output_image_topic': '/yolo_cls_processed_image',
+ 'class_names_yaml': str({0: 'continue', 1: 'end'}),
+ }
+ ],
+ )
+ )
+
+ return actions
+
+
+def generate_launch_description():
+ return launch.LaunchDescription([OpaqueFunction(function=_launch_setup)])
diff --git a/perception_setup/launch/yolo/yolo_detect.launch.py b/perception_setup/launch/yolo/yolo_detect.launch.py
new file mode 100644
index 0000000..34bff69
--- /dev/null
+++ b/perception_setup/launch/yolo/yolo_detect.launch.py
@@ -0,0 +1,168 @@
+# SPDX-License-Identifier: MIT
+
+"""Isaac ROS YOLOv8 TensorRT inference pipeline.
+
+1. ImageFormatConverterNode
+ Input: IMAGE_INPUT_TOPIC (launch arg)
+ Output: CONVERTED_IMAGE_TOPIC
+ Purpose: Convert camera image to desired encoding (e.g. bgra8 -> rgb8)
+
+2. DNNImageEncoderNode
+ Input: CONVERTED_IMAGE_TOPIC
+ Output: TENSOR_OUTPUT_TOPIC
+ Purpose: Resize + normalize image and convert to network tensor
+
+3. TensorRTNode
+ Input: TENSOR_OUTPUT_TOPIC
+ Output: TENSOR_INPUT_TOPIC
+ Purpose: Run TensorRT inference on encoded tensor
+
+4. YoloV8DecoderNode
+ Input: TENSOR_INPUT_TOPIC
+ Output: DETECTION_TOPIC
+ Purpose: Convert network output tensor -> Detection2DArray
+
+5. (Optional) YOLOv8Visualizer
+ Input: DETECTION_TOPIC + encoder resize image
+ Output: VISUALIZED_IMAGE_TOPIC
+ Purpose: Draw bounding boxes on image
+"""
+
+import os
+
+import launch
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import (
+ IncludeLaunchDescription,
+ OpaqueFunction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def _launch_setup(context, *args, **kwargs):
+ pkg_dir = get_package_share_directory('perception_setup')
+ models_dir = os.path.join(pkg_dir, 'models')
+ config_path = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_detect.yaml')
+ with open(config_path) as f:
+ cfg = yaml.safe_load(f)
+
+ model_file_path = os.path.join(models_dir, 'best.onnx')
+ engine_file_path = os.path.join(models_dir, 'best.engine')
+
+ image_format_converter = ComposableNode(
+ package='isaac_ros_image_proc',
+ plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
+ name='image_format_converter',
+ parameters=[
+ {
+ 'encoding_desired': 'rgb8',
+ 'image_width': 896,
+ 'image_height': 504,
+ 'input_qos': 'sensor_data',
+ }
+ ],
+ remappings=[
+ ('image_raw', '/realsense_d555/color/image_rect'),
+ ('image', '/yolov8/internal/converted_image'),
+ ],
+ )
+
+ tensor_rt_node = ComposableNode(
+ name='tensor_rt',
+ package='isaac_ros_tensor_rt',
+ plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode',
+ parameters=[
+ {
+ 'model_file_path': model_file_path,
+ 'engine_file_path': engine_file_path,
+ 'output_binding_names': cfg['output_binding_names'],
+ 'output_tensor_names': cfg['output_tensor_names'],
+ 'input_tensor_names': cfg['input_tensor_names'],
+ 'input_binding_names': cfg['input_binding_names'],
+ 'verbose': True,
+ 'force_engine_update': False,
+ 'tensor_output_topic': '/yolov8/tensor_pub',
+ }
+ ],
+ remappings=[
+ ('tensor_pub', '/yolov8/tensor_pub'),
+ ('tensor_sub', '/yolov8/tensor_sub'),
+ ],
+ )
+
+ yolov8_decoder_node = ComposableNode(
+ name='yolov8_decoder_node',
+ package='isaac_ros_yolov8',
+ plugin='nvidia::isaac_ros::yolov8::YoloV8DecoderNode',
+ parameters=[
+ {
+ 'tensor_input_topic': '/yolov8/tensor_sub',
+ 'confidence_threshold': float(cfg['confidence_threshold']),
+ 'nms_threshold': float(cfg['nms_threshold']),
+ 'num_classes': int(cfg['num_classes']),
+ 'detections_topic': '/yolov8_detections_output',
+ }
+ ],
+ )
+
+ tensor_rt_container = ComposableNodeContainer(
+ name='detect_tensor_rt_container',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ image_format_converter,
+ tensor_rt_node,
+ yolov8_decoder_node,
+ ],
+ output='screen',
+ arguments=['--ros-args', '--log-level', 'INFO'],
+ namespace='',
+ )
+
+ encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder')
+ yolov8_encoder_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py')
+ ),
+ launch_arguments={
+ 'input_image_width': str(896),
+ 'input_image_height': str(504),
+ 'network_image_width': str(cfg['network_image_width']),
+ 'network_image_height': str(cfg['network_image_height']),
+ 'image_mean': str(cfg['image_mean']),
+ 'image_stddev': str(cfg['image_stddev']),
+ 'attach_to_shared_component_container': 'True',
+ 'component_container_name': 'detect_tensor_rt_container',
+ 'dnn_image_encoder_namespace': 'yolov8_encoder/internal',
+ 'image_input_topic': '/yolov8/internal/converted_image',
+ 'camera_info_input_topic': '/realsense_d555/color/camera_info',
+ 'tensor_output_topic': '/yolov8/tensor_pub',
+ }.items(),
+ )
+
+ actions = [tensor_rt_container, yolov8_encoder_launch]
+
+ actions.append(
+ Node(
+ package='isaac_ros_yolov8',
+ executable='isaac_ros_yolov8_visualizer.py',
+ name='yolov8_visualizer',
+ parameters=[
+ {
+ 'detections_topic': '/yolov8_detections_output',
+ 'image_topic': '/yolov8_encoder/internal/resize/image',
+ 'output_image_topic': '/yolov8_processed_image',
+ 'class_names_yaml': str({0: 'valve'}),
+ }
+ ],
+ )
+ )
+
+ return actions
+
+
+def generate_launch_description():
+ return launch.LaunchDescription([OpaqueFunction(function=_launch_setup)])
diff --git a/perception_setup/launch/yolo/yolo_obb.launch.py b/perception_setup/launch/yolo/yolo_obb.launch.py
new file mode 100644
index 0000000..0af1ac7
--- /dev/null
+++ b/perception_setup/launch/yolo/yolo_obb.launch.py
@@ -0,0 +1,159 @@
+# SPDX-License-Identifier: MIT
+
+"""Isaac ROS YOLO-OBB TensorRT inference pipeline.
+
+1. ImageFormatConverterNode
+ Input: IMAGE_INPUT_TOPIC (launch arg)
+ Output: CONVERTED_IMAGE_TOPIC
+
+2. DNNImageEncoderNode
+ Input: CONVERTED_IMAGE_TOPIC
+ Output: TENSOR_OUTPUT_TOPIC
+
+3. TensorRTNode
+ Input: TENSOR_OUTPUT_TOPIC
+ Output: TENSOR_INPUT_TOPIC
+ Expected output tensor shape: [1, num_detections, 7]
+
+4. YoloV26OBBDecoderNode
+ Input: TENSOR_INPUT_TOPIC
+ Output: DETECTION_TOPIC (Detection2DArray with BoundingBox2D.center.theta = OBB angle)
+"""
+
+import os
+
+import launch
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import (
+ IncludeLaunchDescription,
+ OpaqueFunction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def _launch_setup(context, *args, **kwargs):
+ pkg_dir = get_package_share_directory('perception_setup')
+ models_dir = os.path.join(pkg_dir, 'models')
+ config_path = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_obb.yaml')
+ with open(config_path) as f:
+ cfg = yaml.safe_load(f)
+
+ model_file_path = os.path.join(models_dir, 'obb_best.onnx')
+ engine_file_path = os.path.join(models_dir, 'obb_best.engine')
+
+ image_format_converter = ComposableNode(
+ package='isaac_ros_image_proc',
+ plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
+ name='image_format_converter',
+ parameters=[
+ {
+ 'encoding_desired': 'rgb8',
+ 'image_width': 896,
+ 'image_height': 504,
+ 'input_qos': 'sensor_data',
+ }
+ ],
+ remappings=[
+ ('image_raw', '/realsense_d555/color/image_rect'),
+ ('image', '/yolo_obb/internal/converted_image'),
+ ],
+ )
+
+ tensor_rt_node = ComposableNode(
+ name='tensor_rt',
+ package='isaac_ros_tensor_rt',
+ plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode',
+ parameters=[
+ {
+ 'model_file_path': model_file_path,
+ 'engine_file_path': engine_file_path,
+ 'output_binding_names': cfg['output_binding_names'],
+ 'output_tensor_names': cfg['output_tensor_names'],
+ 'input_tensor_names': cfg['input_tensor_names'],
+ 'input_binding_names': cfg['input_binding_names'],
+ 'verbose': True,
+ 'force_engine_update': False,
+ 'tensor_output_topic': '/yolo_obb/tensor_pub',
+ }
+ ],
+ remappings=[
+ ('tensor_pub', '/yolo_obb/tensor_pub'),
+ ('tensor_sub', '/yolo_obb/tensor_sub'),
+ ],
+ )
+
+ yolo_obb_decoder_node = ComposableNode(
+ name='yolo_obb_decoder_node',
+ package='isaac_ros_yolov26_obb',
+ plugin='nvidia::isaac_ros::yolov26_obb::YoloV26OBBDecoderNode',
+ parameters=[
+ {
+ 'tensor_input_topic': '/yolo_obb/tensor_sub',
+ 'confidence_threshold': float(cfg['confidence_threshold']),
+ 'num_detections': int(cfg['num_detections']),
+ 'detections_topic': '/obb_detections_output',
+ }
+ ],
+ )
+
+ tensor_rt_container = ComposableNodeContainer(
+ name='obb_tensor_rt_container',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ image_format_converter,
+ tensor_rt_node,
+ yolo_obb_decoder_node,
+ ],
+ output='screen',
+ arguments=['--ros-args', '--log-level', 'INFO'],
+ namespace='',
+ )
+
+ encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder')
+ yolo_obb_encoder_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py')
+ ),
+ launch_arguments={
+ 'input_image_width': str(896),
+ 'input_image_height': str(504),
+ 'network_image_width': str(cfg['network_image_width']),
+ 'network_image_height': str(cfg['network_image_height']),
+ 'image_mean': str(cfg['image_mean']),
+ 'image_stddev': str(cfg['image_stddev']),
+ 'attach_to_shared_component_container': 'True',
+ 'component_container_name': 'obb_tensor_rt_container',
+ 'dnn_image_encoder_namespace': 'yolo_obb_encoder/internal',
+ 'image_input_topic': '/yolo_obb/internal/converted_image',
+ 'camera_info_input_topic': '/realsense_d555/color/camera_info',
+ 'tensor_output_topic': '/yolo_obb/tensor_pub',
+ }.items(),
+ )
+
+ actions = [tensor_rt_container, yolo_obb_encoder_launch]
+
+ actions.append(
+ Node(
+ package='isaac_ros_yolov26_obb',
+ executable='isaac_ros_yolov26_obb_visualizer.py',
+ name='yolo_obb_visualizer',
+ parameters=[
+ {
+ 'detections_topic': '/obb_detections_output',
+ 'image_topic': '/yolo_obb_encoder/internal/resize/image',
+ 'output_image_topic': '/yolo_obb_processed_image',
+ 'class_names_yaml': str({0: 'valve'}),
+ }
+ ],
+ )
+ )
+
+ return actions
+
+
+def generate_launch_description():
+ return launch.LaunchDescription([OpaqueFunction(function=_launch_setup)])
diff --git a/perception_setup/launch/yolo/yolo_seg.launch.py b/perception_setup/launch/yolo/yolo_seg.launch.py
new file mode 100644
index 0000000..2fea4b3
--- /dev/null
+++ b/perception_setup/launch/yolo/yolo_seg.launch.py
@@ -0,0 +1,172 @@
+# SPDX-License-Identifier: MIT
+
+"""Isaac ROS YOLO Instance-Segmentation TensorRT inference pipeline.
+
+1. ImageFormatConverterNode
+ Input: IMAGE_INPUT_TOPIC (launch arg)
+ Output: CONVERTED_IMAGE_TOPIC
+
+2. DNNImageEncoderNode
+ Input: CONVERTED_IMAGE_TOPIC
+ Output: TENSOR_OUTPUT_TOPIC
+
+3. TensorRTNode
+ Input: TENSOR_OUTPUT_TOPIC
+ Output: TENSOR_INPUT_TOPIC
+ Expected output tensors:
+ output0 [1, 300, 38] detections with mask coefficients
+ output1 [1, 32, 160, 160] prototype masks
+
+4. YoloV26SegDecoderNode
+ Input: TENSOR_INPUT_TOPIC
+ Output: DETECTION_TOPIC (Detection2DArray), MASK_TOPIC (mono8 Image)
+
+5. (Optional) YoloSegVisualizer -> VISUALIZED_IMAGE_TOPIC
+"""
+
+import os
+
+import launch
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import (
+ IncludeLaunchDescription,
+ OpaqueFunction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def _launch_setup(context, *args, **kwargs):
+ pkg_dir = get_package_share_directory('perception_setup')
+ models_dir = os.path.join(pkg_dir, 'models')
+ config_path = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_seg.yaml')
+ with open(config_path) as f:
+ cfg = yaml.safe_load(f)
+
+ model_file_path = os.path.join(models_dir, 'seg_pipe_real.onnx')
+ engine_file_path = os.path.join(models_dir, 'seg_pipe_real.engine')
+
+ image_format_converter = ComposableNode(
+ package='isaac_ros_image_proc',
+ plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
+ name='image_format_converter',
+ parameters=[
+ {
+ 'encoding_desired': 'rgb8',
+ 'image_width': 720,
+ 'image_height': 540,
+ 'input_qos': 'sensor_data',
+ }
+ ],
+ remappings=[
+ ('image_raw', '/blackfly_s/image_raw'),
+ ('image', '/yolo_seg/internal/converted_image'),
+ ],
+ )
+
+ tensor_rt_node = ComposableNode(
+ name='tensor_rt',
+ package='isaac_ros_tensor_rt',
+ plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode',
+ parameters=[
+ {
+ 'model_file_path': model_file_path,
+ 'engine_file_path': engine_file_path,
+ 'output_binding_names': cfg['output_binding_names'],
+ 'output_tensor_names': cfg['output_tensor_names'],
+ 'input_tensor_names': cfg['input_tensor_names'],
+ 'input_binding_names': cfg['input_binding_names'],
+ 'verbose': True,
+ 'force_engine_update': False,
+ 'tensor_output_topic': '/yolo_seg/tensor_pub',
+ }
+ ],
+ remappings=[
+ ('tensor_pub', '/yolo_seg/tensor_pub'),
+ ('tensor_sub', '/yolo_seg/tensor_sub'),
+ ],
+ )
+
+ yolo_seg_decoder_node = ComposableNode(
+ name='yolo_seg_decoder_node',
+ package='isaac_ros_yolov26_seg',
+ plugin='nvidia::isaac_ros::yolov26_seg::YoloV26SegDecoderNode',
+ parameters=[
+ {
+ 'tensor_input_topic': '/yolo_seg/tensor_sub',
+ 'confidence_threshold': float(cfg['confidence_threshold']),
+ 'num_detections': int(cfg['num_detections']),
+ 'mask_width': int(cfg['mask_width']),
+ 'mask_height': int(cfg['mask_height']),
+ 'num_protos': int(cfg['num_protos']),
+ 'network_image_width': int(cfg['network_image_width']),
+ 'network_image_height': int(cfg['network_image_height']),
+ 'output_mask_width': 640,
+ 'output_mask_height': 640,
+ 'detections_topic': '/seg_detections_output',
+ 'mask_topic': '/yolo_seg_mask',
+ }
+ ],
+ )
+
+ tensor_rt_container = ComposableNodeContainer(
+ name='seg_tensor_rt_container',
+ package='rclcpp_components',
+ executable='component_container_mt',
+ composable_node_descriptions=[
+ image_format_converter,
+ tensor_rt_node,
+ yolo_seg_decoder_node,
+ ],
+ output='screen',
+ arguments=['--ros-args', '--log-level', 'INFO'],
+ namespace='',
+ )
+
+ encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder')
+ yolo_seg_encoder_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py')
+ ),
+ launch_arguments={
+ 'input_image_width': str(720),
+ 'input_image_height': str(540),
+ 'network_image_width': str(cfg['network_image_width']),
+ 'network_image_height': str(cfg['network_image_height']),
+ 'image_mean': str(cfg['image_mean']),
+ 'image_stddev': str(cfg['image_stddev']),
+ 'attach_to_shared_component_container': 'True',
+ 'component_container_name': 'seg_tensor_rt_container',
+ 'dnn_image_encoder_namespace': 'yolo_seg_encoder/internal',
+ 'image_input_topic': '/yolo_seg/internal/converted_image',
+ 'camera_info_input_topic': '/blackfly_s/camera_info',
+ 'tensor_output_topic': '/yolo_seg/tensor_pub',
+ }.items(),
+ )
+
+ actions = [tensor_rt_container, yolo_seg_encoder_launch]
+
+ actions.append(
+ Node(
+ package='isaac_ros_yolov26_seg',
+ executable='isaac_ros_yolov26_seg_visualizer.py',
+ name='yolo_seg_visualizer',
+ parameters=[
+ {
+ 'detections_topic': '/seg_detections_output',
+ 'image_topic': '/yolo_seg_encoder/internal/resize/image',
+ 'mask_topic': '/yolo_seg_mask',
+ 'output_image_topic': '/yolo_seg_processed_image',
+ 'class_names_yaml': str({0: 'pipeline'}),
+ }
+ ],
+ )
+ )
+
+ return actions
+
+
+def generate_launch_description():
+ return launch.LaunchDescription([OpaqueFunction(function=_launch_setup)])
diff --git a/perception_setup/launch/zed_composable_node.launch.py b/perception_setup/launch/zed_composable_node.launch.py
deleted file mode 100644
index 5a5afa8..0000000
--- a/perception_setup/launch/zed_composable_node.launch.py
+++ /dev/null
@@ -1,107 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.conditions import IfCondition, UnlessCondition
-from launch.substitutions import Command, LaunchConfiguration
-from launch_ros.actions import LoadComposableNodes, Node
-from launch_ros.descriptions import ComposableNode
-
-# Define the default container name for composable nodes
-DEFAULT_CONTAINER_NAME = 'my_perception_container'
-
-
-def generate_launch_description() -> LaunchDescription:
- # Declare launch arguments
- container_name_arg = DeclareLaunchArgument(
- 'container_name',
- default_value=DEFAULT_CONTAINER_NAME,
- description='Name of the container to load the composable nodes into',
- )
- run_composable_arg = DeclareLaunchArgument(
- 'run_composable',
- default_value='False',
- description='Run the ZED node as a standalone node if set to false',
- )
-
- container_name = LaunchConfiguration('container_name')
- run_composable = LaunchConfiguration('run_composable')
-
- # Configuration file path (adjust the path according to your setup)
- config_file_common = os.path.join(
- get_package_share_directory('perception_setup'),
- 'config',
- 'zed_driver_params.yaml',
- )
-
- # URDF/xacro file to be loaded by the Robot State Publisher node
- xacro_path = os.path.join(
- get_package_share_directory('zed_wrapper'), 'urdf', 'zed_descr.urdf.xacro'
- )
-
- # Robot State Publisher node (publishing static TFs for the camera)
- robot_state_publisher_node = Node(
- package='robot_state_publisher',
- namespace='zed',
- executable='robot_state_publisher',
- name='zed_state_publisher',
- output='screen',
- parameters=[
- {
- 'robot_description': Command(
- [
- 'xacro',
- ' ',
- str(xacro_path),
- ' ',
- 'camera_name:=zed',
- ' ',
- 'camera_model:=zed2i',
- ]
- )
- }
- ],
- )
-
- # Define the ZED composable node
- zed_composable_node = ComposableNode(
- package='zed_components',
- namespace='zed',
- name='zed_node',
- plugin='stereolabs::ZedCamera',
- parameters=[
- config_file_common, # Common parameters
- ],
- extra_arguments=[{'use_intra_process_comms': True}],
- condition=IfCondition(run_composable),
- )
-
- zed_node = Node(
- package='zed_wrapper',
- namespace='zed',
- executable='zed_wrapper',
- name='zed_node',
- output='screen',
- parameters=[
- config_file_common, # Common parameters
- ],
- condition=UnlessCondition(run_composable),
- )
-
- # Load the ZED composable node into the existing container
- load_zed_node = LoadComposableNodes(
- target_container=container_name,
- composable_node_descriptions=[zed_composable_node],
- condition=IfCondition(run_composable),
- )
-
- return LaunchDescription(
- [
- container_name_arg,
- run_composable_arg,
- robot_state_publisher_node,
- load_zed_node,
- zed_node,
- ]
- )
diff --git a/perception_setup/launch/zed_yolo.launch.py b/perception_setup/launch/zed_yolo.launch.py
deleted file mode 100644
index f8eef77..0000000
--- a/perception_setup/launch/zed_yolo.launch.py
+++ /dev/null
@@ -1,296 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import Command, LaunchConfiguration
-from launch_ros.actions import ComposableNodeContainer, Node
-from launch_ros.descriptions import ComposableNode
-
-# Define the default container name for composable nodes
-DEFAULT_CONTAINER_NAME = 'zed_yolov8_container'
-
-
-def generate_launch_description():
- # Declare common launch arguments
- config_file_common = os.path.join(
- get_package_share_directory('perception_setup'),
- 'config',
- 'zed_driver_params.yaml',
- )
-
- # URDF/xacro file to be loaded by the Robot State Publisher node
- xacro_path = os.path.join(
- get_package_share_directory('zed_wrapper'), 'urdf', 'zed_descr.urdf.xacro'
- )
-
- # Define the default values for the YOLOv8 composable node configurations
-
- image_format_converter_node_left = ComposableNode(
- package='isaac_ros_image_proc',
- plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
- name='image_format_node_left',
- parameters=[
- {
- 'encoding_desired': 'rgb8',
- 'image_width': 1280,
- 'image_height': 720,
- }
- ],
- remappings=[
- ('image_raw', 'zed_node/left/image_rect_color'),
- ('image', 'image_rect'),
- ],
- )
- zed_wrapper_component = ComposableNode(
- package='zed_components',
- name='zed_node',
- plugin='stereolabs::ZedCamera',
- parameters=[
- config_file_common, # Common parameters
- ],
- remappings=[
- ('zed_node/left/camera_info', '/camera_info_rect'),
- ],
- extra_arguments=[{'use_intra_process_comms': True}],
- )
-
- valve_detection_node = ComposableNode(
- package='valve_detection',
- plugin='ValveDetectionNode',
- name='valve_detection_node',
- parameters=[
- os.path.join(
- get_package_share_directory('valve_detection'),
- 'config',
- 'valve_detection_params.yaml',
- )
- ],
- )
-
- rsp_node = Node(
- package='robot_state_publisher',
- executable='robot_state_publisher',
- name='zed_state_publisher',
- output='screen',
- parameters=[
- {
- 'robot_description': Command(
- [
- 'xacro',
- ' ',
- str(xacro_path),
- ' ',
- 'camera_name:=zed',
- ' ',
- 'camera_model:=zed2i',
- ]
- )
- }
- ],
- )
-
- # TensorRT parameters
- model_file_path = LaunchConfiguration('model_file_path')
- engine_file_path = LaunchConfiguration('engine_file_path')
- input_tensor_names = LaunchConfiguration('input_tensor_names')
- input_binding_names = LaunchConfiguration('input_binding_names')
- output_tensor_names = LaunchConfiguration('output_tensor_names')
- output_binding_names = LaunchConfiguration('output_binding_names')
- verbose = LaunchConfiguration('verbose')
- force_engine_update = LaunchConfiguration('force_engine_update')
-
- # YOLOv8 Decoder parameters
- confidence_threshold = LaunchConfiguration('confidence_threshold')
- nms_threshold = LaunchConfiguration('nms_threshold')
-
- tensor_rt_node = ComposableNode(
- name='tensor_rt',
- package='isaac_ros_tensor_rt',
- plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode',
- parameters=[
- {
- 'model_file_path': model_file_path,
- 'engine_file_path': engine_file_path,
- 'output_binding_names': output_binding_names,
- 'output_tensor_names': output_tensor_names,
- 'input_tensor_names': input_tensor_names,
- 'input_binding_names': input_binding_names,
- 'verbose': verbose,
- 'force_engine_update': force_engine_update,
- }
- ],
- )
- yolov8_decoder_node = ComposableNode(
- name='yolov8_decoder_node',
- package='isaac_ros_yolov8',
- plugin='nvidia::isaac_ros::yolov8::YoloV8DecoderNode',
- parameters=[
- {
- 'confidence_threshold': confidence_threshold,
- 'nms_threshold': nms_threshold,
- 'num_classes': 1,
- }
- ],
- )
-
- network_image_width = LaunchConfiguration('network_image_width')
- network_image_height = LaunchConfiguration('network_image_height')
- input_encoding = LaunchConfiguration('input_encoding')
- image_mean = LaunchConfiguration('image_mean')
- image_stddev = LaunchConfiguration('image_stddev')
- image_input_topic = LaunchConfiguration('image_input_topic')
- camera_info_input_topic = LaunchConfiguration('camera_info_input_topic')
-
- encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder')
-
- network_image_width = DeclareLaunchArgument(
- 'network_image_width',
- default_value='640',
- description='The input image width that the network expects',
- )
- network_image_height = DeclareLaunchArgument(
- 'network_image_height',
- default_value='640',
- description='The input image height that the network expects',
- )
- input_encoding = DeclareLaunchArgument(
- 'input_encoding',
- default_value='rgb8',
- description='The desired image format encoding',
- )
- image_mean = DeclareLaunchArgument(
- 'image_mean',
- default_value='[0.0, 0.0, 0.0]',
- description='The mean for image normalization',
- )
- image_stddev = DeclareLaunchArgument(
- 'image_stddev',
- default_value='[1.0, 1.0, 1.0]',
- description='The standard deviation for image normalization',
- )
- model_file_path = DeclareLaunchArgument(
- 'model_file_path',
- default_value=os.path.join(
- get_package_share_directory('perception_setup'), 'models', 'best200.onnx'
- ),
- description='Path to the ONNX model file',
- )
- engine_file_path = DeclareLaunchArgument(
- 'engine_file_path',
- default_value=os.path.join(
- get_package_share_directory('perception_setup'),
- 'models',
- 'yolo-04-04.engine',
- ),
- description='Path to the TensorRT engine file',
- )
- input_tensor_names = DeclareLaunchArgument(
- 'input_tensor_names',
- default_value='["input_tensor"]',
- description='A list of tensor names to bound to the specified input binding names',
- )
- input_binding_names = DeclareLaunchArgument(
- 'input_binding_names',
- default_value='["images"]',
- description='A list of input tensor binding names (specified by model)',
- )
- image_input_topic = DeclareLaunchArgument(
- 'image_input_topic',
- default_value='/image_rect',
- description='Input image topic name',
- )
- camera_info_input_topic = DeclareLaunchArgument(
- 'camera_info_input_topic',
- default_value='/camera_info_rect',
- description='Input camera info topic name',
- )
- output_tensor_names = DeclareLaunchArgument(
- 'output_tensor_names',
- default_value='["output_tensor"]',
- description='A list of tensor names to bound to the specified output binding names',
- )
- output_binding_names = DeclareLaunchArgument(
- 'output_binding_names',
- default_value='["output0"]',
- description='A list of output tensor binding names (specified by model)',
- )
- verbose = DeclareLaunchArgument(
- 'verbose',
- default_value='False',
- description='Whether TensorRT should verbosely log or not',
- )
- force_engine_update = DeclareLaunchArgument(
- 'force_engine_update',
- default_value='False',
- description='Whether TensorRT should update the TensorRT engine file or not',
- )
- confidence_threshold = DeclareLaunchArgument(
- 'confidence_threshold',
- default_value='0.25',
- description='Confidence threshold to filter candidate detections during NMS',
- )
- nms_threshold = DeclareLaunchArgument(
- 'nms_threshold', default_value='0.45', description='NMS IOU threshold'
- )
-
- yolov8_encoder_launch = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- [os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py')]
- ),
- launch_arguments={
- 'input_image_width': '1280',
- 'input_image_height': '720',
- 'network_image_width': LaunchConfiguration('network_image_width'),
- 'network_image_height': LaunchConfiguration('network_image_height'),
- 'image_mean': LaunchConfiguration('image_mean'),
- 'image_stddev': LaunchConfiguration('image_stddev'),
- 'attach_to_shared_component_container': 'True',
- 'component_container_name': 'yolo_container',
- 'dnn_image_encoder_namespace': 'yolov8_encoder',
- 'image_input_topic': LaunchConfiguration('image_input_topic'),
- 'camera_info_input_topic': LaunchConfiguration('camera_info_input_topic'),
- 'tensor_output_topic': '/tensor_pub',
- 'input_encoding': LaunchConfiguration('input_encoding'),
- }.items(),
- )
-
- return LaunchDescription(
- [
- network_image_width,
- network_image_height,
- input_encoding,
- image_mean,
- image_stddev,
- model_file_path,
- engine_file_path,
- input_tensor_names,
- input_binding_names,
- image_input_topic,
- camera_info_input_topic,
- output_tensor_names,
- output_binding_names,
- verbose,
- force_engine_update,
- confidence_threshold,
- nms_threshold,
- rsp_node,
- yolov8_encoder_launch,
- ComposableNodeContainer(
- name='yolo_container',
- namespace='',
- package='rclcpp_components',
- executable='component_container_mt',
- composable_node_descriptions=[
- tensor_rt_node,
- yolov8_decoder_node,
- image_format_converter_node_left,
- zed_wrapper_component,
- valve_detection_node,
- ],
- output='screen',
- arguments=['--ros-args', '--log-level', 'INFO'],
- ),
- ]
- )
diff --git a/perception_setup/package.xml b/perception_setup/package.xml
index 2ee227c..9f6fc68 100644
--- a/perception_setup/package.xml
+++ b/perception_setup/package.xml
@@ -7,13 +7,35 @@
jorgen
MIT
+ camera_info_manager
+ vortex_utils_ros
+
+ rclcpp
+ rclcpp_components
+ sensor_msgs
+ cv_bridge
+ yaml-cpp
+
ament_cmake
ament_lint_auto
ament_lint_common
ros2launch
-
+ launch_ros
+
+ valve_detection
+ image_filtering
+ aruco_detector
+ image_to_gstreamer
ament_cmake
diff --git a/perception_setup/scripts/camera_info_publisher.py b/perception_setup/scripts/camera_info_publisher.py
new file mode 100755
index 0000000..2b6a132
--- /dev/null
+++ b/perception_setup/scripts/camera_info_publisher.py
@@ -0,0 +1,73 @@
+#!/usr/bin/env python3
+
+import rclpy
+import yaml
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from sensor_msgs.msg import CameraInfo, Image
+from vortex_utils_ros.qos_profiles import reliable_profile
+
+
+class CameraInfoPublisher(Node):
+ def __init__(self):
+ super().__init__("camera_info_publisher")
+
+ self.declare_parameter("camera_info_file", Parameter.Type.STRING)
+ self.declare_parameter("camera_info_topic", Parameter.Type.STRING)
+ self.declare_parameter("image_topic", Parameter.Type.STRING)
+
+ file_path = self.get_parameter("camera_info_file").value
+ topic_name = self.get_parameter("camera_info_topic").value
+ image_topic = self.get_parameter("image_topic").value
+
+ if not file_path:
+ raise RuntimeError("Parameter 'camera_info_file' must be provided")
+
+ if not topic_name:
+ raise RuntimeError("Parameter 'camera_info_topic' must be provided")
+
+ if not image_topic:
+ raise RuntimeError("Parameter 'image_topic' must be provided")
+
+ with open(file_path) as f:
+ data = yaml.safe_load(f)
+
+ msg = CameraInfo()
+
+ msg.width = int(data["image_width"])
+ msg.height = int(data["image_height"])
+ msg.distortion_model = data["distortion_model"]
+
+ msg.k = data["camera_matrix"]["data"]
+ msg.d = data["distortion_coefficients"]["data"]
+ msg.r = data["rectification_matrix"]["data"]
+ msg.p = data["projection_matrix"]["data"]
+
+ self.publisher = self.create_publisher(
+ CameraInfo, topic_name, reliable_profile(10)
+ )
+ self.msg = msg
+
+ self.create_subscription(
+ Image, image_topic, self._image_callback, reliable_profile(1)
+ )
+
+ self.get_logger().info(
+ f"Publishing CameraInfo on {topic_name} synced to {image_topic}"
+ )
+ self.get_logger().info(f"Loaded calibration: {file_path}")
+
+ def _image_callback(self, img_msg: Image):
+ self.msg.header.stamp = img_msg.header.stamp
+ self.msg.header.frame_id = img_msg.header.frame_id
+ self.publisher.publish(self.msg)
+
+
+def main():
+ rclpy.init()
+ node = CameraInfoPublisher()
+ rclpy.spin(node)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/perception_setup/scripts/image_crop.py b/perception_setup/scripts/image_crop.py
new file mode 100755
index 0000000..5198d30
--- /dev/null
+++ b/perception_setup/scripts/image_crop.py
@@ -0,0 +1,126 @@
+#!/usr/bin/env python3
+
+import rclpy
+from cv_bridge import CvBridge
+from message_filters import ApproximateTimeSynchronizer, Subscriber
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from sensor_msgs.msg import CameraInfo, Image
+from vortex_utils_ros.qos_profiles import sensor_data_profile
+
+
+class ImageCrop(Node):
+ def __init__(self):
+ super().__init__("image_crop")
+
+ self.declare_parameter("image_topic", Parameter.Type.STRING)
+ self.declare_parameter("camera_info_topic", Parameter.Type.STRING)
+ self.declare_parameter("output_image_topic", Parameter.Type.STRING)
+ self.declare_parameter("output_camera_info_topic", Parameter.Type.STRING)
+ self.declare_parameter("crop.x_offset", Parameter.Type.INTEGER)
+ self.declare_parameter("crop.y_offset", Parameter.Type.INTEGER)
+ self.declare_parameter("crop.width", Parameter.Type.INTEGER)
+ self.declare_parameter("crop.height", Parameter.Type.INTEGER)
+ self.declare_parameter("enable_crop", Parameter.Type.BOOL)
+
+ image_topic = self.get_parameter("image_topic").value
+ info_topic = self.get_parameter("camera_info_topic").value
+ out_image_topic = self.get_parameter("output_image_topic").value
+ out_info_topic = self.get_parameter("output_camera_info_topic").value
+
+ self.x = self.get_parameter("crop.x_offset").value
+ self.y = self.get_parameter("crop.y_offset").value
+ self.w = self.get_parameter("crop.width").value
+ self.h = self.get_parameter("crop.height").value
+ self.enable_crop = self.get_parameter("enable_crop").value
+
+ self.bridge = CvBridge()
+
+ image_sub = Subscriber(
+ self, Image, image_topic, qos_profile=sensor_data_profile(10)
+ )
+ info_sub = Subscriber(
+ self, CameraInfo, info_topic, qos_profile=sensor_data_profile(10)
+ )
+
+ self.sync = ApproximateTimeSynchronizer(
+ [image_sub, info_sub], queue_size=10, slop=0.05
+ )
+ self.sync.registerCallback(self.callback)
+
+ self.image_pub = self.create_publisher(
+ Image, out_image_topic, sensor_data_profile(10)
+ )
+ self.info_pub = self.create_publisher(
+ CameraInfo, out_info_topic, sensor_data_profile(10)
+ )
+
+ if self.enable_crop:
+ self.get_logger().info(
+ f"image_crop: {image_topic} -> {out_image_topic} "
+ f"[x={self.x}, y={self.y}, w={self.w or 'full'}, h={self.h or 'full'}]"
+ )
+ else:
+ self.get_logger().info(
+ f"image_crop: passthrough {image_topic} -> {out_image_topic}"
+ )
+
+ def callback(self, image_msg: Image, info_msg: CameraInfo):
+ if not self.enable_crop:
+ self.image_pub.publish(image_msg)
+ self.info_pub.publish(info_msg)
+ return
+
+ cv_image = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding="passthrough")
+
+ img_h, img_w = cv_image.shape[:2]
+
+ x = self.x
+ y = self.y
+ w = self.w if self.w > 0 else img_w - x
+ h = self.h if self.h > 0 else img_h - y
+
+ cropped = cv_image[y : y + h, x : x + w]
+
+ # Republish cropped image
+ out_image = self.bridge.cv2_to_imgmsg(cropped, encoding=image_msg.encoding)
+ out_image.header = image_msg.header
+ self.image_pub.publish(out_image)
+
+ # Update camera info: shift principal point by crop offset, update size
+ out_info = CameraInfo()
+ out_info.header = info_msg.header
+ out_info.width = w
+ out_info.height = h
+ out_info.distortion_model = info_msg.distortion_model
+ out_info.d = info_msg.d
+
+ k = list(info_msg.k)
+ k[2] -= x # cx
+ k[5] -= y # cy
+ out_info.k = k
+
+ out_info.r = info_msg.r
+
+ p = list(info_msg.p)
+ p[2] -= x # cx
+ p[6] -= y # cy
+ out_info.p = p
+
+ out_info.roi.x_offset = x
+ out_info.roi.y_offset = y
+ out_info.roi.width = w
+ out_info.roi.height = h
+ out_info.roi.do_rectify = False
+
+ self.info_pub.publish(out_info)
+
+
+def main():
+ rclpy.init()
+ node = ImageCrop()
+ rclpy.spin(node)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/perception_setup/scripts/tmux_perception.sh b/perception_setup/scripts/tmux_perception.sh
new file mode 100755
index 0000000..a75e487
--- /dev/null
+++ b/perception_setup/scripts/tmux_perception.sh
@@ -0,0 +1,59 @@
+#!/bin/bash
+# Launch perception stack in a tmux session.
+#
+# Usage:
+# ./tmux_perception.sh # camera + OBB (defaults)
+# ./tmux_perception.sh realsense_d555 obb # explicit camera + pipeline
+# ./tmux_perception.sh blackfly_s seg # blackfly + segmentation
+#
+# Each component runs in its own tmux window so you can monitor/restart
+# them independently.
+
+set -euo pipefail
+
+SESSION="perception"
+CAMERA="${1:-realsense_d555}"
+PIPELINE="${2:-obb}"
+
+# Map camera name to launch file
+declare -A CAMERA_LAUNCH=(
+ [realsense_d555]="realsense_d555.launch.py"
+ [blackfly_s]="blackfly_s.launch.py"
+)
+
+# Map pipeline name to YOLO launch file
+declare -A PIPELINE_LAUNCH=(
+ [detect]="yolo_detect.launch.py"
+ [obb]="yolo_obb.launch.py"
+ [seg]="yolo_seg.launch.py"
+ [cls]="yolo_cls.launch.py"
+)
+
+CAMERA_FILE="${CAMERA_LAUNCH[$CAMERA]:-}"
+PIPELINE_FILE="${PIPELINE_LAUNCH[$PIPELINE]:-}"
+
+if [[ -z "$CAMERA_FILE" ]]; then
+ echo "Unknown camera: $CAMERA (available: ${!CAMERA_LAUNCH[*]})"
+ exit 1
+fi
+
+if [[ -z "$PIPELINE_FILE" ]]; then
+ echo "Unknown pipeline: $PIPELINE (available: ${!PIPELINE_LAUNCH[*]})"
+ exit 1
+fi
+
+# Kill existing session if present
+tmux kill-session -t "$SESSION" 2>/dev/null || true
+
+# Window 1: Camera
+tmux new-session -d -s "$SESSION" -n "camera"
+tmux send-keys -t "$SESSION:camera" \
+ "ros2 launch perception_setup $CAMERA_FILE" Enter
+
+# Window 2: YOLO pipeline
+tmux new-window -t "$SESSION" -n "$PIPELINE"
+tmux send-keys -t "$SESSION:$PIPELINE" \
+ "ros2 launch perception_setup $PIPELINE_FILE camera:=$CAMERA" Enter
+
+# Attach
+tmux attach -t "$SESSION"
diff --git a/perception_setup/src/image_undistort.cpp b/perception_setup/src/image_undistort.cpp
new file mode 100644
index 0000000..dd09978
--- /dev/null
+++ b/perception_setup/src/image_undistort.cpp
@@ -0,0 +1,187 @@
+#include "perception_setup/image_undistort.hpp"
+
+#include
+
+#include
+#include
+#include
+
+namespace perception_setup {
+
+ImageUndistort::ImageUndistort(const rclcpp::NodeOptions& options)
+ : Node("image_undistort", options) {
+ const auto image_topic = declare_parameter("image_topic");
+ const auto info_topic =
+ declare_parameter("camera_info_topic", "");
+ const auto camera_info_file =
+ declare_parameter("camera_info_file", "");
+ const auto raw_info_topic =
+ declare_parameter("raw_camera_info_topic");
+ const auto out_topic = declare_parameter("output_image_topic");
+ const auto out_info_topic =
+ declare_parameter("output_camera_info_topic");
+ const auto enable_undistort = declare_parameter("enable_undistort");
+ const auto image_qos_str =
+ declare_parameter("image_qos", "sensor_data");
+
+ const auto image_qos = (image_qos_str == "reliable")
+ ? rclcpp::QoS(10).reliable()
+ : rclcpp::QoS(10).best_effort();
+ const auto reliable_qos = rclcpp::QoS(10).reliable();
+ const auto sensor_data_qos = rclcpp::QoS(10).best_effort();
+
+ image_pub_ =
+ create_publisher(out_topic, image_qos);
+ info_pub_ = create_publisher(out_info_topic,
+ reliable_qos);
+
+ if (enable_undistort) {
+ if (!camera_info_file.empty()) {
+ init_maps_from_file(camera_info_file);
+ } else {
+ info_sub_ = create_subscription(
+ info_topic, rclcpp::QoS(1).reliable(),
+ [this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
+ info_callback(msg);
+ });
+ }
+
+ image_sub_ = create_subscription(
+ image_topic, sensor_data_qos,
+ [this](const sensor_msgs::msg::Image::SharedPtr msg) {
+ image_callback(msg);
+ });
+
+ RCLCPP_INFO(get_logger(), "image_undistort: %s -> %s",
+ image_topic.c_str(), out_topic.c_str());
+ } else {
+ image_sub_ = create_subscription(
+ image_topic, sensor_data_qos,
+ [this](const sensor_msgs::msg::Image::SharedPtr msg) {
+ relay_image(msg);
+ });
+
+ info_sub_ = create_subscription(
+ raw_info_topic, rclcpp::QoS(1).reliable(),
+ [this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
+ relay_camera_info(msg);
+ });
+
+ RCLCPP_INFO(get_logger(), "image_undistort: passthrough %s -> %s",
+ image_topic.c_str(), out_topic.c_str());
+ }
+}
+
+void ImageUndistort::init_maps_from_file(const std::string& path) {
+ const YAML::Node data = YAML::LoadFile(path);
+
+ const auto k_vec = data["camera_matrix"]["data"].as>();
+ const auto d_vec =
+ data["distortion_coefficients"]["data"].as>();
+ const int w = data["image_width"].as();
+ const int h = data["image_height"].as();
+
+ const cv::Mat k(3, 3, CV_64F, const_cast(k_vec.data()));
+ const cv::Mat d(1, static_cast(d_vec.size()), CV_64F,
+ const_cast(d_vec.data()));
+
+ build_maps(k, d, w, h);
+ RCLCPP_INFO(get_logger(), "Undistortion maps initialised from file (%dx%d)",
+ w, h);
+}
+
+void ImageUndistort::build_maps(const cv::Mat& k,
+ const cv::Mat& d,
+ int w,
+ int h) {
+ const cv::Mat new_k = cv::getOptimalNewCameraMatrix(k, d, {w, h}, 0.0);
+ cv::initUndistortRectifyMap(k, d, cv::noArray(), new_k, {w, h}, CV_16SC2,
+ map1_, map2_);
+
+ rectified_info_ = sensor_msgs::msg::CameraInfo{};
+ rectified_info_.width = static_cast(w);
+ rectified_info_.height = static_cast(h);
+ rectified_info_.distortion_model = "plumb_bob";
+ rectified_info_.d = {0.0, 0.0, 0.0, 0.0, 0.0};
+ rectified_info_.k = {new_k.at(0, 0),
+ 0.0,
+ new_k.at(0, 2),
+ 0.0,
+ new_k.at(1, 1),
+ new_k.at(1, 2),
+ 0.0,
+ 0.0,
+ 1.0};
+ rectified_info_.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
+ rectified_info_.p = {new_k.at(0, 0),
+ 0.0,
+ new_k.at(0, 2),
+ 0.0,
+ 0.0,
+ new_k.at(1, 1),
+ new_k.at(1, 2),
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0,
+ 0.0};
+
+ maps_ready_ = true;
+}
+
+void ImageUndistort::info_callback(
+ const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
+ if (maps_ready_)
+ return;
+
+ const cv::Mat k(3, 3, CV_64F, const_cast(msg->k.data()));
+ const cv::Mat d(1, static_cast(msg->d.size()), CV_64F,
+ const_cast(msg->d.data()));
+
+ build_maps(k, d, static_cast(msg->width),
+ static_cast(msg->height));
+ RCLCPP_INFO(get_logger(), "Undistortion maps initialised (%dx%d)",
+ msg->width, msg->height);
+
+ info_sub_.reset();
+}
+
+void ImageUndistort::image_callback(
+ const sensor_msgs::msg::Image::SharedPtr msg) {
+ if (!maps_ready_) {
+ RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000,
+ "Waiting for camera_info...");
+ return;
+ }
+
+ cv_bridge::CvImageConstPtr cv_ptr;
+ try {
+ cv_ptr = cv_bridge::toCvShare(msg);
+ } catch (const cv_bridge::Exception& e) {
+ RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", e.what());
+ return;
+ }
+
+ cv::Mat undistorted;
+ cv::remap(cv_ptr->image, undistorted, map1_, map2_, cv::INTER_LINEAR);
+
+ auto out_msg = cv_bridge::CvImage(msg->header, msg->encoding, undistorted)
+ .toImageMsg();
+ image_pub_->publish(*out_msg);
+
+ rectified_info_.header = msg->header;
+ info_pub_->publish(rectified_info_);
+}
+
+void ImageUndistort::relay_image(const sensor_msgs::msg::Image::SharedPtr msg) {
+ image_pub_->publish(*msg);
+}
+
+void ImageUndistort::relay_camera_info(
+ const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
+ info_pub_->publish(*msg);
+}
+
+} // namespace perception_setup
+
+RCLCPP_COMPONENTS_REGISTER_NODE(perception_setup::ImageUndistort)
diff --git a/scripts/fastdds_profile.xml b/scripts/fastdds_profile.xml
new file mode 100644
index 0000000..11b32e0
--- /dev/null
+++ b/scripts/fastdds_profile.xml
@@ -0,0 +1,22 @@
+
+
+
+
+
+ udp_eno1
+ UDPv4
+
+ eno1
+
+
+
+
+
+
+ udp_eno1
+
+ false
+
+
+
+
diff --git a/scripts/pt_to_onnx.py b/scripts/pt_to_onnx.py
new file mode 100755
index 0000000..951ccd1
--- /dev/null
+++ b/scripts/pt_to_onnx.py
@@ -0,0 +1,9 @@
+from ultralytics import YOLO
+
+# Load model
+model = YOLO("obb_best.pt")
+
+# Export to ONNX
+model.export(format="onnx", opset=12, dynamic=False, simplify=True)
+
+print("Export complete")
diff --git a/scripts/set_MTU_nic.sh b/scripts/set_MTU_nic.sh
index cf9ac0e..98a03da 100755
--- a/scripts/set_MTU_nic.sh
+++ b/scripts/set_MTU_nic.sh
@@ -1,6 +1,5 @@
-sudo ip link set enP5p1s0f0 down
-sudo ip link set enP5p1s0f0 mtu 9000
-sudo ip link set enP5p1s0f0 up
-sudo nmcli connection modify "Wired connection 3" 802-3-ethernet.mtu 9000
+sudo ip link set enP5p1s0f3 down
+sudo ip link set enP5p1s0f3 mtu 9000
+sudo ip link set enP5p1s0f3 up
sudo systemctl restart NetworkManager
-ip link show enP5p1s0f0
+ip link show enP5p1s0f3
diff --git a/scripts/set_fastdds_eno1.sh b/scripts/set_fastdds_eno1.sh
new file mode 100644
index 0000000..04d77b1
--- /dev/null
+++ b/scripts/set_fastdds_eno1.sh
@@ -0,0 +1,30 @@
+#!/usr/bin/env bash
+
+# Source this file, do not execute it.
+
+if [[ "${BASH_SOURCE[0]}" == "${0}" ]]; then
+ echo "[Fast DDS] Please source this script:"
+ echo " source ${0}"
+ exit 1
+fi
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+PROFILE_FILE="${SCRIPT_DIR}/../config/fastdds_multi_interface.xml"
+
+if [[ ! -f "${PROFILE_FILE}" ]]; then
+ echo "[Fast DDS] Error: profile not found at ${PROFILE_FILE}"
+ return 1
+fi
+
+export FASTRTPS_DEFAULT_PROFILES_FILE="${PROFILE_FILE}"
+export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+export ROS_DOMAIN_ID=0
+unset ROS_LOCALHOST_ONLY
+
+if command -v ros2 >/dev/null 2>&1; then
+ ros2 daemon stop >/dev/null 2>&1 || true
+fi
+
+echo "[Fast DDS] Profile: ${FASTRTPS_DEFAULT_PROFILES_FILE}"
+echo "[Fast DDS] RMW_IMPLEMENTATION=${RMW_IMPLEMENTATION}"
+echo "[Fast DDS] ROS_DOMAIN_ID=${ROS_DOMAIN_ID}"
diff --git a/scripts/tmux_nautilus.sh b/scripts/tmux_nautilus.sh
new file mode 100755
index 0000000..4583d4b
--- /dev/null
+++ b/scripts/tmux_nautilus.sh
@@ -0,0 +1,9 @@
+#!/bin/bash
+
+tmux new-session -d -s "nautilus"
+
+tmux split-window -v -t "nautilus:0"
+tmux split-window -h -t "nautilus:0.0"
+tmux split-window -h -t "nautilus:0.2"
+
+tmux attach-session -t "nautilus"
diff --git a/scripts/unset_fastdds.sh b/scripts/unset_fastdds.sh
new file mode 100755
index 0000000..ffff08a
--- /dev/null
+++ b/scripts/unset_fastdds.sh
@@ -0,0 +1,20 @@
+#!/usr/bin/env bash
+# Source this script:
+# source unset_fastdds.sh
+
+if [[ "${BASH_SOURCE[0]}" == "${0}" ]]; then
+ echo "Please source this script instead of executing it:"
+ echo " source ${0}"
+ return 1 2>/dev/null || exit 1
+fi
+
+unset FASTDDS_DEFAULT_PROFILES_FILE
+unset RMW_IMPLEMENTATION
+unset ROS_DOMAIN_ID
+unset ROS_LOCALHOST_ONLY
+
+if command -v ros2 >/dev/null 2>&1; then
+ ros2 daemon stop >/dev/null 2>&1 || true
+fi
+
+echo "Cleared Fast DDS/ROS environment variables from this shell."