From c0c8af5a39d2683a33ec37ad11d2cd513f624caf Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Tue, 10 Mar 2026 22:57:02 +0100 Subject: [PATCH 01/37] refactor: perception_setup launch files and add camera_info_publisher - Created a new launch file `camera_info.launch.py` to publish camera info from a YAML configuration. - Removed the old `perception.launch.py`, `pipeline_detection.launch.py`, `zed_composable_node.launch.py`, and `zed_yolo.launch.py` files to streamline the launch process. - Introduced `yolo_detect.launch.py` for the YOLOv8 TensorRT inference pipeline, including necessary configurations and nodes. - Added a new script `camera_info_publisher.py` to handle the publishing of camera information. - Updated `package.xml` to include new dependencies for the added functionality. - Created a placeholder for `yolo_obb.launch.py` for future development. --- .gitignore | 2 +- perception_setup/CMakeLists.txt | 5 + .../config/aruco_detector_params.yaml | 33 -- .../config/blackfly_s_params.yaml | 155 --------- .../config/d555_color_camera_factory.yaml | 31 ++ .../config/downwards_cam_calib.yaml | 20 -- .../config/downwards_cam_params.yaml | 35 --- .../config/front_camera_calib.yaml | 20 -- .../config/front_camera_calib_downscale.yaml | 20 -- .../config/front_camera_params.yaml | 35 --- .../config/gripper_camera_calib.yaml | 20 -- .../gripper_camera_calib_downscale.yaml | 20 -- .../config/gripper_camera_params.yaml | 35 --- .../config/image_filtering_params.yaml | 24 -- .../config/pipeline_filtering_params.yaml | 16 - .../config/pipeline_line_fitting_params.yaml | 17 - .../config/valve_detection_params.yaml | 6 - perception_setup/config/yolo_detect.yaml | 40 +++ perception_setup/config/yolo_obb.yaml | 0 .../config/zed_driver_params.yaml | 170 ---------- perception_setup/launch/camera_info.launch.py | 36 +++ perception_setup/launch/perception.launch.py | 244 --------------- .../launch/pipeline_detection.launch.py | 122 -------- perception_setup/launch/yolo_detect.launch.py | 225 +++++++++++++ perception_setup/launch/yolo_obb.launch.py | 0 .../launch/zed_composable_node.launch.py | 107 ------- perception_setup/launch/zed_yolo.launch.py | 296 ------------------ perception_setup/package.xml | 7 + .../scripts/camera_info_publisher.py | 67 ++++ 29 files changed, 412 insertions(+), 1396 deletions(-) delete mode 100644 perception_setup/config/aruco_detector_params.yaml delete mode 100644 perception_setup/config/blackfly_s_params.yaml create mode 100644 perception_setup/config/d555_color_camera_factory.yaml delete mode 100644 perception_setup/config/downwards_cam_calib.yaml delete mode 100644 perception_setup/config/downwards_cam_params.yaml delete mode 100644 perception_setup/config/front_camera_calib.yaml delete mode 100644 perception_setup/config/front_camera_calib_downscale.yaml delete mode 100644 perception_setup/config/front_camera_params.yaml delete mode 100644 perception_setup/config/gripper_camera_calib.yaml delete mode 100644 perception_setup/config/gripper_camera_calib_downscale.yaml delete mode 100644 perception_setup/config/gripper_camera_params.yaml delete mode 100644 perception_setup/config/image_filtering_params.yaml delete mode 100644 perception_setup/config/pipeline_filtering_params.yaml delete mode 100644 perception_setup/config/pipeline_line_fitting_params.yaml delete mode 100644 perception_setup/config/valve_detection_params.yaml create mode 100644 perception_setup/config/yolo_detect.yaml create mode 100644 perception_setup/config/yolo_obb.yaml delete mode 100644 perception_setup/config/zed_driver_params.yaml create mode 100644 perception_setup/launch/camera_info.launch.py delete mode 100644 perception_setup/launch/perception.launch.py delete mode 100644 perception_setup/launch/pipeline_detection.launch.py create mode 100644 perception_setup/launch/yolo_detect.launch.py create mode 100644 perception_setup/launch/yolo_obb.launch.py delete mode 100644 perception_setup/launch/zed_composable_node.launch.py delete mode 100644 perception_setup/launch/zed_yolo.launch.py create mode 100755 perception_setup/scripts/camera_info_publisher.py diff --git a/.gitignore b/.gitignore index 584b7b2..808f144 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,5 @@ # VSCode -.vscode/* +.vscode/ !.vscode/settings.json !.vscode/tasks.json !.vscode/launch.json diff --git a/perception_setup/CMakeLists.txt b/perception_setup/CMakeLists.txt index eb12bfb..45eaf5f 100644 --- a/perception_setup/CMakeLists.txt +++ b/perception_setup/CMakeLists.txt @@ -3,6 +3,11 @@ project(perception_setup) find_package(ament_cmake REQUIRED) +install(PROGRAMS + scripts/camera_info_publisher.py + DESTINATION lib/${PROJECT_NAME} +) + install(DIRECTORY launch config 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/blackfly_s_params.yaml b/perception_setup/config/blackfly_s_params.yaml deleted file mode 100644 index 0e75d96..0000000 --- a/perception_setup/config/blackfly_s_params.yaml +++ /dev/null @@ -1,155 +0,0 @@ -# -# config file for blackfly S cameras (USB3 and GigE) -# -# This file maps the ros parameters to the corresponding Spinnaker "nodes" in the camera. -# For more details on how to modify this file, see the README on camera configuration files. -parameters: - # - # -------- image format control - # - - name: pixel_format - type: enum - # Check available values with SpinView. Not all are supported by ROS! - # Some formats are e.g. "Mono8", "BayerRG8", "BGR8", "BayerRG16" - # default is "BayerRG8" - node: ImageFormatControl/PixelFormat - - name: binning_x - type: int - node: ImageFormatControl/BinningHorizontal - - name: binning_y - type: int - node: ImageFormatControl/BinningVertical - - name: image_width - type: int - node: ImageFormatControl/Width - - name: image_height - type: int - node: ImageFormatControl/Height - - name: offset_x # offset must come after image width reduction! - type: int - node: ImageFormatControl/OffsetX - - name: offset_y - type: int - node: ImageFormatControl/OffsetY - # - # -------- analog control - # - - name: gain_auto - type: enum - # valid values are "Continuous", "Off" - node: AnalogControl/GainAuto - - name: gain - type: float - node: AnalogControl/Gain - # - # -------- device link throughput limiting - # - - name: device_link_throughput_limit - type: int - node: DeviceControl/DeviceLinkThroughputLimit - # - # -------- transport layer control (GigE) - # - - name: gev_scps_packet_size - type: int - # default is 1400. Set to 9000 to enable jumbo frames, ensure NIC MTU set >= 9000 - node: TransportLayerControl/GigEVision/GevSCPSPacketSize - # - # -------- digital IO control - # - - name: line0_selector # black wire: opto-isolated input - type: enum - node: DigitalIOControl/LineSelector - - name: line1_selector # white wire: opto-isolated output - type: enum - node: DigitalIOControl/LineSelector - - name: line1_linemode # valid values: "Input", "Output" - type: enum - node: DigitalIOControl/LineMode - - name: line2_selector # red wire: non-isolated input/output - type: enum - node: DigitalIOControl/LineSelector - - name: line2_v33enable # red wire: 3.3V power - type: bool - node: DigitalIOControl/V3_3Enable - - name: line3_selector # green wire: aux voltage input and non-isolated input - type: enum - node: DigitalIOControl/LineSelector - - name: line3_linemode # valid values: "Input", "Output" - type: enum - node: DigitalIOControl/LineMode - # - # -------- acquisition control - # - - name: exposure_auto - type: enum - # valid values are "Off", "Continuous" - node: AcquisitionControl/ExposureAuto - - name: exposure_time - type: float - node: AcquisitionControl/ExposureTime - - name: frame_rate_enable - type: bool - node: AcquisitionControl/AcquisitionFrameRateEnable - - name: frame_rate - type: float - node: AcquisitionControl/AcquisitionFrameRate - - name: trigger_selector - type: enum - # valid values are e.g. "FrameStart", "AcquisitionStart", "FrameBurstStart" - node: AcquisitionControl/TriggerSelector - - name: trigger_mode - type: enum - # valid values are "On" and "Off" - node: AcquisitionControl/TriggerMode - - name: trigger_source - type: enum - # valid values are "Line<0,1,2>", "UserOutput<0,1,2>", "Counter<0,1>", - # "LogicBlock<0,1> - node: AcquisitionControl/TriggerSource - - name: trigger_delay - # value >= 9 - type: float - node: AcquisitionControl/TriggerDelay - - name: trigger_overlap - type: enum - # valid values: "Off" and "ReadOut" - node: AcquisitionControl/TriggerOverlap - - name: trigger_activation - type: enum - # valid values: "LevelLow", "LevelHigh", "FallingEdge", "RisingEdge", "AnyEdge", - node: AcquisitionControl/TriggerActivation - # - # --------- chunk control - # - - name: chunk_mode_active - type: bool - node: ChunkDataControl/ChunkModeActive - - name: chunk_selector_frame_id - type: enum - # valid values: "FrameID" - node: ChunkDataControl/ChunkSelector - - name: chunk_enable_frame_id - type: bool - node: ChunkDataControl/ChunkEnable - - name: chunk_selector_exposure_time - type: enum - # valid values: "ExposureTime" - node: ChunkDataControl/ChunkSelector - - name: chunk_enable_exposure_time - type: bool - node: ChunkDataControl/ChunkEnable - - name: chunk_selector_gain - type: enum - # valid values: "Gain" - node: ChunkDataControl/ChunkSelector - - name: chunk_enable_gain - type: bool - node: ChunkDataControl/ChunkEnable - - name: chunk_selector_timestamp - type: enum - # valid values: "Timestamp" - node: ChunkDataControl/ChunkSelector - - name: chunk_enable_timestamp - type: bool - node: ChunkDataControl/ChunkEnable diff --git a/perception_setup/config/d555_color_camera_factory.yaml b/perception_setup/config/d555_color_camera_factory.yaml new file mode 100644 index 0000000..620fec8 --- /dev/null +++ b/perception_setup/config/d555_color_camera_factory.yaml @@ -0,0 +1,31 @@ +image_width: 848 +image_height: 480 +camera_name: d555_color +camera_info_topic: "/realsense/D555_409122300281_Color/camera_info" +camera_matrix: + rows: 3 + cols: 3 + data: [452.121520996094, 0.0, 438.254058837891, + 0.0, 451.737976074219, 248.703216552734, + 0.0, 0.0, 1.0] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.0548105500638485, + 0.0597584918141365, + 0.000486430013552308, + 0.00031599000794813, + -0.0192314591258764] +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: [452.121520996094, 0.0, 438.254058837891, 0.0, + 0.0, 451.737976074219, 248.703216552734, 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/front_camera_params.yaml b/perception_setup/config/front_camera_params.yaml deleted file mode 100644 index cf8cccf..0000000 --- a/perception_setup/config/front_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/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_detect.yaml b/perception_setup/config/yolo_detect.yaml new file mode 100644 index 0000000..89427e5 --- /dev/null +++ b/perception_setup/config/yolo_detect.yaml @@ -0,0 +1,40 @@ +# Topics +image_input_topic: "/realsense/D555_409122300281_Color" +camera_info_input_topic: "/realsense/D555_409122300281_Color/camera_info" +converted_image_topic: "/yolo/detect/converted_image" +tensor_output_topic: "/tensor_pub" + +# Model and engine file paths +model_file_path: "best.onnx" # We assume the output dimensions are [1, 5, 8400] +engine_file_path: "best.engine" + +# Visualizer and class names for the visualizer overlay. +enable_visualizer: true +class_names: + 0: "valve" + +# TensorRT +verbose: true +force_engine_update: false + +# Tensor I/O bindings +input_tensor_names: ["input_tensor"] +input_binding_names: ["images"] +output_tensor_names: ["output_tensor"] +output_binding_names: ["output0"] + +# Input image +input_image_width: 848 +input_image_height: 480 +encoding_desired: "rgb8" + +# 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_obb.yaml b/perception_setup/config/yolo_obb.yaml new file mode 100644 index 0000000..e69de29 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/launch/camera_info.launch.py b/perception_setup/launch/camera_info.launch.py new file mode 100644 index 0000000..e9c26a5 --- /dev/null +++ b/perception_setup/launch/camera_info.launch.py @@ -0,0 +1,36 @@ +import os +import yaml + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + + pkg_dir = get_package_share_directory("perception_setup") + + config_file = os.path.join( + pkg_dir, + "config", + "d555_color_camera_factory.yaml" + ) + + with open(config_file, "r") as f: + cfg = yaml.safe_load(f) + + camera_info_topic = cfg["camera_info_topic"] + + return LaunchDescription([ + + Node( + package="perception_setup", + executable="camera_info_publisher.py", + name="camera_info_publisher", + parameters=[{ + "camera_info_file": config_file, + "camera_info_topic": camera_info_topic, + }], + output="screen" + ) + ]) \ No newline at end of file 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_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/yolo_detect.launch.py b/perception_setup/launch/yolo_detect.launch.py new file mode 100644 index 0000000..dda1535 --- /dev/null +++ b/perception_setup/launch/yolo_detect.launch.py @@ -0,0 +1,225 @@ +# SPDX-License-Identifier: MIT + +""" +Wrapper launch file for the Isaac ROS YOLOv8 TensorRT inference pipeline. + +All parameters are loaded from a YAML config file. Missing parameters +will cause the launch to fail immediately. + +Pipeline: + + 1. Image format converter (e.g. bgra8 → rgb8) + 2. DNN image encoder (resize + normalise → tensor) + 3. TensorRT inference node + 4. YOLOv8 decoder node (tensor → Detection2DArray) + 5. (optional) Visualizer (overlay bboxes on the resized image) +""" + +import os +import yaml + +from ament_index_python.packages import get_package_share_directory + +import launch +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): + + config_path = LaunchConfiguration("config_file").perform(context) + + with open(config_path, "r") as f: + cfg = yaml.safe_load(f) + + # Validate required config keys + required_keys = [ + "model_file_path", + "engine_file_path", + "input_tensor_names", + "input_binding_names", + "output_tensor_names", + "output_binding_names", + "verbose", + "force_engine_update", + "input_image_width", + "input_image_height", + "encoding_desired", + "network_image_width", + "network_image_height", + "image_mean", + "image_stddev", + "confidence_threshold", + "nms_threshold", + "num_classes", + "image_input_topic", + "camera_info_input_topic", + "converted_image_topic", + "tensor_output_topic", + "enable_visualizer", + "class_names", + ] + + for key in required_keys: + if key not in cfg: + raise RuntimeError(f"Missing required config key: '{key}'") + + # Load configuration + model_file_path = str(cfg["model_file_path"]) + engine_file_path = str(cfg["engine_file_path"]) + + input_tensor_names = cfg["input_tensor_names"] + input_binding_names = cfg["input_binding_names"] + output_tensor_names = cfg["output_tensor_names"] + output_binding_names = cfg["output_binding_names"] + + verbose = bool(cfg["verbose"]) + force_engine_update = bool(cfg["force_engine_update"]) + + input_image_width = int(cfg["input_image_width"]) + input_image_height = int(cfg["input_image_height"]) + encoding_desired = str(cfg["encoding_desired"]) + + network_image_width = int(cfg["network_image_width"]) + network_image_height = int(cfg["network_image_height"]) + image_mean = cfg["image_mean"] + image_stddev = cfg["image_stddev"] + + confidence_threshold = float(cfg["confidence_threshold"]) + nms_threshold = float(cfg["nms_threshold"]) + num_classes = int(cfg["num_classes"]) + + image_input_topic = str(cfg["image_input_topic"]) + camera_info_input_topic = str(cfg["camera_info_input_topic"]) + converted_image_topic = str(cfg["converted_image_topic"]) + tensor_output_topic = str(cfg["tensor_output_topic"]) + + enable_visualizer = bool(cfg["enable_visualizer"]) + class_names = cfg["class_names"] + + # Image format converter + image_format_converter = ComposableNode( + package="isaac_ros_image_proc", + plugin="nvidia::isaac_ros::image_proc::ImageFormatConverterNode", + name="image_format_converter", + parameters=[{ + "encoding_desired": encoding_desired, + "image_width": input_image_width, + "image_height": input_image_height, + "input_qos": "sensor_data", + }], + remappings=[ + ("image_raw", image_input_topic), + ("image", converted_image_topic), + ], + ) + + # TensorRT inference node + 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 + 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": num_classes, + }], + ) + + # Component container + tensor_rt_container = ComposableNodeContainer( + name="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="", + ) + + # DNN image encoder + 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(input_image_width), + "input_image_height": str(input_image_height), + "network_image_width": str(network_image_width), + "network_image_height": str(network_image_height), + "image_mean": str(image_mean), + "image_stddev": str(image_stddev), + "attach_to_shared_component_container": "True", + "component_container_name": "tensor_rt_container", + "dnn_image_encoder_namespace": "yolov8_encoder", + "image_input_topic": converted_image_topic, + "camera_info_input_topic": camera_info_input_topic, + "tensor_output_topic": tensor_output_topic, + }.items(), + ) + + actions = [ + tensor_rt_container, + yolov8_encoder_launch, + ] + + # Optional visualizer + if enable_visualizer: + actions.append( + Node( + package="isaac_ros_yolov8", + executable="isaac_ros_yolov8_visualizer.py", + name="yolov8_visualizer", + parameters=[{ + "class_names_yaml": str(class_names), + }], + ) + ) + + return actions + + +def generate_launch_description(): + + pkg_dir = get_package_share_directory("perception_setup") + default_config = os.path.join(pkg_dir, "config", "yolo_detect.yaml") + + return launch.LaunchDescription([ + DeclareLaunchArgument( + "config_file", + default_value=default_config, + description="Path to YOLO pipeline config YAML", + ), + OpaqueFunction(function=_launch_setup), + ]) \ No newline at end of file diff --git a/perception_setup/launch/yolo_obb.launch.py b/perception_setup/launch/yolo_obb.launch.py new file mode 100644 index 0000000..e69de29 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..87673a8 100644 --- a/perception_setup/package.xml +++ b/perception_setup/package.xml @@ -7,12 +7,19 @@ jorgen MIT + camera_info_manager + ament_cmake ament_lint_auto ament_lint_common ros2launch + launch_ros + isaac_ros_yolov8 + isaac_ros_tensor_rt + isaac_ros_dnn_image_encoder + isaac_ros_image_proc 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..4c85b6c --- /dev/null +++ b/perception_setup/scripts/camera_info_publisher.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import CameraInfo + +from rclpy.qos import QoSProfile +from rclpy.qos import DurabilityPolicy +from rclpy.qos import ReliabilityPolicy +from rclpy.parameter import Parameter + +import yaml + + +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) + + file_path = self.get_parameter("camera_info_file").value + topic_name = self.get_parameter("camera_info_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") + + with open(file_path, "r") 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"] + + qos = QoSProfile(depth=1) + qos.reliability = ReliabilityPolicy.RELIABLE + qos.durability = DurabilityPolicy.TRANSIENT_LOCAL + + self.publisher = self.create_publisher(CameraInfo, topic_name, qos) + + # publish once + self.publisher.publish(msg) + + self.get_logger().info(f"Published CameraInfo on {topic_name}") + self.get_logger().info(f"Loaded calibration: {file_path}") + + +def main(): + rclpy.init() + node = CameraInfoPublisher() + rclpy.spin(node) + + +if __name__ == "__main__": + main() \ No newline at end of file From 357b66401b118743b7821c57b28c215ab95bb0d0 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Fri, 13 Mar 2026 22:46:52 +0100 Subject: [PATCH 02/37] refactor: update YOLOv8 launch and config files for improved topic management and clarity --- perception_setup/config/yolo_detect.yaml | 20 +- perception_setup/launch/yolo_detect.launch.py | 271 ++++++++++-------- 2 files changed, 154 insertions(+), 137 deletions(-) diff --git a/perception_setup/config/yolo_detect.yaml b/perception_setup/config/yolo_detect.yaml index 89427e5..3a68d94 100644 --- a/perception_setup/config/yolo_detect.yaml +++ b/perception_setup/config/yolo_detect.yaml @@ -1,11 +1,11 @@ # Topics -image_input_topic: "/realsense/D555_409122300281_Color" -camera_info_input_topic: "/realsense/D555_409122300281_Color/camera_info" -converted_image_topic: "/yolo/detect/converted_image" -tensor_output_topic: "/tensor_pub" +image_input_topic: "/zed_node/left/image_rect_color" +camera_info_input_topic: "/zed_node/depth/camera_info" +visualized_image_topic: "/yolov8_processed_image" # Output topic for visualized images with detection overlays +detection_topic: "/detections_output" # Output topic for detection results (Detection2DArray) # Model and engine file paths -model_file_path: "best.onnx" # We assume the output dimensions are [1, 5, 8400] +model_file_path: "best.onnx" # The output dimensions must be [1, 5, 8400] (see yolov8_encoder_node.cpp for details) engine_file_path: "best.engine" # Visualizer and class names for the visualizer overlay. @@ -13,6 +13,11 @@ enable_visualizer: true class_names: 0: "valve" +# Input image +input_image_width: 1280 +input_image_height: 720 +encoding_desired: "rgb8" + # TensorRT verbose: true force_engine_update: false @@ -23,11 +28,6 @@ input_binding_names: ["images"] output_tensor_names: ["output_tensor"] output_binding_names: ["output0"] -# Input image -input_image_width: 848 -input_image_height: 480 -encoding_desired: "rgb8" - # Network network_image_width: 640 network_image_height: 640 diff --git a/perception_setup/launch/yolo_detect.launch.py b/perception_setup/launch/yolo_detect.launch.py index dda1535..83f9695 100644 --- a/perception_setup/launch/yolo_detect.launch.py +++ b/perception_setup/launch/yolo_detect.launch.py @@ -1,18 +1,32 @@ # SPDX-License-Identifier: MIT """ -Wrapper launch file for the Isaac ROS YOLOv8 TensorRT inference pipeline. - -All parameters are loaded from a YAML config file. Missing parameters -will cause the launch to fail immediately. - -Pipeline: - - 1. Image format converter (e.g. bgra8 → rgb8) - 2. DNN image encoder (resize + normalise → tensor) - 3. TensorRT inference node - 4. YOLOv8 decoder node (tensor → Detection2DArray) - 5. (optional) Visualizer (overlay bboxes on the resized image) +Isaac ROS YOLOv8 TensorRT inference pipeline + +1. ImageFormatConverterNode + Input: image_input_topic + Output: /yolov8/internal/converted_image + Purpose: Convert camera image to desired encoding (e.g. bgra8 -> rgb8) + +2. DNNImageEncoderNode + Input: /yolov8/internal/converted_image + Output: /tensor_pub + Purpose: Resize + normalize image and convert to network tensor + +3. TensorRTNode + Input: /tensor_pub + Output: /tensor_sub + Purpose: Run TensorRT inference on encoded tensor + +4. YoloV8DecoderNode + Input: /tensor_sub + Output: /yolov8/internal/detection_topic + Purpose: Convert network output tensor -> Detection2DArray + +5. (Optional) YOLOv8Visualizer + Input: /yolov8/internal/detection_topic + /yolov8/internal/converted_image + Output: overlay visualization + Purpose: Draw bounding boxes on image """ import os @@ -28,164 +42,165 @@ from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode +# Internal pipeline topics +CONVERTED_IMAGE_TOPIC = '/yolov8/internal/converted_image' +ENCODER_RESIZE_TOPIC = '/yolov8_encoder/internal/resize/image' +TENSOR_OUTPUT_TOPIC = '/tensor_pub' +TENSOR_INPUT_TOPIC = '/tensor_sub' def _launch_setup(context, *args, **kwargs): - config_path = LaunchConfiguration("config_file").perform(context) + config_path = LaunchConfiguration('config_file').perform(context) - with open(config_path, "r") as f: + with open(config_path, 'r') as f: cfg = yaml.safe_load(f) - # Validate required config keys required_keys = [ - "model_file_path", - "engine_file_path", - "input_tensor_names", - "input_binding_names", - "output_tensor_names", - "output_binding_names", - "verbose", - "force_engine_update", - "input_image_width", - "input_image_height", - "encoding_desired", - "network_image_width", - "network_image_height", - "image_mean", - "image_stddev", - "confidence_threshold", - "nms_threshold", - "num_classes", - "image_input_topic", - "camera_info_input_topic", - "converted_image_topic", - "tensor_output_topic", - "enable_visualizer", - "class_names", + 'model_file_path', + 'engine_file_path', + 'input_tensor_names', + 'input_binding_names', + 'output_tensor_names', + 'output_binding_names', + 'verbose', + 'force_engine_update', + 'input_image_width', + 'input_image_height', + 'encoding_desired', + 'network_image_width', + 'network_image_height', + 'image_mean', + 'image_stddev', + 'confidence_threshold', + 'nms_threshold', + 'num_classes', + 'detection_topic', + 'image_input_topic', + 'camera_info_input_topic', + 'enable_visualizer', + 'visualized_image_topic', + 'class_names', ] for key in required_keys: if key not in cfg: - raise RuntimeError(f"Missing required config key: '{key}'") + raise RuntimeError(f'Missing required config key: \'{key}\'') - # Load configuration - model_file_path = str(cfg["model_file_path"]) - engine_file_path = str(cfg["engine_file_path"]) + model_file_path = str(cfg['model_file_path']) + engine_file_path = str(cfg['engine_file_path']) - input_tensor_names = cfg["input_tensor_names"] - input_binding_names = cfg["input_binding_names"] - output_tensor_names = cfg["output_tensor_names"] - output_binding_names = cfg["output_binding_names"] + input_tensor_names = cfg['input_tensor_names'] + input_binding_names = cfg['input_binding_names'] + output_tensor_names = cfg['output_tensor_names'] + output_binding_names = cfg['output_binding_names'] - verbose = bool(cfg["verbose"]) - force_engine_update = bool(cfg["force_engine_update"]) + verbose = bool(cfg['verbose']) + force_engine_update = bool(cfg['force_engine_update']) - input_image_width = int(cfg["input_image_width"]) - input_image_height = int(cfg["input_image_height"]) - encoding_desired = str(cfg["encoding_desired"]) + input_image_width = int(cfg['input_image_width']) + input_image_height = int(cfg['input_image_height']) + encoding_desired = str(cfg['encoding_desired']) - network_image_width = int(cfg["network_image_width"]) - network_image_height = int(cfg["network_image_height"]) - image_mean = cfg["image_mean"] - image_stddev = cfg["image_stddev"] + network_image_width = int(cfg['network_image_width']) + network_image_height = int(cfg['network_image_height']) + image_mean = cfg['image_mean'] + image_stddev = cfg['image_stddev'] - confidence_threshold = float(cfg["confidence_threshold"]) - nms_threshold = float(cfg["nms_threshold"]) - num_classes = int(cfg["num_classes"]) + confidence_threshold = float(cfg['confidence_threshold']) + nms_threshold = float(cfg['nms_threshold']) + num_classes = int(cfg['num_classes']) + detection_topic = str(cfg['detection_topic']) - image_input_topic = str(cfg["image_input_topic"]) - camera_info_input_topic = str(cfg["camera_info_input_topic"]) - converted_image_topic = str(cfg["converted_image_topic"]) - tensor_output_topic = str(cfg["tensor_output_topic"]) + image_input_topic = str(cfg['image_input_topic']) + camera_info_input_topic = str(cfg['camera_info_input_topic']) - enable_visualizer = bool(cfg["enable_visualizer"]) - class_names = cfg["class_names"] + enable_visualizer = bool(cfg['enable_visualizer']) + visualized_image_topic = str(cfg['visualized_image_topic']) + class_names = cfg['class_names'] - # Image format converter image_format_converter = ComposableNode( - package="isaac_ros_image_proc", - plugin="nvidia::isaac_ros::image_proc::ImageFormatConverterNode", - name="image_format_converter", + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_converter', parameters=[{ - "encoding_desired": encoding_desired, - "image_width": input_image_width, - "image_height": input_image_height, - "input_qos": "sensor_data", + 'encoding_desired': encoding_desired, + 'image_width': input_image_width, + 'image_height': input_image_height, + 'input_qos': 'sensor_data', }], remappings=[ - ("image_raw", image_input_topic), - ("image", converted_image_topic), + ('image_raw', image_input_topic), + ('image', CONVERTED_IMAGE_TOPIC), ], ) - # TensorRT inference node tensor_rt_node = ComposableNode( - name="tensor_rt", - package="isaac_ros_tensor_rt", - plugin="nvidia::isaac_ros::dnn_inference::TensorRTNode", + 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, + '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, + 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, }], ) - # YOLOv8 decoder yolov8_decoder_node = ComposableNode( - name="yolov8_decoder_node", - package="isaac_ros_yolov8", - plugin="nvidia::isaac_ros::yolov8::YoloV8DecoderNode", + 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": num_classes, + 'tensor_input_topic': TENSOR_INPUT_TOPIC, + 'confidence_threshold': confidence_threshold, + 'nms_threshold': nms_threshold, + 'num_classes': num_classes, + 'detections_topic': detection_topic, }], ) - # Component container tensor_rt_container = ComposableNodeContainer( - name="tensor_rt_container", - package="rclcpp_components", - executable="component_container_mt", + name='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="", + output='screen', + arguments=['--ros-args', '--log-level', 'INFO'], + namespace='', ) - # DNN image encoder - encoder_dir = get_package_share_directory("isaac_ros_dnn_image_encoder") + 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', + 'dnn_image_encoder.launch.py', ) ), launch_arguments={ - "input_image_width": str(input_image_width), - "input_image_height": str(input_image_height), - "network_image_width": str(network_image_width), - "network_image_height": str(network_image_height), - "image_mean": str(image_mean), - "image_stddev": str(image_stddev), - "attach_to_shared_component_container": "True", - "component_container_name": "tensor_rt_container", - "dnn_image_encoder_namespace": "yolov8_encoder", - "image_input_topic": converted_image_topic, - "camera_info_input_topic": camera_info_input_topic, - "tensor_output_topic": tensor_output_topic, + 'input_image_width': str(input_image_width), + 'input_image_height': str(input_image_height), + 'network_image_width': str(network_image_width), + 'network_image_height': str(network_image_height), + 'image_mean': str(image_mean), + 'image_stddev': str(image_stddev), + 'attach_to_shared_component_container': 'True', + 'component_container_name': 'tensor_rt_container', + 'dnn_image_encoder_namespace': 'yolov8_encoder/internal', + 'image_input_topic': CONVERTED_IMAGE_TOPIC, + 'camera_info_input_topic': camera_info_input_topic, + 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, }.items(), ) @@ -194,15 +209,17 @@ def _launch_setup(context, *args, **kwargs): yolov8_encoder_launch, ] - # Optional visualizer if enable_visualizer: actions.append( Node( - package="isaac_ros_yolov8", - executable="isaac_ros_yolov8_visualizer.py", - name="yolov8_visualizer", + package='isaac_ros_yolov8', + executable='isaac_ros_yolov8_visualizer.py', + name='yolov8_visualizer', parameters=[{ - "class_names_yaml": str(class_names), + 'detections_topic': detection_topic, + 'image_topic': ENCODER_RESIZE_TOPIC, + 'output_image_topic': visualized_image_topic, + 'class_names_yaml': str(class_names), }], ) ) @@ -212,14 +229,14 @@ def _launch_setup(context, *args, **kwargs): def generate_launch_description(): - pkg_dir = get_package_share_directory("perception_setup") - default_config = os.path.join(pkg_dir, "config", "yolo_detect.yaml") + pkg_dir = get_package_share_directory('perception_setup') + default_config = os.path.join(pkg_dir, 'config', 'yolo_detect.yaml') return launch.LaunchDescription([ DeclareLaunchArgument( - "config_file", + 'config_file', default_value=default_config, - description="Path to YOLO pipeline config YAML", + description='Path to YOLO pipeline config YAML', ), OpaqueFunction(function=_launch_setup), - ]) \ No newline at end of file + ]) From 108bffb40de9575a8e309d1d35ffe2997a6db854 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Sat, 14 Mar 2026 12:39:43 +0100 Subject: [PATCH 03/37] feat: implement YOLO-OBB TensorRT inference pipeline and update dependencies --- perception_setup/config/yolo_obb.yaml | 44 ++++ perception_setup/launch/yolo_obb.launch.py | 237 +++++++++++++++++++++ perception_setup/package.xml | 1 + 3 files changed, 282 insertions(+) diff --git a/perception_setup/config/yolo_obb.yaml b/perception_setup/config/yolo_obb.yaml index e69de29..3ceeaae 100644 --- a/perception_setup/config/yolo_obb.yaml +++ b/perception_setup/config/yolo_obb.yaml @@ -0,0 +1,44 @@ +# SPDX-License-Identifier: MIT + +# Topics +image_input_topic: "/zed_node/left/image_rect_color" +camera_info_input_topic: "/zed_node/depth/camera_info" +visualized_image_topic: "/yolo_obb_processed_image" +detection_topic: "/obb_detections_output" + +# Model and engine file paths (relative to perception_setup/models/) +model_file_path: "obb_best.onnx" +engine_file_path: "obb_best.engine" + +# Visualizer +enable_visualizer: true +class_names: + 0: "valve" + +# Input image dimensions +input_image_width: 1280 +input_image_height: 720 +encoding_desired: "rgb8" + +# TensorRT +verbose: true +force_engine_update: false + +# 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/launch/yolo_obb.launch.py b/perception_setup/launch/yolo_obb.launch.py index e69de29..fd65e87 100644 --- a/perception_setup/launch/yolo_obb.launch.py +++ b/perception_setup/launch/yolo_obb.launch.py @@ -0,0 +1,237 @@ +# SPDX-License-Identifier: MIT + +""" +Isaac ROS YOLO-OBB TensorRT inference pipeline + +1. ImageFormatConverterNode + Input: image_input_topic + Output: /yolo_obb/internal/converted_image + Purpose: Convert camera image encoding (e.g. bgra8 -> rgb8) + +2. DNNImageEncoderNode + Input: /yolo_obb/internal/converted_image + Output: /tensor_pub + Purpose: Resize + normalize image and convert to network tensor + +3. TensorRTNode + Input: /tensor_pub + Output: /tensor_sub + Purpose: Run TensorRT inference + Expected output tensor shape: [1, num_detections, 7] + +4. YoloV26OBBDecoderNode + Input: /tensor_sub + Output: detection_topic (Detection2DArray with BoundingBox2D.center.theta = OBB angle) + Purpose: Filter by confidence and publish OBB detections + Note: NMS is embedded in the model - no NMS is applied here. +""" + +import os +import yaml + +from ament_index_python.packages import get_package_share_directory + +import launch +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 + +CONVERTED_IMAGE_TOPIC = '/yolo_obb/internal/converted_image' +ENCODER_RESIZE_TOPIC = '/yolo_obb_encoder/internal/resize/image' +TENSOR_OUTPUT_TOPIC = '/tensor_pub' +TENSOR_INPUT_TOPIC = '/tensor_sub' + + +def _launch_setup(context, *args, **kwargs): + + config_path = LaunchConfiguration('config_file').perform(context) + + with open(config_path, 'r') as f: + cfg = yaml.safe_load(f) + + required_keys = [ + 'model_file_path', + 'engine_file_path', + 'input_tensor_names', + 'input_binding_names', + 'output_tensor_names', + 'output_binding_names', + 'verbose', + 'force_engine_update', + 'input_image_width', + 'input_image_height', + 'encoding_desired', + 'network_image_width', + 'network_image_height', + 'image_mean', + 'image_stddev', + 'confidence_threshold', + 'num_detections', + 'detection_topic', + 'image_input_topic', + 'camera_info_input_topic', + 'enable_visualizer', + 'visualized_image_topic', + 'class_names', + ] + + for key in required_keys: + if key not in cfg: + raise RuntimeError(f"Missing required config key: '{key}'") + + # Resolve model paths relative to perception_setup/models/ + pkg_dir = get_package_share_directory('perception_setup') + models_dir = os.path.join(pkg_dir, 'models') + + model_file_path = os.path.join(models_dir, str(cfg['model_file_path'])) + engine_file_path = os.path.join(models_dir, str(cfg['engine_file_path'])) + + input_tensor_names = cfg['input_tensor_names'] + input_binding_names = cfg['input_binding_names'] + output_tensor_names = cfg['output_tensor_names'] + output_binding_names = cfg['output_binding_names'] + + verbose = bool(cfg['verbose']) + force_engine_update = bool(cfg['force_engine_update']) + + input_image_width = int(cfg['input_image_width']) + input_image_height = int(cfg['input_image_height']) + encoding_desired = str(cfg['encoding_desired']) + + network_image_width = int(cfg['network_image_width']) + network_image_height = int(cfg['network_image_height']) + image_mean = cfg['image_mean'] + image_stddev = cfg['image_stddev'] + + confidence_threshold = float(cfg['confidence_threshold']) + num_detections = int(cfg['num_detections']) + detection_topic = str(cfg['detection_topic']) + + image_input_topic = str(cfg['image_input_topic']) + camera_info_input_topic = str(cfg['camera_info_input_topic']) + + enable_visualizer = bool(cfg['enable_visualizer']) + visualized_image_topic = str(cfg['visualized_image_topic']) + class_names = cfg['class_names'] + + image_format_converter = ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_converter', + parameters=[{ + 'encoding_desired': encoding_desired, + 'image_width': input_image_width, + 'image_height': input_image_height, + 'input_qos': 'sensor_data', + }], + remappings=[ + ('image_raw', image_input_topic), + ('image', CONVERTED_IMAGE_TOPIC), + ], + ) + + 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, + 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + }], + ) + + 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': TENSOR_INPUT_TOPIC, + 'confidence_threshold': confidence_threshold, + 'num_detections': num_detections, + 'detections_topic': detection_topic, + }], + ) + + tensor_rt_container = ComposableNodeContainer( + name='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(input_image_width), + 'input_image_height': str(input_image_height), + 'network_image_width': str(network_image_width), + 'network_image_height': str(network_image_height), + 'image_mean': str(image_mean), + 'image_stddev': str(image_stddev), + 'attach_to_shared_component_container': 'True', + 'component_container_name': 'tensor_rt_container', + 'dnn_image_encoder_namespace': 'yolo_obb_encoder/internal', + 'image_input_topic': CONVERTED_IMAGE_TOPIC, + 'camera_info_input_topic': camera_info_input_topic, + 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + }.items(), + ) + + actions = [tensor_rt_container, yolo_obb_encoder_launch] + + if enable_visualizer: + actions.append( + Node( + package='isaac_ros_yolov26_obb', + executable='isaac_ros_yolov26_obb_visualizer.py', + name='yolo_obb_visualizer', + parameters=[{ + 'detections_topic': detection_topic, + 'image_topic': ENCODER_RESIZE_TOPIC, + 'output_image_topic': visualized_image_topic, + 'class_names_yaml': str(class_names), + }], + ) + ) + + return actions + + +def generate_launch_description(): + + pkg_dir = get_package_share_directory('perception_setup') + default_config = os.path.join(pkg_dir, 'config', 'yolo_obb.yaml') + + return launch.LaunchDescription([ + DeclareLaunchArgument( + 'config_file', + default_value=default_config, + description='Path to YOLO OBB pipeline config YAML', + ), + OpaqueFunction(function=_launch_setup), + ]) diff --git a/perception_setup/package.xml b/perception_setup/package.xml index 87673a8..abcbc7a 100644 --- a/perception_setup/package.xml +++ b/perception_setup/package.xml @@ -17,6 +17,7 @@ ros2launch launch_ros isaac_ros_yolov8 + isaac_ros_yolov26_obb isaac_ros_tensor_rt isaac_ros_dnn_image_encoder isaac_ros_image_proc From a70a1ae6c4789d6c0a54bcc79bb130e198515f47 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Wed, 18 Mar 2026 00:18:22 +0100 Subject: [PATCH 04/37] feat: add YOLO segmentation pipeline configuration and launch files --- perception_setup/config/yolo_detect.yaml | 2 +- perception_setup/config/yolo_obb.yaml | 2 - perception_setup/config/yolo_seg.yaml | 53 ++++ perception_setup/launch/yolo_detect.launch.py | 3 +- perception_setup/launch/yolo_obb.launch.py | 3 +- perception_setup/launch/yolo_seg.launch.py | 266 ++++++++++++++++++ perception_setup/package.xml | 1 + 7 files changed, 325 insertions(+), 5 deletions(-) create mode 100644 perception_setup/config/yolo_seg.yaml create mode 100644 perception_setup/launch/yolo_seg.launch.py diff --git a/perception_setup/config/yolo_detect.yaml b/perception_setup/config/yolo_detect.yaml index 3a68d94..15497f0 100644 --- a/perception_setup/config/yolo_detect.yaml +++ b/perception_setup/config/yolo_detect.yaml @@ -2,7 +2,7 @@ image_input_topic: "/zed_node/left/image_rect_color" camera_info_input_topic: "/zed_node/depth/camera_info" visualized_image_topic: "/yolov8_processed_image" # Output topic for visualized images with detection overlays -detection_topic: "/detections_output" # Output topic for detection results (Detection2DArray) +detection_topic: "/yolov8_detections_output" # Output topic for detection results (Detection2DArray) # Model and engine file paths model_file_path: "best.onnx" # The output dimensions must be [1, 5, 8400] (see yolov8_encoder_node.cpp for details) diff --git a/perception_setup/config/yolo_obb.yaml b/perception_setup/config/yolo_obb.yaml index 3ceeaae..d8a6568 100644 --- a/perception_setup/config/yolo_obb.yaml +++ b/perception_setup/config/yolo_obb.yaml @@ -1,5 +1,3 @@ -# SPDX-License-Identifier: MIT - # Topics image_input_topic: "/zed_node/left/image_rect_color" camera_info_input_topic: "/zed_node/depth/camera_info" diff --git a/perception_setup/config/yolo_seg.yaml b/perception_setup/config/yolo_seg.yaml new file mode 100644 index 0000000..c99249d --- /dev/null +++ b/perception_setup/config/yolo_seg.yaml @@ -0,0 +1,53 @@ +# Topics +image_input_topic: "/cam_down/image_color" +camera_info_input_topic: "/cam_down/camera_info" +visualized_image_topic: "/yolo_seg_processed_image" +detection_topic: "/seg_detections_output" +mask_topic: "/yolo_seg_mask" + +# Model and engine file paths (relative to perception_setup/models/) +model_file_path: "down_cam_seg.onnx" +engine_file_path: "down_cam_seg.engine" + +# Visualizer +enable_visualizer: true +class_names: + 0: "pipeline" + +# Input image dimensions +input_image_width: 1440 +input_image_height: 1080 +encoding_desired: "rgb8" + +# Output mask resolution (upscaled from 160x160 via bilinear interpolation) +output_mask_width: 1440 +output_mask_height: 1080 + +# TensorRT +verbose: true +force_engine_update: false + +# 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.aaaaaaa +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/launch/yolo_detect.launch.py b/perception_setup/launch/yolo_detect.launch.py index 83f9695..04c9767 100644 --- a/perception_setup/launch/yolo_detect.launch.py +++ b/perception_setup/launch/yolo_detect.launch.py @@ -47,6 +47,7 @@ ENCODER_RESIZE_TOPIC = '/yolov8_encoder/internal/resize/image' TENSOR_OUTPUT_TOPIC = '/tensor_pub' TENSOR_INPUT_TOPIC = '/tensor_sub' +DNN_IMAGE_ENCODER_NAMESPACE = 'yolov8_encoder/internal' def _launch_setup(context, *args, **kwargs): @@ -197,7 +198,7 @@ def _launch_setup(context, *args, **kwargs): 'image_stddev': str(image_stddev), 'attach_to_shared_component_container': 'True', 'component_container_name': 'tensor_rt_container', - 'dnn_image_encoder_namespace': 'yolov8_encoder/internal', + 'dnn_image_encoder_namespace': DNN_IMAGE_ENCODER_NAMESPACE, 'image_input_topic': CONVERTED_IMAGE_TOPIC, 'camera_info_input_topic': camera_info_input_topic, 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, diff --git a/perception_setup/launch/yolo_obb.launch.py b/perception_setup/launch/yolo_obb.launch.py index fd65e87..10fc6da 100644 --- a/perception_setup/launch/yolo_obb.launch.py +++ b/perception_setup/launch/yolo_obb.launch.py @@ -43,6 +43,7 @@ ENCODER_RESIZE_TOPIC = '/yolo_obb_encoder/internal/resize/image' TENSOR_OUTPUT_TOPIC = '/tensor_pub' TENSOR_INPUT_TOPIC = '/tensor_sub' +DNN_IMAGE_ENCODER_NAMESPACE = 'yolo_obb_encoder/internal' def _launch_setup(context, *args, **kwargs): @@ -195,7 +196,7 @@ def _launch_setup(context, *args, **kwargs): 'image_stddev': str(image_stddev), 'attach_to_shared_component_container': 'True', 'component_container_name': 'tensor_rt_container', - 'dnn_image_encoder_namespace': 'yolo_obb_encoder/internal', + 'dnn_image_encoder_namespace': DNN_IMAGE_ENCODER_NAMESPACE, 'image_input_topic': CONVERTED_IMAGE_TOPIC, 'camera_info_input_topic': camera_info_input_topic, 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, diff --git a/perception_setup/launch/yolo_seg.launch.py b/perception_setup/launch/yolo_seg.launch.py new file mode 100644 index 0000000..e45b4a6 --- /dev/null +++ b/perception_setup/launch/yolo_seg.launch.py @@ -0,0 +1,266 @@ +# SPDX-License-Identifier: MIT + +""" +Isaac ROS YOLO Instance-Segmentation TensorRT inference pipeline + +1. ImageFormatConverterNode + Input: image_input_topic + Output: /yolo_seg/internal/converted_image + Purpose: Convert camera image encoding (e.g. bgra8 -> rgb8) + +2. DNNImageEncoderNode + Input: /yolo_seg/internal/converted_image + Output: /tensor_pub + Purpose: Resize + normalize image and convert to network tensor + +3. TensorRTNode + Input: /tensor_pub + Output: /tensor_sub + Purpose: Run TensorRT inference + Expected output tensors: + output0 [1, 300, 38] detections with mask coefficients + output1 [1, 32, 160, 160] prototype masks + +4. YoloV26SegDecoderNode + Input: /tensor_sub (NitrosTensorList with both output tensors) + Output: detection_topic (Detection2DArray) + mask_topic (mono8 Image - combined binary mask) + Purpose: Decode detections and compute instance segmentation mask + +5. (Optional) YoloSegVisualizer + Input: detection_topic + encoder resize image + mask_topic + Output: overlay visualization + Purpose: Draw mask overlay and bounding boxes on image +""" + +import os +import yaml + +from ament_index_python.packages import get_package_share_directory + +import launch +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 + +CONVERTED_IMAGE_TOPIC = '/yolo_seg/internal/converted_image' +ENCODER_RESIZE_TOPIC = '/yolo_seg_encoder/internal/resize/image' +TENSOR_OUTPUT_TOPIC = '/tensor_pub' +TENSOR_INPUT_TOPIC = '/tensor_sub' +DNN_IMAGE_ENCODER_NAMESPACE = 'yolo_seg_encoder/internal' + + +def _launch_setup(context, *args, **kwargs): + + config_path = LaunchConfiguration('config_file').perform(context) + + with open(config_path, 'r') as f: + cfg = yaml.safe_load(f) + + required_keys = [ + 'model_file_path', + 'engine_file_path', + 'input_tensor_names', + 'input_binding_names', + 'output_tensor_names', + 'output_binding_names', + 'verbose', + 'force_engine_update', + 'input_image_width', + 'input_image_height', + 'encoding_desired', + 'network_image_width', + 'network_image_height', + 'image_mean', + 'image_stddev', + 'confidence_threshold', + 'num_detections', + 'mask_width', + 'mask_height', + 'num_protos', + 'output_mask_width', + 'output_mask_height', + 'detection_topic', + 'mask_topic', + 'image_input_topic', + 'camera_info_input_topic', + 'enable_visualizer', + 'visualized_image_topic', + 'class_names', + ] + + for key in required_keys: + if key not in cfg: + raise RuntimeError(f"Missing required config key: '{key}'") + + # Resolve model paths relative to perception_setup/models/ + pkg_dir = get_package_share_directory('perception_setup') + models_dir = os.path.join(pkg_dir, 'models') + + model_file_path = os.path.join(models_dir, str(cfg['model_file_path'])) + engine_file_path = os.path.join(models_dir, str(cfg['engine_file_path'])) + + input_tensor_names = cfg['input_tensor_names'] + input_binding_names = cfg['input_binding_names'] + output_tensor_names = cfg['output_tensor_names'] + output_binding_names = cfg['output_binding_names'] + + verbose = bool(cfg['verbose']) + force_engine_update = bool(cfg['force_engine_update']) + + input_image_width = int(cfg['input_image_width']) + input_image_height = int(cfg['input_image_height']) + encoding_desired = str(cfg['encoding_desired']) + + network_image_width = int(cfg['network_image_width']) + network_image_height = int(cfg['network_image_height']) + image_mean = cfg['image_mean'] + image_stddev = cfg['image_stddev'] + + 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']) + output_mask_width = int(cfg['output_mask_width']) + output_mask_height = int(cfg['output_mask_height']) + detection_topic = str(cfg['detection_topic']) + mask_topic = str(cfg['mask_topic']) + + image_input_topic = str(cfg['image_input_topic']) + camera_info_input_topic = str(cfg['camera_info_input_topic']) + + enable_visualizer = bool(cfg['enable_visualizer']) + visualized_image_topic = str(cfg['visualized_image_topic']) + class_names = cfg['class_names'] + + image_format_converter = ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_converter', + parameters=[{ + 'encoding_desired': encoding_desired, + 'image_width': input_image_width, + 'image_height': input_image_height, + 'input_qos': 'sensor_data', + }], + remappings=[ + ('image_raw', image_input_topic), + ('image', CONVERTED_IMAGE_TOPIC), + ], + ) + + 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, + 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + }], + ) + + 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': TENSOR_INPUT_TOPIC, + 'confidence_threshold': confidence_threshold, + 'num_detections': num_detections, + 'mask_width': mask_width, + 'mask_height': mask_height, + 'num_protos': num_protos, + 'network_image_width': network_image_width, + 'network_image_height': network_image_height, + 'output_mask_width': output_mask_width, + 'output_mask_height': output_mask_height, + 'detections_topic': detection_topic, + 'mask_topic': mask_topic, + }], + ) + + tensor_rt_container = ComposableNodeContainer( + name='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(input_image_width), + 'input_image_height': str(input_image_height), + 'network_image_width': str(network_image_width), + 'network_image_height': str(network_image_height), + 'image_mean': str(image_mean), + 'image_stddev': str(image_stddev), + 'attach_to_shared_component_container': 'True', + 'component_container_name': 'tensor_rt_container', + 'dnn_image_encoder_namespace': DNN_IMAGE_ENCODER_NAMESPACE, + 'image_input_topic': CONVERTED_IMAGE_TOPIC, + 'camera_info_input_topic': camera_info_input_topic, + 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + }.items(), + ) + + actions = [tensor_rt_container, yolo_seg_encoder_launch] + + if enable_visualizer: + actions.append( + Node( + package='isaac_ros_yolov26_seg', + executable='isaac_ros_yolov26_seg_visualizer.py', + name='yolo_seg_visualizer', + parameters=[{ + 'detections_topic': detection_topic, + 'image_topic': ENCODER_RESIZE_TOPIC, + 'mask_topic': mask_topic, + 'output_image_topic': visualized_image_topic, + 'class_names_yaml': str(class_names), + }], + ) + ) + + return actions + + +def generate_launch_description(): + + pkg_dir = get_package_share_directory('perception_setup') + default_config = os.path.join(pkg_dir, 'config', 'yolo_seg.yaml') + + return launch.LaunchDescription([ + DeclareLaunchArgument( + 'config_file', + default_value=default_config, + description='Path to YOLO segmentation pipeline config YAML', + ), + OpaqueFunction(function=_launch_setup), + ]) diff --git a/perception_setup/package.xml b/perception_setup/package.xml index abcbc7a..d572233 100644 --- a/perception_setup/package.xml +++ b/perception_setup/package.xml @@ -18,6 +18,7 @@ launch_ros isaac_ros_yolov8 isaac_ros_yolov26_obb + isaac_ros_yolov26_seg isaac_ros_tensor_rt isaac_ros_dnn_image_encoder isaac_ros_image_proc From bdbb6911b135988db74a1cb381efd1642984591b Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Wed, 18 Mar 2026 00:19:57 +0100 Subject: [PATCH 05/37] docs: remove extraneous characters from segmentation decoder comment in yolo_seg.yaml --- perception_setup/config/yolo_seg.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception_setup/config/yolo_seg.yaml b/perception_setup/config/yolo_seg.yaml index c99249d..0e55e86 100644 --- a/perception_setup/config/yolo_seg.yaml +++ b/perception_setup/config/yolo_seg.yaml @@ -43,7 +43,7 @@ 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.aaaaaaa +# 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 From 8570166165afe1e966fb920032fe19d8cdef639b Mon Sep 17 00:00:00 2001 From: vortexuser Date: Fri, 20 Mar 2026 18:04:26 +0100 Subject: [PATCH 06/37] feat: update tensor topic remappings in YOLO launch files for consistency --- perception_setup/launch/yolo_detect.launch.py | 114 ++++++++++-------- perception_setup/launch/yolo_obb.launch.py | 107 ++++++++-------- 2 files changed, 124 insertions(+), 97 deletions(-) diff --git a/perception_setup/launch/yolo_detect.launch.py b/perception_setup/launch/yolo_detect.launch.py index 04c9767..4c9ed9e 100644 --- a/perception_setup/launch/yolo_detect.launch.py +++ b/perception_setup/launch/yolo_detect.launch.py @@ -1,7 +1,6 @@ # SPDX-License-Identifier: MIT -""" -Isaac ROS YOLOv8 TensorRT inference pipeline +"""Isaac ROS YOLOv8 TensorRT inference pipeline 1. ImageFormatConverterNode Input: image_input_topic @@ -30,30 +29,32 @@ """ import os -import yaml - -from ament_index_python.packages import get_package_share_directory import launch -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +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 # Internal pipeline topics CONVERTED_IMAGE_TOPIC = '/yolov8/internal/converted_image' ENCODER_RESIZE_TOPIC = '/yolov8_encoder/internal/resize/image' -TENSOR_OUTPUT_TOPIC = '/tensor_pub' -TENSOR_INPUT_TOPIC = '/tensor_sub' +TENSOR_OUTPUT_TOPIC = '/yolov8/tensor_pub' +TENSOR_INPUT_TOPIC = '/yolov8/tensor_sub' DNN_IMAGE_ENCODER_NAMESPACE = 'yolov8_encoder/internal' -def _launch_setup(context, *args, **kwargs): +def _launch_setup(context, *args, **kwargs): config_path = LaunchConfiguration('config_file').perform(context) - with open(config_path, 'r') as f: + with open(config_path) as f: cfg = yaml.safe_load(f) required_keys = [ @@ -123,12 +124,14 @@ def _launch_setup(context, *args, **kwargs): package='isaac_ros_image_proc', plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', name='image_format_converter', - parameters=[{ - 'encoding_desired': encoding_desired, - 'image_width': input_image_width, - 'image_height': input_image_height, - 'input_qos': 'sensor_data', - }], + parameters=[ + { + 'encoding_desired': encoding_desired, + 'image_width': input_image_width, + 'image_height': input_image_height, + 'input_qos': 'sensor_data', + } + ], remappings=[ ('image_raw', image_input_topic), ('image', CONVERTED_IMAGE_TOPIC), @@ -139,30 +142,38 @@ def _launch_setup(context, *args, **kwargs): 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, - 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, - }], + 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, + 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + } + ], + remappings=[ + ('tensor_pub', TENSOR_OUTPUT_TOPIC), + ('tensor_sub', TENSOR_INPUT_TOPIC), + ], ) yolov8_decoder_node = ComposableNode( name='yolov8_decoder_node', package='isaac_ros_yolov8', plugin='nvidia::isaac_ros::yolov8::YoloV8DecoderNode', - parameters=[{ - 'tensor_input_topic': TENSOR_INPUT_TOPIC, - 'confidence_threshold': confidence_threshold, - 'nms_threshold': nms_threshold, - 'num_classes': num_classes, - 'detections_topic': detection_topic, - }], + parameters=[ + { + 'tensor_input_topic': TENSOR_INPUT_TOPIC, + 'confidence_threshold': confidence_threshold, + 'nms_threshold': nms_threshold, + 'num_classes': num_classes, + 'detections_topic': detection_topic, + } + ], ) tensor_rt_container = ComposableNodeContainer( @@ -216,12 +227,14 @@ def _launch_setup(context, *args, **kwargs): package='isaac_ros_yolov8', executable='isaac_ros_yolov8_visualizer.py', name='yolov8_visualizer', - parameters=[{ - 'detections_topic': detection_topic, - 'image_topic': ENCODER_RESIZE_TOPIC, - 'output_image_topic': visualized_image_topic, - 'class_names_yaml': str(class_names), - }], + parameters=[ + { + 'detections_topic': detection_topic, + 'image_topic': ENCODER_RESIZE_TOPIC, + 'output_image_topic': visualized_image_topic, + 'class_names_yaml': str(class_names), + } + ], ) ) @@ -229,15 +242,16 @@ def _launch_setup(context, *args, **kwargs): def generate_launch_description(): - pkg_dir = get_package_share_directory('perception_setup') default_config = os.path.join(pkg_dir, 'config', 'yolo_detect.yaml') - return launch.LaunchDescription([ - DeclareLaunchArgument( - 'config_file', - default_value=default_config, - description='Path to YOLO pipeline config YAML', - ), - OpaqueFunction(function=_launch_setup), - ]) + return launch.LaunchDescription( + [ + DeclareLaunchArgument( + 'config_file', + default_value=default_config, + description='Path to YOLO pipeline config YAML', + ), + OpaqueFunction(function=_launch_setup), + ] + ) diff --git a/perception_setup/launch/yolo_obb.launch.py b/perception_setup/launch/yolo_obb.launch.py index 10fc6da..3db5634 100644 --- a/perception_setup/launch/yolo_obb.launch.py +++ b/perception_setup/launch/yolo_obb.launch.py @@ -1,7 +1,6 @@ # SPDX-License-Identifier: MIT -""" -Isaac ROS YOLO-OBB TensorRT inference pipeline +"""Isaac ROS YOLO-OBB TensorRT inference pipeline. 1. ImageFormatConverterNode Input: image_input_topic @@ -27,15 +26,17 @@ """ import os -import yaml - -from ament_index_python.packages import get_package_share_directory import launch -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +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 @@ -47,10 +48,9 @@ def _launch_setup(context, *args, **kwargs): - config_path = LaunchConfiguration('config_file').perform(context) - with open(config_path, 'r') as f: + with open(config_path) as f: cfg = yaml.safe_load(f) required_keys = [ @@ -122,12 +122,14 @@ def _launch_setup(context, *args, **kwargs): package='isaac_ros_image_proc', plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', name='image_format_converter', - parameters=[{ - 'encoding_desired': encoding_desired, - 'image_width': input_image_width, - 'image_height': input_image_height, - 'input_qos': 'sensor_data', - }], + parameters=[ + { + 'encoding_desired': encoding_desired, + 'image_width': input_image_width, + 'image_height': input_image_height, + 'input_qos': 'sensor_data', + } + ], remappings=[ ('image_raw', image_input_topic), ('image', CONVERTED_IMAGE_TOPIC), @@ -138,29 +140,37 @@ def _launch_setup(context, *args, **kwargs): 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, - 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, - }], + 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, + 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + } + ], + remappings=[ + ('tensor_pub', TENSOR_OUTPUT_TOPIC), + ('tensor_sub', TENSOR_INPUT_TOPIC), + ], ) 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': TENSOR_INPUT_TOPIC, - 'confidence_threshold': confidence_threshold, - 'num_detections': num_detections, - 'detections_topic': detection_topic, - }], + parameters=[ + { + 'tensor_input_topic': TENSOR_INPUT_TOPIC, + 'confidence_threshold': confidence_threshold, + 'num_detections': num_detections, + 'detections_topic': detection_topic, + } + ], ) tensor_rt_container = ComposableNodeContainer( @@ -211,12 +221,14 @@ def _launch_setup(context, *args, **kwargs): package='isaac_ros_yolov26_obb', executable='isaac_ros_yolov26_obb_visualizer.py', name='yolo_obb_visualizer', - parameters=[{ - 'detections_topic': detection_topic, - 'image_topic': ENCODER_RESIZE_TOPIC, - 'output_image_topic': visualized_image_topic, - 'class_names_yaml': str(class_names), - }], + parameters=[ + { + 'detections_topic': detection_topic, + 'image_topic': ENCODER_RESIZE_TOPIC, + 'output_image_topic': visualized_image_topic, + 'class_names_yaml': str(class_names), + } + ], ) ) @@ -224,15 +236,16 @@ def _launch_setup(context, *args, **kwargs): def generate_launch_description(): - pkg_dir = get_package_share_directory('perception_setup') default_config = os.path.join(pkg_dir, 'config', 'yolo_obb.yaml') - return launch.LaunchDescription([ - DeclareLaunchArgument( - 'config_file', - default_value=default_config, - description='Path to YOLO OBB pipeline config YAML', - ), - OpaqueFunction(function=_launch_setup), - ]) + return launch.LaunchDescription( + [ + DeclareLaunchArgument( + 'config_file', + default_value=default_config, + description='Path to YOLO OBB pipeline config YAML', + ), + OpaqueFunction(function=_launch_setup), + ] + ) From cf5f7d922d232a1fd87e08f7f29e173626fe010f Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 23 Mar 2026 13:10:57 +0100 Subject: [PATCH 07/37] feat: add pipeline-following segmentation and classification launch files and configurations --- .../config/pipeline_following.yaml | 86 +++++ perception_setup/config/yolo_cls.yaml | 43 +++ perception_setup/config/yolo_seg.yaml | 4 +- .../launch/pipeline_following.launch.py | 304 ++++++++++++++++++ perception_setup/launch/yolo_cls.launch.py | 250 ++++++++++++++ perception_setup/launch/yolo_obb.launch.py | 4 +- perception_setup/launch/yolo_seg.launch.py | 8 +- 7 files changed, 693 insertions(+), 6 deletions(-) create mode 100644 perception_setup/config/pipeline_following.yaml create mode 100644 perception_setup/config/yolo_cls.yaml create mode 100644 perception_setup/launch/pipeline_following.launch.py create mode 100644 perception_setup/launch/yolo_cls.launch.py diff --git a/perception_setup/config/pipeline_following.yaml b/perception_setup/config/pipeline_following.yaml new file mode 100644 index 0000000..04e23cd --- /dev/null +++ b/perception_setup/config/pipeline_following.yaml @@ -0,0 +1,86 @@ +# ============================================================================= +# Pipeline-following: Segmentation -> Classification +# +# Stage 1 (seg): Runs instance segmentation on the camera image and produces +# a binary mask of detected pipelines. +# Stage 2 (cls): Classifies the mask image to determine if the pipeline +# continues or ends. +# ============================================================================= + +# --------------- Segmentation stage --------------- +seg: + image_input_topic: "/cam_down/image_color" + camera_info_input_topic: "/cam_down/camera_info" + visualized_image_topic: "/pipeline/camera/segmentation_debug" + detection_topic: "/pipeline/camera/bboxes" + mask_topic: "/pipeline/camera/segmentation_mask" + + model_file_path: "down_cam_seg.onnx" + engine_file_path: "down_cam_seg.engine" + + enable_visualizer: true + class_names: + 0: "pipeline" + + input_image_width: 1440 + input_image_height: 1080 + encoding_desired: "rgb8" + + output_mask_width: 224 + output_mask_height: 224 + + verbose: true + force_engine_update: false + + input_tensor_names: ["input_tensor"] + input_binding_names: ["images"] + output_tensor_names: ["output_tensor", "mask_tensor"] + output_binding_names: ["output0", "output1"] + + network_image_width: 640 + network_image_height: 640 + image_mean: [0.0, 0.0, 0.0] + image_stddev: [1.0, 1.0, 1.0] + + confidence_threshold: 0.25 + num_detections: 300 + mask_width: 160 + mask_height: 160 + num_protos: 32 + +# --------------- Classification stage --------------- +cls: + # Input is the segmentation mask output from stage 1 + image_input_topic: "/pipeline/camera/segmentation_mask" + camera_info_input_topic: "/cam_down/camera_info" + visualized_image_topic: "/pipeline/camera/classification_debug" + class_topic: "/pipeline/camera/classification" + + model_file_path: "end-of-pipeline-yolov26.onnx" + engine_file_path: "end-of-pipeline-yolov26.engine" + + enable_visualizer: true + class_names: + 0: "continue" + 1: "end" + + # Seg mask is mono8 at output_mask_width x output_mask_height + input_image_width: 224 + input_image_height: 224 + encoding_desired: "rgb8" + + verbose: true + force_engine_update: false + + input_tensor_names: ["input_tensor"] + input_binding_names: ["images"] + output_tensor_names: ["output_tensor"] + output_binding_names: ["output0"] + + network_image_width: 224 + network_image_height: 224 + image_mean: [0.0, 0.0, 0.0] + image_stddev: [1.0, 1.0, 1.0] + + confidence_threshold: 0.25 + num_classes: 2 diff --git a/perception_setup/config/yolo_cls.yaml b/perception_setup/config/yolo_cls.yaml new file mode 100644 index 0000000..d84a8c9 --- /dev/null +++ b/perception_setup/config/yolo_cls.yaml @@ -0,0 +1,43 @@ +# Topics +image_input_topic: "/pipeline/camera/segmentation_mask" +camera_info_input_topic: "/pipeline/camera/camera_info" +visualized_image_topic: "/yolo_cls_processed_image" +class_topic: "/classification_output" + +# Model and engine file paths (relative to perception_setup/models/) +model_file_path: "end-of-pipeline-yolov26.onnx" +engine_file_path: "end-of-pipeline-yolov26.engine" + +# Visualizer +enable_visualizer: true +# Map class indices to human-readable names. +# Update these to match your classification model's classes. +class_names: + 0: "continue" + 1: "end" + +# Input image dimensions (from the subscribed topic) +input_image_width: 224 +input_image_height: 224 +encoding_desired: "rgb8" + +# TensorRT +verbose: true +force_engine_update: false + +# 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_seg.yaml b/perception_setup/config/yolo_seg.yaml index 0e55e86..7031003 100644 --- a/perception_setup/config/yolo_seg.yaml +++ b/perception_setup/config/yolo_seg.yaml @@ -20,8 +20,8 @@ input_image_height: 1080 encoding_desired: "rgb8" # Output mask resolution (upscaled from 160x160 via bilinear interpolation) -output_mask_width: 1440 -output_mask_height: 1080 +output_mask_width: 640 # Set to 640 to match the network input size, which allows you to use its camera info directly +output_mask_height: 640 # Set to 640 to match the network input size, which allows you to use its camera info directly # TensorRT verbose: true diff --git a/perception_setup/launch/pipeline_following.launch.py b/perception_setup/launch/pipeline_following.launch.py new file mode 100644 index 0000000..bb0f6f9 --- /dev/null +++ b/perception_setup/launch/pipeline_following.launch.py @@ -0,0 +1,304 @@ +# SPDX-License-Identifier: MIT + +"""Pipeline-following launch: Segmentation → Classification. + +Runs two TensorRT pipelines in a single launch: + +Stage 1 – Segmentation + camera image → ImageFormatConverter → DNNImageEncoder → TensorRT (seg model) + → YoloV26SegDecoder → binary mask + detections + (optional) visualizer + +Stage 2 – Classification + seg mask → ImageFormatConverter → DNNImageEncoder → TensorRT (cls model) + → YoloV26ClsDecoder → UInt8 class ID + (optional) visualizer + +The segmentation mask output feeds directly into the classification input. +""" + +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 + +# ── Stage 1: Segmentation internal topics ───────────────────────────────── +SEG_CONVERTED_IMAGE_TOPIC = '/yolo_seg/internal/converted_image' +SEG_ENCODER_RESIZE_TOPIC = '/yolo_seg_encoder/internal/resize/image' +SEG_TENSOR_OUTPUT_TOPIC = '/yolo_seg/tensor_pub' +SEG_TENSOR_INPUT_TOPIC = '/yolo_seg/tensor_sub' +SEG_ENCODER_NAMESPACE = 'yolo_seg_encoder/internal' + +# ── Stage 2: Classification internal topics ──────────────────────────────── +CLS_CONVERTED_IMAGE_TOPIC = '/yolo_cls/internal/converted_image' +CLS_ENCODER_RESIZE_TOPIC = '/yolo_cls_encoder/internal/resize/image' +CLS_TENSOR_OUTPUT_TOPIC = '/yolo_cls/tensor_pub' +CLS_TENSOR_INPUT_TOPIC = '/yolo_cls/tensor_sub' +CLS_ENCODER_NAMESPACE = 'yolo_cls_encoder/internal' + + +def _launch_setup(context, *args, **kwargs): + config_path = LaunchConfiguration('config_file').perform(context) + + with open(config_path) as f: + cfg = yaml.safe_load(f) + + seg = cfg['seg'] + cls = cfg['cls'] + + 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') + + # ==================================================================== + # Stage 1 – Segmentation + # ==================================================================== + seg_model = os.path.join(models_dir, str(seg['model_file_path'])) + seg_engine = os.path.join(models_dir, str(seg['engine_file_path'])) + + 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': str(seg['encoding_desired']), + 'image_width': int(seg['input_image_width']), + 'image_height': int(seg['input_image_height']), + 'input_qos': 'sensor_data', + }], + remappings=[ + ('image_raw', str(seg['image_input_topic'])), + ('image', SEG_CONVERTED_IMAGE_TOPIC), + ], + ) + + seg_tensor_rt = ComposableNode( + name='seg_tensor_rt', + package='isaac_ros_tensor_rt', + plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode', + parameters=[{ + 'model_file_path': seg_model, + 'engine_file_path': seg_engine, + 'output_binding_names': seg['output_binding_names'], + 'output_tensor_names': seg['output_tensor_names'], + 'input_tensor_names': seg['input_tensor_names'], + 'input_binding_names': seg['input_binding_names'], + 'verbose': bool(seg['verbose']), + 'force_engine_update': bool(seg['force_engine_update']), + 'tensor_output_topic': SEG_TENSOR_OUTPUT_TOPIC, + }], + remappings=[ + ('tensor_pub', SEG_TENSOR_OUTPUT_TOPIC), + ('tensor_sub', SEG_TENSOR_INPUT_TOPIC), + ], + ) + + seg_decoder = ComposableNode( + name='seg_decoder', + package='isaac_ros_yolov26_seg', + plugin='nvidia::isaac_ros::yolov26_seg::YoloV26SegDecoderNode', + parameters=[{ + 'tensor_input_topic': SEG_TENSOR_INPUT_TOPIC, + 'confidence_threshold': float(seg['confidence_threshold']), + 'num_detections': int(seg['num_detections']), + 'mask_width': int(seg['mask_width']), + 'mask_height': int(seg['mask_height']), + 'num_protos': int(seg['num_protos']), + 'network_image_width': int(seg['network_image_width']), + 'network_image_height': int(seg['network_image_height']), + 'output_mask_width': int(seg['output_mask_width']), + 'output_mask_height': int(seg['output_mask_height']), + 'detections_topic': str(seg['detection_topic']), + 'mask_topic': str(seg['mask_topic']), + }], + ) + + seg_container = ComposableNodeContainer( + name='seg_tensor_rt_container', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + seg_image_format_converter, + seg_tensor_rt, + seg_decoder, + ], + output='screen', + arguments=['--ros-args', '--log-level', 'INFO'], + namespace='', + ) + + seg_encoder_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py') + ), + launch_arguments={ + 'input_image_width': str(seg['input_image_width']), + 'input_image_height': str(seg['input_image_height']), + 'network_image_width': str(seg['network_image_width']), + 'network_image_height': str(seg['network_image_height']), + 'image_mean': str(seg['image_mean']), + 'image_stddev': str(seg['image_stddev']), + 'attach_to_shared_component_container': 'True', + 'component_container_name': 'seg_tensor_rt_container', + 'dnn_image_encoder_namespace': SEG_ENCODER_NAMESPACE, + 'image_input_topic': SEG_CONVERTED_IMAGE_TOPIC, + 'camera_info_input_topic': str(seg['camera_info_input_topic']), + 'tensor_output_topic': SEG_TENSOR_OUTPUT_TOPIC, + }.items(), + ) + + # ==================================================================== + # Stage 2 – Classification (input = seg mask output) + # ==================================================================== + cls_model = os.path.join(models_dir, str(cls['model_file_path'])) + cls_engine = os.path.join(models_dir, str(cls['engine_file_path'])) + + 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': str(cls['encoding_desired']), + 'image_width': int(cls['input_image_width']), + 'image_height': int(cls['input_image_height']), + 'input_qos': 'sensor_data', + }], + remappings=[ + ('image_raw', str(cls['image_input_topic'])), + ('image', CLS_CONVERTED_IMAGE_TOPIC), + ], + ) + + cls_tensor_rt = ComposableNode( + name='cls_tensor_rt', + package='isaac_ros_tensor_rt', + plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode', + parameters=[{ + 'model_file_path': cls_model, + 'engine_file_path': cls_engine, + 'output_binding_names': cls['output_binding_names'], + 'output_tensor_names': cls['output_tensor_names'], + 'input_tensor_names': cls['input_tensor_names'], + 'input_binding_names': cls['input_binding_names'], + 'verbose': bool(cls['verbose']), + 'force_engine_update': bool(cls['force_engine_update']), + 'tensor_output_topic': CLS_TENSOR_OUTPUT_TOPIC, + }], + remappings=[ + ('tensor_pub', CLS_TENSOR_OUTPUT_TOPIC), + ('tensor_sub', CLS_TENSOR_INPUT_TOPIC), + ], + ) + + cls_decoder = ComposableNode( + name='cls_decoder', + package='isaac_ros_yolov26_cls', + plugin='nvidia::isaac_ros::yolov26_cls::YoloV26ClsDecoderNode', + parameters=[{ + 'tensor_input_topic': CLS_TENSOR_INPUT_TOPIC, + 'confidence_threshold': float(cls['confidence_threshold']), + 'num_classes': int(cls['num_classes']), + 'class_topic': str(cls['class_topic']), + }], + ) + + cls_container = ComposableNodeContainer( + name='cls_tensor_rt_container', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + cls_image_format_converter, + cls_tensor_rt, + cls_decoder, + ], + output='screen', + arguments=['--ros-args', '--log-level', 'INFO'], + namespace='', + ) + + cls_encoder_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py') + ), + launch_arguments={ + 'input_image_width': str(cls['input_image_width']), + 'input_image_height': str(cls['input_image_height']), + 'network_image_width': str(cls['network_image_width']), + 'network_image_height': str(cls['network_image_height']), + 'image_mean': str(cls['image_mean']), + 'image_stddev': str(cls['image_stddev']), + 'attach_to_shared_component_container': 'True', + 'component_container_name': 'cls_tensor_rt_container', + 'dnn_image_encoder_namespace': CLS_ENCODER_NAMESPACE, + 'image_input_topic': CLS_CONVERTED_IMAGE_TOPIC, + 'camera_info_input_topic': str(cls['camera_info_input_topic']), + 'tensor_output_topic': CLS_TENSOR_OUTPUT_TOPIC, + }.items(), + ) + + # ==================================================================== + # Assemble actions + # ==================================================================== + actions = [ + seg_container, + seg_encoder_launch, + cls_container, + cls_encoder_launch, + ] + + # Segmentation visualizer + if bool(seg.get('enable_visualizer', False)): + actions.append( + Node( + package='isaac_ros_yolov26_seg', + executable='isaac_ros_yolov26_seg_visualizer.py', + name='seg_visualizer', + parameters=[{ + 'detections_topic': str(seg['detection_topic']), + 'image_topic': SEG_ENCODER_RESIZE_TOPIC, + 'mask_topic': str(seg['mask_topic']), + 'output_image_topic': str(seg['visualized_image_topic']), + 'class_names_yaml': str(seg['class_names']), + }], + ) + ) + + # Classification visualizer + if bool(cls.get('enable_visualizer', False)): + actions.append( + Node( + package='isaac_ros_yolov26_cls', + executable='isaac_ros_yolov26_cls_visualizer.py', + name='cls_visualizer', + parameters=[{ + 'class_topic': str(cls['class_topic']), + 'image_topic': CLS_ENCODER_RESIZE_TOPIC, + 'output_image_topic': str(cls['visualized_image_topic']), + 'class_names_yaml': str(cls['class_names']), + }], + ) + ) + + return actions + + +def generate_launch_description(): + pkg_dir = get_package_share_directory('perception_setup') + default_config = os.path.join(pkg_dir, 'config', 'pipeline_following.yaml') + + return launch.LaunchDescription([ + DeclareLaunchArgument( + 'config_file', + default_value=default_config, + description='Path to pipeline-following config YAML', + ), + OpaqueFunction(function=_launch_setup), + ]) diff --git a/perception_setup/launch/yolo_cls.launch.py b/perception_setup/launch/yolo_cls.launch.py new file mode 100644 index 0000000..10ffa89 --- /dev/null +++ b/perception_setup/launch/yolo_cls.launch.py @@ -0,0 +1,250 @@ +# SPDX-License-Identifier: MIT + +"""Isaac ROS YOLO classification TensorRT inference pipeline. + +1. ImageFormatConverterNode + Input: image_input_topic + Output: /yolo_cls/internal/converted_image + Purpose: Convert camera image encoding (e.g. bgra8 -> rgb8) + +2. DNNImageEncoderNode + Input: /yolo_cls/internal/converted_image + Output: /tensor_pub + Purpose: Resize + normalize image and convert to network tensor + +3. TensorRTNode + Input: /tensor_pub + Output: /tensor_sub + Purpose: Run TensorRT inference + Expected output tensor shape: [1, num_classes] + +4. YoloV26ClsDecoderNode + Input: /tensor_sub + Output: class_topic (UInt8 with predicted class index) + Purpose: Argmax over class scores and publish class ID +""" + +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 + +CONVERTED_IMAGE_TOPIC = '/yolo_cls/internal/converted_image' +ENCODER_RESIZE_TOPIC = '/yolo_cls_encoder/internal/resize/image' +TENSOR_OUTPUT_TOPIC = '/yolo_cls/tensor_pub' +TENSOR_INPUT_TOPIC = '/yolo_cls/tensor_sub' +DNN_IMAGE_ENCODER_NAMESPACE = 'yolo_cls_encoder/internal' + + +def _launch_setup(context, *args, **kwargs): + config_path = LaunchConfiguration('config_file').perform(context) + + with open(config_path) as f: + cfg = yaml.safe_load(f) + + required_keys = [ + 'model_file_path', + 'engine_file_path', + 'input_tensor_names', + 'input_binding_names', + 'output_tensor_names', + 'output_binding_names', + 'verbose', + 'force_engine_update', + 'input_image_width', + 'input_image_height', + 'encoding_desired', + 'network_image_width', + 'network_image_height', + 'image_mean', + 'image_stddev', + 'confidence_threshold', + 'num_classes', + 'class_topic', + 'image_input_topic', + 'camera_info_input_topic', + 'enable_visualizer', + 'visualized_image_topic', + 'class_names', + ] + + for key in required_keys: + if key not in cfg: + raise RuntimeError(f"Missing required config key: '{key}'") + + # Resolve model paths relative to perception_setup/models/ + pkg_dir = get_package_share_directory('perception_setup') + models_dir = os.path.join(pkg_dir, 'models') + + model_file_path = os.path.join(models_dir, str(cfg['model_file_path'])) + engine_file_path = os.path.join(models_dir, str(cfg['engine_file_path'])) + + input_tensor_names = cfg['input_tensor_names'] + input_binding_names = cfg['input_binding_names'] + output_tensor_names = cfg['output_tensor_names'] + output_binding_names = cfg['output_binding_names'] + + verbose = bool(cfg['verbose']) + force_engine_update = bool(cfg['force_engine_update']) + + input_image_width = int(cfg['input_image_width']) + input_image_height = int(cfg['input_image_height']) + encoding_desired = str(cfg['encoding_desired']) + + network_image_width = int(cfg['network_image_width']) + network_image_height = int(cfg['network_image_height']) + image_mean = cfg['image_mean'] + image_stddev = cfg['image_stddev'] + + confidence_threshold = float(cfg['confidence_threshold']) + num_classes = int(cfg['num_classes']) + class_topic = str(cfg['class_topic']) + + image_input_topic = str(cfg['image_input_topic']) + camera_info_input_topic = str(cfg['camera_info_input_topic']) + + enable_visualizer = bool(cfg['enable_visualizer']) + visualized_image_topic = str(cfg['visualized_image_topic']) + class_names = cfg['class_names'] + + image_format_converter = ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_converter', + parameters=[ + { + 'encoding_desired': encoding_desired, + 'image_width': input_image_width, + 'image_height': input_image_height, + 'input_qos': 'sensor_data', + } + ], + remappings=[ + ('image_raw', image_input_topic), + ('image', CONVERTED_IMAGE_TOPIC), + ], + ) + + 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, + 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + } + ], + remappings=[ + ('tensor_pub', TENSOR_OUTPUT_TOPIC), + ('tensor_sub', TENSOR_INPUT_TOPIC), + ], + ) + + 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': TENSOR_INPUT_TOPIC, + 'confidence_threshold': confidence_threshold, + 'num_classes': num_classes, + 'class_topic': class_topic, + } + ], + ) + + tensor_rt_container = ComposableNodeContainer( + name='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(input_image_width), + 'input_image_height': str(input_image_height), + 'network_image_width': str(network_image_width), + 'network_image_height': str(network_image_height), + 'image_mean': str(image_mean), + 'image_stddev': str(image_stddev), + 'attach_to_shared_component_container': 'True', + 'component_container_name': 'tensor_rt_container', + 'dnn_image_encoder_namespace': DNN_IMAGE_ENCODER_NAMESPACE, + 'image_input_topic': CONVERTED_IMAGE_TOPIC, + 'camera_info_input_topic': camera_info_input_topic, + 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + }.items(), + ) + + actions = [tensor_rt_container, yolo_cls_encoder_launch] + + if enable_visualizer: + actions.append( + Node( + package='isaac_ros_yolov26_cls', + executable='isaac_ros_yolov26_cls_visualizer.py', + name='yolo_cls_visualizer', + parameters=[ + { + 'class_topic': class_topic, + 'image_topic': ENCODER_RESIZE_TOPIC, + 'output_image_topic': visualized_image_topic, + 'class_names_yaml': str(class_names), + } + ], + ) + ) + + return actions + + +def generate_launch_description(): + pkg_dir = get_package_share_directory('perception_setup') + default_config = os.path.join(pkg_dir, 'config', 'yolo_cls.yaml') + + return launch.LaunchDescription( + [ + DeclareLaunchArgument( + 'config_file', + default_value=default_config, + description='Path to YOLO classification pipeline config YAML', + ), + OpaqueFunction(function=_launch_setup), + ] + ) diff --git a/perception_setup/launch/yolo_obb.launch.py b/perception_setup/launch/yolo_obb.launch.py index 3db5634..daf0d9a 100644 --- a/perception_setup/launch/yolo_obb.launch.py +++ b/perception_setup/launch/yolo_obb.launch.py @@ -42,8 +42,8 @@ CONVERTED_IMAGE_TOPIC = '/yolo_obb/internal/converted_image' ENCODER_RESIZE_TOPIC = '/yolo_obb_encoder/internal/resize/image' -TENSOR_OUTPUT_TOPIC = '/tensor_pub' -TENSOR_INPUT_TOPIC = '/tensor_sub' +TENSOR_OUTPUT_TOPIC = '/yolo_obb/tensor_pub' +TENSOR_INPUT_TOPIC = '/yolo_obb/tensor_sub' DNN_IMAGE_ENCODER_NAMESPACE = 'yolo_obb_encoder/internal' diff --git a/perception_setup/launch/yolo_seg.launch.py b/perception_setup/launch/yolo_seg.launch.py index e45b4a6..1e6b5dd 100644 --- a/perception_setup/launch/yolo_seg.launch.py +++ b/perception_setup/launch/yolo_seg.launch.py @@ -48,8 +48,8 @@ CONVERTED_IMAGE_TOPIC = '/yolo_seg/internal/converted_image' ENCODER_RESIZE_TOPIC = '/yolo_seg_encoder/internal/resize/image' -TENSOR_OUTPUT_TOPIC = '/tensor_pub' -TENSOR_INPUT_TOPIC = '/tensor_sub' +TENSOR_OUTPUT_TOPIC = '/yolo_seg/tensor_pub' +TENSOR_INPUT_TOPIC = '/yolo_seg/tensor_sub' DNN_IMAGE_ENCODER_NAMESPACE = 'yolo_seg_encoder/internal' @@ -168,6 +168,10 @@ def _launch_setup(context, *args, **kwargs): 'force_engine_update': force_engine_update, 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, }], + remappings=[ + ('tensor_pub', TENSOR_OUTPUT_TOPIC), + ('tensor_sub', TENSOR_INPUT_TOPIC), + ], ) yolo_seg_decoder_node = ComposableNode( From 6273a375e630848ad05f61a8da58b5fb425a4805 Mon Sep 17 00:00:00 2001 From: vortexuser Date: Mon, 23 Mar 2026 22:09:51 +0100 Subject: [PATCH 08/37] feat: add RealSense D555 configuration and launch files; update YOLO input topics and dimensions --- perception_setup/config/realsense_d555.yaml | 91 +++++++++++++++++++ perception_setup/config/yolo_obb.yaml | 8 +- .../launch/realsense_d555.launch.py | 35 +++++++ scripts/set_MTU_nic.sh | 10 +- 4 files changed, 135 insertions(+), 9 deletions(-) create mode 100644 perception_setup/config/realsense_d555.yaml create mode 100644 perception_setup/launch/realsense_d555.launch.py diff --git a/perception_setup/config/realsense_d555.yaml b/perception_setup/config/realsense_d555.yaml new file mode 100644 index 0000000..1fa651e --- /dev/null +++ b/perception_setup/config/realsense_d555.yaml @@ -0,0 +1,91 @@ +depth_module.emitter_enabled: false # Toggle emitter used for depth image (we dont want it underwater) + +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' + +enable_infra1: false +enable_infra2: false +depth_module.infra1_format: 'Y8' +depth_module.infra2_format: 'Y8' + +camera_namespace: 'moby' # Does not work? +camera_name: 'front_camera' +tf_prefix: 'moby/' + +enable_gyro: false +enable_accel: false +enable_motion: false + +publish_tf: false # Publishes with wrong convention for ROS (z-frame for camera/optical frame) + +# serial_no: "''" +# usb_port_id: "''" +# device_type: "''" +# config_file: "''" +# json_file_path: "''" +# initial_reset: 'false' +# accelerate_gpu_with_glsl: 'false' +# rosbag_filename: "''" +# rosbag_loop: 'false' +# log_level: 'info' +# output: 'screen' +# enable_infra: 'false' +# enable_infra1: 'false' +# enable_infra2: 'false' +# depth_module.infra_profile: '0,0,0' +# depth_module.infra_format: 'RGB8' +# depth_module.color_profile: '0,0,0' +# depth_module.color_format: 'RGB8' +# depth_module.exposure: '8500' +# depth_module.gain: '16' +# depth_module.hdr_enabled: 'false' +# depth_module.enable_auto_exposure: 'true' +# depth_module.exposure.1: '7500' +# depth_module.gain.1: '16' +# depth_module.exposure.2: '1' +# depth_module.gain.2: '16' +# enable_sync: 'false' +# depth_module.inter_cam_sync_mode: '0' +# enable_rgbd: 'false' +# gyro_fps: '0' +# accel_fps: '0' +# motion_fps: '0' +# unite_imu_method: '0' +# clip_distance: '-2.' +# angular_velocity_cov: '0.01' +# linear_accel_cov: '0.01' +# diagnostics_period: '0.0' +# publish_tf: 'true' +# tf_publish_rate: '0.0' +# pointcloud.enable: 'false' +# pointcloud.stream_filter: '2' +# pointcloud.stream_index_filter: '0' +# pointcloud.ordered_pc: 'false' +# pointcloud.allow_no_texture_points: 'false' +# align_depth.enable: 'false' +# colorizer.enable: 'false' +# decimation_filter.enable: 'false' +# rotation_filter.enable: 'false' +# rotation_filter.rotation: '0.0' +# spatial_filter.enable: 'false' +# temporal_filter.enable: 'false' +# disparity_filter.enable: 'false' +# hole_filling_filter.enable: 'false' +# hdr_merge.enable: 'false' +# wait_for_device_timeout: '-1.' +# reconnect_timeout: '6.' +# base_frame_id: 'link' +# decimation_filter.filter_magnitude: '2' +# enable_safety: 'false' +# safety_camera.safety_mode: '0' +# enable_labeled_point_cloud: 'false' +# depth_mapping_camera.labeled_point_cloud_profile: '0,0,0' +# enable_occupancy: 'false' +# depth_mapping_camera.occupancy_profile: '0,0,0' diff --git a/perception_setup/config/yolo_obb.yaml b/perception_setup/config/yolo_obb.yaml index d8a6568..05bfaad 100644 --- a/perception_setup/config/yolo_obb.yaml +++ b/perception_setup/config/yolo_obb.yaml @@ -1,6 +1,6 @@ # Topics -image_input_topic: "/zed_node/left/image_rect_color" -camera_info_input_topic: "/zed_node/depth/camera_info" +image_input_topic: "/camera/camera/color/image_raw" +camera_info_input_topic: "/camera/camera/color/camera_info" visualized_image_topic: "/yolo_obb_processed_image" detection_topic: "/obb_detections_output" @@ -14,8 +14,8 @@ class_names: 0: "valve" # Input image dimensions -input_image_width: 1280 -input_image_height: 720 +input_image_width: 896 +input_image_height: 504 encoding_desired: "rgb8" # TensorRT diff --git a/perception_setup/launch/realsense_d555.launch.py b/perception_setup/launch/realsense_d555.launch.py new file mode 100644 index 0000000..034969d --- /dev/null +++ b/perception_setup/launch/realsense_d555.launch.py @@ -0,0 +1,35 @@ +"""Wrapper launch file for the RealSense D555 camera. + +See: https://github.com/vortexntnu/realsense-ros/blob/r/4.57.6/realsense2_camera/launch/rs_launch.py +""" + +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 LaunchConfiguration +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + pkg_dir = get_package_share_directory("perception_setup") + default_config = os.path.join(pkg_dir, "config", "realsense_d555.yaml") + + config_file_arg = DeclareLaunchArgument( + "config_file", + default_value=default_config, + description="Path to a YAML file with node-level parameter overrides for rs_launch.py", + ) + + rs_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [FindPackageShare("realsense2_camera"), "/launch/rs_launch.py"] + ), + launch_arguments={ + "config_file": LaunchConfiguration("config_file"), + }.items(), + ) + + return LaunchDescription([config_file_arg, rs_launch]) diff --git a/scripts/set_MTU_nic.sh b/scripts/set_MTU_nic.sh index cf9ac0e..cb99732 100755 --- a/scripts/set_MTU_nic.sh +++ b/scripts/set_MTU_nic.sh @@ -1,6 +1,6 @@ -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 nmcli connection modify "realsense_d555" 802-3-ethernet.mtu 9000 sudo systemctl restart NetworkManager -ip link show enP5p1s0f0 +ip link show enP5p1s0f3 From 8b15868263f6c286c1d3f8fe11addccf18884a2f Mon Sep 17 00:00:00 2001 From: vortexuser Date: Mon, 23 Mar 2026 22:11:17 +0100 Subject: [PATCH 09/37] fix: update comments for clarity in realsense_d555.yaml configuration --- perception_setup/config/realsense_d555.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/perception_setup/config/realsense_d555.yaml b/perception_setup/config/realsense_d555.yaml index 1fa651e..9225089 100644 --- a/perception_setup/config/realsense_d555.yaml +++ b/perception_setup/config/realsense_d555.yaml @@ -15,13 +15,13 @@ enable_infra2: false depth_module.infra1_format: 'Y8' depth_module.infra2_format: 'Y8' -camera_namespace: 'moby' # Does not work? +camera_namespace: 'moby' # Does nothing? camera_name: 'front_camera' tf_prefix: 'moby/' -enable_gyro: false -enable_accel: false -enable_motion: false +enable_gyro: false # Dont need IMU data (not being used) +enable_accel: false # Dont need IMU data (not being used) +enable_motion: false # Dont need IMU data (not being used) publish_tf: false # Publishes with wrong convention for ROS (z-frame for camera/optical frame) From d33bf7965f8f79315d17a09c45ebf9b05980956b Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Tue, 24 Mar 2026 16:29:42 +0100 Subject: [PATCH 10/37] feat: add RealSense D555 calibration and dual-camera segmentation launch files; update YOLO configurations and remove deprecated files --- .../config/color_realsense_d555_calib.yaml | 33 +++ .../config/d555_color_camera_factory.yaml | 31 --- .../config/pipeline_following.yaml | 31 +-- .../config/pipeline_localization.yaml | 90 ++++++++ perception_setup/config/yolo_cls.yaml | 12 +- perception_setup/config/yolo_detect.yaml | 4 + perception_setup/config/yolo_obb.yaml | 4 + perception_setup/config/yolo_seg.yaml | 4 + perception_setup/launch/camera_info.launch.py | 36 ---- .../launch/pipeline_following.launch.py | 12 +- .../launch/pipeline_localization.launch.py | 196 ++++++++++++++++++ .../launch/realsense_d555.launch.py | 36 +++- perception_setup/launch/yolo_cls.launch.py | 4 +- perception_setup/launch/yolo_detect.launch.py | 4 +- perception_setup/launch/yolo_obb.launch.py | 4 +- perception_setup/launch/yolo_seg.launch.py | 4 +- perception_setup/package.xml | 1 + 17 files changed, 400 insertions(+), 106 deletions(-) create mode 100644 perception_setup/config/color_realsense_d555_calib.yaml delete mode 100644 perception_setup/config/d555_color_camera_factory.yaml create mode 100644 perception_setup/config/pipeline_localization.yaml delete mode 100644 perception_setup/launch/camera_info.launch.py create mode 100644 perception_setup/launch/pipeline_localization.launch.py diff --git a/perception_setup/config/color_realsense_d555_calib.yaml b/perception_setup/config/color_realsense_d555_calib.yaml new file mode 100644 index 0000000..1e36256 --- /dev/null +++ b/perception_setup/config/color_realsense_d555_calib.yaml @@ -0,0 +1,33 @@ +camera_info_topic: "/camera/camera/color/camera_info" + +image_width: 1280 +image_height: 800 +camera_name: color_camera + +camera_matrix: + rows: 3 + cols: 3 + data: [1042.00796, 0.0, 741.55948, + 0.0, 1039.05486, 569.96599, + 0.0, 0.0, 1.0] + +distortion_model: plumb_bob + +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.360444, 0.191541, 0.002454, -0.000152, -0.064295] + +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: [1042.00796, 0.0, 741.55948, 0.0, + 0.0, 1039.05486, 569.96599, 0.0, + 0.0, 0.0, 1.0, 0.0] diff --git a/perception_setup/config/d555_color_camera_factory.yaml b/perception_setup/config/d555_color_camera_factory.yaml deleted file mode 100644 index 620fec8..0000000 --- a/perception_setup/config/d555_color_camera_factory.yaml +++ /dev/null @@ -1,31 +0,0 @@ -image_width: 848 -image_height: 480 -camera_name: d555_color -camera_info_topic: "/realsense/D555_409122300281_Color/camera_info" -camera_matrix: - rows: 3 - cols: 3 - data: [452.121520996094, 0.0, 438.254058837891, - 0.0, 451.737976074219, 248.703216552734, - 0.0, 0.0, 1.0] -distortion_model: plumb_bob -distortion_coefficients: - rows: 1 - cols: 5 - data: [-0.0548105500638485, - 0.0597584918141365, - 0.000486430013552308, - 0.00031599000794813, - -0.0192314591258764] -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: [452.121520996094, 0.0, 438.254058837891, 0.0, - 0.0, 451.737976074219, 248.703216552734, 0.0, - 0.0, 0.0, 1.0, 0.0] diff --git a/perception_setup/config/pipeline_following.yaml b/perception_setup/config/pipeline_following.yaml index 04e23cd..3356e76 100644 --- a/perception_setup/config/pipeline_following.yaml +++ b/perception_setup/config/pipeline_following.yaml @@ -1,13 +1,7 @@ -# ============================================================================= -# Pipeline-following: Segmentation -> Classification -# -# Stage 1 (seg): Runs instance segmentation on the camera image and produces -# a binary mask of detected pipelines. -# Stage 2 (cls): Classifies the mask image to determine if the pipeline -# continues or ends. -# ============================================================================= - -# --------------- Segmentation stage --------------- +# Pipeline-following +# Stage 1: Runs instance segmentation on the camera image and produces a binary mask of detected pipelines. +# Stage 2: Classifies the mask image to determine if the pipeline continues or ends. + seg: image_input_topic: "/cam_down/image_color" camera_info_input_topic: "/cam_down/camera_info" @@ -26,12 +20,16 @@ seg: input_image_height: 1080 encoding_desired: "rgb8" - output_mask_width: 224 - output_mask_height: 224 + output_mask_width: 640 # Set to 640 to match the network input size, which allows you to use its camera info directly + output_mask_height: 640 # Set to 640 to match the network input size, which allows you to use its camera info directly verbose: true force_engine_update: false + # ============================================================================================================================================================== + # NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model + # ============================================================================================================================================================== + input_tensor_names: ["input_tensor"] input_binding_names: ["images"] output_tensor_names: ["output_tensor", "mask_tensor"] @@ -48,7 +46,6 @@ seg: mask_height: 160 num_protos: 32 -# --------------- Classification stage --------------- cls: # Input is the segmentation mask output from stage 1 image_input_topic: "/pipeline/camera/segmentation_mask" @@ -65,13 +62,17 @@ cls: 1: "end" # Seg mask is mono8 at output_mask_width x output_mask_height - input_image_width: 224 - input_image_height: 224 + input_image_width: 640 + input_image_height: 640 encoding_desired: "rgb8" verbose: true force_engine_update: false + # ============================================================================================================================================================== + # NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model + # ============================================================================================================================================================== + input_tensor_names: ["input_tensor"] input_binding_names: ["images"] output_tensor_names: ["output_tensor"] diff --git a/perception_setup/config/pipeline_localization.yaml b/perception_setup/config/pipeline_localization.yaml new file mode 100644 index 0000000..0b71497 --- /dev/null +++ b/perception_setup/config/pipeline_localization.yaml @@ -0,0 +1,90 @@ +# Pipeline-localization: Dual-camera segmentation +# Runs two segmentation pipelines on different camera streams. + +seg_front: + image_input_topic: "/cam/image_color" + camera_info_input_topic: "/cam/camera_info" + visualized_image_topic: "/localization/front/segmentation_debug" + detection_topic: "/localization/front/detections" + mask_topic: "/localization/front/segmentation_mask" + + model_file_path: "down_cam_seg.onnx" + engine_file_path: "down_cam_seg.engine" + + enable_visualizer: true + class_names: + 0: "pipeline" + + input_image_width: 1440 + input_image_height: 1080 + encoding_desired: "rgb8" + + output_mask_width: 640 + output_mask_height: 640 + + verbose: true + force_engine_update: false + + # ============================================================================================================================================================== + # NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different segmentation model + # ============================================================================================================================================================== + + input_tensor_names: ["input_tensor"] + input_binding_names: ["images"] + output_tensor_names: ["output_tensor", "mask_tensor"] + output_binding_names: ["output0", "output1"] + + network_image_width: 640 + network_image_height: 640 + image_mean: [0.0, 0.0, 0.0] + image_stddev: [1.0, 1.0, 1.0] + + confidence_threshold: 0.25 + num_detections: 300 + mask_width: 160 + mask_height: 160 + num_protos: 32 + +seg_down: + image_input_topic: "/cam_down/image_color" + camera_info_input_topic: "/cam_down/camera_info" + visualized_image_topic: "/localization/down/segmentation_debug" + detection_topic: "/localization/down/detections" + mask_topic: "/localization/down/segmentation_mask" + + model_file_path: "down_cam_seg.onnx" + engine_file_path: "down_cam_seg.engine" + + enable_visualizer: true + class_names: + 0: "pipeline" + + input_image_width: 1440 + input_image_height: 1080 + encoding_desired: "rgb8" + + output_mask_width: 640 + output_mask_height: 640 + + verbose: true + force_engine_update: false + + # ============================================================================================================================================================== + # NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model + # ============================================================================================================================================================== + + input_tensor_names: ["input_tensor"] + input_binding_names: ["images"] + output_tensor_names: ["output_tensor", "mask_tensor"] + output_binding_names: ["output0", "output1"] + + network_image_width: 640 + network_image_height: 640 + image_mean: [0.0, 0.0, 0.0] + image_stddev: [1.0, 1.0, 1.0] + + confidence_threshold: 0.25 + num_detections: 300 + mask_width: 160 + mask_height: 160 + num_protos: 32 diff --git a/perception_setup/config/yolo_cls.yaml b/perception_setup/config/yolo_cls.yaml index d84a8c9..54c8b61 100644 --- a/perception_setup/config/yolo_cls.yaml +++ b/perception_setup/config/yolo_cls.yaml @@ -1,6 +1,6 @@ # Topics -image_input_topic: "/pipeline/camera/segmentation_mask" -camera_info_input_topic: "/pipeline/camera/camera_info" +image_input_topic: "/yolo_seg_mask" +camera_info_input_topic: "/cam_down/camera_info" visualized_image_topic: "/yolo_cls_processed_image" class_topic: "/classification_output" @@ -17,14 +17,18 @@ class_names: 1: "end" # Input image dimensions (from the subscribed topic) -input_image_width: 224 -input_image_height: 224 +input_image_width: 640 +input_image_height: 640 encoding_desired: "rgb8" # TensorRT verbose: true force_engine_update: false +# ============================================================================================================================================================== +# NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model +# ============================================================================================================================================================== + # Tensor I/O bindings # Classification model output shape: [1, num_classes] input_tensor_names: ["input_tensor"] diff --git a/perception_setup/config/yolo_detect.yaml b/perception_setup/config/yolo_detect.yaml index 15497f0..26deee3 100644 --- a/perception_setup/config/yolo_detect.yaml +++ b/perception_setup/config/yolo_detect.yaml @@ -22,6 +22,10 @@ encoding_desired: "rgb8" verbose: true force_engine_update: false +# ============================================================================================================================================================== +# NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model +# ============================================================================================================================================================== + # Tensor I/O bindings input_tensor_names: ["input_tensor"] input_binding_names: ["images"] diff --git a/perception_setup/config/yolo_obb.yaml b/perception_setup/config/yolo_obb.yaml index 05bfaad..6caa900 100644 --- a/perception_setup/config/yolo_obb.yaml +++ b/perception_setup/config/yolo_obb.yaml @@ -22,6 +22,10 @@ encoding_desired: "rgb8" verbose: true force_engine_update: false +# ============================================================================================================================================================== +# NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model +# ============================================================================================================================================================== + # Tensor I/O bindings # OBB model exported with NMS; output shape: [1, 300, 7] input_tensor_names: ["input_tensor"] diff --git a/perception_setup/config/yolo_seg.yaml b/perception_setup/config/yolo_seg.yaml index 7031003..73a0bd1 100644 --- a/perception_setup/config/yolo_seg.yaml +++ b/perception_setup/config/yolo_seg.yaml @@ -27,6 +27,10 @@ output_mask_height: 640 # Set to 640 to match the network input size, which allo verbose: true force_engine_update: false +# ============================================================================================================================================================== +# NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model +# ============================================================================================================================================================== + # Tensor I/O bindings # Segmentation model with embedded NMS; two outputs: # output0: [1, 300, 38] detections with 32 mask coefficients diff --git a/perception_setup/launch/camera_info.launch.py b/perception_setup/launch/camera_info.launch.py deleted file mode 100644 index e9c26a5..0000000 --- a/perception_setup/launch/camera_info.launch.py +++ /dev/null @@ -1,36 +0,0 @@ -import os -import yaml - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - - pkg_dir = get_package_share_directory("perception_setup") - - config_file = os.path.join( - pkg_dir, - "config", - "d555_color_camera_factory.yaml" - ) - - with open(config_file, "r") as f: - cfg = yaml.safe_load(f) - - camera_info_topic = cfg["camera_info_topic"] - - return LaunchDescription([ - - Node( - package="perception_setup", - executable="camera_info_publisher.py", - name="camera_info_publisher", - parameters=[{ - "camera_info_file": config_file, - "camera_info_topic": camera_info_topic, - }], - output="screen" - ) - ]) \ No newline at end of file diff --git a/perception_setup/launch/pipeline_following.launch.py b/perception_setup/launch/pipeline_following.launch.py index bb0f6f9..9674630 100644 --- a/perception_setup/launch/pipeline_following.launch.py +++ b/perception_setup/launch/pipeline_following.launch.py @@ -30,14 +30,14 @@ from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode -# ── Stage 1: Segmentation internal topics ───────────────────────────────── +# Stage 1: Segmentation internal topics SEG_CONVERTED_IMAGE_TOPIC = '/yolo_seg/internal/converted_image' SEG_ENCODER_RESIZE_TOPIC = '/yolo_seg_encoder/internal/resize/image' SEG_TENSOR_OUTPUT_TOPIC = '/yolo_seg/tensor_pub' SEG_TENSOR_INPUT_TOPIC = '/yolo_seg/tensor_sub' SEG_ENCODER_NAMESPACE = 'yolo_seg_encoder/internal' -# ── Stage 2: Classification internal topics ──────────────────────────────── +# Stage 2: Classification internal topics CLS_CONVERTED_IMAGE_TOPIC = '/yolo_cls/internal/converted_image' CLS_ENCODER_RESIZE_TOPIC = '/yolo_cls_encoder/internal/resize/image' CLS_TENSOR_OUTPUT_TOPIC = '/yolo_cls/tensor_pub' @@ -58,9 +58,7 @@ def _launch_setup(context, *args, **kwargs): models_dir = os.path.join(pkg_dir, 'models') encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder') - # ==================================================================== # Stage 1 – Segmentation - # ==================================================================== seg_model = os.path.join(models_dir, str(seg['model_file_path'])) seg_engine = os.path.join(models_dir, str(seg['engine_file_path'])) @@ -155,9 +153,7 @@ def _launch_setup(context, *args, **kwargs): }.items(), ) - # ==================================================================== - # Stage 2 – Classification (input = seg mask output) - # ==================================================================== + # Stage 2 – Classification cls_model = os.path.join(models_dir, str(cls['model_file_path'])) cls_engine = os.path.join(models_dir, str(cls['engine_file_path'])) @@ -244,9 +240,7 @@ def _launch_setup(context, *args, **kwargs): }.items(), ) - # ==================================================================== # Assemble actions - # ==================================================================== actions = [ seg_container, seg_encoder_launch, diff --git a/perception_setup/launch/pipeline_localization.launch.py b/perception_setup/launch/pipeline_localization.launch.py new file mode 100644 index 0000000..ebc0143 --- /dev/null +++ b/perception_setup/launch/pipeline_localization.launch.py @@ -0,0 +1,196 @@ +# SPDX-License-Identifier: MIT + +"""Pipeline-localization launch: Dual-camera segmentation. + +Runs two independent YOLO segmentation pipelines on different camera streams +(front + down). Each pipeline gets its own container and unique topic namespace +to avoid node/topic collisions. + +Stage 1 – Front camera segmentation + /cam/image_color → seg pipeline → /localization/front/segmentation_mask + +Stage 2 – Down camera segmentation + /cam_down/image_color → seg pipeline → /localization/down/segmentation_mask +""" + +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 _build_seg_pipeline(cfg, prefix, models_dir, encoder_dir): + """Build a complete segmentation pipeline with unique names. + + Args: + cfg: Config dict for this segmentation instance. + prefix: Unique prefix string (e.g. 'front', 'down') used to + differentiate container names, node names, and internal topics. + models_dir: Absolute path to the models directory. + encoder_dir: Package share directory for isaac_ros_dnn_image_encoder. + + Returns: + List of launch actions for this pipeline. + """ + # Unique internal topics per pipeline + converted_image_topic = f'/yolo_seg_{prefix}/internal/converted_image' + encoder_resize_topic = f'/yolo_seg_{prefix}_encoder/internal/resize/image' + tensor_output_topic = f'/yolo_seg_{prefix}/tensor_pub' + tensor_input_topic = f'/yolo_seg_{prefix}/tensor_sub' + encoder_namespace = f'yolo_seg_{prefix}_encoder/internal' + container_name = f'seg_{prefix}_tensor_rt_container' + + model_path = os.path.join(models_dir, str(cfg['model_file_path'])) + engine_path = os.path.join(models_dir, str(cfg['engine_file_path'])) + + image_format_converter = ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name=f'seg_{prefix}_image_format_converter', + parameters=[{ + 'encoding_desired': str(cfg['encoding_desired']), + 'image_width': int(cfg['input_image_width']), + 'image_height': int(cfg['input_image_height']), + 'input_qos': 'sensor_data', + }], + remappings=[ + ('image_raw', str(cfg['image_input_topic'])), + ('image', converted_image_topic), + ], + ) + + tensor_rt = ComposableNode( + name=f'seg_{prefix}_tensor_rt', + package='isaac_ros_tensor_rt', + plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode', + parameters=[{ + 'model_file_path': model_path, + 'engine_file_path': engine_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': bool(cfg['verbose']), + 'force_engine_update': bool(cfg['force_engine_update']), + 'tensor_output_topic': tensor_output_topic, + }], + remappings=[ + ('tensor_pub', tensor_output_topic), + ('tensor_sub', tensor_input_topic), + ], + ) + + decoder = ComposableNode( + name=f'seg_{prefix}_decoder', + package='isaac_ros_yolov26_seg', + plugin='nvidia::isaac_ros::yolov26_seg::YoloV26SegDecoderNode', + parameters=[{ + 'tensor_input_topic': tensor_input_topic, + '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': int(cfg['output_mask_width']), + 'output_mask_height': int(cfg['output_mask_height']), + 'detections_topic': str(cfg['detection_topic']), + 'mask_topic': str(cfg['mask_topic']), + }], + ) + + container = ComposableNodeContainer( + name=container_name, + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + image_format_converter, + tensor_rt, + decoder, + ], + output='screen', + arguments=['--ros-args', '--log-level', 'INFO'], + namespace='', + ) + + encoder_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py') + ), + launch_arguments={ + 'input_image_width': str(cfg['input_image_width']), + 'input_image_height': str(cfg['input_image_height']), + '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': container_name, + 'dnn_image_encoder_namespace': encoder_namespace, + 'image_input_topic': converted_image_topic, + 'camera_info_input_topic': str(cfg['camera_info_input_topic']), + 'tensor_output_topic': tensor_output_topic, + }.items(), + ) + + actions = [container, encoder_launch] + + if bool(cfg.get('enable_visualizer', False)): + actions.append( + Node( + package='isaac_ros_yolov26_seg', + executable='isaac_ros_yolov26_seg_visualizer.py', + name=f'seg_{prefix}_visualizer', + parameters=[{ + 'detections_topic': str(cfg['detection_topic']), + 'image_topic': encoder_resize_topic, + 'mask_topic': str(cfg['mask_topic']), + 'output_image_topic': str(cfg['visualized_image_topic']), + 'class_names_yaml': str(cfg['class_names']), + }], + ) + ) + + return actions + + +def _launch_setup(context, *args, **kwargs): + config_path = LaunchConfiguration('config_file').perform(context) + + with open(config_path) as f: + cfg = yaml.safe_load(f) + + 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') + + actions = [] + actions += _build_seg_pipeline(cfg['seg_front'], 'front', models_dir, encoder_dir) + actions += _build_seg_pipeline(cfg['seg_down'], 'down', models_dir, encoder_dir) + + return actions + + +def generate_launch_description(): + pkg_dir = get_package_share_directory('perception_setup') + default_config = os.path.join(pkg_dir, 'config', 'pipeline_localization.yaml') + + return launch.LaunchDescription([ + DeclareLaunchArgument( + 'config_file', + default_value=default_config, + description='Path to pipeline-localization config YAML', + ), + OpaqueFunction(function=_launch_setup), + ]) diff --git a/perception_setup/launch/realsense_d555.launch.py b/perception_setup/launch/realsense_d555.launch.py index 034969d..76d7384 100644 --- a/perception_setup/launch/realsense_d555.launch.py +++ b/perception_setup/launch/realsense_d555.launch.py @@ -4,22 +4,35 @@ """ import os +import yaml 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 LaunchConfiguration +from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): pkg_dir = get_package_share_directory("perception_setup") - default_config = os.path.join(pkg_dir, "config", "realsense_d555.yaml") + + default_rs_config = os.path.join(pkg_dir, "config", "realsense_d555.yaml") + camera_info_file = os.path.join( + pkg_dir, + "config", + "color_realsense_d555_calib.yaml", + ) + + with open(camera_info_file, "r") as f: + cfg = yaml.safe_load(f) + + camera_info_topic = cfg["camera_info_topic"] config_file_arg = DeclareLaunchArgument( "config_file", - default_value=default_config, + default_value=default_rs_config, description="Path to a YAML file with node-level parameter overrides for rs_launch.py", ) @@ -32,4 +45,21 @@ def generate_launch_description(): }.items(), ) - return LaunchDescription([config_file_arg, rs_launch]) + camera_info_pub = Node( + package="perception_setup", + executable="camera_info_publisher.py", + name="camera_info_publisher", + parameters=[ + { + "camera_info_file": camera_info_file, + "camera_info_topic": camera_info_topic, + } + ], + output="screen", + ) + + return LaunchDescription([ + config_file_arg, + rs_launch, + camera_info_pub, + ]) diff --git a/perception_setup/launch/yolo_cls.launch.py b/perception_setup/launch/yolo_cls.launch.py index 10ffa89..2df21ae 100644 --- a/perception_setup/launch/yolo_cls.launch.py +++ b/perception_setup/launch/yolo_cls.launch.py @@ -173,7 +173,7 @@ def _launch_setup(context, *args, **kwargs): ) tensor_rt_container = ComposableNodeContainer( - name='tensor_rt_container', + name='cls_tensor_rt_container', package='rclcpp_components', executable='component_container_mt', composable_node_descriptions=[ @@ -204,7 +204,7 @@ def _launch_setup(context, *args, **kwargs): 'image_mean': str(image_mean), 'image_stddev': str(image_stddev), 'attach_to_shared_component_container': 'True', - 'component_container_name': 'tensor_rt_container', + 'component_container_name': 'cls_tensor_rt_container', 'dnn_image_encoder_namespace': DNN_IMAGE_ENCODER_NAMESPACE, 'image_input_topic': CONVERTED_IMAGE_TOPIC, 'camera_info_input_topic': camera_info_input_topic, diff --git a/perception_setup/launch/yolo_detect.launch.py b/perception_setup/launch/yolo_detect.launch.py index 4c9ed9e..29c3d31 100644 --- a/perception_setup/launch/yolo_detect.launch.py +++ b/perception_setup/launch/yolo_detect.launch.py @@ -177,7 +177,7 @@ def _launch_setup(context, *args, **kwargs): ) tensor_rt_container = ComposableNodeContainer( - name='tensor_rt_container', + name='detect_tensor_rt_container', package='rclcpp_components', executable='component_container_mt', composable_node_descriptions=[ @@ -208,7 +208,7 @@ def _launch_setup(context, *args, **kwargs): 'image_mean': str(image_mean), 'image_stddev': str(image_stddev), 'attach_to_shared_component_container': 'True', - 'component_container_name': 'tensor_rt_container', + 'component_container_name': 'detect_tensor_rt_container', 'dnn_image_encoder_namespace': DNN_IMAGE_ENCODER_NAMESPACE, 'image_input_topic': CONVERTED_IMAGE_TOPIC, 'camera_info_input_topic': camera_info_input_topic, diff --git a/perception_setup/launch/yolo_obb.launch.py b/perception_setup/launch/yolo_obb.launch.py index daf0d9a..cfdd5da 100644 --- a/perception_setup/launch/yolo_obb.launch.py +++ b/perception_setup/launch/yolo_obb.launch.py @@ -174,7 +174,7 @@ def _launch_setup(context, *args, **kwargs): ) tensor_rt_container = ComposableNodeContainer( - name='tensor_rt_container', + name='obb_tensor_rt_container', package='rclcpp_components', executable='component_container_mt', composable_node_descriptions=[ @@ -205,7 +205,7 @@ def _launch_setup(context, *args, **kwargs): 'image_mean': str(image_mean), 'image_stddev': str(image_stddev), 'attach_to_shared_component_container': 'True', - 'component_container_name': 'tensor_rt_container', + 'component_container_name': 'obb_tensor_rt_container', 'dnn_image_encoder_namespace': DNN_IMAGE_ENCODER_NAMESPACE, 'image_input_topic': CONVERTED_IMAGE_TOPIC, 'camera_info_input_topic': camera_info_input_topic, diff --git a/perception_setup/launch/yolo_seg.launch.py b/perception_setup/launch/yolo_seg.launch.py index 1e6b5dd..b9a1423 100644 --- a/perception_setup/launch/yolo_seg.launch.py +++ b/perception_setup/launch/yolo_seg.launch.py @@ -195,7 +195,7 @@ def _launch_setup(context, *args, **kwargs): ) tensor_rt_container = ComposableNodeContainer( - name='tensor_rt_container', + name='seg_tensor_rt_container', package='rclcpp_components', executable='component_container_mt', composable_node_descriptions=[ @@ -226,7 +226,7 @@ def _launch_setup(context, *args, **kwargs): 'image_mean': str(image_mean), 'image_stddev': str(image_stddev), 'attach_to_shared_component_container': 'True', - 'component_container_name': 'tensor_rt_container', + 'component_container_name': 'seg_tensor_rt_container', 'dnn_image_encoder_namespace': DNN_IMAGE_ENCODER_NAMESPACE, 'image_input_topic': CONVERTED_IMAGE_TOPIC, 'camera_info_input_topic': camera_info_input_topic, diff --git a/perception_setup/package.xml b/perception_setup/package.xml index d572233..6c79d28 100644 --- a/perception_setup/package.xml +++ b/perception_setup/package.xml @@ -19,6 +19,7 @@ isaac_ros_yolov8 isaac_ros_yolov26_obb isaac_ros_yolov26_seg + isaac_ros_yolov26_cls isaac_ros_tensor_rt isaac_ros_dnn_image_encoder isaac_ros_image_proc From 058c24abc25cd5e5b559c284fb80317ed98bb545 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Tue, 24 Mar 2026 23:10:59 +0100 Subject: [PATCH 11/37] feat: centralize camera configurations in cameras.yaml; update launch files to reference camera settings --- perception_setup/config/cameras.yaml | 27 +++++++++++ .../config/pipeline_following.yaml | 5 +- .../config/pipeline_localization.yaml | 10 +--- perception_setup/config/yolo_detect.yaml | 8 ++-- perception_setup/config/yolo_obb.yaml | 8 ++-- perception_setup/config/yolo_seg.yaml | 8 ++-- perception_setup/launch/yolo_cls.launch.py | 42 +++++----------- perception_setup/launch/yolo_seg.launch.py | 48 +++++-------------- 8 files changed, 61 insertions(+), 95 deletions(-) create mode 100644 perception_setup/config/cameras.yaml diff --git a/perception_setup/config/cameras.yaml b/perception_setup/config/cameras.yaml new file mode 100644 index 0000000..5112ae8 --- /dev/null +++ b/perception_setup/config/cameras.yaml @@ -0,0 +1,27 @@ +# Centralized camera definitions +# Referenced by pipeline configs via "camera: " +# Fields: image_topic, camera_info_topic, image_width, image_height + +cam_down: + image_topic: "/cam_down/image_color" + camera_info_topic: "/cam_down/camera_info" + image_width: 1440 + image_height: 1080 + +cam_front: + image_topic: "/cam/image_color" + camera_info_topic: "/cam/camera_info" + image_width: 1440 + image_height: 1080 + +zed_left: + image_topic: "/zed_node/left/image_rect_color" + camera_info_topic: "/zed_node/depth/camera_info" + image_width: 1280 + image_height: 720 + +realsense_d555: + image_topic: "/camera/camera/color/image_raw" + camera_info_topic: "/camera/camera/color/camera_info" + image_width: 896 + image_height: 504 diff --git a/perception_setup/config/pipeline_following.yaml b/perception_setup/config/pipeline_following.yaml index 3356e76..2c2098b 100644 --- a/perception_setup/config/pipeline_following.yaml +++ b/perception_setup/config/pipeline_following.yaml @@ -3,8 +3,7 @@ # Stage 2: Classifies the mask image to determine if the pipeline continues or ends. seg: - image_input_topic: "/cam_down/image_color" - camera_info_input_topic: "/cam_down/camera_info" + camera: "cam_down" visualized_image_topic: "/pipeline/camera/segmentation_debug" detection_topic: "/pipeline/camera/bboxes" mask_topic: "/pipeline/camera/segmentation_mask" @@ -16,8 +15,6 @@ seg: class_names: 0: "pipeline" - input_image_width: 1440 - input_image_height: 1080 encoding_desired: "rgb8" output_mask_width: 640 # Set to 640 to match the network input size, which allows you to use its camera info directly diff --git a/perception_setup/config/pipeline_localization.yaml b/perception_setup/config/pipeline_localization.yaml index 0b71497..03ea9d7 100644 --- a/perception_setup/config/pipeline_localization.yaml +++ b/perception_setup/config/pipeline_localization.yaml @@ -2,8 +2,7 @@ # Runs two segmentation pipelines on different camera streams. seg_front: - image_input_topic: "/cam/image_color" - camera_info_input_topic: "/cam/camera_info" + camera: "cam_front" visualized_image_topic: "/localization/front/segmentation_debug" detection_topic: "/localization/front/detections" mask_topic: "/localization/front/segmentation_mask" @@ -15,8 +14,6 @@ seg_front: class_names: 0: "pipeline" - input_image_width: 1440 - input_image_height: 1080 encoding_desired: "rgb8" output_mask_width: 640 @@ -46,8 +43,7 @@ seg_front: num_protos: 32 seg_down: - image_input_topic: "/cam_down/image_color" - camera_info_input_topic: "/cam_down/camera_info" + camera: "cam_down" visualized_image_topic: "/localization/down/segmentation_debug" detection_topic: "/localization/down/detections" mask_topic: "/localization/down/segmentation_mask" @@ -59,8 +55,6 @@ seg_down: class_names: 0: "pipeline" - input_image_width: 1440 - input_image_height: 1080 encoding_desired: "rgb8" output_mask_width: 640 diff --git a/perception_setup/config/yolo_detect.yaml b/perception_setup/config/yolo_detect.yaml index 26deee3..9228ad8 100644 --- a/perception_setup/config/yolo_detect.yaml +++ b/perception_setup/config/yolo_detect.yaml @@ -1,6 +1,7 @@ +# Camera source (resolved from cameras.yaml) +camera: "zed_left" + # Topics -image_input_topic: "/zed_node/left/image_rect_color" -camera_info_input_topic: "/zed_node/depth/camera_info" visualized_image_topic: "/yolov8_processed_image" # Output topic for visualized images with detection overlays detection_topic: "/yolov8_detections_output" # Output topic for detection results (Detection2DArray) @@ -13,9 +14,6 @@ enable_visualizer: true class_names: 0: "valve" -# Input image -input_image_width: 1280 -input_image_height: 720 encoding_desired: "rgb8" # TensorRT diff --git a/perception_setup/config/yolo_obb.yaml b/perception_setup/config/yolo_obb.yaml index 6caa900..641c2cc 100644 --- a/perception_setup/config/yolo_obb.yaml +++ b/perception_setup/config/yolo_obb.yaml @@ -1,6 +1,7 @@ +# Camera source (resolved from cameras.yaml) +camera: "realsense_d555" + # Topics -image_input_topic: "/camera/camera/color/image_raw" -camera_info_input_topic: "/camera/camera/color/camera_info" visualized_image_topic: "/yolo_obb_processed_image" detection_topic: "/obb_detections_output" @@ -13,9 +14,6 @@ enable_visualizer: true class_names: 0: "valve" -# Input image dimensions -input_image_width: 896 -input_image_height: 504 encoding_desired: "rgb8" # TensorRT diff --git a/perception_setup/config/yolo_seg.yaml b/perception_setup/config/yolo_seg.yaml index 73a0bd1..a88a663 100644 --- a/perception_setup/config/yolo_seg.yaml +++ b/perception_setup/config/yolo_seg.yaml @@ -1,6 +1,7 @@ +# Camera source (resolved from cameras.yaml) +camera: "cam_down" + # Topics -image_input_topic: "/cam_down/image_color" -camera_info_input_topic: "/cam_down/camera_info" visualized_image_topic: "/yolo_seg_processed_image" detection_topic: "/seg_detections_output" mask_topic: "/yolo_seg_mask" @@ -14,9 +15,6 @@ enable_visualizer: true class_names: 0: "pipeline" -# Input image dimensions -input_image_width: 1440 -input_image_height: 1080 encoding_desired: "rgb8" # Output mask resolution (upscaled from 160x160 via bilinear interpolation) diff --git a/perception_setup/launch/yolo_cls.launch.py b/perception_setup/launch/yolo_cls.launch.py index 2df21ae..89ea3ee 100644 --- a/perception_setup/launch/yolo_cls.launch.py +++ b/perception_setup/launch/yolo_cls.launch.py @@ -52,38 +52,18 @@ def _launch_setup(context, *args, **kwargs): with open(config_path) as f: cfg = yaml.safe_load(f) - required_keys = [ - 'model_file_path', - 'engine_file_path', - 'input_tensor_names', - 'input_binding_names', - 'output_tensor_names', - 'output_binding_names', - 'verbose', - 'force_engine_update', - 'input_image_width', - 'input_image_height', - 'encoding_desired', - 'network_image_width', - 'network_image_height', - 'image_mean', - 'image_stddev', - 'confidence_threshold', - 'num_classes', - 'class_topic', - 'image_input_topic', - 'camera_info_input_topic', - 'enable_visualizer', - 'visualized_image_topic', - 'class_names', - ] - - for key in required_keys: - if key not in cfg: - raise RuntimeError(f"Missing required config key: '{key}'") - - # Resolve model paths relative to perception_setup/models/ pkg_dir = get_package_share_directory('perception_setup') + + # Resolve camera reference from cameras.yaml + if 'camera' in cfg: + cameras_path = os.path.join(pkg_dir, 'config', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras[cfg['camera']] + cfg['image_input_topic'] = cam['image_topic'] + cfg['camera_info_input_topic'] = cam['camera_info_topic'] + cfg['input_image_width'] = cam['image_width'] + cfg['input_image_height'] = cam['image_height'] models_dir = os.path.join(pkg_dir, 'models') model_file_path = os.path.join(models_dir, str(cfg['model_file_path'])) diff --git a/perception_setup/launch/yolo_seg.launch.py b/perception_setup/launch/yolo_seg.launch.py index b9a1423..a470886 100644 --- a/perception_setup/launch/yolo_seg.launch.py +++ b/perception_setup/launch/yolo_seg.launch.py @@ -60,44 +60,18 @@ def _launch_setup(context, *args, **kwargs): with open(config_path, 'r') as f: cfg = yaml.safe_load(f) - required_keys = [ - 'model_file_path', - 'engine_file_path', - 'input_tensor_names', - 'input_binding_names', - 'output_tensor_names', - 'output_binding_names', - 'verbose', - 'force_engine_update', - 'input_image_width', - 'input_image_height', - 'encoding_desired', - 'network_image_width', - 'network_image_height', - 'image_mean', - 'image_stddev', - 'confidence_threshold', - 'num_detections', - 'mask_width', - 'mask_height', - 'num_protos', - 'output_mask_width', - 'output_mask_height', - 'detection_topic', - 'mask_topic', - 'image_input_topic', - 'camera_info_input_topic', - 'enable_visualizer', - 'visualized_image_topic', - 'class_names', - ] - - for key in required_keys: - if key not in cfg: - raise RuntimeError(f"Missing required config key: '{key}'") - - # Resolve model paths relative to perception_setup/models/ pkg_dir = get_package_share_directory('perception_setup') + + # Resolve camera reference from cameras.yaml + if 'camera' in cfg: + cameras_path = os.path.join(pkg_dir, 'config', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras[cfg['camera']] + cfg['image_input_topic'] = cam['image_topic'] + cfg['camera_info_input_topic'] = cam['camera_info_topic'] + cfg['input_image_width'] = cam['image_width'] + cfg['input_image_height'] = cam['image_height'] models_dir = os.path.join(pkg_dir, 'models') model_file_path = os.path.join(models_dir, str(cfg['model_file_path'])) From f7ea0eafb049f5f82e37bccbde95c707f5983238 Mon Sep 17 00:00:00 2001 From: vortexuser Date: Thu, 26 Mar 2026 14:44:55 +0100 Subject: [PATCH 12/37] Add YOLO launch files and image processing scripts --- perception_setup/CMakeLists.txt | 2 + perception_setup/config/cameras.yaml | 27 --- .../config/cameras/blackfly_s_params.yaml | 155 +++++++++++++++ perception_setup/config/cameras/cameras.yaml | 9 + .../color_realsense_d555_calib.yaml | 16 +- .../config/cameras/downwards_cam_calib.yaml | 31 +++ .../config/cameras/downwards_cam_params.yaml | 35 ++++ .../config/{ => cameras}/realsense_d555.yaml | 0 .../config/image_processing/image_crop.yaml | 16 ++ .../image_processing/image_undistort.yaml | 4 + .../config/pipeline_following.yaml | 8 +- .../config/pipeline_localization.yaml | 8 +- .../config/{ => yolo}/yolo_cls.yaml | 6 +- .../config/{ => yolo}/yolo_detect.yaml | 4 +- .../config/{ => yolo}/yolo_obb.yaml | 2 +- .../config/{ => yolo}/yolo_seg.yaml | 4 +- .../helpers/camera_info_publisher.launch.py | 39 ++++ .../launch/helpers/image_crop.launch.py | 37 ++++ .../launch/helpers/image_undistort.launch.py | 35 ++++ .../launch/pipeline_following.launch.py | 179 ++++++++++-------- .../launch/pipeline_localization.launch.py | 115 ++++++----- .../launch/realsense_d555.launch.py | 50 +++-- .../launch/{ => yolo}/yolo_cls.launch.py | 4 +- .../launch/{ => yolo}/yolo_detect.launch.py | 15 +- .../launch/{ => yolo}/yolo_obb.launch.py | 16 +- .../launch/{ => yolo}/yolo_seg.launch.py | 127 +++++++------ .../scripts/camera_info_publisher.py | 26 ++- perception_setup/scripts/image_crop.py | 108 +++++++++++ perception_setup/scripts/image_undistort.py | 113 +++++++++++ scripts/pt_to_onnx.py | 9 + 30 files changed, 936 insertions(+), 264 deletions(-) delete mode 100644 perception_setup/config/cameras.yaml create mode 100644 perception_setup/config/cameras/blackfly_s_params.yaml create mode 100644 perception_setup/config/cameras/cameras.yaml rename perception_setup/config/{ => cameras}/color_realsense_d555_calib.yaml (52%) create mode 100644 perception_setup/config/cameras/downwards_cam_calib.yaml create mode 100644 perception_setup/config/cameras/downwards_cam_params.yaml rename perception_setup/config/{ => cameras}/realsense_d555.yaml (100%) create mode 100644 perception_setup/config/image_processing/image_crop.yaml create mode 100644 perception_setup/config/image_processing/image_undistort.yaml rename perception_setup/config/{ => yolo}/yolo_cls.yaml (86%) rename perception_setup/config/{ => yolo}/yolo_detect.yaml (87%) rename perception_setup/config/{ => yolo}/yolo_obb.yaml (89%) rename perception_setup/config/{ => yolo}/yolo_seg.yaml (91%) create mode 100644 perception_setup/launch/helpers/camera_info_publisher.launch.py create mode 100644 perception_setup/launch/helpers/image_crop.launch.py create mode 100644 perception_setup/launch/helpers/image_undistort.launch.py rename perception_setup/launch/{ => yolo}/yolo_cls.launch.py (98%) rename perception_setup/launch/{ => yolo}/yolo_detect.launch.py (92%) rename perception_setup/launch/{ => yolo}/yolo_obb.launch.py (93%) rename perception_setup/launch/{ => yolo}/yolo_seg.launch.py (72%) create mode 100755 perception_setup/scripts/image_crop.py create mode 100755 perception_setup/scripts/image_undistort.py create mode 100755 scripts/pt_to_onnx.py diff --git a/perception_setup/CMakeLists.txt b/perception_setup/CMakeLists.txt index 45eaf5f..317b0e4 100644 --- a/perception_setup/CMakeLists.txt +++ b/perception_setup/CMakeLists.txt @@ -5,6 +5,8 @@ find_package(ament_cmake REQUIRED) install(PROGRAMS scripts/camera_info_publisher.py + scripts/image_undistort.py + scripts/image_crop.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/perception_setup/config/cameras.yaml b/perception_setup/config/cameras.yaml deleted file mode 100644 index 5112ae8..0000000 --- a/perception_setup/config/cameras.yaml +++ /dev/null @@ -1,27 +0,0 @@ -# Centralized camera definitions -# Referenced by pipeline configs via "camera: " -# Fields: image_topic, camera_info_topic, image_width, image_height - -cam_down: - image_topic: "/cam_down/image_color" - camera_info_topic: "/cam_down/camera_info" - image_width: 1440 - image_height: 1080 - -cam_front: - image_topic: "/cam/image_color" - camera_info_topic: "/cam/camera_info" - image_width: 1440 - image_height: 1080 - -zed_left: - image_topic: "/zed_node/left/image_rect_color" - camera_info_topic: "/zed_node/depth/camera_info" - image_width: 1280 - image_height: 720 - -realsense_d555: - image_topic: "/camera/camera/color/image_raw" - camera_info_topic: "/camera/camera/color/camera_info" - image_width: 896 - image_height: 504 diff --git a/perception_setup/config/cameras/blackfly_s_params.yaml b/perception_setup/config/cameras/blackfly_s_params.yaml new file mode 100644 index 0000000..0e75d96 --- /dev/null +++ b/perception_setup/config/cameras/blackfly_s_params.yaml @@ -0,0 +1,155 @@ +# +# config file for blackfly S cameras (USB3 and GigE) +# +# This file maps the ros parameters to the corresponding Spinnaker "nodes" in the camera. +# For more details on how to modify this file, see the README on camera configuration files. +parameters: + # + # -------- image format control + # + - name: pixel_format + type: enum + # Check available values with SpinView. Not all are supported by ROS! + # Some formats are e.g. "Mono8", "BayerRG8", "BGR8", "BayerRG16" + # default is "BayerRG8" + node: ImageFormatControl/PixelFormat + - name: binning_x + type: int + node: ImageFormatControl/BinningHorizontal + - name: binning_y + type: int + node: ImageFormatControl/BinningVertical + - name: image_width + type: int + node: ImageFormatControl/Width + - name: image_height + type: int + node: ImageFormatControl/Height + - name: offset_x # offset must come after image width reduction! + type: int + node: ImageFormatControl/OffsetX + - name: offset_y + type: int + node: ImageFormatControl/OffsetY + # + # -------- analog control + # + - name: gain_auto + type: enum + # valid values are "Continuous", "Off" + node: AnalogControl/GainAuto + - name: gain + type: float + node: AnalogControl/Gain + # + # -------- device link throughput limiting + # + - name: device_link_throughput_limit + type: int + node: DeviceControl/DeviceLinkThroughputLimit + # + # -------- transport layer control (GigE) + # + - name: gev_scps_packet_size + type: int + # default is 1400. Set to 9000 to enable jumbo frames, ensure NIC MTU set >= 9000 + node: TransportLayerControl/GigEVision/GevSCPSPacketSize + # + # -------- digital IO control + # + - name: line0_selector # black wire: opto-isolated input + type: enum + node: DigitalIOControl/LineSelector + - name: line1_selector # white wire: opto-isolated output + type: enum + node: DigitalIOControl/LineSelector + - name: line1_linemode # valid values: "Input", "Output" + type: enum + node: DigitalIOControl/LineMode + - name: line2_selector # red wire: non-isolated input/output + type: enum + node: DigitalIOControl/LineSelector + - name: line2_v33enable # red wire: 3.3V power + type: bool + node: DigitalIOControl/V3_3Enable + - name: line3_selector # green wire: aux voltage input and non-isolated input + type: enum + node: DigitalIOControl/LineSelector + - name: line3_linemode # valid values: "Input", "Output" + type: enum + node: DigitalIOControl/LineMode + # + # -------- acquisition control + # + - name: exposure_auto + type: enum + # valid values are "Off", "Continuous" + node: AcquisitionControl/ExposureAuto + - name: exposure_time + type: float + node: AcquisitionControl/ExposureTime + - name: frame_rate_enable + type: bool + node: AcquisitionControl/AcquisitionFrameRateEnable + - name: frame_rate + type: float + node: AcquisitionControl/AcquisitionFrameRate + - name: trigger_selector + type: enum + # valid values are e.g. "FrameStart", "AcquisitionStart", "FrameBurstStart" + node: AcquisitionControl/TriggerSelector + - name: trigger_mode + type: enum + # valid values are "On" and "Off" + node: AcquisitionControl/TriggerMode + - name: trigger_source + type: enum + # valid values are "Line<0,1,2>", "UserOutput<0,1,2>", "Counter<0,1>", + # "LogicBlock<0,1> + node: AcquisitionControl/TriggerSource + - name: trigger_delay + # value >= 9 + type: float + node: AcquisitionControl/TriggerDelay + - name: trigger_overlap + type: enum + # valid values: "Off" and "ReadOut" + node: AcquisitionControl/TriggerOverlap + - name: trigger_activation + type: enum + # valid values: "LevelLow", "LevelHigh", "FallingEdge", "RisingEdge", "AnyEdge", + node: AcquisitionControl/TriggerActivation + # + # --------- chunk control + # + - name: chunk_mode_active + type: bool + node: ChunkDataControl/ChunkModeActive + - name: chunk_selector_frame_id + type: enum + # valid values: "FrameID" + node: ChunkDataControl/ChunkSelector + - name: chunk_enable_frame_id + type: bool + node: ChunkDataControl/ChunkEnable + - name: chunk_selector_exposure_time + type: enum + # valid values: "ExposureTime" + node: ChunkDataControl/ChunkSelector + - name: chunk_enable_exposure_time + type: bool + node: ChunkDataControl/ChunkEnable + - name: chunk_selector_gain + type: enum + # valid values: "Gain" + node: ChunkDataControl/ChunkSelector + - name: chunk_enable_gain + type: bool + node: ChunkDataControl/ChunkEnable + - name: chunk_selector_timestamp + type: enum + # valid values: "Timestamp" + node: ChunkDataControl/ChunkSelector + - name: chunk_enable_timestamp + type: bool + node: ChunkDataControl/ChunkEnable diff --git a/perception_setup/config/cameras/cameras.yaml b/perception_setup/config/cameras/cameras.yaml new file mode 100644 index 0000000..7be25ca --- /dev/null +++ b/perception_setup/config/cameras/cameras.yaml @@ -0,0 +1,9 @@ +# Centralized camera definitions +# Referenced by pipeline configs via "camera: " +# Fields: image_topic, camera_info_topic, image_width, image_height + +realsense_d555: + image_topic: "/camera/camera/color/image_raw" + camera_info_topic: "/camera/camera/color/camera_info_rect" + image_width: 896 + image_height: 504 diff --git a/perception_setup/config/color_realsense_d555_calib.yaml b/perception_setup/config/cameras/color_realsense_d555_calib.yaml similarity index 52% rename from perception_setup/config/color_realsense_d555_calib.yaml rename to perception_setup/config/cameras/color_realsense_d555_calib.yaml index 1e36256..4df1c1c 100644 --- a/perception_setup/config/color_realsense_d555_calib.yaml +++ b/perception_setup/config/cameras/color_realsense_d555_calib.yaml @@ -1,14 +1,14 @@ -camera_info_topic: "/camera/camera/color/camera_info" +camera_info_topic: "/camera/camera/color_raw/camera_info" -image_width: 1280 -image_height: 800 +image_width: 896 +image_height: 504 camera_name: color_camera camera_matrix: rows: 3 cols: 3 - data: [1042.00796, 0.0, 741.55948, - 0.0, 1039.05486, 569.96599, + data: [687.47, 0.0, 410.31, + 0.0, 572.95, 252.74, 0.0, 0.0, 1.0] distortion_model: plumb_bob @@ -16,7 +16,7 @@ distortion_model: plumb_bob distortion_coefficients: rows: 1 cols: 5 - data: [-0.360444, 0.191541, 0.002454, -0.000152, -0.064295] + data: [0.268120, 0.659922, 0.004430, -0.024228, -0.353035] rectification_matrix: rows: 3 @@ -28,6 +28,6 @@ rectification_matrix: projection_matrix: rows: 3 cols: 4 - data: [1042.00796, 0.0, 741.55948, 0.0, - 0.0, 1039.05486, 569.96599, 0.0, + 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/cameras/downwards_cam_calib.yaml b/perception_setup/config/cameras/downwards_cam_calib.yaml new file mode 100644 index 0000000..a30e05e --- /dev/null +++ b/perception_setup/config/cameras/downwards_cam_calib.yaml @@ -0,0 +1,31 @@ +image_width: 1440 +image_height: 1080 +camera_name: "downwards_cam" + +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/cameras/downwards_cam_params.yaml b/perception_setup/config/cameras/downwards_cam_params.yaml new file mode 100644 index 0000000..4f2f522 --- /dev/null +++ b/perception_setup/config/cameras/downwards_cam_params.yaml @@ -0,0 +1,35 @@ +/**: + 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': 'BGR8' + '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/realsense_d555.yaml b/perception_setup/config/cameras/realsense_d555.yaml similarity index 100% rename from perception_setup/config/realsense_d555.yaml rename to perception_setup/config/cameras/realsense_d555.yaml diff --git a/perception_setup/config/image_processing/image_crop.yaml b/perception_setup/config/image_processing/image_crop.yaml new file mode 100644 index 0000000..4768a3a --- /dev/null +++ b/perception_setup/config/image_processing/image_crop.yaml @@ -0,0 +1,16 @@ +# Image crop configuration +# x_offset / y_offset: top-left corner of the crop region (pixels) +# width / height: size of the crop region (pixels) +# Set to 0 to use the full image dimension. + +image_topic: "/camera/camera/depth/image_rect_raw" +camera_info_topic: "/camera/camera/depth/camera_info" + +output_image_topic: "/front_cam/depth/image_rect_raw/cropped" +output_camera_info_topic: "/front_cam/depth/camera_info/cropped" + +crop: + x_offset: 260 # x_left boundary + y_offset: 190 # y_upper boundary + width: 485 # x_right (730) - x_left (245) + height: 245 # y_lower (435) - y_upper (190) diff --git a/perception_setup/config/image_processing/image_undistort.yaml b/perception_setup/config/image_processing/image_undistort.yaml new file mode 100644 index 0000000..bf7f509 --- /dev/null +++ b/perception_setup/config/image_processing/image_undistort.yaml @@ -0,0 +1,4 @@ +image_topic: "/camera/camera/color/image_raw" +camera_info_topic: "/camera/camera/color_raw/camera_info" +output_image_topic: "/front_cam/image_undistorted" +output_camera_info_topic: "/front_cam/camera_info_undistorted" diff --git a/perception_setup/config/pipeline_following.yaml b/perception_setup/config/pipeline_following.yaml index 2c2098b..b395265 100644 --- a/perception_setup/config/pipeline_following.yaml +++ b/perception_setup/config/pipeline_following.yaml @@ -3,7 +3,7 @@ # Stage 2: Classifies the mask image to determine if the pipeline continues or ends. seg: - camera: "cam_down" + camera: "realsense_d555" visualized_image_topic: "/pipeline/camera/segmentation_debug" detection_topic: "/pipeline/camera/bboxes" mask_topic: "/pipeline/camera/segmentation_mask" @@ -24,7 +24,7 @@ seg: force_engine_update: false # ============================================================================================================================================================== - # NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model + # NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model # ============================================================================================================================================================== input_tensor_names: ["input_tensor"] @@ -46,7 +46,7 @@ seg: cls: # Input is the segmentation mask output from stage 1 image_input_topic: "/pipeline/camera/segmentation_mask" - camera_info_input_topic: "/cam_down/camera_info" + camera_info_input_topic: "/camera/camera/color/camera_info_rect" visualized_image_topic: "/pipeline/camera/classification_debug" class_topic: "/pipeline/camera/classification" @@ -67,7 +67,7 @@ cls: force_engine_update: false # ============================================================================================================================================================== - # NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model + # NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model # ============================================================================================================================================================== input_tensor_names: ["input_tensor"] diff --git a/perception_setup/config/pipeline_localization.yaml b/perception_setup/config/pipeline_localization.yaml index 03ea9d7..1ba1477 100644 --- a/perception_setup/config/pipeline_localization.yaml +++ b/perception_setup/config/pipeline_localization.yaml @@ -2,7 +2,7 @@ # Runs two segmentation pipelines on different camera streams. seg_front: - camera: "cam_front" + camera: "realsense_d555" visualized_image_topic: "/localization/front/segmentation_debug" detection_topic: "/localization/front/detections" mask_topic: "/localization/front/segmentation_mask" @@ -23,7 +23,7 @@ seg_front: force_engine_update: false # ============================================================================================================================================================== - # NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different segmentation model + # NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different segmentation model # ============================================================================================================================================================== input_tensor_names: ["input_tensor"] @@ -43,7 +43,7 @@ seg_front: num_protos: 32 seg_down: - camera: "cam_down" + camera: "realsense_d555" visualized_image_topic: "/localization/down/segmentation_debug" detection_topic: "/localization/down/detections" mask_topic: "/localization/down/segmentation_mask" @@ -64,7 +64,7 @@ seg_down: force_engine_update: false # ============================================================================================================================================================== - # NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model + # NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model # ============================================================================================================================================================== input_tensor_names: ["input_tensor"] diff --git a/perception_setup/config/yolo_cls.yaml b/perception_setup/config/yolo/yolo_cls.yaml similarity index 86% rename from perception_setup/config/yolo_cls.yaml rename to perception_setup/config/yolo/yolo_cls.yaml index 54c8b61..4fa15b2 100644 --- a/perception_setup/config/yolo_cls.yaml +++ b/perception_setup/config/yolo/yolo_cls.yaml @@ -1,6 +1,6 @@ # Topics image_input_topic: "/yolo_seg_mask" -camera_info_input_topic: "/cam_down/camera_info" +camera_info_input_topic: "/camera/camera/color/camera_info_rect" visualized_image_topic: "/yolo_cls_processed_image" class_topic: "/classification_output" @@ -20,13 +20,13 @@ class_names: input_image_width: 640 input_image_height: 640 encoding_desired: "rgb8" - + # TensorRT verbose: true force_engine_update: false # ============================================================================================================================================================== -# NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model +# NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model # ============================================================================================================================================================== # Tensor I/O bindings diff --git a/perception_setup/config/yolo_detect.yaml b/perception_setup/config/yolo/yolo_detect.yaml similarity index 87% rename from perception_setup/config/yolo_detect.yaml rename to perception_setup/config/yolo/yolo_detect.yaml index 9228ad8..a977d98 100644 --- a/perception_setup/config/yolo_detect.yaml +++ b/perception_setup/config/yolo/yolo_detect.yaml @@ -1,5 +1,5 @@ # Camera source (resolved from cameras.yaml) -camera: "zed_left" +camera: "realsense_d555" # Topics visualized_image_topic: "/yolov8_processed_image" # Output topic for visualized images with detection overlays @@ -21,7 +21,7 @@ verbose: true force_engine_update: false # ============================================================================================================================================================== -# NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model +# NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model # ============================================================================================================================================================== # Tensor I/O bindings diff --git a/perception_setup/config/yolo_obb.yaml b/perception_setup/config/yolo/yolo_obb.yaml similarity index 89% rename from perception_setup/config/yolo_obb.yaml rename to perception_setup/config/yolo/yolo_obb.yaml index 641c2cc..164b071 100644 --- a/perception_setup/config/yolo_obb.yaml +++ b/perception_setup/config/yolo/yolo_obb.yaml @@ -21,7 +21,7 @@ verbose: true force_engine_update: false # ============================================================================================================================================================== -# NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model +# NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model # ============================================================================================================================================================== # Tensor I/O bindings diff --git a/perception_setup/config/yolo_seg.yaml b/perception_setup/config/yolo/yolo_seg.yaml similarity index 91% rename from perception_setup/config/yolo_seg.yaml rename to perception_setup/config/yolo/yolo_seg.yaml index a88a663..ec77c6b 100644 --- a/perception_setup/config/yolo_seg.yaml +++ b/perception_setup/config/yolo/yolo_seg.yaml @@ -1,5 +1,5 @@ # Camera source (resolved from cameras.yaml) -camera: "cam_down" +camera: "realsense_d555" # Topics visualized_image_topic: "/yolo_seg_processed_image" @@ -26,7 +26,7 @@ verbose: true force_engine_update: false # ============================================================================================================================================================== -# NOTE: Below wont change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model +# NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model # ============================================================================================================================================================== # Tensor I/O bindings diff --git a/perception_setup/launch/helpers/camera_info_publisher.launch.py b/perception_setup/launch/helpers/camera_info_publisher.launch.py new file mode 100644 index 0000000..e4b9fdc --- /dev/null +++ b/perception_setup/launch/helpers/camera_info_publisher.launch.py @@ -0,0 +1,39 @@ +"""Launch file that publishes camera info without starting the RealSense camera.""" + +import os + +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + pkg_dir = get_package_share_directory("perception_setup") + + camera_info_file = os.path.join( + pkg_dir, + "config", + "cameras", + "color_realsense_d555_calib.yaml", + ) + + with open(camera_info_file) as f: + cfg = yaml.safe_load(f) + + camera_info_topic = cfg["camera_info_topic"] + + camera_info_pub = Node( + package="perception_setup", + executable="camera_info_publisher.py", + name="camera_info_publisher", + parameters=[ + { + "camera_info_file": camera_info_file, + "camera_info_topic": camera_info_topic, + } + ], + output="screen", + ) + + return LaunchDescription([camera_info_pub]) diff --git a/perception_setup/launch/helpers/image_crop.launch.py b/perception_setup/launch/helpers/image_crop.launch.py new file mode 100644 index 0000000..c76864c --- /dev/null +++ b/perception_setup/launch/helpers/image_crop.launch.py @@ -0,0 +1,37 @@ +import os + +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + pkg_dir = get_package_share_directory("perception_setup") + config_file = os.path.join(pkg_dir, "config", "image_processing", "image_crop.yaml") + + with open(config_file) as f: + cfg = yaml.safe_load(f) + + params = { + "image_topic": cfg["image_topic"], + "camera_info_topic": cfg["camera_info_topic"], + "output_image_topic": cfg["output_image_topic"], + "output_camera_info_topic": cfg["output_camera_info_topic"], + "crop.x_offset": int(cfg["crop"]["x_offset"]), + "crop.y_offset": int(cfg["crop"]["y_offset"]), + "crop.width": int(cfg["crop"]["width"]), + "crop.height": int(cfg["crop"]["height"]), + } + + return LaunchDescription( + [ + Node( + package="perception_setup", + executable="image_crop.py", + name="image_crop", + parameters=[params], + output="screen", + ) + ] + ) diff --git a/perception_setup/launch/helpers/image_undistort.launch.py b/perception_setup/launch/helpers/image_undistort.launch.py new file mode 100644 index 0000000..492822c --- /dev/null +++ b/perception_setup/launch/helpers/image_undistort.launch.py @@ -0,0 +1,35 @@ +import os + +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + pkg_dir = get_package_share_directory("perception_setup") + config_file = os.path.join( + pkg_dir, "config", "image_processing", "image_undistort.yaml" + ) + + with open(config_file) as f: + cfg = yaml.safe_load(f) + + return LaunchDescription( + [ + Node( + package="perception_setup", + executable="image_undistort.py", + name="image_undistort", + parameters=[ + { + "image_topic": cfg["image_topic"], + "camera_info_topic": cfg["camera_info_topic"], + "output_image_topic": cfg["output_image_topic"], + "output_camera_info_topic": cfg["output_camera_info_topic"], + } + ], + output="screen", + ) + ] + ) diff --git a/perception_setup/launch/pipeline_following.launch.py b/perception_setup/launch/pipeline_following.launch.py index 9674630..8503014 100644 --- a/perception_setup/launch/pipeline_following.launch.py +++ b/perception_setup/launch/pipeline_following.launch.py @@ -56,6 +56,17 @@ def _launch_setup(context, *args, **kwargs): pkg_dir = get_package_share_directory('perception_setup') models_dir = os.path.join(pkg_dir, 'models') + + # Resolve camera reference from cameras.yaml for seg stage + if 'camera' in seg: + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras[seg['camera']] + seg['image_input_topic'] = cam['image_topic'] + seg['camera_info_input_topic'] = cam['camera_info_topic'] + seg['input_image_width'] = cam['image_width'] + seg['input_image_height'] = cam['image_height'] encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder') # Stage 1 – Segmentation @@ -66,12 +77,14 @@ def _launch_setup(context, *args, **kwargs): package='isaac_ros_image_proc', plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', name='seg_image_format_converter', - parameters=[{ - 'encoding_desired': str(seg['encoding_desired']), - 'image_width': int(seg['input_image_width']), - 'image_height': int(seg['input_image_height']), - 'input_qos': 'sensor_data', - }], + parameters=[ + { + 'encoding_desired': str(seg['encoding_desired']), + 'image_width': int(seg['input_image_width']), + 'image_height': int(seg['input_image_height']), + 'input_qos': 'sensor_data', + } + ], remappings=[ ('image_raw', str(seg['image_input_topic'])), ('image', SEG_CONVERTED_IMAGE_TOPIC), @@ -82,17 +95,19 @@ def _launch_setup(context, *args, **kwargs): name='seg_tensor_rt', package='isaac_ros_tensor_rt', plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode', - parameters=[{ - 'model_file_path': seg_model, - 'engine_file_path': seg_engine, - 'output_binding_names': seg['output_binding_names'], - 'output_tensor_names': seg['output_tensor_names'], - 'input_tensor_names': seg['input_tensor_names'], - 'input_binding_names': seg['input_binding_names'], - 'verbose': bool(seg['verbose']), - 'force_engine_update': bool(seg['force_engine_update']), - 'tensor_output_topic': SEG_TENSOR_OUTPUT_TOPIC, - }], + parameters=[ + { + 'model_file_path': seg_model, + 'engine_file_path': seg_engine, + 'output_binding_names': seg['output_binding_names'], + 'output_tensor_names': seg['output_tensor_names'], + 'input_tensor_names': seg['input_tensor_names'], + 'input_binding_names': seg['input_binding_names'], + 'verbose': bool(seg['verbose']), + 'force_engine_update': bool(seg['force_engine_update']), + 'tensor_output_topic': SEG_TENSOR_OUTPUT_TOPIC, + } + ], remappings=[ ('tensor_pub', SEG_TENSOR_OUTPUT_TOPIC), ('tensor_sub', SEG_TENSOR_INPUT_TOPIC), @@ -103,20 +118,22 @@ def _launch_setup(context, *args, **kwargs): name='seg_decoder', package='isaac_ros_yolov26_seg', plugin='nvidia::isaac_ros::yolov26_seg::YoloV26SegDecoderNode', - parameters=[{ - 'tensor_input_topic': SEG_TENSOR_INPUT_TOPIC, - 'confidence_threshold': float(seg['confidence_threshold']), - 'num_detections': int(seg['num_detections']), - 'mask_width': int(seg['mask_width']), - 'mask_height': int(seg['mask_height']), - 'num_protos': int(seg['num_protos']), - 'network_image_width': int(seg['network_image_width']), - 'network_image_height': int(seg['network_image_height']), - 'output_mask_width': int(seg['output_mask_width']), - 'output_mask_height': int(seg['output_mask_height']), - 'detections_topic': str(seg['detection_topic']), - 'mask_topic': str(seg['mask_topic']), - }], + parameters=[ + { + 'tensor_input_topic': SEG_TENSOR_INPUT_TOPIC, + 'confidence_threshold': float(seg['confidence_threshold']), + 'num_detections': int(seg['num_detections']), + 'mask_width': int(seg['mask_width']), + 'mask_height': int(seg['mask_height']), + 'num_protos': int(seg['num_protos']), + 'network_image_width': int(seg['network_image_width']), + 'network_image_height': int(seg['network_image_height']), + 'output_mask_width': int(seg['output_mask_width']), + 'output_mask_height': int(seg['output_mask_height']), + 'detections_topic': str(seg['detection_topic']), + 'mask_topic': str(seg['mask_topic']), + } + ], ) seg_container = ComposableNodeContainer( @@ -161,12 +178,14 @@ def _launch_setup(context, *args, **kwargs): package='isaac_ros_image_proc', plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', name='cls_image_format_converter', - parameters=[{ - 'encoding_desired': str(cls['encoding_desired']), - 'image_width': int(cls['input_image_width']), - 'image_height': int(cls['input_image_height']), - 'input_qos': 'sensor_data', - }], + parameters=[ + { + 'encoding_desired': str(cls['encoding_desired']), + 'image_width': int(cls['input_image_width']), + 'image_height': int(cls['input_image_height']), + 'input_qos': 'sensor_data', + } + ], remappings=[ ('image_raw', str(cls['image_input_topic'])), ('image', CLS_CONVERTED_IMAGE_TOPIC), @@ -177,17 +196,19 @@ def _launch_setup(context, *args, **kwargs): name='cls_tensor_rt', package='isaac_ros_tensor_rt', plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode', - parameters=[{ - 'model_file_path': cls_model, - 'engine_file_path': cls_engine, - 'output_binding_names': cls['output_binding_names'], - 'output_tensor_names': cls['output_tensor_names'], - 'input_tensor_names': cls['input_tensor_names'], - 'input_binding_names': cls['input_binding_names'], - 'verbose': bool(cls['verbose']), - 'force_engine_update': bool(cls['force_engine_update']), - 'tensor_output_topic': CLS_TENSOR_OUTPUT_TOPIC, - }], + parameters=[ + { + 'model_file_path': cls_model, + 'engine_file_path': cls_engine, + 'output_binding_names': cls['output_binding_names'], + 'output_tensor_names': cls['output_tensor_names'], + 'input_tensor_names': cls['input_tensor_names'], + 'input_binding_names': cls['input_binding_names'], + 'verbose': bool(cls['verbose']), + 'force_engine_update': bool(cls['force_engine_update']), + 'tensor_output_topic': CLS_TENSOR_OUTPUT_TOPIC, + } + ], remappings=[ ('tensor_pub', CLS_TENSOR_OUTPUT_TOPIC), ('tensor_sub', CLS_TENSOR_INPUT_TOPIC), @@ -198,12 +219,14 @@ def _launch_setup(context, *args, **kwargs): name='cls_decoder', package='isaac_ros_yolov26_cls', plugin='nvidia::isaac_ros::yolov26_cls::YoloV26ClsDecoderNode', - parameters=[{ - 'tensor_input_topic': CLS_TENSOR_INPUT_TOPIC, - 'confidence_threshold': float(cls['confidence_threshold']), - 'num_classes': int(cls['num_classes']), - 'class_topic': str(cls['class_topic']), - }], + parameters=[ + { + 'tensor_input_topic': CLS_TENSOR_INPUT_TOPIC, + 'confidence_threshold': float(cls['confidence_threshold']), + 'num_classes': int(cls['num_classes']), + 'class_topic': str(cls['class_topic']), + } + ], ) cls_container = ComposableNodeContainer( @@ -255,13 +278,15 @@ def _launch_setup(context, *args, **kwargs): package='isaac_ros_yolov26_seg', executable='isaac_ros_yolov26_seg_visualizer.py', name='seg_visualizer', - parameters=[{ - 'detections_topic': str(seg['detection_topic']), - 'image_topic': SEG_ENCODER_RESIZE_TOPIC, - 'mask_topic': str(seg['mask_topic']), - 'output_image_topic': str(seg['visualized_image_topic']), - 'class_names_yaml': str(seg['class_names']), - }], + parameters=[ + { + 'detections_topic': str(seg['detection_topic']), + 'image_topic': SEG_ENCODER_RESIZE_TOPIC, + 'mask_topic': str(seg['mask_topic']), + 'output_image_topic': str(seg['visualized_image_topic']), + 'class_names_yaml': str(seg['class_names']), + } + ], ) ) @@ -272,12 +297,14 @@ def _launch_setup(context, *args, **kwargs): package='isaac_ros_yolov26_cls', executable='isaac_ros_yolov26_cls_visualizer.py', name='cls_visualizer', - parameters=[{ - 'class_topic': str(cls['class_topic']), - 'image_topic': CLS_ENCODER_RESIZE_TOPIC, - 'output_image_topic': str(cls['visualized_image_topic']), - 'class_names_yaml': str(cls['class_names']), - }], + parameters=[ + { + 'class_topic': str(cls['class_topic']), + 'image_topic': CLS_ENCODER_RESIZE_TOPIC, + 'output_image_topic': str(cls['visualized_image_topic']), + 'class_names_yaml': str(cls['class_names']), + } + ], ) ) @@ -288,11 +315,13 @@ def generate_launch_description(): pkg_dir = get_package_share_directory('perception_setup') default_config = os.path.join(pkg_dir, 'config', 'pipeline_following.yaml') - return launch.LaunchDescription([ - DeclareLaunchArgument( - 'config_file', - default_value=default_config, - description='Path to pipeline-following config YAML', - ), - OpaqueFunction(function=_launch_setup), - ]) + return launch.LaunchDescription( + [ + DeclareLaunchArgument( + 'config_file', + default_value=default_config, + description='Path to pipeline-following config YAML', + ), + OpaqueFunction(function=_launch_setup), + ] + ) diff --git a/perception_setup/launch/pipeline_localization.launch.py b/perception_setup/launch/pipeline_localization.launch.py index ebc0143..14d8806 100644 --- a/perception_setup/launch/pipeline_localization.launch.py +++ b/perception_setup/launch/pipeline_localization.launch.py @@ -57,12 +57,14 @@ def _build_seg_pipeline(cfg, prefix, models_dir, encoder_dir): package='isaac_ros_image_proc', plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', name=f'seg_{prefix}_image_format_converter', - parameters=[{ - 'encoding_desired': str(cfg['encoding_desired']), - 'image_width': int(cfg['input_image_width']), - 'image_height': int(cfg['input_image_height']), - 'input_qos': 'sensor_data', - }], + parameters=[ + { + 'encoding_desired': str(cfg['encoding_desired']), + 'image_width': int(cfg['input_image_width']), + 'image_height': int(cfg['input_image_height']), + 'input_qos': 'sensor_data', + } + ], remappings=[ ('image_raw', str(cfg['image_input_topic'])), ('image', converted_image_topic), @@ -73,17 +75,19 @@ def _build_seg_pipeline(cfg, prefix, models_dir, encoder_dir): name=f'seg_{prefix}_tensor_rt', package='isaac_ros_tensor_rt', plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode', - parameters=[{ - 'model_file_path': model_path, - 'engine_file_path': engine_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': bool(cfg['verbose']), - 'force_engine_update': bool(cfg['force_engine_update']), - 'tensor_output_topic': tensor_output_topic, - }], + parameters=[ + { + 'model_file_path': model_path, + 'engine_file_path': engine_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': bool(cfg['verbose']), + 'force_engine_update': bool(cfg['force_engine_update']), + 'tensor_output_topic': tensor_output_topic, + } + ], remappings=[ ('tensor_pub', tensor_output_topic), ('tensor_sub', tensor_input_topic), @@ -94,20 +98,22 @@ def _build_seg_pipeline(cfg, prefix, models_dir, encoder_dir): name=f'seg_{prefix}_decoder', package='isaac_ros_yolov26_seg', plugin='nvidia::isaac_ros::yolov26_seg::YoloV26SegDecoderNode', - parameters=[{ - 'tensor_input_topic': tensor_input_topic, - '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': int(cfg['output_mask_width']), - 'output_mask_height': int(cfg['output_mask_height']), - 'detections_topic': str(cfg['detection_topic']), - 'mask_topic': str(cfg['mask_topic']), - }], + parameters=[ + { + 'tensor_input_topic': tensor_input_topic, + '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': int(cfg['output_mask_width']), + 'output_mask_height': int(cfg['output_mask_height']), + 'detections_topic': str(cfg['detection_topic']), + 'mask_topic': str(cfg['mask_topic']), + } + ], ) container = ComposableNodeContainer( @@ -152,13 +158,15 @@ def _build_seg_pipeline(cfg, prefix, models_dir, encoder_dir): package='isaac_ros_yolov26_seg', executable='isaac_ros_yolov26_seg_visualizer.py', name=f'seg_{prefix}_visualizer', - parameters=[{ - 'detections_topic': str(cfg['detection_topic']), - 'image_topic': encoder_resize_topic, - 'mask_topic': str(cfg['mask_topic']), - 'output_image_topic': str(cfg['visualized_image_topic']), - 'class_names_yaml': str(cfg['class_names']), - }], + parameters=[ + { + 'detections_topic': str(cfg['detection_topic']), + 'image_topic': encoder_resize_topic, + 'mask_topic': str(cfg['mask_topic']), + 'output_image_topic': str(cfg['visualized_image_topic']), + 'class_names_yaml': str(cfg['class_names']), + } + ], ) ) @@ -175,6 +183,19 @@ def _launch_setup(context, *args, **kwargs): models_dir = os.path.join(pkg_dir, 'models') encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder') + # Resolve camera references from cameras.yaml + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + + for seg_cfg in [cfg['seg_front'], cfg['seg_down']]: + if 'camera' in seg_cfg: + cam = cameras[seg_cfg['camera']] + seg_cfg['image_input_topic'] = cam['image_topic'] + seg_cfg['camera_info_input_topic'] = cam['camera_info_topic'] + seg_cfg['input_image_width'] = cam['image_width'] + seg_cfg['input_image_height'] = cam['image_height'] + actions = [] actions += _build_seg_pipeline(cfg['seg_front'], 'front', models_dir, encoder_dir) actions += _build_seg_pipeline(cfg['seg_down'], 'down', models_dir, encoder_dir) @@ -186,11 +207,13 @@ def generate_launch_description(): pkg_dir = get_package_share_directory('perception_setup') default_config = os.path.join(pkg_dir, 'config', 'pipeline_localization.yaml') - return launch.LaunchDescription([ - DeclareLaunchArgument( - 'config_file', - default_value=default_config, - description='Path to pipeline-localization config YAML', - ), - OpaqueFunction(function=_launch_setup), - ]) + return launch.LaunchDescription( + [ + DeclareLaunchArgument( + 'config_file', + default_value=default_config, + description='Path to pipeline-localization config YAML', + ), + OpaqueFunction(function=_launch_setup), + ] + ) diff --git a/perception_setup/launch/realsense_d555.launch.py b/perception_setup/launch/realsense_d555.launch.py index 76d7384..51317c7 100644 --- a/perception_setup/launch/realsense_d555.launch.py +++ b/perception_setup/launch/realsense_d555.launch.py @@ -1,11 +1,17 @@ """Wrapper launch file for the RealSense D555 camera. +Starts: + - RealSense driver (rs_launch.py) + - camera_info_publisher (publishes calibration on color_raw/camera_info) + - image_undistort (undistorts color image) + - image_crop (crops depth image) + See: https://github.com/vortexntnu/realsense-ros/blob/r/4.57.6/realsense2_camera/launch/rs_launch.py """ import os -import yaml +import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription @@ -18,17 +24,17 @@ def generate_launch_description(): pkg_dir = get_package_share_directory("perception_setup") - default_rs_config = os.path.join(pkg_dir, "config", "realsense_d555.yaml") + default_rs_config = os.path.join( + pkg_dir, "config", "cameras", "realsense_d555.yaml" + ) camera_info_file = os.path.join( - pkg_dir, - "config", - "color_realsense_d555_calib.yaml", + pkg_dir, "config", "cameras", "color_realsense_d555_calib.yaml" ) - with open(camera_info_file, "r") as f: - cfg = yaml.safe_load(f) + with open(camera_info_file) as f: + calib = yaml.safe_load(f) - camera_info_topic = cfg["camera_info_topic"] + helpers_dir = os.path.join(pkg_dir, "launch", "helpers") config_file_arg = DeclareLaunchArgument( "config_file", @@ -52,14 +58,30 @@ def generate_launch_description(): parameters=[ { "camera_info_file": camera_info_file, - "camera_info_topic": camera_info_topic, + "camera_info_topic": calib["camera_info_topic"], } ], output="screen", ) - return LaunchDescription([ - config_file_arg, - rs_launch, - camera_info_pub, - ]) + image_undistort = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(helpers_dir, "image_undistort.launch.py") + ), + ) + + image_crop = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(helpers_dir, "image_crop.launch.py") + ), + ) + + return LaunchDescription( + [ + config_file_arg, + rs_launch, + camera_info_pub, + image_undistort, + image_crop, + ] + ) diff --git a/perception_setup/launch/yolo_cls.launch.py b/perception_setup/launch/yolo/yolo_cls.launch.py similarity index 98% rename from perception_setup/launch/yolo_cls.launch.py rename to perception_setup/launch/yolo/yolo_cls.launch.py index 89ea3ee..860f713 100644 --- a/perception_setup/launch/yolo_cls.launch.py +++ b/perception_setup/launch/yolo/yolo_cls.launch.py @@ -56,7 +56,7 @@ def _launch_setup(context, *args, **kwargs): # Resolve camera reference from cameras.yaml if 'camera' in cfg: - cameras_path = os.path.join(pkg_dir, 'config', 'cameras.yaml') + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') with open(cameras_path) as f: cameras = yaml.safe_load(f) cam = cameras[cfg['camera']] @@ -216,7 +216,7 @@ def _launch_setup(context, *args, **kwargs): def generate_launch_description(): pkg_dir = get_package_share_directory('perception_setup') - default_config = os.path.join(pkg_dir, 'config', 'yolo_cls.yaml') + default_config = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_cls.yaml') return launch.LaunchDescription( [ diff --git a/perception_setup/launch/yolo_detect.launch.py b/perception_setup/launch/yolo/yolo_detect.launch.py similarity index 92% rename from perception_setup/launch/yolo_detect.launch.py rename to perception_setup/launch/yolo/yolo_detect.launch.py index 29c3d31..b255bba 100644 --- a/perception_setup/launch/yolo_detect.launch.py +++ b/perception_setup/launch/yolo/yolo_detect.launch.py @@ -57,6 +57,19 @@ def _launch_setup(context, *args, **kwargs): with open(config_path) as f: cfg = yaml.safe_load(f) + pkg_dir = get_package_share_directory('perception_setup') + + # Resolve camera reference from cameras.yaml + if 'camera' in cfg: + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras[cfg['camera']] + cfg['image_input_topic'] = cam['image_topic'] + cfg['camera_info_input_topic'] = cam['camera_info_topic'] + cfg['input_image_width'] = cam['image_width'] + cfg['input_image_height'] = cam['image_height'] + required_keys = [ 'model_file_path', 'engine_file_path', @@ -243,7 +256,7 @@ def _launch_setup(context, *args, **kwargs): def generate_launch_description(): pkg_dir = get_package_share_directory('perception_setup') - default_config = os.path.join(pkg_dir, 'config', 'yolo_detect.yaml') + default_config = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_detect.yaml') return launch.LaunchDescription( [ diff --git a/perception_setup/launch/yolo_obb.launch.py b/perception_setup/launch/yolo/yolo_obb.launch.py similarity index 93% rename from perception_setup/launch/yolo_obb.launch.py rename to perception_setup/launch/yolo/yolo_obb.launch.py index cfdd5da..6987d86 100644 --- a/perception_setup/launch/yolo_obb.launch.py +++ b/perception_setup/launch/yolo/yolo_obb.launch.py @@ -53,6 +53,19 @@ def _launch_setup(context, *args, **kwargs): with open(config_path) as f: cfg = yaml.safe_load(f) + pkg_dir = get_package_share_directory('perception_setup') + + # Resolve camera reference from cameras.yaml + if 'camera' in cfg: + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras[cfg['camera']] + cfg['image_input_topic'] = cam['image_topic'] + cfg['camera_info_input_topic'] = cam['camera_info_topic'] + cfg['input_image_width'] = cam['image_width'] + cfg['input_image_height'] = cam['image_height'] + required_keys = [ 'model_file_path', 'engine_file_path', @@ -84,7 +97,6 @@ def _launch_setup(context, *args, **kwargs): raise RuntimeError(f"Missing required config key: '{key}'") # Resolve model paths relative to perception_setup/models/ - pkg_dir = get_package_share_directory('perception_setup') models_dir = os.path.join(pkg_dir, 'models') model_file_path = os.path.join(models_dir, str(cfg['model_file_path'])) @@ -237,7 +249,7 @@ def _launch_setup(context, *args, **kwargs): def generate_launch_description(): pkg_dir = get_package_share_directory('perception_setup') - default_config = os.path.join(pkg_dir, 'config', 'yolo_obb.yaml') + default_config = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_obb.yaml') return launch.LaunchDescription( [ diff --git a/perception_setup/launch/yolo_seg.launch.py b/perception_setup/launch/yolo/yolo_seg.launch.py similarity index 72% rename from perception_setup/launch/yolo_seg.launch.py rename to perception_setup/launch/yolo/yolo_seg.launch.py index a470886..76c4d73 100644 --- a/perception_setup/launch/yolo_seg.launch.py +++ b/perception_setup/launch/yolo/yolo_seg.launch.py @@ -1,7 +1,6 @@ # SPDX-License-Identifier: MIT -""" -Isaac ROS YOLO Instance-Segmentation TensorRT inference pipeline +"""Isaac ROS YOLO Instance-Segmentation TensorRT inference pipeline 1. ImageFormatConverterNode Input: image_input_topic @@ -34,15 +33,17 @@ """ import os -import yaml - -from ament_index_python.packages import get_package_share_directory import launch -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +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 @@ -54,17 +55,16 @@ def _launch_setup(context, *args, **kwargs): - config_path = LaunchConfiguration('config_file').perform(context) - with open(config_path, 'r') as f: + with open(config_path) as f: cfg = yaml.safe_load(f) pkg_dir = get_package_share_directory('perception_setup') # Resolve camera reference from cameras.yaml if 'camera' in cfg: - cameras_path = os.path.join(pkg_dir, 'config', 'cameras.yaml') + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') with open(cameras_path) as f: cameras = yaml.safe_load(f) cam = cameras[cfg['camera']] @@ -115,12 +115,14 @@ def _launch_setup(context, *args, **kwargs): package='isaac_ros_image_proc', plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', name='image_format_converter', - parameters=[{ - 'encoding_desired': encoding_desired, - 'image_width': input_image_width, - 'image_height': input_image_height, - 'input_qos': 'sensor_data', - }], + parameters=[ + { + 'encoding_desired': encoding_desired, + 'image_width': input_image_width, + 'image_height': input_image_height, + 'input_qos': 'sensor_data', + } + ], remappings=[ ('image_raw', image_input_topic), ('image', CONVERTED_IMAGE_TOPIC), @@ -131,17 +133,19 @@ def _launch_setup(context, *args, **kwargs): 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, - 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, - }], + 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, + 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + } + ], remappings=[ ('tensor_pub', TENSOR_OUTPUT_TOPIC), ('tensor_sub', TENSOR_INPUT_TOPIC), @@ -152,20 +156,22 @@ def _launch_setup(context, *args, **kwargs): name='yolo_seg_decoder_node', package='isaac_ros_yolov26_seg', plugin='nvidia::isaac_ros::yolov26_seg::YoloV26SegDecoderNode', - parameters=[{ - 'tensor_input_topic': TENSOR_INPUT_TOPIC, - 'confidence_threshold': confidence_threshold, - 'num_detections': num_detections, - 'mask_width': mask_width, - 'mask_height': mask_height, - 'num_protos': num_protos, - 'network_image_width': network_image_width, - 'network_image_height': network_image_height, - 'output_mask_width': output_mask_width, - 'output_mask_height': output_mask_height, - 'detections_topic': detection_topic, - 'mask_topic': mask_topic, - }], + parameters=[ + { + 'tensor_input_topic': TENSOR_INPUT_TOPIC, + 'confidence_threshold': confidence_threshold, + 'num_detections': num_detections, + 'mask_width': mask_width, + 'mask_height': mask_height, + 'num_protos': num_protos, + 'network_image_width': network_image_width, + 'network_image_height': network_image_height, + 'output_mask_width': output_mask_width, + 'output_mask_height': output_mask_height, + 'detections_topic': detection_topic, + 'mask_topic': mask_topic, + } + ], ) tensor_rt_container = ComposableNodeContainer( @@ -216,13 +222,15 @@ def _launch_setup(context, *args, **kwargs): package='isaac_ros_yolov26_seg', executable='isaac_ros_yolov26_seg_visualizer.py', name='yolo_seg_visualizer', - parameters=[{ - 'detections_topic': detection_topic, - 'image_topic': ENCODER_RESIZE_TOPIC, - 'mask_topic': mask_topic, - 'output_image_topic': visualized_image_topic, - 'class_names_yaml': str(class_names), - }], + parameters=[ + { + 'detections_topic': detection_topic, + 'image_topic': ENCODER_RESIZE_TOPIC, + 'mask_topic': mask_topic, + 'output_image_topic': visualized_image_topic, + 'class_names_yaml': str(class_names), + } + ], ) ) @@ -230,15 +238,16 @@ def _launch_setup(context, *args, **kwargs): def generate_launch_description(): - pkg_dir = get_package_share_directory('perception_setup') - default_config = os.path.join(pkg_dir, 'config', 'yolo_seg.yaml') - - return launch.LaunchDescription([ - DeclareLaunchArgument( - 'config_file', - default_value=default_config, - description='Path to YOLO segmentation pipeline config YAML', - ), - OpaqueFunction(function=_launch_setup), - ]) + default_config = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_seg.yaml') + + return launch.LaunchDescription( + [ + DeclareLaunchArgument( + 'config_file', + default_value=default_config, + description='Path to YOLO segmentation pipeline config YAML', + ), + OpaqueFunction(function=_launch_setup), + ] + ) diff --git a/perception_setup/scripts/camera_info_publisher.py b/perception_setup/scripts/camera_info_publisher.py index 4c85b6c..75dcd48 100755 --- a/perception_setup/scripts/camera_info_publisher.py +++ b/perception_setup/scripts/camera_info_publisher.py @@ -1,20 +1,14 @@ #!/usr/bin/env python3 import rclpy +import yaml from rclpy.node import Node - -from sensor_msgs.msg import CameraInfo - -from rclpy.qos import QoSProfile -from rclpy.qos import DurabilityPolicy -from rclpy.qos import ReliabilityPolicy from rclpy.parameter import Parameter - -import yaml +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from sensor_msgs.msg import CameraInfo class CameraInfoPublisher(Node): - def __init__(self): super().__init__("camera_info_publisher") @@ -30,7 +24,7 @@ def __init__(self): if not topic_name: raise RuntimeError("Parameter 'camera_info_topic' must be provided") - with open(file_path, "r") as f: + with open(file_path) as f: data = yaml.safe_load(f) msg = CameraInfo() @@ -49,13 +43,17 @@ def __init__(self): qos.durability = DurabilityPolicy.TRANSIENT_LOCAL self.publisher = self.create_publisher(CameraInfo, topic_name, qos) + self.msg = msg - # publish once - self.publisher.publish(msg) + self.create_timer(1.0, self.publish_camera_info) - self.get_logger().info(f"Published CameraInfo on {topic_name}") + self.get_logger().info(f"Publishing CameraInfo on {topic_name} at 1 Hz") self.get_logger().info(f"Loaded calibration: {file_path}") + def publish_camera_info(self): + self.msg.header.stamp = self.get_clock().now().to_msg() + self.publisher.publish(self.msg) + def main(): rclpy.init() @@ -64,4 +62,4 @@ def main(): if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/perception_setup/scripts/image_crop.py b/perception_setup/scripts/image_crop.py new file mode 100755 index 0000000..09f99aa --- /dev/null +++ b/perception_setup/scripts/image_crop.py @@ -0,0 +1,108 @@ +#!/usr/bin/env python3 + +import rclpy +from cv_bridge import CvBridge +from message_filters import ApproximateTimeSynchronizer, Subscriber +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo, Image + + +class ImageCrop(Node): + def __init__(self): + super().__init__("image_crop") + + self.declare_parameter("image_topic", "/camera/camera/depth/image_rect_raw") + self.declare_parameter("camera_info_topic", "/camera/camera/depth/camera_info") + self.declare_parameter( + "output_image_topic", "/camera/camera/depth/image_rect_raw/cropped" + ) + self.declare_parameter( + "output_camera_info_topic", "/camera/camera/depth/camera_info/cropped" + ) + self.declare_parameter("crop.x_offset", 0) + self.declare_parameter("crop.y_offset", 0) + self.declare_parameter("crop.width", 0) + self.declare_parameter("crop.height", 0) + + 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.bridge = CvBridge() + + image_sub = Subscriber(self, Image, image_topic) + info_sub = Subscriber(self, CameraInfo, info_topic) + + 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, 10) + self.info_pub = self.create_publisher(CameraInfo, out_info_topic, 10) + + 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'}]" + ) + + def callback(self, image_msg: Image, info_msg: CameraInfo): + 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/image_undistort.py b/perception_setup/scripts/image_undistort.py new file mode 100755 index 0000000..54421b1 --- /dev/null +++ b/perception_setup/scripts/image_undistort.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 + +import cv2 +import numpy as np +import rclpy +from cv_bridge import CvBridge +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from sensor_msgs.msg import CameraInfo, Image + + +class ImageUndistort(Node): + def __init__(self): + super().__init__("image_undistort") + + self.declare_parameter("image_topic", "/camera/camera/color/image_raw") + self.declare_parameter( + "camera_info_topic", "/camera/camera/color_raw/camera_info" + ) + self.declare_parameter( + "output_image_topic", "/camera/camera/color/image_undistorted" + ) + self.declare_parameter( + "output_camera_info_topic", "/camera/camera/color/camera_info_undistorted" + ) + + image_topic = self.get_parameter("image_topic").value + info_topic = self.get_parameter("camera_info_topic").value + out_topic = self.get_parameter("output_image_topic").value + out_info_topic = self.get_parameter("output_camera_info_topic").value + + self.bridge = CvBridge() + self.map1 = None + self.map2 = None + self.rectified_info = None + + # Camera info only needs to be received once — use transient local so we + # also catch messages published before this node started (latched). + info_qos = QoSProfile(depth=1) + info_qos.durability = DurabilityPolicy.TRANSIENT_LOCAL + info_qos.reliability = ReliabilityPolicy.RELIABLE + + self.create_subscription(CameraInfo, info_topic, self.info_callback, info_qos) + self.create_subscription(Image, image_topic, self.image_callback, 10) + + self.pub = self.create_publisher(Image, out_topic, 10) + self.info_pub = self.create_publisher(CameraInfo, out_info_topic, info_qos) + + self.get_logger().info( + f"image_undistort: {image_topic} -> {out_topic}, {info_topic} -> {out_info_topic}" + ) + + def info_callback(self, msg: CameraInfo): + if self.map1 is not None: + return + K = np.array(msg.k, dtype=np.float64).reshape(3, 3) + D = np.array(msg.d, dtype=np.float64) + h, w = msg.height, msg.width + new_K, _ = cv2.getOptimalNewCameraMatrix(K, D, (w, h), alpha=0) + self.map1, self.map2 = cv2.initUndistortRectifyMap( + K, D, None, new_K, (w, h), cv2.CV_16SC2 + ) + # Build the rectified camera_info (zero distortion, updated K and P) + self.rectified_info = CameraInfo() + self.rectified_info.width = w + self.rectified_info.height = h + self.rectified_info.distortion_model = "plumb_bob" + self.rectified_info.d = [0.0, 0.0, 0.0, 0.0, 0.0] + self.rectified_info.k = new_K.flatten().tolist() + self.rectified_info.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + P = [ + new_K[0, 0], + 0.0, + new_K[0, 2], + 0.0, + 0.0, + new_K[1, 1], + new_K[1, 2], + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + ] + self.rectified_info.p = P + self.get_logger().info(f"Undistortion maps initialised ({w}x{h})") + + def image_callback(self, msg: Image): + if self.map1 is None: + self.get_logger().warn( + "Waiting for camera_info...", throttle_duration_sec=5.0 + ) + return + + cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") + undistorted = cv2.remap(cv_image, self.map1, self.map2, cv2.INTER_LINEAR) + + out_msg = self.bridge.cv2_to_imgmsg(undistorted, encoding=msg.encoding) + out_msg.header = msg.header + self.pub.publish(out_msg) + + self.rectified_info.header = msg.header + self.info_pub.publish(self.rectified_info) + + +def main(): + rclpy.init() + node = ImageUndistort() + rclpy.spin(node) + + +if __name__ == "__main__": + main() 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") From 6110d832b4aaa4ede7866781081621c1c95d2199 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Sat, 28 Mar 2026 03:36:56 +0100 Subject: [PATCH 13/37] feat: add visual inspection pipeline and refactor camera configurations - Introduced a new visual inspection pipeline with a YAML configuration file for image filtering and ArUco detection. - Removed camera source definitions from YOLO configuration files to centralize camera settings in cameras.yaml. - Added new launch files for the FLIR Blackfly S BFS-PGE-16S2C-CS and RealSense D555 cameras. - Implemented image processing nodes for cropping and undistorting images with configurable parameters. - Enhanced pipeline launch files to dynamically resolve camera settings based on the enable_undistort flag. - Updated image_crop and image_undistort scripts to support passthrough functionality when undistortion is disabled. - Added a valve intervention pipeline that integrates YOLO OBB detection and valve landmark detection. --- ...am_calib.yaml => bfs_pge_16s2c_calib.yaml} | 2 +- ..._params.yaml => bfs_pge_16s2c_params.yaml} | 0 perception_setup/config/cameras/cameras.yaml | 51 +++++- .../cameras/color_realsense_d555_calib.yaml | 2 - .../config/cameras/realsense_d555.yaml | 5 +- .../config/image_processing/image_crop.yaml | 16 +- .../image_processing/image_undistort.yaml | 10 +- .../config/pipeline_following.yaml | 4 +- .../config/pipeline_localization.yaml | 2 +- .../config/valve_intervention.yaml | 85 ++++++++++ .../config/visual_inspection.yaml | 68 ++++++++ perception_setup/config/yolo/yolo_detect.yaml | 3 - perception_setup/config/yolo/yolo_obb.yaml | 3 - perception_setup/config/yolo/yolo_seg.yaml | 3 - .../launch/cameras/bfs_pge_16s2c.launch.py | 94 +++++++++++ .../{ => cameras}/realsense_d555.launch.py | 8 +- .../camera_info_publisher.launch.py | 10 +- .../image_crop.launch.py | 14 +- .../image_undistort.launch.py | 15 +- .../launch/pipeline_following.launch.py | 39 +++-- .../launch/pipeline_localization.launch.py | 12 +- .../launch/valve_intervention.launch.py | 151 ++++++++++++++++++ .../launch/visual_inspection.launch.py | 127 +++++++++++++++ .../launch/yolo/yolo_detect.launch.py | 24 ++- .../launch/yolo/yolo_obb.launch.py | 24 ++- .../launch/yolo/yolo_seg.launch.py | 24 ++- perception_setup/scripts/image_crop.py | 41 +++-- perception_setup/scripts/image_undistort.py | 57 ++++--- 28 files changed, 764 insertions(+), 130 deletions(-) rename perception_setup/config/cameras/{downwards_cam_calib.yaml => bfs_pge_16s2c_calib.yaml} (95%) rename perception_setup/config/cameras/{downwards_cam_params.yaml => bfs_pge_16s2c_params.yaml} (100%) create mode 100644 perception_setup/config/valve_intervention.yaml create mode 100644 perception_setup/config/visual_inspection.yaml create mode 100644 perception_setup/launch/cameras/bfs_pge_16s2c.launch.py rename perception_setup/launch/{ => cameras}/realsense_d555.launch.py (90%) rename perception_setup/launch/{helpers => image_processing}/camera_info_publisher.launch.py (76%) rename perception_setup/launch/{helpers => image_processing}/image_crop.launch.py (66%) rename perception_setup/launch/{helpers => image_processing}/image_undistort.launch.py (55%) create mode 100644 perception_setup/launch/valve_intervention.launch.py create mode 100644 perception_setup/launch/visual_inspection.launch.py diff --git a/perception_setup/config/cameras/downwards_cam_calib.yaml b/perception_setup/config/cameras/bfs_pge_16s2c_calib.yaml similarity index 95% rename from perception_setup/config/cameras/downwards_cam_calib.yaml rename to perception_setup/config/cameras/bfs_pge_16s2c_calib.yaml index a30e05e..95c2df4 100644 --- a/perception_setup/config/cameras/downwards_cam_calib.yaml +++ b/perception_setup/config/cameras/bfs_pge_16s2c_calib.yaml @@ -1,6 +1,6 @@ image_width: 1440 image_height: 1080 -camera_name: "downwards_cam" +camera_name: "bfs_pge_16s2c" camera_matrix: rows: 3 diff --git a/perception_setup/config/cameras/downwards_cam_params.yaml b/perception_setup/config/cameras/bfs_pge_16s2c_params.yaml similarity index 100% rename from perception_setup/config/cameras/downwards_cam_params.yaml rename to perception_setup/config/cameras/bfs_pge_16s2c_params.yaml diff --git a/perception_setup/config/cameras/cameras.yaml b/perception_setup/config/cameras/cameras.yaml index 7be25ca..3210677 100644 --- a/perception_setup/config/cameras/cameras.yaml +++ b/perception_setup/config/cameras/cameras.yaml @@ -1,9 +1,52 @@ # Centralized camera definitions -# Referenced by pipeline configs via "camera: " -# Fields: image_topic, camera_info_topic, image_width, image_height +# Referenced by pipeline/YOLO/image-processing configs via "camera: " +# +# raw_*: Topics published directly by the camera driver (input to image processing) +# image_topic / camera_info_topic: Processed topics (output of image_undistort / image_crop) +# — downstream nodes (YOLO, pipelines) subscribe to these realsense_d555: - image_topic: "/camera/camera/color/image_raw" - camera_info_topic: "/camera/camera/color/camera_info_rect" + # Raw topics from the camera driver + raw_image_topic: "/camera/camera/color/image_raw" + raw_camera_info_topic: "/camera/camera/color/camera_info_rect" + raw_depth_topic: "/camera/camera/depth/image_rect_raw" + raw_depth_camera_info_topic: "/camera/camera/depth/camera_info" + + # Calibration camera info (published by camera_info_publisher, contains distortion coefficients) + calibration_camera_info_topic: "/camera/camera/color_raw/camera_info" + + # Image processing flags + enable_undistort: true + enable_crop: true + + # Processed topics (output of image_undistort / image_crop) + image_topic: "/realsense_d555/color/image" + camera_info_topic: "/realsense_d555/color/camera_info" + depth_image_topic: "/realsense_d555/depth/image" + depth_camera_info_topic: "/realsense_d555/depth/camera_info" + + # Image properties image_width: 896 image_height: 504 + encoding: "rgb8" + +bfs_pge_16s2c: + # Raw topics from spinnaker_camera_driver (node name: bfs_pge_16s2c) + raw_image_topic: "/bfs_pge_16s2c/image_raw" + raw_camera_info_topic: "/bfs_pge_16s2c/camera_info" + + # Driver publishes calibrated camera_info directly via camerainfo_url — no separate publisher needed. + calibration_camera_info_topic: "/bfs_pge_16s2c/camera_info" + + # Image processing flags + enable_undistort: false + enable_crop: false + + # Processed topics (output of image_undistort / image_crop) + image_topic: "/bfs_pge_16s2c/color/image" + camera_info_topic: "/bfs_pge_16s2c/color/camera_info" + + # Image properties (720x540 after 2x binning from 1440x1080) + image_width: 720 + image_height: 540 + encoding: "bgr8" diff --git a/perception_setup/config/cameras/color_realsense_d555_calib.yaml b/perception_setup/config/cameras/color_realsense_d555_calib.yaml index 4df1c1c..e141ccb 100644 --- a/perception_setup/config/cameras/color_realsense_d555_calib.yaml +++ b/perception_setup/config/cameras/color_realsense_d555_calib.yaml @@ -1,5 +1,3 @@ -camera_info_topic: "/camera/camera/color_raw/camera_info" - image_width: 896 image_height: 504 camera_name: color_camera diff --git a/perception_setup/config/cameras/realsense_d555.yaml b/perception_setup/config/cameras/realsense_d555.yaml index 9225089..8a53f2e 100644 --- a/perception_setup/config/cameras/realsense_d555.yaml +++ b/perception_setup/config/cameras/realsense_d555.yaml @@ -15,15 +15,16 @@ enable_infra2: false depth_module.infra1_format: 'Y8' depth_module.infra2_format: 'Y8' -camera_namespace: 'moby' # Does nothing? +camera_namespace: 'nautilus' # Does nothing? camera_name: 'front_camera' -tf_prefix: 'moby/' +tf_prefix: 'nautilus/' enable_gyro: false # Dont need IMU data (not being used) enable_accel: false # Dont need IMU data (not being used) enable_motion: false # Dont need IMU data (not being used) publish_tf: false # Publishes with wrong convention for ROS (z-frame for camera/optical frame) +enable_sync: false # Software sync (needs to be tested) # serial_no: "''" # usb_port_id: "''" diff --git a/perception_setup/config/image_processing/image_crop.yaml b/perception_setup/config/image_processing/image_crop.yaml index 4768a3a..82461a6 100644 --- a/perception_setup/config/image_processing/image_crop.yaml +++ b/perception_setup/config/image_processing/image_crop.yaml @@ -1,15 +1,13 @@ # Image crop configuration -# x_offset / y_offset: top-left corner of the crop region (pixels) -# width / height: size of the crop region (pixels) -# Set to 0 to use the full image dimension. +# Reads raw depth image from camera and crops to the specified region. +# Topics are resolved from cameras.yaml based on the camera reference. +# +# crop: top-left corner (x_offset, y_offset) and size (width, height) in pixels +# Set width or height to 0 to use the full image dimension. -image_topic: "/camera/camera/depth/image_rect_raw" -camera_info_topic: "/camera/camera/depth/camera_info" +camera: "realsense_d555" -output_image_topic: "/front_cam/depth/image_rect_raw/cropped" -output_camera_info_topic: "/front_cam/depth/camera_info/cropped" - -crop: +crop: # These values were eye-balled from the raw depth image. x_offset: 260 # x_left boundary y_offset: 190 # y_upper boundary width: 485 # x_right (730) - x_left (245) diff --git a/perception_setup/config/image_processing/image_undistort.yaml b/perception_setup/config/image_processing/image_undistort.yaml index bf7f509..8713926 100644 --- a/perception_setup/config/image_processing/image_undistort.yaml +++ b/perception_setup/config/image_processing/image_undistort.yaml @@ -1,4 +1,6 @@ -image_topic: "/camera/camera/color/image_raw" -camera_info_topic: "/camera/camera/color_raw/camera_info" -output_image_topic: "/front_cam/image_undistorted" -output_camera_info_topic: "/front_cam/camera_info_undistorted" +# Image undistortion configuration +# Reads raw image from camera, applies undistortion using calibration data, +# and publishes the corrected image. +# Topics are resolved from cameras.yaml based on the camera reference. + +camera: "realsense_d555" diff --git a/perception_setup/config/pipeline_following.yaml b/perception_setup/config/pipeline_following.yaml index b395265..3a77188 100644 --- a/perception_setup/config/pipeline_following.yaml +++ b/perception_setup/config/pipeline_following.yaml @@ -3,7 +3,7 @@ # Stage 2: Classifies the mask image to determine if the pipeline continues or ends. seg: - camera: "realsense_d555" + camera: "bfs_pge_16s2c" visualized_image_topic: "/pipeline/camera/segmentation_debug" detection_topic: "/pipeline/camera/bboxes" mask_topic: "/pipeline/camera/segmentation_mask" @@ -44,9 +44,9 @@ seg: num_protos: 32 cls: + camera: "bfs_pge_16s2c" # Used to resolve camera_info_input_topic # Input is the segmentation mask output from stage 1 image_input_topic: "/pipeline/camera/segmentation_mask" - camera_info_input_topic: "/camera/camera/color/camera_info_rect" visualized_image_topic: "/pipeline/camera/classification_debug" class_topic: "/pipeline/camera/classification" diff --git a/perception_setup/config/pipeline_localization.yaml b/perception_setup/config/pipeline_localization.yaml index 1ba1477..63110c4 100644 --- a/perception_setup/config/pipeline_localization.yaml +++ b/perception_setup/config/pipeline_localization.yaml @@ -43,7 +43,7 @@ seg_front: num_protos: 32 seg_down: - camera: "realsense_d555" + camera: "bfs_pge_16s2c" visualized_image_topic: "/localization/down/segmentation_debug" detection_topic: "/localization/down/detections" mask_topic: "/localization/down/segmentation_mask" diff --git a/perception_setup/config/valve_intervention.yaml b/perception_setup/config/valve_intervention.yaml new file mode 100644 index 0000000..00d66b3 --- /dev/null +++ b/perception_setup/config/valve_intervention.yaml @@ -0,0 +1,85 @@ +# Valve intervention pipeline configuration +# Chains: camera → yolo_obb → valve_detection + +camera: "realsense_d555" + +# ── YOLO OBB ──────────────────────────────────────────────────────────────── +yolo_obb: + # Topics + visualized_image_topic: "/yolo_obb_processed_image" + detection_topic: "/obb_detections_output" + + # Model and engine file paths (relative to perception_setup/models/) + model_file_path: "obb_best.onnx" + engine_file_path: "obb_best.engine" + + # Visualizer + enable_visualizer: true + class_names: + 0: "valve" + + encoding_desired: "rgb8" + + # TensorRT + verbose: true + force_engine_update: false + + # Tensor I/O bindings + 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 + confidence_threshold: 0.25 + num_detections: 300 + +# ── Valve Detection ───────────────────────────────────────────────────────── +valve_detection: + # Inputs — detections and depth topics wired automatically from yolo_obb / cameras.yaml + color_image_info_topic: "/yolo_obb_encoder/internal/resize/camera_info" + + # Depth-to-color extrinsic (looked up from /tf_static) + depth_frame_id: "front_camera_depth_optical" + color_frame_id: "front_camera_color_optical" + + # Output + landmarks_pub_topic: "/valve_landmarks" + + # YOLO letterbox reference size + yolo_img_width: 640 + yolo_img_height: 640 + + # Annulus and plane fit + annulus_radius_ratio: 0.8 + plane_ransac_threshold: 0.01 + plane_ransac_max_iterations: 50 + + # Duplicate-detection suppression + iou_duplicate_threshold: 0.5 + + # Pose offset + valve_handle_offset: 0.065 + + # Output frame + output_frame_id: "front_camera_depth_optical" + drone: "nautilus" + + # Behaviour toggles + debug_visualize: false + + # Debug visualization (only active when debug_visualize: true) + debug: + valve_poses_pub_topic: "/valve_poses" + depth_cloud_pub_topic: "/valve_depth_cloud" + depth_colormap_pub_topic: "/valve_detection_depth_colormap" + depth_colormap_value_min: 0.1 + depth_colormap_value_max: 1125.5 + annulus_pub_topic: "/bbx_annulus_pcl" + plane_pub_topic: "/annulus_plane_pcl" diff --git a/perception_setup/config/visual_inspection.yaml b/perception_setup/config/visual_inspection.yaml new file mode 100644 index 0000000..5771126 --- /dev/null +++ b/perception_setup/config/visual_inspection.yaml @@ -0,0 +1,68 @@ +# Visual inspection pipeline: camera → image_filtering → aruco_detector +# +# camera: Key from cameras.yaml — determines which camera driver to launch +# and which image/camera_info topics to feed into the pipeline. + +camera: "realsense_d555" + +# --------------------------------------------------------------------------- +# Image filtering +# sub_topic is resolved automatically from cameras.yaml (based on enable_undistort flag) +# --------------------------------------------------------------------------- +image_filtering: + pub_topic: "/visual_inspection/filtered_image" + output_encoding: "rgb8" + filter_params: + filter_type: "no_filter" + flip: + flip_code: 1 + unsharpening: + blur_size: 8 + erosion: + size: 1 + dilation: + size: 1 + white_balancing: + contrast_percentage: 0.1 + ebus: + erosion_size: 2 + blur_size: 30 + mask_weight: 5 + otsu: + gsc_weight_r: 1.0 + gsc_weight_g: 0.0 + gsc_weight_b: 0.0 + gamma_auto_correction: true + gamma_auto_correction_weight: 4.0 + otsu_segmentation: true + erosion_size: 10 + dilation_size: 10 + +# --------------------------------------------------------------------------- +# ArUco detector +# subs.image_topic is wired automatically to image_filtering.pub_topic +# subs.camera_info_topic is resolved automatically from cameras.yaml +# --------------------------------------------------------------------------- +aruco_detector: + pubs: + aruco_image: "/visual_inspection/aruco/image" + aruco_poses: "/visual_inspection/aruco/markers" + board_pose: "/visual_inspection/aruco/board" + landmarks: "/visual_inspection/aruco/landmarks" + + logger_service_name: "/toggle_marker_logger" + + detect_board: false + visualize: true + log_markers: true + publish_detections: true + publish_landmarks: false + + aruco: + marker_size: 0.150 + dictionary: "DICT_ARUCO_ORIGINAL" + + board: + xDist: 0.430 + yDist: 0.830 + ids: [28, 7, 96, 19] diff --git a/perception_setup/config/yolo/yolo_detect.yaml b/perception_setup/config/yolo/yolo_detect.yaml index a977d98..aa2f0d8 100644 --- a/perception_setup/config/yolo/yolo_detect.yaml +++ b/perception_setup/config/yolo/yolo_detect.yaml @@ -1,6 +1,3 @@ -# Camera source (resolved from cameras.yaml) -camera: "realsense_d555" - # Topics visualized_image_topic: "/yolov8_processed_image" # Output topic for visualized images with detection overlays detection_topic: "/yolov8_detections_output" # Output topic for detection results (Detection2DArray) diff --git a/perception_setup/config/yolo/yolo_obb.yaml b/perception_setup/config/yolo/yolo_obb.yaml index 164b071..0f65033 100644 --- a/perception_setup/config/yolo/yolo_obb.yaml +++ b/perception_setup/config/yolo/yolo_obb.yaml @@ -1,6 +1,3 @@ -# Camera source (resolved from cameras.yaml) -camera: "realsense_d555" - # Topics visualized_image_topic: "/yolo_obb_processed_image" detection_topic: "/obb_detections_output" diff --git a/perception_setup/config/yolo/yolo_seg.yaml b/perception_setup/config/yolo/yolo_seg.yaml index ec77c6b..e91312d 100644 --- a/perception_setup/config/yolo/yolo_seg.yaml +++ b/perception_setup/config/yolo/yolo_seg.yaml @@ -1,6 +1,3 @@ -# Camera source (resolved from cameras.yaml) -camera: "realsense_d555" - # Topics visualized_image_topic: "/yolo_seg_processed_image" detection_topic: "/seg_detections_output" diff --git a/perception_setup/launch/cameras/bfs_pge_16s2c.launch.py b/perception_setup/launch/cameras/bfs_pge_16s2c.launch.py new file mode 100644 index 0000000..9e9d747 --- /dev/null +++ b/perception_setup/launch/cameras/bfs_pge_16s2c.launch.py @@ -0,0 +1,94 @@ +"""Wrapper launch file for the FLIR Blackfly S BFS-PGE-16S2C-CS downwards camera. + +Starts: + - Spinnaker camera driver component in a dedicated container +""" + +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 +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + pkg_dir = get_package_share_directory("perception_setup") + + default_cam_config = os.path.join( + pkg_dir, "config", "cameras", "bfs_pge_16s2c_params.yaml" + ) + default_spinnaker_map = os.path.join( + pkg_dir, "config", "cameras", "blackfly_s_params.yaml" + ) + calib_path = os.path.join( + pkg_dir, "config", "cameras", "bfs_pge_16s2c_calib.yaml" + ) + calib_url = f"file://{calib_path}" + + enable_camera_arg = DeclareLaunchArgument( + "enable_camera", + default_value="true", + description="Enable FLIR BFS-PGE-16S2C-CS camera component", + ) + + camera_name_arg = DeclareLaunchArgument( + "camera_name", + default_value="bfs_pge_16s2c", + description="Camera node name (also used as topic namespace by the driver)", + ) + serial_arg = DeclareLaunchArgument( + "serial", + default_value="", + description="FLIR serial number (empty selects first available camera)", + ) + camera_params_arg = DeclareLaunchArgument( + "camera_params_file", + default_value=default_cam_config, + description="Path to ROS parameters for camera_driver_node", + ) + parameter_file_arg = DeclareLaunchArgument( + "parameter_file", + default_value=default_spinnaker_map, + description="Path to Spinnaker node-map parameter definition YAML", + ) + + flir_container = ComposableNodeContainer( + name="bfs_pge_16s2c_container", + namespace="", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="spinnaker_camera_driver", + plugin="spinnaker_camera_driver::CameraDriver", + name=LaunchConfiguration("camera_name"), + parameters=[ + LaunchConfiguration("camera_params_file"), + { + "parameter_file": LaunchConfiguration("parameter_file"), + "serial_number": LaunchConfiguration("serial"), + "camerainfo_url": calib_url, + }, + ], + remappings=[("~/control", "/exposure_control/control")], + extra_arguments=[{"use_intra_process_comms": True}], + condition=IfCondition(LaunchConfiguration("enable_camera")), + ) + ], + output="screen", + ) + + return LaunchDescription( + [ + enable_camera_arg, + camera_name_arg, + serial_arg, + camera_params_arg, + parameter_file_arg, + flir_container, + ] + ) diff --git a/perception_setup/launch/realsense_d555.launch.py b/perception_setup/launch/cameras/realsense_d555.launch.py similarity index 90% rename from perception_setup/launch/realsense_d555.launch.py rename to perception_setup/launch/cameras/realsense_d555.launch.py index 51317c7..774914c 100644 --- a/perception_setup/launch/realsense_d555.launch.py +++ b/perception_setup/launch/cameras/realsense_d555.launch.py @@ -31,8 +31,10 @@ def generate_launch_description(): pkg_dir, "config", "cameras", "color_realsense_d555_calib.yaml" ) - with open(camera_info_file) as f: - calib = yaml.safe_load(f) + cameras_path = os.path.join(pkg_dir, "config", "cameras", "cameras.yaml") + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras["realsense_d555"] helpers_dir = os.path.join(pkg_dir, "launch", "helpers") @@ -58,7 +60,7 @@ def generate_launch_description(): parameters=[ { "camera_info_file": camera_info_file, - "camera_info_topic": calib["camera_info_topic"], + "camera_info_topic": cam["calibration_camera_info_topic"], } ], output="screen", diff --git a/perception_setup/launch/helpers/camera_info_publisher.launch.py b/perception_setup/launch/image_processing/camera_info_publisher.launch.py similarity index 76% rename from perception_setup/launch/helpers/camera_info_publisher.launch.py rename to perception_setup/launch/image_processing/camera_info_publisher.launch.py index e4b9fdc..9faf528 100644 --- a/perception_setup/launch/helpers/camera_info_publisher.launch.py +++ b/perception_setup/launch/image_processing/camera_info_publisher.launch.py @@ -18,10 +18,10 @@ def generate_launch_description(): "color_realsense_d555_calib.yaml", ) - with open(camera_info_file) as f: - cfg = yaml.safe_load(f) - - camera_info_topic = cfg["camera_info_topic"] + cameras_path = os.path.join(pkg_dir, "config", "cameras", "cameras.yaml") + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras["realsense_d555"] camera_info_pub = Node( package="perception_setup", @@ -30,7 +30,7 @@ def generate_launch_description(): parameters=[ { "camera_info_file": camera_info_file, - "camera_info_topic": camera_info_topic, + "camera_info_topic": cam["calibration_camera_info_topic"], } ], output="screen", diff --git a/perception_setup/launch/helpers/image_crop.launch.py b/perception_setup/launch/image_processing/image_crop.launch.py similarity index 66% rename from perception_setup/launch/helpers/image_crop.launch.py rename to perception_setup/launch/image_processing/image_crop.launch.py index c76864c..f700ba0 100644 --- a/perception_setup/launch/helpers/image_crop.launch.py +++ b/perception_setup/launch/image_processing/image_crop.launch.py @@ -13,11 +13,17 @@ def generate_launch_description(): with open(config_file) as f: cfg = yaml.safe_load(f) + cameras_path = os.path.join(pkg_dir, "config", "cameras", "cameras.yaml") + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras[cfg["camera"]] + params = { - "image_topic": cfg["image_topic"], - "camera_info_topic": cfg["camera_info_topic"], - "output_image_topic": cfg["output_image_topic"], - "output_camera_info_topic": cfg["output_camera_info_topic"], + "image_topic": cam["raw_depth_topic"], + "camera_info_topic": cam["raw_depth_camera_info_topic"], + "output_image_topic": cam["depth_image_topic"], + "output_camera_info_topic": cam["depth_camera_info_topic"], + "enable_crop": bool(cam["enable_crop"]), "crop.x_offset": int(cfg["crop"]["x_offset"]), "crop.y_offset": int(cfg["crop"]["y_offset"]), "crop.width": int(cfg["crop"]["width"]), diff --git a/perception_setup/launch/helpers/image_undistort.launch.py b/perception_setup/launch/image_processing/image_undistort.launch.py similarity index 55% rename from perception_setup/launch/helpers/image_undistort.launch.py rename to perception_setup/launch/image_processing/image_undistort.launch.py index 492822c..0c01fe2 100644 --- a/perception_setup/launch/helpers/image_undistort.launch.py +++ b/perception_setup/launch/image_processing/image_undistort.launch.py @@ -15,6 +15,11 @@ def generate_launch_description(): with open(config_file) as f: cfg = yaml.safe_load(f) + cameras_path = os.path.join(pkg_dir, "config", "cameras", "cameras.yaml") + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras[cfg["camera"]] + return LaunchDescription( [ Node( @@ -23,10 +28,12 @@ def generate_launch_description(): name="image_undistort", parameters=[ { - "image_topic": cfg["image_topic"], - "camera_info_topic": cfg["camera_info_topic"], - "output_image_topic": cfg["output_image_topic"], - "output_camera_info_topic": cfg["output_camera_info_topic"], + "image_topic": cam["raw_image_topic"], + "camera_info_topic": cam["calibration_camera_info_topic"], + "raw_camera_info_topic": cam["raw_camera_info_topic"], + "output_image_topic": cam["image_topic"], + "output_camera_info_topic": cam["camera_info_topic"], + "enable_undistort": bool(cam["enable_undistort"]), } ], output="screen", diff --git a/perception_setup/launch/pipeline_following.launch.py b/perception_setup/launch/pipeline_following.launch.py index 8503014..d653c08 100644 --- a/perception_setup/launch/pipeline_following.launch.py +++ b/perception_setup/launch/pipeline_following.launch.py @@ -1,16 +1,16 @@ # SPDX-License-Identifier: MIT -"""Pipeline-following launch: Segmentation → Classification. +"""Pipeline-following launch: Segmentation -> Classification. Runs two TensorRT pipelines in a single launch: -Stage 1 – Segmentation - camera image → ImageFormatConverter → DNNImageEncoder → TensorRT (seg model) - → YoloV26SegDecoder → binary mask + detections + (optional) visualizer +Stage 1 - Segmentation + camera image -> ImageFormatConverter -> DNNImageEncoder -> TensorRT (seg model) + -> YoloV26SegDecoder -> binary mask + detections + (optional) visualizer -Stage 2 – Classification - seg mask → ImageFormatConverter → DNNImageEncoder → TensorRT (cls model) - → YoloV26ClsDecoder → UInt8 class ID + (optional) visualizer +Stage 2 - Classification + seg mask -> ImageFormatConverter -> DNNImageEncoder -> TensorRT (cls model) + -> YoloV26ClsDecoder -> UInt8 class ID + (optional) visualizer The segmentation mask output feeds directly into the classification input. """ @@ -57,16 +57,29 @@ def _launch_setup(context, *args, **kwargs): pkg_dir = get_package_share_directory('perception_setup') models_dir = os.path.join(pkg_dir, 'models') - # Resolve camera reference from cameras.yaml for seg stage + # Resolve camera references from cameras.yaml + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + if 'camera' in seg: - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) cam = cameras[seg['camera']] - seg['image_input_topic'] = cam['image_topic'] - seg['camera_info_input_topic'] = cam['camera_info_topic'] + if cam.get('enable_undistort', True): + seg['image_input_topic'] = cam['image_topic'] + seg['camera_info_input_topic'] = cam['camera_info_topic'] + else: + seg['image_input_topic'] = cam['raw_image_topic'] + seg['camera_info_input_topic'] = cam['raw_camera_info_topic'] seg['input_image_width'] = cam['image_width'] seg['input_image_height'] = cam['image_height'] + + if 'camera' in cls: + cam = cameras[cls['camera']] + if cam.get('enable_undistort', True): + cls['camera_info_input_topic'] = cam['camera_info_topic'] + else: + cls['camera_info_input_topic'] = cam['raw_camera_info_topic'] + encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder') # Stage 1 – Segmentation diff --git a/perception_setup/launch/pipeline_localization.launch.py b/perception_setup/launch/pipeline_localization.launch.py index 14d8806..062a1cc 100644 --- a/perception_setup/launch/pipeline_localization.launch.py +++ b/perception_setup/launch/pipeline_localization.launch.py @@ -7,10 +7,10 @@ to avoid node/topic collisions. Stage 1 – Front camera segmentation - /cam/image_color → seg pipeline → /localization/front/segmentation_mask + /cam/image_color -> seg pipeline -> /localization/front/segmentation_mask Stage 2 – Down camera segmentation - /cam_down/image_color → seg pipeline → /localization/down/segmentation_mask + /cam_down/image_color -> seg pipeline -> /localization/down/segmentation_mask """ import os @@ -191,8 +191,12 @@ def _launch_setup(context, *args, **kwargs): for seg_cfg in [cfg['seg_front'], cfg['seg_down']]: if 'camera' in seg_cfg: cam = cameras[seg_cfg['camera']] - seg_cfg['image_input_topic'] = cam['image_topic'] - seg_cfg['camera_info_input_topic'] = cam['camera_info_topic'] + if cam.get('enable_undistort', True): + seg_cfg['image_input_topic'] = cam['image_topic'] + seg_cfg['camera_info_input_topic'] = cam['camera_info_topic'] + else: + seg_cfg['image_input_topic'] = cam['raw_image_topic'] + seg_cfg['camera_info_input_topic'] = cam['raw_camera_info_topic'] seg_cfg['input_image_width'] = cam['image_width'] seg_cfg['input_image_height'] = cam['image_height'] diff --git a/perception_setup/launch/valve_intervention.launch.py b/perception_setup/launch/valve_intervention.launch.py new file mode 100644 index 0000000..b6b70b1 --- /dev/null +++ b/perception_setup/launch/valve_intervention.launch.py @@ -0,0 +1,151 @@ +"""Valve intervention pipeline launch. + +Starts: + - Camera driver (realsense_d555 by default, configured via valve_intervention.yaml) + - yolo_obb pipeline (input: camera image -> output: OBB detections) + - valve_detection node (input: detections + depth + camera_info -> output: valve landmarks) + +All tunable parameters live in perception_setup/config/valve_intervention.yaml. +""" + +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 Node + + +def _flatten(d, parent_key=''): + """Recursively flatten a nested dict to dot-notation ROS parameter keys.""" + items = {} + for k, v in d.items(): + full_key = f'{parent_key}.{k}' if parent_key else k + if isinstance(v, dict): + items.update(_flatten(v, full_key)) + else: + items[full_key] = v + return items + + +def _launch_setup(context, *args, **kwargs): + pkg_dir = get_package_share_directory('perception_setup') + + config_path = LaunchConfiguration('config_file').perform(context) + with open(config_path) as f: + cfg = yaml.safe_load(f) + + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + + camera_key = cfg['camera'] + cam = cameras[camera_key] + + # --- Camera driver --- + camera_launch_path = os.path.join( + pkg_dir, 'launch', 'cameras', f'{camera_key}.launch.py' + ) + camera_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(camera_launch_path) + ) + + # --- YOLO OBB pipeline --- + yolo_obb_config_path = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_obb.yaml') + + # Write a temporary merged YOLO config that includes camera-resolved fields + yolo_cfg = dict(cfg['yolo_obb']) + + # Resolve camera image topics for YOLO input + if cam.get('enable_undistort', True): + yolo_cfg['image_input_topic'] = cam['image_topic'] + yolo_cfg['camera_info_input_topic'] = cam['camera_info_topic'] + else: + yolo_cfg['image_input_topic'] = cam['raw_image_topic'] + yolo_cfg['camera_info_input_topic'] = cam['raw_camera_info_topic'] + yolo_cfg['input_image_width'] = cam['image_width'] + yolo_cfg['input_image_height'] = cam['image_height'] + + yolo_obb_launch_path = os.path.join( + pkg_dir, 'launch', 'yolo', 'yolo_obb.launch.py' + ) + yolo_obb_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(yolo_obb_launch_path), + launch_arguments={ + 'config_file': yolo_obb_config_path, + 'camera': camera_key, + }.items(), + ) + + # --- Valve Detection node --- + vd = cfg['valve_detection'] + + # Wire topics: detections from yolo_obb, color camera_info from encoder resize, + # depth image and depth camera_info from cameras.yaml + valve_params = { + 'detections_sub_topic': str(cfg['yolo_obb']['detection_topic']), + 'color_image_info_topic': str(vd['color_image_info_topic']), + 'depth_image_sub_topic': cam.get('depth_image_topic', cam.get('raw_depth_topic', '')), + 'depth_image_info_topic': cam.get('depth_camera_info_topic', cam.get('raw_depth_camera_info_topic', '')), + + # Extrinsic frame IDs (looked up from /tf_static) + 'depth_frame_id': str(vd['depth_frame_id']), + 'color_frame_id': str(vd['color_frame_id']), + + # Output + 'landmarks_pub_topic': str(vd['landmarks_pub_topic']), + + # YOLO letterbox + 'yolo_img_width': int(vd['yolo_img_width']), + 'yolo_img_height': int(vd['yolo_img_height']), + + # Annulus and plane fit + 'annulus_radius_ratio': float(vd['annulus_radius_ratio']), + 'plane_ransac_threshold': float(vd['plane_ransac_threshold']), + 'plane_ransac_max_iterations': int(vd['plane_ransac_max_iterations']), + + # Duplicate suppression + 'iou_duplicate_threshold': float(vd['iou_duplicate_threshold']), + + # Pose offset + 'valve_handle_offset': float(vd['valve_handle_offset']), + + # Output frame + 'output_frame_id': str(vd['output_frame_id']), + 'drone': str(vd['drone']), + + # Debug + 'debug_visualize': bool(vd['debug_visualize']), + } + + # Add debug params (flattened with dot notation) + valve_params.update(_flatten(vd['debug'], 'debug')) + + valve_detection_node = Node( + package='valve_detection', + executable='valve_detection_node', + name='valve_detection_node', + parameters=[valve_params], + output='screen', + ) + + return [camera_launch, yolo_obb_launch, valve_detection_node] + + +def generate_launch_description(): + pkg_dir = get_package_share_directory('perception_setup') + default_config = os.path.join(pkg_dir, 'config', 'valve_intervention.yaml') + + return LaunchDescription( + [ + DeclareLaunchArgument( + 'config_file', + default_value=default_config, + description='Path to valve intervention config YAML', + ), + 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..fdf84b9 --- /dev/null +++ b/perception_setup/launch/visual_inspection.launch.py @@ -0,0 +1,127 @@ +"""Visual inspection pipeline launch. + +Starts: + - Camera driver (realsense_d555 or bfs_pge_16s2c, configured via visual_inspection.yaml) + - image_filtering_node (input: camera image -> output: filtered image) + - aruco_detector_node (input: filtered image -> output: marker poses / board pose) + +All tunable parameters live in perception_setup/config/visual_inspection.yaml. +""" + +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 Node + + +def _flatten(d, parent_key=''): + """Recursively flatten a nested dict to dot-notation ROS parameter keys.""" + items = {} + for k, v in d.items(): + full_key = f'{parent_key}.{k}' if parent_key else k + if isinstance(v, dict): + items.update(_flatten(v, full_key)) + else: + items[full_key] = v + return items + + +def _launch_setup(context, *args, **kwargs): + pkg_dir = get_package_share_directory('perception_setup') + + config_path = LaunchConfiguration('config_file').perform(context) + with open(config_path) as f: + cfg = yaml.safe_load(f) + + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + + camera_key = cfg['camera'] + cam = cameras[camera_key] + + # Resolve image and camera_info topics based on enable_undistort flag + if cam.get('enable_undistort', True): + image_topic = cam['image_topic'] + camera_info_topic = cam['camera_info_topic'] + else: + image_topic = cam['raw_image_topic'] + camera_info_topic = cam['raw_camera_info_topic'] + + # --- Camera driver --- + camera_launch_path = os.path.join( + pkg_dir, 'launch', 'cameras', f'{camera_key}.launch.py' + ) + camera_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(camera_launch_path) + ) + + # --- image_filtering --- + filt = cfg['image_filtering'] + filtering_params = { + 'sub_topic': image_topic, + 'pub_topic': str(filt['pub_topic']), + 'output_encoding': str(filt['output_encoding']), + } + filtering_params.update(_flatten(filt['filter_params'], 'filter_params')) + + image_filtering_node = Node( + package='image_filtering', + executable='image_filtering_node', + name='image_filtering_node', + parameters=[filtering_params], + output='screen', + ) + + # --- aruco_detector --- + aruco = cfg['aruco_detector'] + aruco_params = { + 'subs.image_topic': str(filt['pub_topic']), + 'subs.camera_info_topic': camera_info_topic, + 'detect_board': bool(aruco['detect_board']), + 'visualize': bool(aruco['visualize']), + 'log_markers': bool(aruco['log_markers']), + 'publish_detections': bool(aruco['publish_detections']), + 'publish_landmarks': bool(aruco['publish_landmarks']), + 'logger_service_name': str(aruco['logger_service_name']), + 'pubs.aruco_image': str(aruco['pubs']['aruco_image']), + 'pubs.aruco_poses': str(aruco['pubs']['aruco_poses']), + 'pubs.board_pose': str(aruco['pubs']['board_pose']), + 'pubs.landmarks': str(aruco['pubs']['landmarks']), + 'aruco.marker_size': float(aruco['aruco']['marker_size']), + 'aruco.dictionary': str(aruco['aruco']['dictionary']), + 'board.xDist': float(aruco['board']['xDist']), + 'board.yDist': float(aruco['board']['yDist']), + 'board.ids': list(aruco['board']['ids']), + } + + aruco_detector_node = Node( + package='aruco_detector', + executable='aruco_detector_node', + name='aruco_detector_node', + parameters=[aruco_params], + output='screen', + ) + + return [camera_launch, image_filtering_node, aruco_detector_node] + + +def generate_launch_description(): + pkg_dir = get_package_share_directory('perception_setup') + default_config = os.path.join(pkg_dir, 'config', 'visual_inspection.yaml') + + return LaunchDescription( + [ + DeclareLaunchArgument( + 'config_file', + default_value=default_config, + description='Path to visual inspection config YAML', + ), + OpaqueFunction(function=_launch_setup), + ] + ) diff --git a/perception_setup/launch/yolo/yolo_detect.launch.py b/perception_setup/launch/yolo/yolo_detect.launch.py index b255bba..adbd561 100644 --- a/perception_setup/launch/yolo/yolo_detect.launch.py +++ b/perception_setup/launch/yolo/yolo_detect.launch.py @@ -59,16 +59,19 @@ def _launch_setup(context, *args, **kwargs): pkg_dir = get_package_share_directory('perception_setup') - # Resolve camera reference from cameras.yaml - if 'camera' in cfg: - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras[cfg['camera']] + # Resolve camera from launch argument + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras[LaunchConfiguration('camera').perform(context)] + if cam.get('enable_undistort', True): cfg['image_input_topic'] = cam['image_topic'] cfg['camera_info_input_topic'] = cam['camera_info_topic'] - cfg['input_image_width'] = cam['image_width'] - cfg['input_image_height'] = cam['image_height'] + else: + cfg['image_input_topic'] = cam['raw_image_topic'] + cfg['camera_info_input_topic'] = cam['raw_camera_info_topic'] + cfg['input_image_width'] = cam['image_width'] + cfg['input_image_height'] = cam['image_height'] required_keys = [ 'model_file_path', @@ -265,6 +268,11 @@ def generate_launch_description(): default_value=default_config, description='Path to YOLO pipeline config YAML', ), + DeclareLaunchArgument( + 'camera', + default_value='realsense_d555', + description='Camera name (key in cameras.yaml)', + ), OpaqueFunction(function=_launch_setup), ] ) diff --git a/perception_setup/launch/yolo/yolo_obb.launch.py b/perception_setup/launch/yolo/yolo_obb.launch.py index 6987d86..fd58f96 100644 --- a/perception_setup/launch/yolo/yolo_obb.launch.py +++ b/perception_setup/launch/yolo/yolo_obb.launch.py @@ -55,16 +55,19 @@ def _launch_setup(context, *args, **kwargs): pkg_dir = get_package_share_directory('perception_setup') - # Resolve camera reference from cameras.yaml - if 'camera' in cfg: - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras[cfg['camera']] + # Resolve camera from launch argument + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras[LaunchConfiguration('camera').perform(context)] + if cam.get('enable_undistort', True): cfg['image_input_topic'] = cam['image_topic'] cfg['camera_info_input_topic'] = cam['camera_info_topic'] - cfg['input_image_width'] = cam['image_width'] - cfg['input_image_height'] = cam['image_height'] + else: + cfg['image_input_topic'] = cam['raw_image_topic'] + cfg['camera_info_input_topic'] = cam['raw_camera_info_topic'] + cfg['input_image_width'] = cam['image_width'] + cfg['input_image_height'] = cam['image_height'] required_keys = [ 'model_file_path', @@ -258,6 +261,11 @@ def generate_launch_description(): default_value=default_config, description='Path to YOLO OBB pipeline config YAML', ), + DeclareLaunchArgument( + 'camera', + default_value='realsense_d555', + description='Camera name (key in cameras.yaml)', + ), OpaqueFunction(function=_launch_setup), ] ) diff --git a/perception_setup/launch/yolo/yolo_seg.launch.py b/perception_setup/launch/yolo/yolo_seg.launch.py index 76c4d73..abcd034 100644 --- a/perception_setup/launch/yolo/yolo_seg.launch.py +++ b/perception_setup/launch/yolo/yolo_seg.launch.py @@ -62,16 +62,19 @@ def _launch_setup(context, *args, **kwargs): pkg_dir = get_package_share_directory('perception_setup') - # Resolve camera reference from cameras.yaml - if 'camera' in cfg: - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras[cfg['camera']] + # Resolve camera from launch argument + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras[LaunchConfiguration('camera').perform(context)] + if cam.get('enable_undistort', True): cfg['image_input_topic'] = cam['image_topic'] cfg['camera_info_input_topic'] = cam['camera_info_topic'] - cfg['input_image_width'] = cam['image_width'] - cfg['input_image_height'] = cam['image_height'] + else: + cfg['image_input_topic'] = cam['raw_image_topic'] + cfg['camera_info_input_topic'] = cam['raw_camera_info_topic'] + cfg['input_image_width'] = cam['image_width'] + cfg['input_image_height'] = cam['image_height'] models_dir = os.path.join(pkg_dir, 'models') model_file_path = os.path.join(models_dir, str(cfg['model_file_path'])) @@ -248,6 +251,11 @@ def generate_launch_description(): default_value=default_config, description='Path to YOLO segmentation pipeline config YAML', ), + DeclareLaunchArgument( + 'camera', + default_value='realsense_d555', + description='Camera name (key in cameras.yaml)', + ), OpaqueFunction(function=_launch_setup), ] ) diff --git a/perception_setup/scripts/image_crop.py b/perception_setup/scripts/image_crop.py index 09f99aa..eeb4902 100755 --- a/perception_setup/scripts/image_crop.py +++ b/perception_setup/scripts/image_crop.py @@ -4,6 +4,7 @@ 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 @@ -11,18 +12,15 @@ class ImageCrop(Node): def __init__(self): super().__init__("image_crop") - self.declare_parameter("image_topic", "/camera/camera/depth/image_rect_raw") - self.declare_parameter("camera_info_topic", "/camera/camera/depth/camera_info") - self.declare_parameter( - "output_image_topic", "/camera/camera/depth/image_rect_raw/cropped" - ) - self.declare_parameter( - "output_camera_info_topic", "/camera/camera/depth/camera_info/cropped" - ) - self.declare_parameter("crop.x_offset", 0) - self.declare_parameter("crop.y_offset", 0) - self.declare_parameter("crop.width", 0) - self.declare_parameter("crop.height", 0) + 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 @@ -33,6 +31,7 @@ def __init__(self): 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() @@ -47,12 +46,22 @@ def __init__(self): self.image_pub = self.create_publisher(Image, out_image_topic, 10) self.info_pub = self.create_publisher(CameraInfo, out_info_topic, 10) - 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'}]" - ) + 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] diff --git a/perception_setup/scripts/image_undistort.py b/perception_setup/scripts/image_undistort.py index 54421b1..a4ca1e1 100755 --- a/perception_setup/scripts/image_undistort.py +++ b/perception_setup/scripts/image_undistort.py @@ -5,6 +5,7 @@ import rclpy from cv_bridge import CvBridge from rclpy.node import Node +from rclpy.parameter import Parameter from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy from sensor_msgs.msg import CameraInfo, Image @@ -13,42 +14,46 @@ class ImageUndistort(Node): def __init__(self): super().__init__("image_undistort") - self.declare_parameter("image_topic", "/camera/camera/color/image_raw") - self.declare_parameter( - "camera_info_topic", "/camera/camera/color_raw/camera_info" - ) - self.declare_parameter( - "output_image_topic", "/camera/camera/color/image_undistorted" - ) - self.declare_parameter( - "output_camera_info_topic", "/camera/camera/color/camera_info_undistorted" - ) + self.declare_parameter("image_topic", Parameter.Type.STRING) + self.declare_parameter("camera_info_topic", Parameter.Type.STRING) + self.declare_parameter("raw_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("enable_undistort", Parameter.Type.BOOL) image_topic = self.get_parameter("image_topic").value info_topic = self.get_parameter("camera_info_topic").value + raw_info_topic = self.get_parameter("raw_camera_info_topic").value out_topic = self.get_parameter("output_image_topic").value out_info_topic = self.get_parameter("output_camera_info_topic").value + enable_undistort = self.get_parameter("enable_undistort").value - self.bridge = CvBridge() - self.map1 = None - self.map2 = None - self.rectified_info = None - - # Camera info only needs to be received once — use transient local so we - # also catch messages published before this node started (latched). info_qos = QoSProfile(depth=1) info_qos.durability = DurabilityPolicy.TRANSIENT_LOCAL info_qos.reliability = ReliabilityPolicy.RELIABLE - self.create_subscription(CameraInfo, info_topic, self.info_callback, info_qos) - self.create_subscription(Image, image_topic, self.image_callback, 10) - self.pub = self.create_publisher(Image, out_topic, 10) self.info_pub = self.create_publisher(CameraInfo, out_info_topic, info_qos) - self.get_logger().info( - f"image_undistort: {image_topic} -> {out_topic}, {info_topic} -> {out_info_topic}" - ) + if enable_undistort: + self.bridge = CvBridge() + self.map1 = None + self.map2 = None + self.rectified_info = None + + # Camera info only needs to be received once — use transient local so we + # also catch messages published before this node started (latched). + self.create_subscription(CameraInfo, info_topic, self.info_callback, info_qos) + self.create_subscription(Image, image_topic, self.image_callback, 10) + self.get_logger().info( + f"image_undistort: {image_topic} -> {out_topic}" + ) + else: + self.create_subscription(Image, image_topic, self.relay_image, 10) + self.create_subscription(CameraInfo, raw_info_topic, self.relay_camera_info, info_qos) + self.get_logger().info( + f"image_undistort: passthrough {image_topic} -> {out_topic}" + ) def info_callback(self, msg: CameraInfo): if self.map1 is not None: @@ -102,6 +107,12 @@ def image_callback(self, msg: Image): self.rectified_info.header = msg.header self.info_pub.publish(self.rectified_info) + def relay_image(self, msg: Image): + self.pub.publish(msg) + + def relay_camera_info(self, msg: CameraInfo): + self.info_pub.publish(msg) + def main(): rclpy.init() From 07b45d409a352509e0dd792668f7e3f2fe736050 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Sat, 28 Mar 2026 03:38:05 +0100 Subject: [PATCH 14/37] refactor: format code for consistency and clarity across launch and script files --- .../launch/cameras/bfs_pge_16s2c.launch.py | 4 +-- .../launch/valve_intervention.launch.py | 26 +++++++------- .../launch/visual_inspection.launch.py | 6 +++- .../launch/yolo/yolo_detect.launch.py | 2 +- .../launch/yolo/yolo_seg.launch.py | 2 +- perception_setup/scripts/image_crop.py | 16 ++++----- perception_setup/scripts/image_undistort.py | 34 ++++++++++--------- 7 files changed, 46 insertions(+), 44 deletions(-) diff --git a/perception_setup/launch/cameras/bfs_pge_16s2c.launch.py b/perception_setup/launch/cameras/bfs_pge_16s2c.launch.py index 9e9d747..3ee746e 100644 --- a/perception_setup/launch/cameras/bfs_pge_16s2c.launch.py +++ b/perception_setup/launch/cameras/bfs_pge_16s2c.launch.py @@ -24,9 +24,7 @@ def generate_launch_description(): default_spinnaker_map = os.path.join( pkg_dir, "config", "cameras", "blackfly_s_params.yaml" ) - calib_path = os.path.join( - pkg_dir, "config", "cameras", "bfs_pge_16s2c_calib.yaml" - ) + calib_path = os.path.join(pkg_dir, "config", "cameras", "bfs_pge_16s2c_calib.yaml") calib_url = f"file://{calib_path}" enable_camera_arg = DeclareLaunchArgument( diff --git a/perception_setup/launch/valve_intervention.launch.py b/perception_setup/launch/valve_intervention.launch.py index b6b70b1..04d79d2 100644 --- a/perception_setup/launch/valve_intervention.launch.py +++ b/perception_setup/launch/valve_intervention.launch.py @@ -13,7 +13,11 @@ 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.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + OpaqueFunction, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -69,9 +73,7 @@ def _launch_setup(context, *args, **kwargs): yolo_cfg['input_image_width'] = cam['image_width'] yolo_cfg['input_image_height'] = cam['image_height'] - yolo_obb_launch_path = os.path.join( - pkg_dir, 'launch', 'yolo', 'yolo_obb.launch.py' - ) + yolo_obb_launch_path = os.path.join(pkg_dir, 'launch', 'yolo', 'yolo_obb.launch.py') yolo_obb_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource(yolo_obb_launch_path), launch_arguments={ @@ -88,35 +90,31 @@ def _launch_setup(context, *args, **kwargs): valve_params = { 'detections_sub_topic': str(cfg['yolo_obb']['detection_topic']), 'color_image_info_topic': str(vd['color_image_info_topic']), - 'depth_image_sub_topic': cam.get('depth_image_topic', cam.get('raw_depth_topic', '')), - 'depth_image_info_topic': cam.get('depth_camera_info_topic', cam.get('raw_depth_camera_info_topic', '')), - + 'depth_image_sub_topic': cam.get( + 'depth_image_topic', cam.get('raw_depth_topic', '') + ), + 'depth_image_info_topic': cam.get( + 'depth_camera_info_topic', cam.get('raw_depth_camera_info_topic', '') + ), # Extrinsic frame IDs (looked up from /tf_static) 'depth_frame_id': str(vd['depth_frame_id']), 'color_frame_id': str(vd['color_frame_id']), - # Output 'landmarks_pub_topic': str(vd['landmarks_pub_topic']), - # YOLO letterbox 'yolo_img_width': int(vd['yolo_img_width']), 'yolo_img_height': int(vd['yolo_img_height']), - # Annulus and plane fit 'annulus_radius_ratio': float(vd['annulus_radius_ratio']), 'plane_ransac_threshold': float(vd['plane_ransac_threshold']), 'plane_ransac_max_iterations': int(vd['plane_ransac_max_iterations']), - # Duplicate suppression 'iou_duplicate_threshold': float(vd['iou_duplicate_threshold']), - # Pose offset 'valve_handle_offset': float(vd['valve_handle_offset']), - # Output frame 'output_frame_id': str(vd['output_frame_id']), 'drone': str(vd['drone']), - # Debug 'debug_visualize': bool(vd['debug_visualize']), } diff --git a/perception_setup/launch/visual_inspection.launch.py b/perception_setup/launch/visual_inspection.launch.py index fdf84b9..a8e93fb 100644 --- a/perception_setup/launch/visual_inspection.launch.py +++ b/perception_setup/launch/visual_inspection.launch.py @@ -13,7 +13,11 @@ 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.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + OpaqueFunction, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node diff --git a/perception_setup/launch/yolo/yolo_detect.launch.py b/perception_setup/launch/yolo/yolo_detect.launch.py index adbd561..de12120 100644 --- a/perception_setup/launch/yolo/yolo_detect.launch.py +++ b/perception_setup/launch/yolo/yolo_detect.launch.py @@ -1,6 +1,6 @@ # SPDX-License-Identifier: MIT -"""Isaac ROS YOLOv8 TensorRT inference pipeline +"""Isaac ROS YOLOv8 TensorRT inference pipeline. 1. ImageFormatConverterNode Input: image_input_topic diff --git a/perception_setup/launch/yolo/yolo_seg.launch.py b/perception_setup/launch/yolo/yolo_seg.launch.py index abcd034..1153c3b 100644 --- a/perception_setup/launch/yolo/yolo_seg.launch.py +++ b/perception_setup/launch/yolo/yolo_seg.launch.py @@ -1,6 +1,6 @@ # SPDX-License-Identifier: MIT -"""Isaac ROS YOLO Instance-Segmentation TensorRT inference pipeline +"""Isaac ROS YOLO Instance-Segmentation TensorRT inference pipeline. 1. ImageFormatConverterNode Input: image_input_topic diff --git a/perception_setup/scripts/image_crop.py b/perception_setup/scripts/image_crop.py index eeb4902..cc0f055 100755 --- a/perception_setup/scripts/image_crop.py +++ b/perception_setup/scripts/image_crop.py @@ -86,17 +86,17 @@ def callback(self, image_msg: Image, info_msg: CameraInfo): 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 + 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 + 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 diff --git a/perception_setup/scripts/image_undistort.py b/perception_setup/scripts/image_undistort.py index a4ca1e1..843c783 100755 --- a/perception_setup/scripts/image_undistort.py +++ b/perception_setup/scripts/image_undistort.py @@ -43,14 +43,16 @@ def __init__(self): # Camera info only needs to be received once — use transient local so we # also catch messages published before this node started (latched). - self.create_subscription(CameraInfo, info_topic, self.info_callback, info_qos) - self.create_subscription(Image, image_topic, self.image_callback, 10) - self.get_logger().info( - f"image_undistort: {image_topic} -> {out_topic}" + self.create_subscription( + CameraInfo, info_topic, self.info_callback, info_qos ) + self.create_subscription(Image, image_topic, self.image_callback, 10) + self.get_logger().info(f"image_undistort: {image_topic} -> {out_topic}") else: self.create_subscription(Image, image_topic, self.relay_image, 10) - self.create_subscription(CameraInfo, raw_info_topic, self.relay_camera_info, info_qos) + self.create_subscription( + CameraInfo, raw_info_topic, self.relay_camera_info, info_qos + ) self.get_logger().info( f"image_undistort: passthrough {image_topic} -> {out_topic}" ) @@ -58,12 +60,12 @@ def __init__(self): def info_callback(self, msg: CameraInfo): if self.map1 is not None: return - K = np.array(msg.k, dtype=np.float64).reshape(3, 3) - D = np.array(msg.d, dtype=np.float64) + k = np.array(msg.k, dtype=np.float64).reshape(3, 3) + d = np.array(msg.d, dtype=np.float64) h, w = msg.height, msg.width - new_K, _ = cv2.getOptimalNewCameraMatrix(K, D, (w, h), alpha=0) + new_k, _ = cv2.getOptimalNewCameraMatrix(k, d, (w, h), alpha=0) self.map1, self.map2 = cv2.initUndistortRectifyMap( - K, D, None, new_K, (w, h), cv2.CV_16SC2 + k, d, None, new_k, (w, h), cv2.CV_16SC2 ) # Build the rectified camera_info (zero distortion, updated K and P) self.rectified_info = CameraInfo() @@ -71,23 +73,23 @@ def info_callback(self, msg: CameraInfo): self.rectified_info.height = h self.rectified_info.distortion_model = "plumb_bob" self.rectified_info.d = [0.0, 0.0, 0.0, 0.0, 0.0] - self.rectified_info.k = new_K.flatten().tolist() + self.rectified_info.k = new_k.flatten().tolist() self.rectified_info.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] - P = [ - new_K[0, 0], + p = [ + new_k[0, 0], 0.0, - new_K[0, 2], + new_k[0, 2], 0.0, 0.0, - new_K[1, 1], - new_K[1, 2], + new_k[1, 1], + new_k[1, 2], 0.0, 0.0, 0.0, 1.0, 0.0, ] - self.rectified_info.p = P + self.rectified_info.p = p self.get_logger().info(f"Undistortion maps initialised ({w}x{h})") def image_callback(self, msg: Image): From eb9bd39c7f988c2a93aad03e56f0eb5308fc3c0a Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Sat, 28 Mar 2026 22:18:03 +0100 Subject: [PATCH 15/37] feat: update camera configurations and processing scripts for improved functionality and reliability --- perception_setup/config/cameras/cameras.yaml | 75 +++++++++++- .../config/valve_intervention.yaml | 13 +- .../config/visual_inspection.yaml | 50 ++++++-- perception_setup/config/yolo/yolo_obb.yaml | 4 +- .../launch/cameras/realsense_d555.launch.py | 2 +- .../launch/valve_intervention.launch.py | 113 +++++++++++++----- .../launch/visual_inspection.launch.py | 53 ++++++-- perception_setup/package.xml | 4 + .../scripts/camera_info_publisher.py | 10 +- perception_setup/scripts/image_crop.py | 17 ++- perception_setup/scripts/image_undistort.py | 22 ++-- 11 files changed, 276 insertions(+), 87 deletions(-) diff --git a/perception_setup/config/cameras/cameras.yaml b/perception_setup/config/cameras/cameras.yaml index 3210677..753ae76 100644 --- a/perception_setup/config/cameras/cameras.yaml +++ b/perception_setup/config/cameras/cameras.yaml @@ -8,12 +8,19 @@ realsense_d555: # Raw topics from the camera driver raw_image_topic: "/camera/camera/color/image_raw" - raw_camera_info_topic: "/camera/camera/color/camera_info_rect" + raw_camera_info_topic: "/camera/camera/color/camera_info" raw_depth_topic: "/camera/camera/depth/image_rect_raw" raw_depth_camera_info_topic: "/camera/camera/depth/camera_info" # Calibration camera info (published by camera_info_publisher, contains distortion coefficients) calibration_camera_info_topic: "/camera/camera/color_raw/camera_info" + # Path to the calibration YAML (relative to perception_setup/config/cameras/). + # Used by pipeline launch files to start the camera_info_publisher when use_rosbag is true. + calibration_file: "color_realsense_d555_calib.yaml" + + # When true, skip launching the camera driver and subscribe directly to the + # raw topics (e.g. when replaying a rosbag). + use_rosbag: true # Image processing flags enable_undistort: true @@ -25,6 +32,10 @@ realsense_d555: depth_image_topic: "/realsense_d555/depth/image" depth_camera_info_topic: "/realsense_d555/depth/camera_info" + # TF frame IDs (without drone prefix — prepended at runtime) + color_frame_id: "front_camera_color_optical" + depth_frame_id: "front_camera_depth_optical" + # Image properties image_width: 896 image_height: 504 @@ -38,6 +49,10 @@ bfs_pge_16s2c: # Driver publishes calibrated camera_info directly via camerainfo_url — no separate publisher needed. calibration_camera_info_topic: "/bfs_pge_16s2c/camera_info" + # When true, skip launching the camera driver and subscribe directly to the + # raw topics (e.g. when replaying a rosbag). + use_rosbag: false + # Image processing flags enable_undistort: false enable_crop: false @@ -46,7 +61,65 @@ bfs_pge_16s2c: image_topic: "/bfs_pge_16s2c/color/image" camera_info_topic: "/bfs_pge_16s2c/color/camera_info" + # TF frame IDs (without drone prefix — prepended at runtime) + color_frame_id: "bfs_pge_16s2c_color_optical" + # Image properties (720x540 after 2x binning from 1440x1080) image_width: 720 image_height: 540 encoding: "bgr8" + +# ── Simulator cameras ─────────────────────────────────────────────────────── + +sim_front_camera: + # Topics from the simulator (prefixed with robot namespace at runtime) + raw_image_topic: "/nautilus/front_camera/image_color" + raw_camera_info_topic: "/nautilus/front_camera/camera_info" + raw_depth_topic: "/nautilus/depth_camera/image_depth" + raw_depth_camera_info_topic: "/nautilus/depth_camera/camera_info" + + # No physical camera driver to launch + use_rosbag: true + + # Simulator images are already undistorted + enable_undistort: false + enable_crop: false + + # Raw and processed topics are the same (no processing needed) + image_topic: "/nautilus/front_camera/image_color" + camera_info_topic: "/nautilus/front_camera/camera_info" + depth_image_topic: "/nautilus/depth_camera/image_depth" + depth_camera_info_topic: "/nautilus/depth_camera/camera_info" + + # TF frame IDs (without drone prefix — prepended at runtime) + color_frame_id: "front_camera_color_optical" + depth_frame_id: "front_camera_depth_optical" + + # Image properties + image_width: 1920 + image_height: 1080 + encoding: "rgb8" + +sim_down_camera: + # Topics from the simulator + raw_image_topic: "/nautilus/down_camera/image_color" + raw_camera_info_topic: "/nautilus/down_camera/camera_info" + + # No physical camera driver to launch + use_rosbag: true + + # Simulator images are already undistorted + enable_undistort: false + enable_crop: false + + # Raw and processed topics are the same + image_topic: "/nautilus/down_camera/image_color" + camera_info_topic: "/nautilus/down_camera/camera_info" + + # TF frame IDs (without drone prefix — prepended at runtime) + color_frame_id: "down_camera_color_optical" + + # Image properties + image_width: 1440 + image_height: 1080 + encoding: "rgb8" diff --git a/perception_setup/config/valve_intervention.yaml b/perception_setup/config/valve_intervention.yaml index 00d66b3..b558682 100644 --- a/perception_setup/config/valve_intervention.yaml +++ b/perception_setup/config/valve_intervention.yaml @@ -45,10 +45,6 @@ valve_detection: # Inputs — detections and depth topics wired automatically from yolo_obb / cameras.yaml color_image_info_topic: "/yolo_obb_encoder/internal/resize/camera_info" - # Depth-to-color extrinsic (looked up from /tf_static) - depth_frame_id: "front_camera_depth_optical" - color_frame_id: "front_camera_color_optical" - # Output landmarks_pub_topic: "/valve_landmarks" @@ -59,20 +55,19 @@ valve_detection: # Annulus and plane fit annulus_radius_ratio: 0.8 plane_ransac_threshold: 0.01 - plane_ransac_max_iterations: 50 + plane_ransac_max_iterations: 150 # Duplicate-detection suppression iou_duplicate_threshold: 0.5 # Pose offset - valve_handle_offset: 0.065 + valve_handle_offset: 0.02 - # Output frame - output_frame_id: "front_camera_depth_optical" + # Drone namespace (prepended to frame IDs from cameras.yaml) drone: "nautilus" # Behaviour toggles - debug_visualize: false + debug_visualize: true # Debug visualization (only active when debug_visualize: true) debug: diff --git a/perception_setup/config/visual_inspection.yaml b/perception_setup/config/visual_inspection.yaml index 5771126..7280b8b 100644 --- a/perception_setup/config/visual_inspection.yaml +++ b/perception_setup/config/visual_inspection.yaml @@ -3,17 +3,19 @@ # camera: Key from cameras.yaml — determines which camera driver to launch # and which image/camera_info topics to feed into the pipeline. -camera: "realsense_d555" +camera: "sim_front_camera" # --------------------------------------------------------------------------- # Image filtering # sub_topic is resolved automatically from cameras.yaml (based on enable_undistort flag) # --------------------------------------------------------------------------- image_filtering: - pub_topic: "/visual_inspection/filtered_image" + pub_topic: "/filtered_image" + input_encoding: "rgb8" output_encoding: "rgb8" + filter_params: - filter_type: "no_filter" + filter_type: "remove_grid" flip: flip_code: 1 unsharpening: @@ -21,22 +23,48 @@ image_filtering: erosion: size: 1 dilation: - size: 1 + size: 3 white_balancing: - contrast_percentage: 0.1 + contrast_percentage: 10.0 ebus: erosion_size: 2 blur_size: 30 mask_weight: 5 otsu: - gsc_weight_r: 1.0 - gsc_weight_g: 0.0 - gsc_weight_b: 0.0 + gsc_weight_r: 1.0 # Grayscale red weight + gsc_weight_g: 0.0 # Grayscale green weight + gsc_weight_b: 0.0 # Grayscale blue weight gamma_auto_correction: true gamma_auto_correction_weight: 4.0 otsu_segmentation: true - erosion_size: 10 - dilation_size: 10 + erosion_size: 2 + dilation_size: 2 + remove_grid: + threshold_green: 0.5 + threshold_binary: 30.0 + inpaint_radius: 1.0 + rotation: 0 + height: 400 + width: 400 + overlap: + percentage_threshold: 20.0 # Percentage (0-100) to cap the pixel intensity difference + median_binary: # finds the median of each n x n square around each pixel + kernel_size: 3 # must be odd > 1 + threshold: 100 # [0, 255] + invert: false + binary: + threshold: 20. # in percent + maxval: 255. + invert: true + temporal_noise: + median_kernel_size: 3 + power_law_weight: 4.0 # Weight for the gamma function + erotion_size: 2 + dilation_size: 4 + canny_high: 90 # upper weight for canny edge detection + canny_low: 30 # lower weight ---------- || ---------- + edge_protection_radius: 2 # Radius for protecting edges + # --------------------------------------------------------------------------- # ArUco detector @@ -54,7 +82,7 @@ aruco_detector: detect_board: false visualize: true - log_markers: true + log_markers: false publish_detections: true publish_landmarks: false diff --git a/perception_setup/config/yolo/yolo_obb.yaml b/perception_setup/config/yolo/yolo_obb.yaml index 0f65033..665bb38 100644 --- a/perception_setup/config/yolo/yolo_obb.yaml +++ b/perception_setup/config/yolo/yolo_obb.yaml @@ -3,8 +3,8 @@ visualized_image_topic: "/yolo_obb_processed_image" detection_topic: "/obb_detections_output" # Model and engine file paths (relative to perception_setup/models/) -model_file_path: "obb_best.onnx" -engine_file_path: "obb_best.engine" +model_file_path: "obb_best_simulator.onnx" +engine_file_path: "obb_best_simulator.engine" # Visualizer enable_visualizer: true diff --git a/perception_setup/launch/cameras/realsense_d555.launch.py b/perception_setup/launch/cameras/realsense_d555.launch.py index 774914c..a00ea1d 100644 --- a/perception_setup/launch/cameras/realsense_d555.launch.py +++ b/perception_setup/launch/cameras/realsense_d555.launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): cameras = yaml.safe_load(f) cam = cameras["realsense_d555"] - helpers_dir = os.path.join(pkg_dir, "launch", "helpers") + helpers_dir = os.path.join(pkg_dir, "launch", "image_processing") config_file_arg = DeclareLaunchArgument( "config_file", diff --git a/perception_setup/launch/valve_intervention.launch.py b/perception_setup/launch/valve_intervention.launch.py index 04d79d2..37f5099 100644 --- a/perception_setup/launch/valve_intervention.launch.py +++ b/perception_setup/launch/valve_intervention.launch.py @@ -2,6 +2,7 @@ Starts: - Camera driver (realsense_d555 by default, configured via valve_intervention.yaml) + - Image processing (undistort / crop, if enabled in cameras.yaml) - yolo_obb pipeline (input: camera image -> output: OBB detections) - valve_detection node (input: detections + depth + camera_info -> output: valve landmarks) @@ -9,6 +10,7 @@ """ import os +import tempfile import yaml from ament_index_python.packages import get_package_share_directory @@ -49,35 +51,85 @@ def _launch_setup(context, *args, **kwargs): camera_key = cfg['camera'] cam = cameras[camera_key] - # --- Camera driver --- - camera_launch_path = os.path.join( - pkg_dir, 'launch', 'cameras', f'{camera_key}.launch.py' - ) - camera_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(camera_launch_path) - ) + actions = [] + + # --- Camera driver (skipped when use_rosbag: true) --- + if cam.get('use_rosbag', False): + # When using a rosbag, launch undistort/crop separately if enabled + # (the camera driver launch bundles them, but we skip the driver). + if cam.get('enable_undistort', False): + # Undistort needs calibration camera_info — launch the publisher. + if 'calibration_file' in cam: + calib_path = os.path.join( + pkg_dir, 'config', 'cameras', cam['calibration_file'] + ) + actions.append(Node( + package='perception_setup', + executable='camera_info_publisher.py', + name='camera_info_publisher', + parameters=[{ + 'camera_info_file': calib_path, + 'camera_info_topic': cam['calibration_camera_info_topic'], + }], + output='screen', + )) + actions.append(IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + pkg_dir, 'launch', 'image_processing', + 'image_undistort.launch.py', + )) + )) + if cam.get('enable_crop', False): + actions.append(IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + pkg_dir, 'launch', 'image_processing', + 'image_crop.launch.py', + )) + )) + else: + # Camera driver launch includes undistort/crop if configured + camera_launch_path = os.path.join( + pkg_dir, 'launch', 'cameras', f'{camera_key}.launch.py' + ) + actions.append(IncludeLaunchDescription( + PythonLaunchDescriptionSource(camera_launch_path) + )) + + # --- Resolve topics based on enable_undistort (independent of use_rosbag) --- + if cam.get('enable_undistort', True): + color_image_topic = cam['image_topic'] + color_info_topic = cam['camera_info_topic'] + else: + color_image_topic = cam['raw_image_topic'] + color_info_topic = cam['raw_camera_info_topic'] - # --- YOLO OBB pipeline --- - yolo_obb_config_path = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_obb.yaml') + if cam.get('enable_crop', True): + depth_image_topic = cam.get('depth_image_topic', cam.get('raw_depth_topic', '')) + depth_info_topic = cam.get( + 'depth_camera_info_topic', cam.get('raw_depth_camera_info_topic', '') + ) + else: + depth_image_topic = cam.get('raw_depth_topic', '') + depth_info_topic = cam.get('raw_depth_camera_info_topic', '') - # Write a temporary merged YOLO config that includes camera-resolved fields + # --- YOLO OBB pipeline --- yolo_cfg = dict(cfg['yolo_obb']) - - # Resolve camera image topics for YOLO input - if cam.get('enable_undistort', True): - yolo_cfg['image_input_topic'] = cam['image_topic'] - yolo_cfg['camera_info_input_topic'] = cam['camera_info_topic'] - else: - yolo_cfg['image_input_topic'] = cam['raw_image_topic'] - yolo_cfg['camera_info_input_topic'] = cam['raw_camera_info_topic'] + yolo_cfg['image_input_topic'] = color_image_topic + yolo_cfg['camera_info_input_topic'] = color_info_topic yolo_cfg['input_image_width'] = cam['image_width'] yolo_cfg['input_image_height'] = cam['image_height'] + tmp_yolo_cfg = tempfile.NamedTemporaryFile( + mode='w', suffix='.yaml', prefix='yolo_obb_', delete=False + ) + yaml.dump(yolo_cfg, tmp_yolo_cfg, default_flow_style=False) + tmp_yolo_cfg.close() + yolo_obb_launch_path = os.path.join(pkg_dir, 'launch', 'yolo', 'yolo_obb.launch.py') yolo_obb_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource(yolo_obb_launch_path), launch_arguments={ - 'config_file': yolo_obb_config_path, + 'config_file': tmp_yolo_cfg.name, 'camera': camera_key, }.items(), ) @@ -85,20 +137,14 @@ def _launch_setup(context, *args, **kwargs): # --- Valve Detection node --- vd = cfg['valve_detection'] - # Wire topics: detections from yolo_obb, color camera_info from encoder resize, - # depth image and depth camera_info from cameras.yaml valve_params = { 'detections_sub_topic': str(cfg['yolo_obb']['detection_topic']), 'color_image_info_topic': str(vd['color_image_info_topic']), - 'depth_image_sub_topic': cam.get( - 'depth_image_topic', cam.get('raw_depth_topic', '') - ), - 'depth_image_info_topic': cam.get( - 'depth_camera_info_topic', cam.get('raw_depth_camera_info_topic', '') - ), - # Extrinsic frame IDs (looked up from /tf_static) - 'depth_frame_id': str(vd['depth_frame_id']), - 'color_frame_id': str(vd['color_frame_id']), + 'depth_image_sub_topic': depth_image_topic, + 'depth_image_info_topic': depth_info_topic, + # Extrinsic frame IDs (from cameras.yaml, looked up from /tf_static) + 'depth_frame_id': str(cam.get('depth_frame_id', '')), + 'color_frame_id': str(cam.get('color_frame_id', '')), # Output 'landmarks_pub_topic': str(vd['landmarks_pub_topic']), # YOLO letterbox @@ -112,8 +158,8 @@ def _launch_setup(context, *args, **kwargs): 'iou_duplicate_threshold': float(vd['iou_duplicate_threshold']), # Pose offset 'valve_handle_offset': float(vd['valve_handle_offset']), - # Output frame - 'output_frame_id': str(vd['output_frame_id']), + # Output frame (defaults to depth_frame_id from cameras.yaml) + 'output_frame_id': str(cam.get('depth_frame_id', '')), 'drone': str(vd['drone']), # Debug 'debug_visualize': bool(vd['debug_visualize']), @@ -130,7 +176,8 @@ def _launch_setup(context, *args, **kwargs): output='screen', ) - return [camera_launch, yolo_obb_launch, valve_detection_node] + actions += [yolo_obb_launch, valve_detection_node] + return actions def generate_launch_description(): diff --git a/perception_setup/launch/visual_inspection.launch.py b/perception_setup/launch/visual_inspection.launch.py index a8e93fb..b6b9e78 100644 --- a/perception_setup/launch/visual_inspection.launch.py +++ b/perception_setup/launch/visual_inspection.launch.py @@ -49,7 +49,7 @@ def _launch_setup(context, *args, **kwargs): camera_key = cfg['camera'] cam = cameras[camera_key] - # Resolve image and camera_info topics based on enable_undistort flag + # Resolve image and camera_info topics based on enable_undistort. if cam.get('enable_undistort', True): image_topic = cam['image_topic'] camera_info_topic = cam['camera_info_topic'] @@ -57,19 +57,53 @@ def _launch_setup(context, *args, **kwargs): image_topic = cam['raw_image_topic'] camera_info_topic = cam['raw_camera_info_topic'] - # --- Camera driver --- - camera_launch_path = os.path.join( - pkg_dir, 'launch', 'cameras', f'{camera_key}.launch.py' - ) - camera_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(camera_launch_path) - ) + # --- Camera driver / image processing --- + actions = [] + if cam.get('use_rosbag', False): + # Rosbag provides raw topics; launch undistort/crop separately if enabled. + if cam.get('enable_undistort', False): + if 'calibration_file' in cam: + calib_path = os.path.join( + pkg_dir, 'config', 'cameras', cam['calibration_file'] + ) + actions.append(Node( + package='perception_setup', + executable='camera_info_publisher.py', + name='camera_info_publisher', + parameters=[{ + 'camera_info_file': calib_path, + 'camera_info_topic': cam['calibration_camera_info_topic'], + }], + output='screen', + )) + actions.append(IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + pkg_dir, 'launch', 'image_processing', + 'image_undistort.launch.py', + )) + )) + if cam.get('enable_crop', False): + actions.append(IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + pkg_dir, 'launch', 'image_processing', + 'image_crop.launch.py', + )) + )) + else: + # Camera driver launch includes undistort/crop if configured. + camera_launch_path = os.path.join( + pkg_dir, 'launch', 'cameras', f'{camera_key}.launch.py' + ) + actions.append(IncludeLaunchDescription( + PythonLaunchDescriptionSource(camera_launch_path) + )) # --- image_filtering --- filt = cfg['image_filtering'] filtering_params = { 'sub_topic': image_topic, 'pub_topic': str(filt['pub_topic']), + 'input_encoding': str(filt['input_encoding']), 'output_encoding': str(filt['output_encoding']), } filtering_params.update(_flatten(filt['filter_params'], 'filter_params')) @@ -112,7 +146,8 @@ def _launch_setup(context, *args, **kwargs): output='screen', ) - return [camera_launch, image_filtering_node, aruco_detector_node] + actions += [image_filtering_node, aruco_detector_node] + return actions def generate_launch_description(): diff --git a/perception_setup/package.xml b/perception_setup/package.xml index 6c79d28..b50e52c 100644 --- a/perception_setup/package.xml +++ b/perception_setup/package.xml @@ -8,6 +8,7 @@ MIT camera_info_manager + vortex_utils_ros ament_cmake @@ -23,6 +24,9 @@ isaac_ros_tensor_rt isaac_ros_dnn_image_encoder isaac_ros_image_proc + valve_detection + image_filtering + aruco_detector ament_cmake diff --git a/perception_setup/scripts/camera_info_publisher.py b/perception_setup/scripts/camera_info_publisher.py index 75dcd48..b918091 100755 --- a/perception_setup/scripts/camera_info_publisher.py +++ b/perception_setup/scripts/camera_info_publisher.py @@ -4,8 +4,8 @@ import yaml from rclpy.node import Node from rclpy.parameter import Parameter -from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy from sensor_msgs.msg import CameraInfo +from vortex_utils_ros.qos_profiles import reliable_profile class CameraInfoPublisher(Node): @@ -38,11 +38,9 @@ def __init__(self): msg.r = data["rectification_matrix"]["data"] msg.p = data["projection_matrix"]["data"] - qos = QoSProfile(depth=1) - qos.reliability = ReliabilityPolicy.RELIABLE - qos.durability = DurabilityPolicy.TRANSIENT_LOCAL - - self.publisher = self.create_publisher(CameraInfo, topic_name, qos) + self.publisher = self.create_publisher( + CameraInfo, topic_name, reliable_profile(1) + ) self.msg = msg self.create_timer(1.0, self.publish_camera_info) diff --git a/perception_setup/scripts/image_crop.py b/perception_setup/scripts/image_crop.py index cc0f055..ed52541 100755 --- a/perception_setup/scripts/image_crop.py +++ b/perception_setup/scripts/image_crop.py @@ -6,6 +6,7 @@ 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, sensor_data_profile class ImageCrop(Node): @@ -35,16 +36,24 @@ def __init__(self): self.bridge = CvBridge() - image_sub = Subscriber(self, Image, image_topic) - info_sub = Subscriber(self, CameraInfo, info_topic) + 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, 10) - self.info_pub = self.create_publisher(CameraInfo, out_info_topic, 10) + 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( diff --git a/perception_setup/scripts/image_undistort.py b/perception_setup/scripts/image_undistort.py index 843c783..41a8fe3 100755 --- a/perception_setup/scripts/image_undistort.py +++ b/perception_setup/scripts/image_undistort.py @@ -6,8 +6,8 @@ from cv_bridge import CvBridge from rclpy.node import Node from rclpy.parameter import Parameter -from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy from sensor_msgs.msg import CameraInfo, Image +from vortex_utils_ros.qos_profiles import reliable_profile, sensor_data_profile class ImageUndistort(Node): @@ -28,12 +28,8 @@ def __init__(self): out_info_topic = self.get_parameter("output_camera_info_topic").value enable_undistort = self.get_parameter("enable_undistort").value - info_qos = QoSProfile(depth=1) - info_qos.durability = DurabilityPolicy.TRANSIENT_LOCAL - info_qos.reliability = ReliabilityPolicy.RELIABLE - - self.pub = self.create_publisher(Image, out_topic, 10) - self.info_pub = self.create_publisher(CameraInfo, out_info_topic, info_qos) + self.pub = self.create_publisher(Image, out_topic, reliable_profile(10)) + self.info_pub = self.create_publisher(CameraInfo, out_info_topic, reliable_profile(10)) if enable_undistort: self.bridge = CvBridge() @@ -44,14 +40,18 @@ def __init__(self): # Camera info only needs to be received once — use transient local so we # also catch messages published before this node started (latched). self.create_subscription( - CameraInfo, info_topic, self.info_callback, info_qos + CameraInfo, info_topic, self.info_callback, reliable_profile(1) + ) + self.create_subscription( + Image, image_topic, self.image_callback, sensor_data_profile(10) ) - self.create_subscription(Image, image_topic, self.image_callback, 10) self.get_logger().info(f"image_undistort: {image_topic} -> {out_topic}") else: - self.create_subscription(Image, image_topic, self.relay_image, 10) self.create_subscription( - CameraInfo, raw_info_topic, self.relay_camera_info, info_qos + Image, image_topic, self.relay_image, sensor_data_profile(10) + ) + self.create_subscription( + CameraInfo, raw_info_topic, self.relay_camera_info, reliable_profile(1) ) self.get_logger().info( f"image_undistort: passthrough {image_topic} -> {out_topic}" From 652eea49d86fc81fa2fe5c9af794755555a23c7f Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Sat, 28 Mar 2026 22:57:45 +0100 Subject: [PATCH 16/37] refactor: standardize comment formatting and improve code readability in configuration and launch files --- .../config/valve_intervention.yaml | 2 +- .../config/visual_inspection.yaml | 2 +- .../launch/valve_intervention.launch.py | 68 ++++++++++++------- .../launch/visual_inspection.launch.py | 68 ++++++++++++------- perception_setup/scripts/image_crop.py | 2 +- perception_setup/scripts/image_undistort.py | 4 +- 6 files changed, 92 insertions(+), 54 deletions(-) diff --git a/perception_setup/config/valve_intervention.yaml b/perception_setup/config/valve_intervention.yaml index b558682..da15935 100644 --- a/perception_setup/config/valve_intervention.yaml +++ b/perception_setup/config/valve_intervention.yaml @@ -1,5 +1,5 @@ # Valve intervention pipeline configuration -# Chains: camera → yolo_obb → valve_detection +# Chains: camera -> yolo_obb -> valve_detection camera: "realsense_d555" diff --git a/perception_setup/config/visual_inspection.yaml b/perception_setup/config/visual_inspection.yaml index 7280b8b..3e157a9 100644 --- a/perception_setup/config/visual_inspection.yaml +++ b/perception_setup/config/visual_inspection.yaml @@ -1,4 +1,4 @@ -# Visual inspection pipeline: camera → image_filtering → aruco_detector +# Visual inspection pipeline: camera -> image_filtering -> aruco_detector # # camera: Key from cameras.yaml — determines which camera driver to launch # and which image/camera_info topics to feed into the pipeline. diff --git a/perception_setup/launch/valve_intervention.launch.py b/perception_setup/launch/valve_intervention.launch.py index 37f5099..b79f537 100644 --- a/perception_setup/launch/valve_intervention.launch.py +++ b/perception_setup/launch/valve_intervention.launch.py @@ -63,37 +63,55 @@ def _launch_setup(context, *args, **kwargs): calib_path = os.path.join( pkg_dir, 'config', 'cameras', cam['calibration_file'] ) - actions.append(Node( - package='perception_setup', - executable='camera_info_publisher.py', - name='camera_info_publisher', - parameters=[{ - 'camera_info_file': calib_path, - 'camera_info_topic': cam['calibration_camera_info_topic'], - }], - output='screen', - )) - actions.append(IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join( - pkg_dir, 'launch', 'image_processing', - 'image_undistort.launch.py', - )) - )) + actions.append( + Node( + package='perception_setup', + executable='camera_info_publisher.py', + name='camera_info_publisher', + parameters=[ + { + 'camera_info_file': calib_path, + 'camera_info_topic': cam[ + 'calibration_camera_info_topic' + ], + } + ], + output='screen', + ) + ) + actions.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + pkg_dir, + 'launch', + 'image_processing', + 'image_undistort.launch.py', + ) + ) + ) + ) if cam.get('enable_crop', False): - actions.append(IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join( - pkg_dir, 'launch', 'image_processing', - 'image_crop.launch.py', - )) - )) + actions.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + pkg_dir, + 'launch', + 'image_processing', + 'image_crop.launch.py', + ) + ) + ) + ) else: # Camera driver launch includes undistort/crop if configured camera_launch_path = os.path.join( pkg_dir, 'launch', 'cameras', f'{camera_key}.launch.py' ) - actions.append(IncludeLaunchDescription( - PythonLaunchDescriptionSource(camera_launch_path) - )) + actions.append( + IncludeLaunchDescription(PythonLaunchDescriptionSource(camera_launch_path)) + ) # --- Resolve topics based on enable_undistort (independent of use_rosbag) --- if cam.get('enable_undistort', True): diff --git a/perception_setup/launch/visual_inspection.launch.py b/perception_setup/launch/visual_inspection.launch.py index b6b9e78..e88a1df 100644 --- a/perception_setup/launch/visual_inspection.launch.py +++ b/perception_setup/launch/visual_inspection.launch.py @@ -66,37 +66,55 @@ def _launch_setup(context, *args, **kwargs): calib_path = os.path.join( pkg_dir, 'config', 'cameras', cam['calibration_file'] ) - actions.append(Node( - package='perception_setup', - executable='camera_info_publisher.py', - name='camera_info_publisher', - parameters=[{ - 'camera_info_file': calib_path, - 'camera_info_topic': cam['calibration_camera_info_topic'], - }], - output='screen', - )) - actions.append(IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join( - pkg_dir, 'launch', 'image_processing', - 'image_undistort.launch.py', - )) - )) + actions.append( + Node( + package='perception_setup', + executable='camera_info_publisher.py', + name='camera_info_publisher', + parameters=[ + { + 'camera_info_file': calib_path, + 'camera_info_topic': cam[ + 'calibration_camera_info_topic' + ], + } + ], + output='screen', + ) + ) + actions.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + pkg_dir, + 'launch', + 'image_processing', + 'image_undistort.launch.py', + ) + ) + ) + ) if cam.get('enable_crop', False): - actions.append(IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join( - pkg_dir, 'launch', 'image_processing', - 'image_crop.launch.py', - )) - )) + actions.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + pkg_dir, + 'launch', + 'image_processing', + 'image_crop.launch.py', + ) + ) + ) + ) else: # Camera driver launch includes undistort/crop if configured. camera_launch_path = os.path.join( pkg_dir, 'launch', 'cameras', f'{camera_key}.launch.py' ) - actions.append(IncludeLaunchDescription( - PythonLaunchDescriptionSource(camera_launch_path) - )) + actions.append( + IncludeLaunchDescription(PythonLaunchDescriptionSource(camera_launch_path)) + ) # --- image_filtering --- filt = cfg['image_filtering'] diff --git a/perception_setup/scripts/image_crop.py b/perception_setup/scripts/image_crop.py index ed52541..5198d30 100755 --- a/perception_setup/scripts/image_crop.py +++ b/perception_setup/scripts/image_crop.py @@ -6,7 +6,7 @@ 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, sensor_data_profile +from vortex_utils_ros.qos_profiles import sensor_data_profile class ImageCrop(Node): diff --git a/perception_setup/scripts/image_undistort.py b/perception_setup/scripts/image_undistort.py index 41a8fe3..09989c4 100755 --- a/perception_setup/scripts/image_undistort.py +++ b/perception_setup/scripts/image_undistort.py @@ -29,7 +29,9 @@ def __init__(self): enable_undistort = self.get_parameter("enable_undistort").value self.pub = self.create_publisher(Image, out_topic, reliable_profile(10)) - self.info_pub = self.create_publisher(CameraInfo, out_info_topic, reliable_profile(10)) + self.info_pub = self.create_publisher( + CameraInfo, out_info_topic, reliable_profile(10) + ) if enable_undistort: self.bridge = CvBridge() From ac443bef49cc2b780dbed4e588d06909d8655eae Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Wed, 1 Apr 2026 18:17:53 +0200 Subject: [PATCH 17/37] Refactor perception_setup launch files and scripts - Removed obsolete launch files: pipeline_localization.launch.py, valve_intervention.launch.py, visual_inspection.launch.py. - Updated YOLO launch files to streamline camera topic resolution from cameras.yaml. - Added a new script (tmux_perception.sh) to launch the perception stack in a tmux session for better monitoring and management. - Modified package.xml to remove unnecessary dependencies and added realsense2_camera and spinnaker_camera_driver. --- ...16s2c_calib.yaml => blackfly_s_calib.yaml} | 2 +- ...ams.yaml => blackfly_s_driver_params.yaml} | 0 perception_setup/config/cameras/cameras.yaml | 107 +----- .../config/cameras/realsense_d555.yaml | 92 ----- .../config/image_processing/image_crop.yaml | 14 - .../image_processing/image_undistort.yaml | 6 - .../config/pipeline_following.yaml | 84 ----- .../config/pipeline_localization.yaml | 84 ----- .../config/valve_intervention.yaml | 80 ----- .../config/visual_inspection.yaml | 96 ----- perception_setup/config/yolo/yolo_detect.yaml | 6 +- perception_setup/config/yolo/yolo_obb.yaml | 2 +- perception_setup/config/yolo/yolo_seg.yaml | 2 +- .../launch/cameras/bfs_pge_16s2c.launch.py | 92 ----- .../launch/cameras/blackfly_s.launch.py | 68 ++++ .../launch/cameras/realsense_d555.launch.py | 120 ++++--- .../camera_info_publisher.launch.py | 39 -- .../image_processing/image_crop.launch.py | 43 --- .../image_undistort.launch.py | 42 --- .../launch/pipeline_following.launch.py | 340 ------------------ .../launch/pipeline_localization.launch.py | 223 ------------ .../launch/valve_intervention.launch.py | 214 ----------- .../launch/visual_inspection.launch.py | 184 ---------- .../launch/yolo/yolo_cls.launch.py | 11 - .../launch/yolo/yolo_detect.launch.py | 52 +-- .../launch/yolo/yolo_obb.launch.py | 54 +-- .../launch/yolo/yolo_seg.launch.py | 24 +- perception_setup/package.xml | 6 +- perception_setup/scripts/tmux_perception.sh | 59 +++ 29 files changed, 247 insertions(+), 1899 deletions(-) rename perception_setup/config/cameras/{bfs_pge_16s2c_calib.yaml => blackfly_s_calib.yaml} (95%) rename perception_setup/config/cameras/{bfs_pge_16s2c_params.yaml => blackfly_s_driver_params.yaml} (100%) delete mode 100644 perception_setup/config/cameras/realsense_d555.yaml delete mode 100644 perception_setup/config/image_processing/image_crop.yaml delete mode 100644 perception_setup/config/image_processing/image_undistort.yaml delete mode 100644 perception_setup/config/pipeline_following.yaml delete mode 100644 perception_setup/config/pipeline_localization.yaml delete mode 100644 perception_setup/config/valve_intervention.yaml delete mode 100644 perception_setup/config/visual_inspection.yaml delete mode 100644 perception_setup/launch/cameras/bfs_pge_16s2c.launch.py create mode 100644 perception_setup/launch/cameras/blackfly_s.launch.py delete mode 100644 perception_setup/launch/image_processing/camera_info_publisher.launch.py delete mode 100644 perception_setup/launch/image_processing/image_crop.launch.py delete mode 100644 perception_setup/launch/image_processing/image_undistort.launch.py delete mode 100644 perception_setup/launch/pipeline_following.launch.py delete mode 100644 perception_setup/launch/pipeline_localization.launch.py delete mode 100644 perception_setup/launch/valve_intervention.launch.py delete mode 100644 perception_setup/launch/visual_inspection.launch.py create mode 100755 perception_setup/scripts/tmux_perception.sh diff --git a/perception_setup/config/cameras/bfs_pge_16s2c_calib.yaml b/perception_setup/config/cameras/blackfly_s_calib.yaml similarity index 95% rename from perception_setup/config/cameras/bfs_pge_16s2c_calib.yaml rename to perception_setup/config/cameras/blackfly_s_calib.yaml index 95c2df4..3670dd7 100644 --- a/perception_setup/config/cameras/bfs_pge_16s2c_calib.yaml +++ b/perception_setup/config/cameras/blackfly_s_calib.yaml @@ -1,6 +1,6 @@ image_width: 1440 image_height: 1080 -camera_name: "bfs_pge_16s2c" +camera_name: "blackfly_s" camera_matrix: rows: 3 diff --git a/perception_setup/config/cameras/bfs_pge_16s2c_params.yaml b/perception_setup/config/cameras/blackfly_s_driver_params.yaml similarity index 100% rename from perception_setup/config/cameras/bfs_pge_16s2c_params.yaml rename to perception_setup/config/cameras/blackfly_s_driver_params.yaml diff --git a/perception_setup/config/cameras/cameras.yaml b/perception_setup/config/cameras/cameras.yaml index 753ae76..5a52fbd 100644 --- a/perception_setup/config/cameras/cameras.yaml +++ b/perception_setup/config/cameras/cameras.yaml @@ -1,125 +1,50 @@ -# Centralized camera definitions -# Referenced by pipeline/YOLO/image-processing configs via "camera: " +# Single source of truth for camera topics and image properties. +# Both camera launch files and YOLO launch files read from here. # -# raw_*: Topics published directly by the camera driver (input to image processing) -# image_topic / camera_info_topic: Processed topics (output of image_undistort / image_crop) -# — downstream nodes (YOLO, pipelines) subscribe to these +# raw_*: Topics published directly by the camera driver +# output_*: Processed topics published by the camera pipeline (undistort/crop) +# — these are what downstream nodes (YOLO, etc.) subscribe to + +blackfly_s: + # No image processing — driver output IS the final output + image_topic: "/blackfly_s/image_raw" + camera_info_topic: "/blackfly_s/camera_info" + image_width: 720 + image_height: 540 + encoding: "bgr8" realsense_d555: - # Raw topics from the camera driver + # Raw topics from the RealSense driver raw_image_topic: "/camera/camera/color/image_raw" raw_camera_info_topic: "/camera/camera/color/camera_info" raw_depth_topic: "/camera/camera/depth/image_rect_raw" raw_depth_camera_info_topic: "/camera/camera/depth/camera_info" - - # Calibration camera info (published by camera_info_publisher, contains distortion coefficients) calibration_camera_info_topic: "/camera/camera/color_raw/camera_info" - # Path to the calibration YAML (relative to perception_setup/config/cameras/). - # Used by pipeline launch files to start the camera_info_publisher when use_rosbag is true. - calibration_file: "color_realsense_d555_calib.yaml" - # When true, skip launching the camera driver and subscribe directly to the - # raw topics (e.g. when replaying a rosbag). - use_rosbag: true - - # Image processing flags - enable_undistort: true - enable_crop: true - - # Processed topics (output of image_undistort / image_crop) + # Processed output topics (after undistort/crop) image_topic: "/realsense_d555/color/image" camera_info_topic: "/realsense_d555/color/camera_info" depth_image_topic: "/realsense_d555/depth/image" depth_camera_info_topic: "/realsense_d555/depth/camera_info" - # TF frame IDs (without drone prefix — prepended at runtime) - color_frame_id: "front_camera_color_optical" - depth_frame_id: "front_camera_depth_optical" - - # Image properties image_width: 896 image_height: 504 encoding: "rgb8" -bfs_pge_16s2c: - # Raw topics from spinnaker_camera_driver (node name: bfs_pge_16s2c) - raw_image_topic: "/bfs_pge_16s2c/image_raw" - raw_camera_info_topic: "/bfs_pge_16s2c/camera_info" - - # Driver publishes calibrated camera_info directly via camerainfo_url — no separate publisher needed. - calibration_camera_info_topic: "/bfs_pge_16s2c/camera_info" - - # When true, skip launching the camera driver and subscribe directly to the - # raw topics (e.g. when replaying a rosbag). - use_rosbag: false - - # Image processing flags - enable_undistort: false - enable_crop: false - - # Processed topics (output of image_undistort / image_crop) - image_topic: "/bfs_pge_16s2c/color/image" - camera_info_topic: "/bfs_pge_16s2c/color/camera_info" - - # TF frame IDs (without drone prefix — prepended at runtime) - color_frame_id: "bfs_pge_16s2c_color_optical" - - # Image properties (720x540 after 2x binning from 1440x1080) - image_width: 720 - image_height: 540 - encoding: "bgr8" - -# ── Simulator cameras ─────────────────────────────────────────────────────── +# Simulator cameras sim_front_camera: - # Topics from the simulator (prefixed with robot namespace at runtime) - raw_image_topic: "/nautilus/front_camera/image_color" - raw_camera_info_topic: "/nautilus/front_camera/camera_info" - raw_depth_topic: "/nautilus/depth_camera/image_depth" - raw_depth_camera_info_topic: "/nautilus/depth_camera/camera_info" - - # No physical camera driver to launch - use_rosbag: true - - # Simulator images are already undistorted - enable_undistort: false - enable_crop: false - - # Raw and processed topics are the same (no processing needed) image_topic: "/nautilus/front_camera/image_color" camera_info_topic: "/nautilus/front_camera/camera_info" depth_image_topic: "/nautilus/depth_camera/image_depth" depth_camera_info_topic: "/nautilus/depth_camera/camera_info" - - # TF frame IDs (without drone prefix — prepended at runtime) - color_frame_id: "front_camera_color_optical" - depth_frame_id: "front_camera_depth_optical" - - # Image properties image_width: 1920 image_height: 1080 encoding: "rgb8" sim_down_camera: - # Topics from the simulator - raw_image_topic: "/nautilus/down_camera/image_color" - raw_camera_info_topic: "/nautilus/down_camera/camera_info" - - # No physical camera driver to launch - use_rosbag: true - - # Simulator images are already undistorted - enable_undistort: false - enable_crop: false - - # Raw and processed topics are the same image_topic: "/nautilus/down_camera/image_color" camera_info_topic: "/nautilus/down_camera/camera_info" - - # TF frame IDs (without drone prefix — prepended at runtime) - color_frame_id: "down_camera_color_optical" - - # Image properties image_width: 1440 image_height: 1080 encoding: "rgb8" diff --git a/perception_setup/config/cameras/realsense_d555.yaml b/perception_setup/config/cameras/realsense_d555.yaml deleted file mode 100644 index 8a53f2e..0000000 --- a/perception_setup/config/cameras/realsense_d555.yaml +++ /dev/null @@ -1,92 +0,0 @@ -depth_module.emitter_enabled: false # Toggle emitter used for depth image (we dont want it underwater) - -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' - -enable_infra1: false -enable_infra2: false -depth_module.infra1_format: 'Y8' -depth_module.infra2_format: 'Y8' - -camera_namespace: 'nautilus' # Does nothing? -camera_name: 'front_camera' -tf_prefix: 'nautilus/' - -enable_gyro: false # Dont need IMU data (not being used) -enable_accel: false # Dont need IMU data (not being used) -enable_motion: false # Dont need IMU data (not being used) - -publish_tf: false # Publishes with wrong convention for ROS (z-frame for camera/optical frame) -enable_sync: false # Software sync (needs to be tested) - -# serial_no: "''" -# usb_port_id: "''" -# device_type: "''" -# config_file: "''" -# json_file_path: "''" -# initial_reset: 'false' -# accelerate_gpu_with_glsl: 'false' -# rosbag_filename: "''" -# rosbag_loop: 'false' -# log_level: 'info' -# output: 'screen' -# enable_infra: 'false' -# enable_infra1: 'false' -# enable_infra2: 'false' -# depth_module.infra_profile: '0,0,0' -# depth_module.infra_format: 'RGB8' -# depth_module.color_profile: '0,0,0' -# depth_module.color_format: 'RGB8' -# depth_module.exposure: '8500' -# depth_module.gain: '16' -# depth_module.hdr_enabled: 'false' -# depth_module.enable_auto_exposure: 'true' -# depth_module.exposure.1: '7500' -# depth_module.gain.1: '16' -# depth_module.exposure.2: '1' -# depth_module.gain.2: '16' -# enable_sync: 'false' -# depth_module.inter_cam_sync_mode: '0' -# enable_rgbd: 'false' -# gyro_fps: '0' -# accel_fps: '0' -# motion_fps: '0' -# unite_imu_method: '0' -# clip_distance: '-2.' -# angular_velocity_cov: '0.01' -# linear_accel_cov: '0.01' -# diagnostics_period: '0.0' -# publish_tf: 'true' -# tf_publish_rate: '0.0' -# pointcloud.enable: 'false' -# pointcloud.stream_filter: '2' -# pointcloud.stream_index_filter: '0' -# pointcloud.ordered_pc: 'false' -# pointcloud.allow_no_texture_points: 'false' -# align_depth.enable: 'false' -# colorizer.enable: 'false' -# decimation_filter.enable: 'false' -# rotation_filter.enable: 'false' -# rotation_filter.rotation: '0.0' -# spatial_filter.enable: 'false' -# temporal_filter.enable: 'false' -# disparity_filter.enable: 'false' -# hole_filling_filter.enable: 'false' -# hdr_merge.enable: 'false' -# wait_for_device_timeout: '-1.' -# reconnect_timeout: '6.' -# base_frame_id: 'link' -# decimation_filter.filter_magnitude: '2' -# enable_safety: 'false' -# safety_camera.safety_mode: '0' -# enable_labeled_point_cloud: 'false' -# depth_mapping_camera.labeled_point_cloud_profile: '0,0,0' -# enable_occupancy: 'false' -# depth_mapping_camera.occupancy_profile: '0,0,0' diff --git a/perception_setup/config/image_processing/image_crop.yaml b/perception_setup/config/image_processing/image_crop.yaml deleted file mode 100644 index 82461a6..0000000 --- a/perception_setup/config/image_processing/image_crop.yaml +++ /dev/null @@ -1,14 +0,0 @@ -# Image crop configuration -# Reads raw depth image from camera and crops to the specified region. -# Topics are resolved from cameras.yaml based on the camera reference. -# -# crop: top-left corner (x_offset, y_offset) and size (width, height) in pixels -# Set width or height to 0 to use the full image dimension. - -camera: "realsense_d555" - -crop: # These values were eye-balled from the raw depth image. - x_offset: 260 # x_left boundary - y_offset: 190 # y_upper boundary - width: 485 # x_right (730) - x_left (245) - height: 245 # y_lower (435) - y_upper (190) diff --git a/perception_setup/config/image_processing/image_undistort.yaml b/perception_setup/config/image_processing/image_undistort.yaml deleted file mode 100644 index 8713926..0000000 --- a/perception_setup/config/image_processing/image_undistort.yaml +++ /dev/null @@ -1,6 +0,0 @@ -# Image undistortion configuration -# Reads raw image from camera, applies undistortion using calibration data, -# and publishes the corrected image. -# Topics are resolved from cameras.yaml based on the camera reference. - -camera: "realsense_d555" diff --git a/perception_setup/config/pipeline_following.yaml b/perception_setup/config/pipeline_following.yaml deleted file mode 100644 index 3a77188..0000000 --- a/perception_setup/config/pipeline_following.yaml +++ /dev/null @@ -1,84 +0,0 @@ -# Pipeline-following -# Stage 1: Runs instance segmentation on the camera image and produces a binary mask of detected pipelines. -# Stage 2: Classifies the mask image to determine if the pipeline continues or ends. - -seg: - camera: "bfs_pge_16s2c" - visualized_image_topic: "/pipeline/camera/segmentation_debug" - detection_topic: "/pipeline/camera/bboxes" - mask_topic: "/pipeline/camera/segmentation_mask" - - model_file_path: "down_cam_seg.onnx" - engine_file_path: "down_cam_seg.engine" - - enable_visualizer: true - class_names: - 0: "pipeline" - - encoding_desired: "rgb8" - - output_mask_width: 640 # Set to 640 to match the network input size, which allows you to use its camera info directly - output_mask_height: 640 # Set to 640 to match the network input size, which allows you to use its camera info directly - - verbose: true - force_engine_update: false - - # ============================================================================================================================================================== - # NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model - # ============================================================================================================================================================== - - input_tensor_names: ["input_tensor"] - input_binding_names: ["images"] - output_tensor_names: ["output_tensor", "mask_tensor"] - output_binding_names: ["output0", "output1"] - - network_image_width: 640 - network_image_height: 640 - image_mean: [0.0, 0.0, 0.0] - image_stddev: [1.0, 1.0, 1.0] - - confidence_threshold: 0.25 - num_detections: 300 - mask_width: 160 - mask_height: 160 - num_protos: 32 - -cls: - camera: "bfs_pge_16s2c" # Used to resolve camera_info_input_topic - # Input is the segmentation mask output from stage 1 - image_input_topic: "/pipeline/camera/segmentation_mask" - visualized_image_topic: "/pipeline/camera/classification_debug" - class_topic: "/pipeline/camera/classification" - - model_file_path: "end-of-pipeline-yolov26.onnx" - engine_file_path: "end-of-pipeline-yolov26.engine" - - enable_visualizer: true - class_names: - 0: "continue" - 1: "end" - - # Seg mask is mono8 at output_mask_width x output_mask_height - input_image_width: 640 - input_image_height: 640 - encoding_desired: "rgb8" - - verbose: true - force_engine_update: false - - # ============================================================================================================================================================== - # NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model - # ============================================================================================================================================================== - - input_tensor_names: ["input_tensor"] - input_binding_names: ["images"] - output_tensor_names: ["output_tensor"] - output_binding_names: ["output0"] - - network_image_width: 224 - network_image_height: 224 - image_mean: [0.0, 0.0, 0.0] - image_stddev: [1.0, 1.0, 1.0] - - confidence_threshold: 0.25 - num_classes: 2 diff --git a/perception_setup/config/pipeline_localization.yaml b/perception_setup/config/pipeline_localization.yaml deleted file mode 100644 index 63110c4..0000000 --- a/perception_setup/config/pipeline_localization.yaml +++ /dev/null @@ -1,84 +0,0 @@ -# Pipeline-localization: Dual-camera segmentation -# Runs two segmentation pipelines on different camera streams. - -seg_front: - camera: "realsense_d555" - visualized_image_topic: "/localization/front/segmentation_debug" - detection_topic: "/localization/front/detections" - mask_topic: "/localization/front/segmentation_mask" - - model_file_path: "down_cam_seg.onnx" - engine_file_path: "down_cam_seg.engine" - - enable_visualizer: true - class_names: - 0: "pipeline" - - encoding_desired: "rgb8" - - output_mask_width: 640 - output_mask_height: 640 - - verbose: true - force_engine_update: false - - # ============================================================================================================================================================== - # NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different segmentation model - # ============================================================================================================================================================== - - input_tensor_names: ["input_tensor"] - input_binding_names: ["images"] - output_tensor_names: ["output_tensor", "mask_tensor"] - output_binding_names: ["output0", "output1"] - - network_image_width: 640 - network_image_height: 640 - image_mean: [0.0, 0.0, 0.0] - image_stddev: [1.0, 1.0, 1.0] - - confidence_threshold: 0.25 - num_detections: 300 - mask_width: 160 - mask_height: 160 - num_protos: 32 - -seg_down: - camera: "bfs_pge_16s2c" - visualized_image_topic: "/localization/down/segmentation_debug" - detection_topic: "/localization/down/detections" - mask_topic: "/localization/down/segmentation_mask" - - model_file_path: "down_cam_seg.onnx" - engine_file_path: "down_cam_seg.engine" - - enable_visualizer: true - class_names: - 0: "pipeline" - - encoding_desired: "rgb8" - - output_mask_width: 640 - output_mask_height: 640 - - verbose: true - force_engine_update: false - - # ============================================================================================================================================================== - # NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model - # ============================================================================================================================================================== - - input_tensor_names: ["input_tensor"] - input_binding_names: ["images"] - output_tensor_names: ["output_tensor", "mask_tensor"] - output_binding_names: ["output0", "output1"] - - network_image_width: 640 - network_image_height: 640 - image_mean: [0.0, 0.0, 0.0] - image_stddev: [1.0, 1.0, 1.0] - - confidence_threshold: 0.25 - num_detections: 300 - mask_width: 160 - mask_height: 160 - num_protos: 32 diff --git a/perception_setup/config/valve_intervention.yaml b/perception_setup/config/valve_intervention.yaml deleted file mode 100644 index da15935..0000000 --- a/perception_setup/config/valve_intervention.yaml +++ /dev/null @@ -1,80 +0,0 @@ -# Valve intervention pipeline configuration -# Chains: camera -> yolo_obb -> valve_detection - -camera: "realsense_d555" - -# ── YOLO OBB ──────────────────────────────────────────────────────────────── -yolo_obb: - # Topics - visualized_image_topic: "/yolo_obb_processed_image" - detection_topic: "/obb_detections_output" - - # Model and engine file paths (relative to perception_setup/models/) - model_file_path: "obb_best.onnx" - engine_file_path: "obb_best.engine" - - # Visualizer - enable_visualizer: true - class_names: - 0: "valve" - - encoding_desired: "rgb8" - - # TensorRT - verbose: true - force_engine_update: false - - # Tensor I/O bindings - 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 - confidence_threshold: 0.25 - num_detections: 300 - -# ── Valve Detection ───────────────────────────────────────────────────────── -valve_detection: - # Inputs — detections and depth topics wired automatically from yolo_obb / cameras.yaml - color_image_info_topic: "/yolo_obb_encoder/internal/resize/camera_info" - - # Output - landmarks_pub_topic: "/valve_landmarks" - - # YOLO letterbox reference size - yolo_img_width: 640 - yolo_img_height: 640 - - # Annulus and plane fit - annulus_radius_ratio: 0.8 - plane_ransac_threshold: 0.01 - plane_ransac_max_iterations: 150 - - # Duplicate-detection suppression - iou_duplicate_threshold: 0.5 - - # Pose offset - valve_handle_offset: 0.02 - - # Drone namespace (prepended to frame IDs from cameras.yaml) - drone: "nautilus" - - # Behaviour toggles - debug_visualize: true - - # Debug visualization (only active when debug_visualize: true) - debug: - valve_poses_pub_topic: "/valve_poses" - depth_cloud_pub_topic: "/valve_depth_cloud" - depth_colormap_pub_topic: "/valve_detection_depth_colormap" - depth_colormap_value_min: 0.1 - depth_colormap_value_max: 1125.5 - annulus_pub_topic: "/bbx_annulus_pcl" - plane_pub_topic: "/annulus_plane_pcl" diff --git a/perception_setup/config/visual_inspection.yaml b/perception_setup/config/visual_inspection.yaml deleted file mode 100644 index 3e157a9..0000000 --- a/perception_setup/config/visual_inspection.yaml +++ /dev/null @@ -1,96 +0,0 @@ -# Visual inspection pipeline: camera -> image_filtering -> aruco_detector -# -# camera: Key from cameras.yaml — determines which camera driver to launch -# and which image/camera_info topics to feed into the pipeline. - -camera: "sim_front_camera" - -# --------------------------------------------------------------------------- -# Image filtering -# sub_topic is resolved automatically from cameras.yaml (based on enable_undistort flag) -# --------------------------------------------------------------------------- -image_filtering: - pub_topic: "/filtered_image" - input_encoding: "rgb8" - output_encoding: "rgb8" - - filter_params: - filter_type: "remove_grid" - flip: - flip_code: 1 - unsharpening: - blur_size: 8 - erosion: - size: 1 - dilation: - size: 3 - white_balancing: - contrast_percentage: 10.0 - ebus: - erosion_size: 2 - blur_size: 30 - mask_weight: 5 - otsu: - gsc_weight_r: 1.0 # Grayscale red weight - gsc_weight_g: 0.0 # Grayscale green weight - gsc_weight_b: 0.0 # Grayscale blue weight - gamma_auto_correction: true - gamma_auto_correction_weight: 4.0 - otsu_segmentation: true - erosion_size: 2 - dilation_size: 2 - remove_grid: - threshold_green: 0.5 - threshold_binary: 30.0 - inpaint_radius: 1.0 - rotation: 0 - height: 400 - width: 400 - overlap: - percentage_threshold: 20.0 # Percentage (0-100) to cap the pixel intensity difference - median_binary: # finds the median of each n x n square around each pixel - kernel_size: 3 # must be odd > 1 - threshold: 100 # [0, 255] - invert: false - binary: - threshold: 20. # in percent - maxval: 255. - invert: true - temporal_noise: - median_kernel_size: 3 - power_law_weight: 4.0 # Weight for the gamma function - erotion_size: 2 - dilation_size: 4 - canny_high: 90 # upper weight for canny edge detection - canny_low: 30 # lower weight ---------- || ---------- - edge_protection_radius: 2 # Radius for protecting edges - - -# --------------------------------------------------------------------------- -# ArUco detector -# subs.image_topic is wired automatically to image_filtering.pub_topic -# subs.camera_info_topic is resolved automatically from cameras.yaml -# --------------------------------------------------------------------------- -aruco_detector: - pubs: - aruco_image: "/visual_inspection/aruco/image" - aruco_poses: "/visual_inspection/aruco/markers" - board_pose: "/visual_inspection/aruco/board" - landmarks: "/visual_inspection/aruco/landmarks" - - logger_service_name: "/toggle_marker_logger" - - detect_board: false - visualize: true - log_markers: false - publish_detections: true - publish_landmarks: false - - aruco: - marker_size: 0.150 - dictionary: "DICT_ARUCO_ORIGINAL" - - board: - xDist: 0.430 - yDist: 0.830 - ids: [28, 7, 96, 19] diff --git a/perception_setup/config/yolo/yolo_detect.yaml b/perception_setup/config/yolo/yolo_detect.yaml index aa2f0d8..32f1514 100644 --- a/perception_setup/config/yolo/yolo_detect.yaml +++ b/perception_setup/config/yolo/yolo_detect.yaml @@ -1,6 +1,6 @@ -# Topics -visualized_image_topic: "/yolov8_processed_image" # Output topic for visualized images with detection overlays -detection_topic: "/yolov8_detections_output" # Output topic for detection results (Detection2DArray) +# Output topics +visualized_image_topic: "/yolov8_processed_image" +detection_topic: "/yolov8_detections_output" # Model and engine file paths model_file_path: "best.onnx" # The output dimensions must be [1, 5, 8400] (see yolov8_encoder_node.cpp for details) diff --git a/perception_setup/config/yolo/yolo_obb.yaml b/perception_setup/config/yolo/yolo_obb.yaml index 665bb38..e7ce647 100644 --- a/perception_setup/config/yolo/yolo_obb.yaml +++ b/perception_setup/config/yolo/yolo_obb.yaml @@ -1,4 +1,4 @@ -# Topics +# Output topics visualized_image_topic: "/yolo_obb_processed_image" detection_topic: "/obb_detections_output" diff --git a/perception_setup/config/yolo/yolo_seg.yaml b/perception_setup/config/yolo/yolo_seg.yaml index e91312d..dcb7492 100644 --- a/perception_setup/config/yolo/yolo_seg.yaml +++ b/perception_setup/config/yolo/yolo_seg.yaml @@ -1,4 +1,4 @@ -# Topics +# Output topics visualized_image_topic: "/yolo_seg_processed_image" detection_topic: "/seg_detections_output" mask_topic: "/yolo_seg_mask" diff --git a/perception_setup/launch/cameras/bfs_pge_16s2c.launch.py b/perception_setup/launch/cameras/bfs_pge_16s2c.launch.py deleted file mode 100644 index 3ee746e..0000000 --- a/perception_setup/launch/cameras/bfs_pge_16s2c.launch.py +++ /dev/null @@ -1,92 +0,0 @@ -"""Wrapper launch file for the FLIR Blackfly S BFS-PGE-16S2C-CS downwards camera. - -Starts: - - Spinnaker camera driver component in a dedicated container -""" - -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 -from launch_ros.descriptions import ComposableNode - - -def generate_launch_description(): - pkg_dir = get_package_share_directory("perception_setup") - - default_cam_config = os.path.join( - pkg_dir, "config", "cameras", "bfs_pge_16s2c_params.yaml" - ) - default_spinnaker_map = os.path.join( - pkg_dir, "config", "cameras", "blackfly_s_params.yaml" - ) - calib_path = os.path.join(pkg_dir, "config", "cameras", "bfs_pge_16s2c_calib.yaml") - calib_url = f"file://{calib_path}" - - enable_camera_arg = DeclareLaunchArgument( - "enable_camera", - default_value="true", - description="Enable FLIR BFS-PGE-16S2C-CS camera component", - ) - - camera_name_arg = DeclareLaunchArgument( - "camera_name", - default_value="bfs_pge_16s2c", - description="Camera node name (also used as topic namespace by the driver)", - ) - serial_arg = DeclareLaunchArgument( - "serial", - default_value="", - description="FLIR serial number (empty selects first available camera)", - ) - camera_params_arg = DeclareLaunchArgument( - "camera_params_file", - default_value=default_cam_config, - description="Path to ROS parameters for camera_driver_node", - ) - parameter_file_arg = DeclareLaunchArgument( - "parameter_file", - default_value=default_spinnaker_map, - description="Path to Spinnaker node-map parameter definition YAML", - ) - - flir_container = ComposableNodeContainer( - name="bfs_pge_16s2c_container", - namespace="", - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=[ - ComposableNode( - package="spinnaker_camera_driver", - plugin="spinnaker_camera_driver::CameraDriver", - name=LaunchConfiguration("camera_name"), - parameters=[ - LaunchConfiguration("camera_params_file"), - { - "parameter_file": LaunchConfiguration("parameter_file"), - "serial_number": LaunchConfiguration("serial"), - "camerainfo_url": calib_url, - }, - ], - remappings=[("~/control", "/exposure_control/control")], - extra_arguments=[{"use_intra_process_comms": True}], - condition=IfCondition(LaunchConfiguration("enable_camera")), - ) - ], - output="screen", - ) - - return LaunchDescription( - [ - enable_camera_arg, - camera_name_arg, - serial_arg, - camera_params_arg, - parameter_file_arg, - flir_container, - ] - ) 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..ef98252 --- /dev/null +++ b/perception_setup/launch/cameras/blackfly_s.launch.py @@ -0,0 +1,68 @@ +"""FLIR Blackfly S downwards camera launch file. + +Starts: + - Spinnaker camera driver component in a dedicated container +""" + +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 +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" + ) + + enable_camera_arg = DeclareLaunchArgument( + "enable_camera", + default_value="true", + description="Enable FLIR Blackfly S camera component", + ) + + 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": "", + "camerainfo_url": f"file://{calib_path}", + }, + ], + remappings=[("~/control", "/exposure_control/control")], + extra_arguments=[{"use_intra_process_comms": True}], + condition=IfCondition(LaunchConfiguration("enable_camera")), + ) + ], + output="screen", + ) + + return LaunchDescription( + [ + enable_camera_arg, + flir_container, + ] + ) diff --git a/perception_setup/launch/cameras/realsense_d555.launch.py b/perception_setup/launch/cameras/realsense_d555.launch.py index a00ea1d..52c35b3 100644 --- a/perception_setup/launch/cameras/realsense_d555.launch.py +++ b/perception_setup/launch/cameras/realsense_d555.launch.py @@ -1,12 +1,12 @@ -"""Wrapper launch file for the RealSense D555 camera. +"""RealSense D555 camera launch file. Starts: - - RealSense driver (rs_launch.py) - - camera_info_publisher (publishes calibration on color_raw/camera_info) - - image_undistort (undistorts color image) - - image_crop (crops depth image) + - RealSense camera driver node + - camera_info_publisher (publishes calibration CameraInfo) + - image_undistort (undistorts color image) + - image_crop (crops depth image) -See: https://github.com/vortexntnu/realsense-ros/blob/r/4.57.6/realsense2_camera/launch/rs_launch.py +Topics are read from cameras.yaml (single source of truth). """ import os @@ -14,76 +14,106 @@ import yaml 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 LaunchConfiguration from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare def generate_launch_description(): pkg_dir = get_package_share_directory("perception_setup") - default_rs_config = os.path.join( - pkg_dir, "config", "cameras", "realsense_d555.yaml" - ) - camera_info_file = os.path.join( - pkg_dir, "config", "cameras", "color_realsense_d555_calib.yaml" - ) - cameras_path = os.path.join(pkg_dir, "config", "cameras", "cameras.yaml") with open(cameras_path) as f: cameras = yaml.safe_load(f) cam = cameras["realsense_d555"] - helpers_dir = os.path.join(pkg_dir, "launch", "image_processing") - - config_file_arg = DeclareLaunchArgument( - "config_file", - default_value=default_rs_config, - description="Path to a YAML file with node-level parameter overrides for rs_launch.py", + calib_file = os.path.join( + pkg_dir, "config", "cameras", "color_realsense_d555_calib.yaml" ) - rs_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [FindPackageShare("realsense2_camera"), "/launch/rs_launch.py"] - ), - launch_arguments={ - "config_file": LaunchConfiguration("config_file"), - }.items(), + realsense_node = Node( + package="realsense2_camera", + executable="realsense2_camera_node", + name="front_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, + } + ], + output="screen", + arguments=["--ros-args", "--log-level", "info"], + emulate_tty=True, ) - camera_info_pub = Node( + camera_info_publisher_node = Node( package="perception_setup", executable="camera_info_publisher.py", name="camera_info_publisher", parameters=[ { - "camera_info_file": camera_info_file, + "camera_info_file": calib_file, "camera_info_topic": cam["calibration_camera_info_topic"], } ], output="screen", ) - image_undistort = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(helpers_dir, "image_undistort.launch.py") - ), + image_undistort_node = Node( + package="perception_setup", + executable="image_undistort.py", + name="image_undistort", + parameters=[ + { + "image_topic": cam["raw_image_topic"], + "camera_info_topic": cam["calibration_camera_info_topic"], + "raw_camera_info_topic": cam["raw_camera_info_topic"], + "output_image_topic": cam["image_topic"], + "output_camera_info_topic": cam["camera_info_topic"], + "enable_undistort": True, + } + ], + output="screen", ) - image_crop = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(helpers_dir, "image_crop.launch.py") - ), + image_crop_node = Node( + package="perception_setup", + executable="image_crop.py", + name="image_crop", + parameters=[ + { + "image_topic": cam["raw_depth_topic"], + "camera_info_topic": cam["raw_depth_camera_info_topic"], + "output_image_topic": cam["depth_image_topic"], + "output_camera_info_topic": cam["depth_camera_info_topic"], + "enable_crop": True, + "crop.x_offset": 260, + "crop.y_offset": 190, + "crop.width": 485, + "crop.height": 245, + } + ], + output="screen", ) return LaunchDescription( [ - config_file_arg, - rs_launch, - camera_info_pub, - image_undistort, - image_crop, + realsense_node, + camera_info_publisher_node, + image_undistort_node, + image_crop_node, ] ) diff --git a/perception_setup/launch/image_processing/camera_info_publisher.launch.py b/perception_setup/launch/image_processing/camera_info_publisher.launch.py deleted file mode 100644 index 9faf528..0000000 --- a/perception_setup/launch/image_processing/camera_info_publisher.launch.py +++ /dev/null @@ -1,39 +0,0 @@ -"""Launch file that publishes camera info without starting the RealSense camera.""" - -import os - -import yaml -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - pkg_dir = get_package_share_directory("perception_setup") - - camera_info_file = os.path.join( - pkg_dir, - "config", - "cameras", - "color_realsense_d555_calib.yaml", - ) - - cameras_path = os.path.join(pkg_dir, "config", "cameras", "cameras.yaml") - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras["realsense_d555"] - - camera_info_pub = Node( - package="perception_setup", - executable="camera_info_publisher.py", - name="camera_info_publisher", - parameters=[ - { - "camera_info_file": camera_info_file, - "camera_info_topic": cam["calibration_camera_info_topic"], - } - ], - output="screen", - ) - - return LaunchDescription([camera_info_pub]) diff --git a/perception_setup/launch/image_processing/image_crop.launch.py b/perception_setup/launch/image_processing/image_crop.launch.py deleted file mode 100644 index f700ba0..0000000 --- a/perception_setup/launch/image_processing/image_crop.launch.py +++ /dev/null @@ -1,43 +0,0 @@ -import os - -import yaml -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - pkg_dir = get_package_share_directory("perception_setup") - config_file = os.path.join(pkg_dir, "config", "image_processing", "image_crop.yaml") - - with open(config_file) as f: - cfg = yaml.safe_load(f) - - cameras_path = os.path.join(pkg_dir, "config", "cameras", "cameras.yaml") - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras[cfg["camera"]] - - params = { - "image_topic": cam["raw_depth_topic"], - "camera_info_topic": cam["raw_depth_camera_info_topic"], - "output_image_topic": cam["depth_image_topic"], - "output_camera_info_topic": cam["depth_camera_info_topic"], - "enable_crop": bool(cam["enable_crop"]), - "crop.x_offset": int(cfg["crop"]["x_offset"]), - "crop.y_offset": int(cfg["crop"]["y_offset"]), - "crop.width": int(cfg["crop"]["width"]), - "crop.height": int(cfg["crop"]["height"]), - } - - return LaunchDescription( - [ - Node( - package="perception_setup", - executable="image_crop.py", - name="image_crop", - parameters=[params], - output="screen", - ) - ] - ) diff --git a/perception_setup/launch/image_processing/image_undistort.launch.py b/perception_setup/launch/image_processing/image_undistort.launch.py deleted file mode 100644 index 0c01fe2..0000000 --- a/perception_setup/launch/image_processing/image_undistort.launch.py +++ /dev/null @@ -1,42 +0,0 @@ -import os - -import yaml -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - pkg_dir = get_package_share_directory("perception_setup") - config_file = os.path.join( - pkg_dir, "config", "image_processing", "image_undistort.yaml" - ) - - with open(config_file) as f: - cfg = yaml.safe_load(f) - - cameras_path = os.path.join(pkg_dir, "config", "cameras", "cameras.yaml") - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras[cfg["camera"]] - - return LaunchDescription( - [ - Node( - package="perception_setup", - executable="image_undistort.py", - name="image_undistort", - parameters=[ - { - "image_topic": cam["raw_image_topic"], - "camera_info_topic": cam["calibration_camera_info_topic"], - "raw_camera_info_topic": cam["raw_camera_info_topic"], - "output_image_topic": cam["image_topic"], - "output_camera_info_topic": cam["camera_info_topic"], - "enable_undistort": bool(cam["enable_undistort"]), - } - ], - output="screen", - ) - ] - ) diff --git a/perception_setup/launch/pipeline_following.launch.py b/perception_setup/launch/pipeline_following.launch.py deleted file mode 100644 index d653c08..0000000 --- a/perception_setup/launch/pipeline_following.launch.py +++ /dev/null @@ -1,340 +0,0 @@ -# SPDX-License-Identifier: MIT - -"""Pipeline-following launch: Segmentation -> Classification. - -Runs two TensorRT pipelines in a single launch: - -Stage 1 - Segmentation - camera image -> ImageFormatConverter -> DNNImageEncoder -> TensorRT (seg model) - -> YoloV26SegDecoder -> binary mask + detections + (optional) visualizer - -Stage 2 - Classification - seg mask -> ImageFormatConverter -> DNNImageEncoder -> TensorRT (cls model) - -> YoloV26ClsDecoder -> UInt8 class ID + (optional) visualizer - -The segmentation mask output feeds directly into the classification input. -""" - -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 - -# Stage 1: Segmentation internal topics -SEG_CONVERTED_IMAGE_TOPIC = '/yolo_seg/internal/converted_image' -SEG_ENCODER_RESIZE_TOPIC = '/yolo_seg_encoder/internal/resize/image' -SEG_TENSOR_OUTPUT_TOPIC = '/yolo_seg/tensor_pub' -SEG_TENSOR_INPUT_TOPIC = '/yolo_seg/tensor_sub' -SEG_ENCODER_NAMESPACE = 'yolo_seg_encoder/internal' - -# Stage 2: Classification internal topics -CLS_CONVERTED_IMAGE_TOPIC = '/yolo_cls/internal/converted_image' -CLS_ENCODER_RESIZE_TOPIC = '/yolo_cls_encoder/internal/resize/image' -CLS_TENSOR_OUTPUT_TOPIC = '/yolo_cls/tensor_pub' -CLS_TENSOR_INPUT_TOPIC = '/yolo_cls/tensor_sub' -CLS_ENCODER_NAMESPACE = 'yolo_cls_encoder/internal' - - -def _launch_setup(context, *args, **kwargs): - config_path = LaunchConfiguration('config_file').perform(context) - - with open(config_path) as f: - cfg = yaml.safe_load(f) - - seg = cfg['seg'] - cls = cfg['cls'] - - pkg_dir = get_package_share_directory('perception_setup') - models_dir = os.path.join(pkg_dir, 'models') - - # Resolve camera references from cameras.yaml - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - - if 'camera' in seg: - cam = cameras[seg['camera']] - if cam.get('enable_undistort', True): - seg['image_input_topic'] = cam['image_topic'] - seg['camera_info_input_topic'] = cam['camera_info_topic'] - else: - seg['image_input_topic'] = cam['raw_image_topic'] - seg['camera_info_input_topic'] = cam['raw_camera_info_topic'] - seg['input_image_width'] = cam['image_width'] - seg['input_image_height'] = cam['image_height'] - - if 'camera' in cls: - cam = cameras[cls['camera']] - if cam.get('enable_undistort', True): - cls['camera_info_input_topic'] = cam['camera_info_topic'] - else: - cls['camera_info_input_topic'] = cam['raw_camera_info_topic'] - - encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder') - - # Stage 1 – Segmentation - seg_model = os.path.join(models_dir, str(seg['model_file_path'])) - seg_engine = os.path.join(models_dir, str(seg['engine_file_path'])) - - 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': str(seg['encoding_desired']), - 'image_width': int(seg['input_image_width']), - 'image_height': int(seg['input_image_height']), - 'input_qos': 'sensor_data', - } - ], - remappings=[ - ('image_raw', str(seg['image_input_topic'])), - ('image', SEG_CONVERTED_IMAGE_TOPIC), - ], - ) - - seg_tensor_rt = ComposableNode( - name='seg_tensor_rt', - package='isaac_ros_tensor_rt', - plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode', - parameters=[ - { - 'model_file_path': seg_model, - 'engine_file_path': seg_engine, - 'output_binding_names': seg['output_binding_names'], - 'output_tensor_names': seg['output_tensor_names'], - 'input_tensor_names': seg['input_tensor_names'], - 'input_binding_names': seg['input_binding_names'], - 'verbose': bool(seg['verbose']), - 'force_engine_update': bool(seg['force_engine_update']), - 'tensor_output_topic': SEG_TENSOR_OUTPUT_TOPIC, - } - ], - remappings=[ - ('tensor_pub', SEG_TENSOR_OUTPUT_TOPIC), - ('tensor_sub', SEG_TENSOR_INPUT_TOPIC), - ], - ) - - seg_decoder = ComposableNode( - name='seg_decoder', - package='isaac_ros_yolov26_seg', - plugin='nvidia::isaac_ros::yolov26_seg::YoloV26SegDecoderNode', - parameters=[ - { - 'tensor_input_topic': SEG_TENSOR_INPUT_TOPIC, - 'confidence_threshold': float(seg['confidence_threshold']), - 'num_detections': int(seg['num_detections']), - 'mask_width': int(seg['mask_width']), - 'mask_height': int(seg['mask_height']), - 'num_protos': int(seg['num_protos']), - 'network_image_width': int(seg['network_image_width']), - 'network_image_height': int(seg['network_image_height']), - 'output_mask_width': int(seg['output_mask_width']), - 'output_mask_height': int(seg['output_mask_height']), - 'detections_topic': str(seg['detection_topic']), - 'mask_topic': str(seg['mask_topic']), - } - ], - ) - - seg_container = ComposableNodeContainer( - name='seg_tensor_rt_container', - package='rclcpp_components', - executable='component_container_mt', - composable_node_descriptions=[ - seg_image_format_converter, - seg_tensor_rt, - seg_decoder, - ], - output='screen', - arguments=['--ros-args', '--log-level', 'INFO'], - namespace='', - ) - - seg_encoder_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py') - ), - launch_arguments={ - 'input_image_width': str(seg['input_image_width']), - 'input_image_height': str(seg['input_image_height']), - 'network_image_width': str(seg['network_image_width']), - 'network_image_height': str(seg['network_image_height']), - 'image_mean': str(seg['image_mean']), - 'image_stddev': str(seg['image_stddev']), - 'attach_to_shared_component_container': 'True', - 'component_container_name': 'seg_tensor_rt_container', - 'dnn_image_encoder_namespace': SEG_ENCODER_NAMESPACE, - 'image_input_topic': SEG_CONVERTED_IMAGE_TOPIC, - 'camera_info_input_topic': str(seg['camera_info_input_topic']), - 'tensor_output_topic': SEG_TENSOR_OUTPUT_TOPIC, - }.items(), - ) - - # Stage 2 – Classification - cls_model = os.path.join(models_dir, str(cls['model_file_path'])) - cls_engine = os.path.join(models_dir, str(cls['engine_file_path'])) - - 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': str(cls['encoding_desired']), - 'image_width': int(cls['input_image_width']), - 'image_height': int(cls['input_image_height']), - 'input_qos': 'sensor_data', - } - ], - remappings=[ - ('image_raw', str(cls['image_input_topic'])), - ('image', CLS_CONVERTED_IMAGE_TOPIC), - ], - ) - - cls_tensor_rt = ComposableNode( - name='cls_tensor_rt', - package='isaac_ros_tensor_rt', - plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode', - parameters=[ - { - 'model_file_path': cls_model, - 'engine_file_path': cls_engine, - 'output_binding_names': cls['output_binding_names'], - 'output_tensor_names': cls['output_tensor_names'], - 'input_tensor_names': cls['input_tensor_names'], - 'input_binding_names': cls['input_binding_names'], - 'verbose': bool(cls['verbose']), - 'force_engine_update': bool(cls['force_engine_update']), - 'tensor_output_topic': CLS_TENSOR_OUTPUT_TOPIC, - } - ], - remappings=[ - ('tensor_pub', CLS_TENSOR_OUTPUT_TOPIC), - ('tensor_sub', CLS_TENSOR_INPUT_TOPIC), - ], - ) - - cls_decoder = ComposableNode( - name='cls_decoder', - package='isaac_ros_yolov26_cls', - plugin='nvidia::isaac_ros::yolov26_cls::YoloV26ClsDecoderNode', - parameters=[ - { - 'tensor_input_topic': CLS_TENSOR_INPUT_TOPIC, - 'confidence_threshold': float(cls['confidence_threshold']), - 'num_classes': int(cls['num_classes']), - 'class_topic': str(cls['class_topic']), - } - ], - ) - - cls_container = ComposableNodeContainer( - name='cls_tensor_rt_container', - package='rclcpp_components', - executable='component_container_mt', - composable_node_descriptions=[ - cls_image_format_converter, - cls_tensor_rt, - cls_decoder, - ], - output='screen', - arguments=['--ros-args', '--log-level', 'INFO'], - namespace='', - ) - - cls_encoder_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py') - ), - launch_arguments={ - 'input_image_width': str(cls['input_image_width']), - 'input_image_height': str(cls['input_image_height']), - 'network_image_width': str(cls['network_image_width']), - 'network_image_height': str(cls['network_image_height']), - 'image_mean': str(cls['image_mean']), - 'image_stddev': str(cls['image_stddev']), - 'attach_to_shared_component_container': 'True', - 'component_container_name': 'cls_tensor_rt_container', - 'dnn_image_encoder_namespace': CLS_ENCODER_NAMESPACE, - 'image_input_topic': CLS_CONVERTED_IMAGE_TOPIC, - 'camera_info_input_topic': str(cls['camera_info_input_topic']), - 'tensor_output_topic': CLS_TENSOR_OUTPUT_TOPIC, - }.items(), - ) - - # Assemble actions - actions = [ - seg_container, - seg_encoder_launch, - cls_container, - cls_encoder_launch, - ] - - # Segmentation visualizer - if bool(seg.get('enable_visualizer', False)): - actions.append( - Node( - package='isaac_ros_yolov26_seg', - executable='isaac_ros_yolov26_seg_visualizer.py', - name='seg_visualizer', - parameters=[ - { - 'detections_topic': str(seg['detection_topic']), - 'image_topic': SEG_ENCODER_RESIZE_TOPIC, - 'mask_topic': str(seg['mask_topic']), - 'output_image_topic': str(seg['visualized_image_topic']), - 'class_names_yaml': str(seg['class_names']), - } - ], - ) - ) - - # Classification visualizer - if bool(cls.get('enable_visualizer', False)): - actions.append( - Node( - package='isaac_ros_yolov26_cls', - executable='isaac_ros_yolov26_cls_visualizer.py', - name='cls_visualizer', - parameters=[ - { - 'class_topic': str(cls['class_topic']), - 'image_topic': CLS_ENCODER_RESIZE_TOPIC, - 'output_image_topic': str(cls['visualized_image_topic']), - 'class_names_yaml': str(cls['class_names']), - } - ], - ) - ) - - return actions - - -def generate_launch_description(): - pkg_dir = get_package_share_directory('perception_setup') - default_config = os.path.join(pkg_dir, 'config', 'pipeline_following.yaml') - - return launch.LaunchDescription( - [ - DeclareLaunchArgument( - 'config_file', - default_value=default_config, - description='Path to pipeline-following config YAML', - ), - OpaqueFunction(function=_launch_setup), - ] - ) diff --git a/perception_setup/launch/pipeline_localization.launch.py b/perception_setup/launch/pipeline_localization.launch.py deleted file mode 100644 index 062a1cc..0000000 --- a/perception_setup/launch/pipeline_localization.launch.py +++ /dev/null @@ -1,223 +0,0 @@ -# SPDX-License-Identifier: MIT - -"""Pipeline-localization launch: Dual-camera segmentation. - -Runs two independent YOLO segmentation pipelines on different camera streams -(front + down). Each pipeline gets its own container and unique topic namespace -to avoid node/topic collisions. - -Stage 1 – Front camera segmentation - /cam/image_color -> seg pipeline -> /localization/front/segmentation_mask - -Stage 2 – Down camera segmentation - /cam_down/image_color -> seg pipeline -> /localization/down/segmentation_mask -""" - -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 _build_seg_pipeline(cfg, prefix, models_dir, encoder_dir): - """Build a complete segmentation pipeline with unique names. - - Args: - cfg: Config dict for this segmentation instance. - prefix: Unique prefix string (e.g. 'front', 'down') used to - differentiate container names, node names, and internal topics. - models_dir: Absolute path to the models directory. - encoder_dir: Package share directory for isaac_ros_dnn_image_encoder. - - Returns: - List of launch actions for this pipeline. - """ - # Unique internal topics per pipeline - converted_image_topic = f'/yolo_seg_{prefix}/internal/converted_image' - encoder_resize_topic = f'/yolo_seg_{prefix}_encoder/internal/resize/image' - tensor_output_topic = f'/yolo_seg_{prefix}/tensor_pub' - tensor_input_topic = f'/yolo_seg_{prefix}/tensor_sub' - encoder_namespace = f'yolo_seg_{prefix}_encoder/internal' - container_name = f'seg_{prefix}_tensor_rt_container' - - model_path = os.path.join(models_dir, str(cfg['model_file_path'])) - engine_path = os.path.join(models_dir, str(cfg['engine_file_path'])) - - image_format_converter = ComposableNode( - package='isaac_ros_image_proc', - plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', - name=f'seg_{prefix}_image_format_converter', - parameters=[ - { - 'encoding_desired': str(cfg['encoding_desired']), - 'image_width': int(cfg['input_image_width']), - 'image_height': int(cfg['input_image_height']), - 'input_qos': 'sensor_data', - } - ], - remappings=[ - ('image_raw', str(cfg['image_input_topic'])), - ('image', converted_image_topic), - ], - ) - - tensor_rt = ComposableNode( - name=f'seg_{prefix}_tensor_rt', - package='isaac_ros_tensor_rt', - plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode', - parameters=[ - { - 'model_file_path': model_path, - 'engine_file_path': engine_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': bool(cfg['verbose']), - 'force_engine_update': bool(cfg['force_engine_update']), - 'tensor_output_topic': tensor_output_topic, - } - ], - remappings=[ - ('tensor_pub', tensor_output_topic), - ('tensor_sub', tensor_input_topic), - ], - ) - - decoder = ComposableNode( - name=f'seg_{prefix}_decoder', - package='isaac_ros_yolov26_seg', - plugin='nvidia::isaac_ros::yolov26_seg::YoloV26SegDecoderNode', - parameters=[ - { - 'tensor_input_topic': tensor_input_topic, - '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': int(cfg['output_mask_width']), - 'output_mask_height': int(cfg['output_mask_height']), - 'detections_topic': str(cfg['detection_topic']), - 'mask_topic': str(cfg['mask_topic']), - } - ], - ) - - container = ComposableNodeContainer( - name=container_name, - package='rclcpp_components', - executable='component_container_mt', - composable_node_descriptions=[ - image_format_converter, - tensor_rt, - decoder, - ], - output='screen', - arguments=['--ros-args', '--log-level', 'INFO'], - namespace='', - ) - - encoder_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py') - ), - launch_arguments={ - 'input_image_width': str(cfg['input_image_width']), - 'input_image_height': str(cfg['input_image_height']), - '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': container_name, - 'dnn_image_encoder_namespace': encoder_namespace, - 'image_input_topic': converted_image_topic, - 'camera_info_input_topic': str(cfg['camera_info_input_topic']), - 'tensor_output_topic': tensor_output_topic, - }.items(), - ) - - actions = [container, encoder_launch] - - if bool(cfg.get('enable_visualizer', False)): - actions.append( - Node( - package='isaac_ros_yolov26_seg', - executable='isaac_ros_yolov26_seg_visualizer.py', - name=f'seg_{prefix}_visualizer', - parameters=[ - { - 'detections_topic': str(cfg['detection_topic']), - 'image_topic': encoder_resize_topic, - 'mask_topic': str(cfg['mask_topic']), - 'output_image_topic': str(cfg['visualized_image_topic']), - 'class_names_yaml': str(cfg['class_names']), - } - ], - ) - ) - - return actions - - -def _launch_setup(context, *args, **kwargs): - config_path = LaunchConfiguration('config_file').perform(context) - - with open(config_path) as f: - cfg = yaml.safe_load(f) - - 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') - - # Resolve camera references from cameras.yaml - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - - for seg_cfg in [cfg['seg_front'], cfg['seg_down']]: - if 'camera' in seg_cfg: - cam = cameras[seg_cfg['camera']] - if cam.get('enable_undistort', True): - seg_cfg['image_input_topic'] = cam['image_topic'] - seg_cfg['camera_info_input_topic'] = cam['camera_info_topic'] - else: - seg_cfg['image_input_topic'] = cam['raw_image_topic'] - seg_cfg['camera_info_input_topic'] = cam['raw_camera_info_topic'] - seg_cfg['input_image_width'] = cam['image_width'] - seg_cfg['input_image_height'] = cam['image_height'] - - actions = [] - actions += _build_seg_pipeline(cfg['seg_front'], 'front', models_dir, encoder_dir) - actions += _build_seg_pipeline(cfg['seg_down'], 'down', models_dir, encoder_dir) - - return actions - - -def generate_launch_description(): - pkg_dir = get_package_share_directory('perception_setup') - default_config = os.path.join(pkg_dir, 'config', 'pipeline_localization.yaml') - - return launch.LaunchDescription( - [ - DeclareLaunchArgument( - 'config_file', - default_value=default_config, - description='Path to pipeline-localization config YAML', - ), - OpaqueFunction(function=_launch_setup), - ] - ) diff --git a/perception_setup/launch/valve_intervention.launch.py b/perception_setup/launch/valve_intervention.launch.py deleted file mode 100644 index b79f537..0000000 --- a/perception_setup/launch/valve_intervention.launch.py +++ /dev/null @@ -1,214 +0,0 @@ -"""Valve intervention pipeline launch. - -Starts: - - Camera driver (realsense_d555 by default, configured via valve_intervention.yaml) - - Image processing (undistort / crop, if enabled in cameras.yaml) - - yolo_obb pipeline (input: camera image -> output: OBB detections) - - valve_detection node (input: detections + depth + camera_info -> output: valve landmarks) - -All tunable parameters live in perception_setup/config/valve_intervention.yaml. -""" - -import os -import tempfile - -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 Node - - -def _flatten(d, parent_key=''): - """Recursively flatten a nested dict to dot-notation ROS parameter keys.""" - items = {} - for k, v in d.items(): - full_key = f'{parent_key}.{k}' if parent_key else k - if isinstance(v, dict): - items.update(_flatten(v, full_key)) - else: - items[full_key] = v - return items - - -def _launch_setup(context, *args, **kwargs): - pkg_dir = get_package_share_directory('perception_setup') - - config_path = LaunchConfiguration('config_file').perform(context) - with open(config_path) as f: - cfg = yaml.safe_load(f) - - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - - camera_key = cfg['camera'] - cam = cameras[camera_key] - - actions = [] - - # --- Camera driver (skipped when use_rosbag: true) --- - if cam.get('use_rosbag', False): - # When using a rosbag, launch undistort/crop separately if enabled - # (the camera driver launch bundles them, but we skip the driver). - if cam.get('enable_undistort', False): - # Undistort needs calibration camera_info — launch the publisher. - if 'calibration_file' in cam: - calib_path = os.path.join( - pkg_dir, 'config', 'cameras', cam['calibration_file'] - ) - actions.append( - Node( - package='perception_setup', - executable='camera_info_publisher.py', - name='camera_info_publisher', - parameters=[ - { - 'camera_info_file': calib_path, - 'camera_info_topic': cam[ - 'calibration_camera_info_topic' - ], - } - ], - output='screen', - ) - ) - actions.append( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - pkg_dir, - 'launch', - 'image_processing', - 'image_undistort.launch.py', - ) - ) - ) - ) - if cam.get('enable_crop', False): - actions.append( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - pkg_dir, - 'launch', - 'image_processing', - 'image_crop.launch.py', - ) - ) - ) - ) - else: - # Camera driver launch includes undistort/crop if configured - camera_launch_path = os.path.join( - pkg_dir, 'launch', 'cameras', f'{camera_key}.launch.py' - ) - actions.append( - IncludeLaunchDescription(PythonLaunchDescriptionSource(camera_launch_path)) - ) - - # --- Resolve topics based on enable_undistort (independent of use_rosbag) --- - if cam.get('enable_undistort', True): - color_image_topic = cam['image_topic'] - color_info_topic = cam['camera_info_topic'] - else: - color_image_topic = cam['raw_image_topic'] - color_info_topic = cam['raw_camera_info_topic'] - - if cam.get('enable_crop', True): - depth_image_topic = cam.get('depth_image_topic', cam.get('raw_depth_topic', '')) - depth_info_topic = cam.get( - 'depth_camera_info_topic', cam.get('raw_depth_camera_info_topic', '') - ) - else: - depth_image_topic = cam.get('raw_depth_topic', '') - depth_info_topic = cam.get('raw_depth_camera_info_topic', '') - - # --- YOLO OBB pipeline --- - yolo_cfg = dict(cfg['yolo_obb']) - yolo_cfg['image_input_topic'] = color_image_topic - yolo_cfg['camera_info_input_topic'] = color_info_topic - yolo_cfg['input_image_width'] = cam['image_width'] - yolo_cfg['input_image_height'] = cam['image_height'] - - tmp_yolo_cfg = tempfile.NamedTemporaryFile( - mode='w', suffix='.yaml', prefix='yolo_obb_', delete=False - ) - yaml.dump(yolo_cfg, tmp_yolo_cfg, default_flow_style=False) - tmp_yolo_cfg.close() - - yolo_obb_launch_path = os.path.join(pkg_dir, 'launch', 'yolo', 'yolo_obb.launch.py') - yolo_obb_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(yolo_obb_launch_path), - launch_arguments={ - 'config_file': tmp_yolo_cfg.name, - 'camera': camera_key, - }.items(), - ) - - # --- Valve Detection node --- - vd = cfg['valve_detection'] - - valve_params = { - 'detections_sub_topic': str(cfg['yolo_obb']['detection_topic']), - 'color_image_info_topic': str(vd['color_image_info_topic']), - 'depth_image_sub_topic': depth_image_topic, - 'depth_image_info_topic': depth_info_topic, - # Extrinsic frame IDs (from cameras.yaml, looked up from /tf_static) - 'depth_frame_id': str(cam.get('depth_frame_id', '')), - 'color_frame_id': str(cam.get('color_frame_id', '')), - # Output - 'landmarks_pub_topic': str(vd['landmarks_pub_topic']), - # YOLO letterbox - 'yolo_img_width': int(vd['yolo_img_width']), - 'yolo_img_height': int(vd['yolo_img_height']), - # Annulus and plane fit - 'annulus_radius_ratio': float(vd['annulus_radius_ratio']), - 'plane_ransac_threshold': float(vd['plane_ransac_threshold']), - 'plane_ransac_max_iterations': int(vd['plane_ransac_max_iterations']), - # Duplicate suppression - 'iou_duplicate_threshold': float(vd['iou_duplicate_threshold']), - # Pose offset - 'valve_handle_offset': float(vd['valve_handle_offset']), - # Output frame (defaults to depth_frame_id from cameras.yaml) - 'output_frame_id': str(cam.get('depth_frame_id', '')), - 'drone': str(vd['drone']), - # Debug - 'debug_visualize': bool(vd['debug_visualize']), - } - - # Add debug params (flattened with dot notation) - valve_params.update(_flatten(vd['debug'], 'debug')) - - valve_detection_node = Node( - package='valve_detection', - executable='valve_detection_node', - name='valve_detection_node', - parameters=[valve_params], - output='screen', - ) - - actions += [yolo_obb_launch, valve_detection_node] - return actions - - -def generate_launch_description(): - pkg_dir = get_package_share_directory('perception_setup') - default_config = os.path.join(pkg_dir, 'config', 'valve_intervention.yaml') - - return LaunchDescription( - [ - DeclareLaunchArgument( - 'config_file', - default_value=default_config, - description='Path to valve intervention config YAML', - ), - OpaqueFunction(function=_launch_setup), - ] - ) diff --git a/perception_setup/launch/visual_inspection.launch.py b/perception_setup/launch/visual_inspection.launch.py deleted file mode 100644 index e88a1df..0000000 --- a/perception_setup/launch/visual_inspection.launch.py +++ /dev/null @@ -1,184 +0,0 @@ -"""Visual inspection pipeline launch. - -Starts: - - Camera driver (realsense_d555 or bfs_pge_16s2c, configured via visual_inspection.yaml) - - image_filtering_node (input: camera image -> output: filtered image) - - aruco_detector_node (input: filtered image -> output: marker poses / board pose) - -All tunable parameters live in perception_setup/config/visual_inspection.yaml. -""" - -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 Node - - -def _flatten(d, parent_key=''): - """Recursively flatten a nested dict to dot-notation ROS parameter keys.""" - items = {} - for k, v in d.items(): - full_key = f'{parent_key}.{k}' if parent_key else k - if isinstance(v, dict): - items.update(_flatten(v, full_key)) - else: - items[full_key] = v - return items - - -def _launch_setup(context, *args, **kwargs): - pkg_dir = get_package_share_directory('perception_setup') - - config_path = LaunchConfiguration('config_file').perform(context) - with open(config_path) as f: - cfg = yaml.safe_load(f) - - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - - camera_key = cfg['camera'] - cam = cameras[camera_key] - - # Resolve image and camera_info topics based on enable_undistort. - if cam.get('enable_undistort', True): - image_topic = cam['image_topic'] - camera_info_topic = cam['camera_info_topic'] - else: - image_topic = cam['raw_image_topic'] - camera_info_topic = cam['raw_camera_info_topic'] - - # --- Camera driver / image processing --- - actions = [] - if cam.get('use_rosbag', False): - # Rosbag provides raw topics; launch undistort/crop separately if enabled. - if cam.get('enable_undistort', False): - if 'calibration_file' in cam: - calib_path = os.path.join( - pkg_dir, 'config', 'cameras', cam['calibration_file'] - ) - actions.append( - Node( - package='perception_setup', - executable='camera_info_publisher.py', - name='camera_info_publisher', - parameters=[ - { - 'camera_info_file': calib_path, - 'camera_info_topic': cam[ - 'calibration_camera_info_topic' - ], - } - ], - output='screen', - ) - ) - actions.append( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - pkg_dir, - 'launch', - 'image_processing', - 'image_undistort.launch.py', - ) - ) - ) - ) - if cam.get('enable_crop', False): - actions.append( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - pkg_dir, - 'launch', - 'image_processing', - 'image_crop.launch.py', - ) - ) - ) - ) - else: - # Camera driver launch includes undistort/crop if configured. - camera_launch_path = os.path.join( - pkg_dir, 'launch', 'cameras', f'{camera_key}.launch.py' - ) - actions.append( - IncludeLaunchDescription(PythonLaunchDescriptionSource(camera_launch_path)) - ) - - # --- image_filtering --- - filt = cfg['image_filtering'] - filtering_params = { - 'sub_topic': image_topic, - 'pub_topic': str(filt['pub_topic']), - 'input_encoding': str(filt['input_encoding']), - 'output_encoding': str(filt['output_encoding']), - } - filtering_params.update(_flatten(filt['filter_params'], 'filter_params')) - - image_filtering_node = Node( - package='image_filtering', - executable='image_filtering_node', - name='image_filtering_node', - parameters=[filtering_params], - output='screen', - ) - - # --- aruco_detector --- - aruco = cfg['aruco_detector'] - aruco_params = { - 'subs.image_topic': str(filt['pub_topic']), - 'subs.camera_info_topic': camera_info_topic, - 'detect_board': bool(aruco['detect_board']), - 'visualize': bool(aruco['visualize']), - 'log_markers': bool(aruco['log_markers']), - 'publish_detections': bool(aruco['publish_detections']), - 'publish_landmarks': bool(aruco['publish_landmarks']), - 'logger_service_name': str(aruco['logger_service_name']), - 'pubs.aruco_image': str(aruco['pubs']['aruco_image']), - 'pubs.aruco_poses': str(aruco['pubs']['aruco_poses']), - 'pubs.board_pose': str(aruco['pubs']['board_pose']), - 'pubs.landmarks': str(aruco['pubs']['landmarks']), - 'aruco.marker_size': float(aruco['aruco']['marker_size']), - 'aruco.dictionary': str(aruco['aruco']['dictionary']), - 'board.xDist': float(aruco['board']['xDist']), - 'board.yDist': float(aruco['board']['yDist']), - 'board.ids': list(aruco['board']['ids']), - } - - aruco_detector_node = Node( - package='aruco_detector', - executable='aruco_detector_node', - name='aruco_detector_node', - parameters=[aruco_params], - output='screen', - ) - - actions += [image_filtering_node, aruco_detector_node] - return actions - - -def generate_launch_description(): - pkg_dir = get_package_share_directory('perception_setup') - default_config = os.path.join(pkg_dir, 'config', 'visual_inspection.yaml') - - return LaunchDescription( - [ - DeclareLaunchArgument( - 'config_file', - default_value=default_config, - description='Path to visual inspection config YAML', - ), - OpaqueFunction(function=_launch_setup), - ] - ) diff --git a/perception_setup/launch/yolo/yolo_cls.launch.py b/perception_setup/launch/yolo/yolo_cls.launch.py index 860f713..ef40f1a 100644 --- a/perception_setup/launch/yolo/yolo_cls.launch.py +++ b/perception_setup/launch/yolo/yolo_cls.launch.py @@ -53,17 +53,6 @@ def _launch_setup(context, *args, **kwargs): cfg = yaml.safe_load(f) pkg_dir = get_package_share_directory('perception_setup') - - # Resolve camera reference from cameras.yaml - if 'camera' in cfg: - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras[cfg['camera']] - cfg['image_input_topic'] = cam['image_topic'] - cfg['camera_info_input_topic'] = cam['camera_info_topic'] - cfg['input_image_width'] = cam['image_width'] - cfg['input_image_height'] = cam['image_height'] models_dir = os.path.join(pkg_dir, 'models') model_file_path = os.path.join(models_dir, str(cfg['model_file_path'])) diff --git a/perception_setup/launch/yolo/yolo_detect.launch.py b/perception_setup/launch/yolo/yolo_detect.launch.py index de12120..f4ec5f6 100644 --- a/perception_setup/launch/yolo/yolo_detect.launch.py +++ b/perception_setup/launch/yolo/yolo_detect.launch.py @@ -59,50 +59,15 @@ def _launch_setup(context, *args, **kwargs): pkg_dir = get_package_share_directory('perception_setup') - # Resolve camera from launch argument + # Resolve camera topics from cameras.yaml (single source of truth) cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') with open(cameras_path) as f: cameras = yaml.safe_load(f) cam = cameras[LaunchConfiguration('camera').perform(context)] - if cam.get('enable_undistort', True): - cfg['image_input_topic'] = cam['image_topic'] - cfg['camera_info_input_topic'] = cam['camera_info_topic'] - else: - cfg['image_input_topic'] = cam['raw_image_topic'] - cfg['camera_info_input_topic'] = cam['raw_camera_info_topic'] - cfg['input_image_width'] = cam['image_width'] - cfg['input_image_height'] = cam['image_height'] - - required_keys = [ - 'model_file_path', - 'engine_file_path', - 'input_tensor_names', - 'input_binding_names', - 'output_tensor_names', - 'output_binding_names', - 'verbose', - 'force_engine_update', - 'input_image_width', - 'input_image_height', - 'encoding_desired', - 'network_image_width', - 'network_image_height', - 'image_mean', - 'image_stddev', - 'confidence_threshold', - 'nms_threshold', - 'num_classes', - 'detection_topic', - 'image_input_topic', - 'camera_info_input_topic', - 'enable_visualizer', - 'visualized_image_topic', - 'class_names', - ] - - for key in required_keys: - if key not in cfg: - raise RuntimeError(f'Missing required config key: \'{key}\'') + image_input_topic = cam['image_topic'] + camera_info_input_topic = cam['camera_info_topic'] + input_image_width = int(cam['image_width']) + input_image_height = int(cam['image_height']) model_file_path = str(cfg['model_file_path']) engine_file_path = str(cfg['engine_file_path']) @@ -115,8 +80,6 @@ def _launch_setup(context, *args, **kwargs): verbose = bool(cfg['verbose']) force_engine_update = bool(cfg['force_engine_update']) - input_image_width = int(cfg['input_image_width']) - input_image_height = int(cfg['input_image_height']) encoding_desired = str(cfg['encoding_desired']) network_image_width = int(cfg['network_image_width']) @@ -129,9 +92,6 @@ def _launch_setup(context, *args, **kwargs): num_classes = int(cfg['num_classes']) detection_topic = str(cfg['detection_topic']) - image_input_topic = str(cfg['image_input_topic']) - camera_info_input_topic = str(cfg['camera_info_input_topic']) - enable_visualizer = bool(cfg['enable_visualizer']) visualized_image_topic = str(cfg['visualized_image_topic']) class_names = cfg['class_names'] @@ -271,7 +231,7 @@ def generate_launch_description(): DeclareLaunchArgument( 'camera', default_value='realsense_d555', - description='Camera name (key in cameras.yaml)', + description='Camera key in cameras.yaml', ), OpaqueFunction(function=_launch_setup), ] diff --git a/perception_setup/launch/yolo/yolo_obb.launch.py b/perception_setup/launch/yolo/yolo_obb.launch.py index fd58f96..e3dcc07 100644 --- a/perception_setup/launch/yolo/yolo_obb.launch.py +++ b/perception_setup/launch/yolo/yolo_obb.launch.py @@ -55,51 +55,16 @@ def _launch_setup(context, *args, **kwargs): pkg_dir = get_package_share_directory('perception_setup') - # Resolve camera from launch argument + # Resolve camera topics from cameras.yaml (single source of truth) cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') with open(cameras_path) as f: cameras = yaml.safe_load(f) cam = cameras[LaunchConfiguration('camera').perform(context)] - if cam.get('enable_undistort', True): - cfg['image_input_topic'] = cam['image_topic'] - cfg['camera_info_input_topic'] = cam['camera_info_topic'] - else: - cfg['image_input_topic'] = cam['raw_image_topic'] - cfg['camera_info_input_topic'] = cam['raw_camera_info_topic'] - cfg['input_image_width'] = cam['image_width'] - cfg['input_image_height'] = cam['image_height'] - - required_keys = [ - 'model_file_path', - 'engine_file_path', - 'input_tensor_names', - 'input_binding_names', - 'output_tensor_names', - 'output_binding_names', - 'verbose', - 'force_engine_update', - 'input_image_width', - 'input_image_height', - 'encoding_desired', - 'network_image_width', - 'network_image_height', - 'image_mean', - 'image_stddev', - 'confidence_threshold', - 'num_detections', - 'detection_topic', - 'image_input_topic', - 'camera_info_input_topic', - 'enable_visualizer', - 'visualized_image_topic', - 'class_names', - ] - - for key in required_keys: - if key not in cfg: - raise RuntimeError(f"Missing required config key: '{key}'") - - # Resolve model paths relative to perception_setup/models/ + image_input_topic = cam['image_topic'] + camera_info_input_topic = cam['camera_info_topic'] + input_image_width = int(cam['image_width']) + input_image_height = int(cam['image_height']) + models_dir = os.path.join(pkg_dir, 'models') model_file_path = os.path.join(models_dir, str(cfg['model_file_path'])) @@ -113,8 +78,6 @@ def _launch_setup(context, *args, **kwargs): verbose = bool(cfg['verbose']) force_engine_update = bool(cfg['force_engine_update']) - input_image_width = int(cfg['input_image_width']) - input_image_height = int(cfg['input_image_height']) encoding_desired = str(cfg['encoding_desired']) network_image_width = int(cfg['network_image_width']) @@ -126,9 +89,6 @@ def _launch_setup(context, *args, **kwargs): num_detections = int(cfg['num_detections']) detection_topic = str(cfg['detection_topic']) - image_input_topic = str(cfg['image_input_topic']) - camera_info_input_topic = str(cfg['camera_info_input_topic']) - enable_visualizer = bool(cfg['enable_visualizer']) visualized_image_topic = str(cfg['visualized_image_topic']) class_names = cfg['class_names'] @@ -264,7 +224,7 @@ def generate_launch_description(): DeclareLaunchArgument( 'camera', default_value='realsense_d555', - description='Camera name (key in cameras.yaml)', + description='Camera key in cameras.yaml', ), OpaqueFunction(function=_launch_setup), ] diff --git a/perception_setup/launch/yolo/yolo_seg.launch.py b/perception_setup/launch/yolo/yolo_seg.launch.py index 1153c3b..c80b0b3 100644 --- a/perception_setup/launch/yolo/yolo_seg.launch.py +++ b/perception_setup/launch/yolo/yolo_seg.launch.py @@ -62,19 +62,16 @@ def _launch_setup(context, *args, **kwargs): pkg_dir = get_package_share_directory('perception_setup') - # Resolve camera from launch argument + # Resolve camera topics from cameras.yaml (single source of truth) cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') with open(cameras_path) as f: cameras = yaml.safe_load(f) cam = cameras[LaunchConfiguration('camera').perform(context)] - if cam.get('enable_undistort', True): - cfg['image_input_topic'] = cam['image_topic'] - cfg['camera_info_input_topic'] = cam['camera_info_topic'] - else: - cfg['image_input_topic'] = cam['raw_image_topic'] - cfg['camera_info_input_topic'] = cam['raw_camera_info_topic'] - cfg['input_image_width'] = cam['image_width'] - cfg['input_image_height'] = cam['image_height'] + image_input_topic = cam['image_topic'] + camera_info_input_topic = cam['camera_info_topic'] + input_image_width = int(cam['image_width']) + input_image_height = int(cam['image_height']) + models_dir = os.path.join(pkg_dir, 'models') model_file_path = os.path.join(models_dir, str(cfg['model_file_path'])) @@ -88,8 +85,6 @@ def _launch_setup(context, *args, **kwargs): verbose = bool(cfg['verbose']) force_engine_update = bool(cfg['force_engine_update']) - input_image_width = int(cfg['input_image_width']) - input_image_height = int(cfg['input_image_height']) encoding_desired = str(cfg['encoding_desired']) network_image_width = int(cfg['network_image_width']) @@ -107,9 +102,6 @@ def _launch_setup(context, *args, **kwargs): detection_topic = str(cfg['detection_topic']) mask_topic = str(cfg['mask_topic']) - image_input_topic = str(cfg['image_input_topic']) - camera_info_input_topic = str(cfg['camera_info_input_topic']) - enable_visualizer = bool(cfg['enable_visualizer']) visualized_image_topic = str(cfg['visualized_image_topic']) class_names = cfg['class_names'] @@ -253,8 +245,8 @@ def generate_launch_description(): ), DeclareLaunchArgument( 'camera', - default_value='realsense_d555', - description='Camera name (key in cameras.yaml)', + default_value='blackfly_s', + description='Camera key in cameras.yaml', ), OpaqueFunction(function=_launch_setup), ] diff --git a/perception_setup/package.xml b/perception_setup/package.xml index b50e52c..63a6471 100644 --- a/perception_setup/package.xml +++ b/perception_setup/package.xml @@ -24,10 +24,8 @@ isaac_ros_tensor_rt isaac_ros_dnn_image_encoder isaac_ros_image_proc - valve_detection - image_filtering - aruco_detector - + realsense2_camera + spinnaker_camera_driver ament_cmake 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" From c527d330216970448f7b18461a9fa0277aa5d3eb Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Thu, 2 Apr 2026 02:16:58 +0200 Subject: [PATCH 18/37] feat: Add image undistort component and update launch files - Introduced a new composable node `ImageUndistort` for undistorting images using camera calibration data. - Updated `CMakeLists.txt` to include necessary dependencies and build the new component. - Modified `cameras.yaml` to reflect changes in topic names and structure for the RealSense D555 camera. - Refactored launch files to integrate the new `ImageUndistort` component, replacing previous image processing nodes. - Enhanced `camera_info_publisher.py` to synchronize CameraInfo messages with image topics. - Added new launch files for visual inspection and valve intervention pipelines, utilizing the undistorted images. - Updated `package.xml` to include new dependencies for image processing and detection. --- perception_setup/CMakeLists.txt | 49 +++ perception_setup/config/cameras/cameras.yaml | 20 +- .../perception_setup/image_undistort.hpp | 33 ++ .../launch/cameras/realsense_d555.launch.py | 120 +++---- .../launch/valve_intervention.launch.py | 292 ++++++++++++++++++ .../launch/visual_inspection.launch.py | 131 ++++++++ perception_setup/package.xml | 9 + .../scripts/camera_info_publisher.py | 18 +- perception_setup/scripts/image_undistort.py | 76 +++-- perception_setup/src/image_undistort.cpp | 154 +++++++++ 10 files changed, 800 insertions(+), 102 deletions(-) create mode 100644 perception_setup/include/perception_setup/image_undistort.hpp create mode 100644 perception_setup/launch/valve_intervention.launch.py create mode 100644 perception_setup/launch/visual_inspection.launch.py create mode 100644 perception_setup/src/image_undistort.cpp diff --git a/perception_setup/CMakeLists.txt b/perception_setup/CMakeLists.txt index 317b0e4..b22bb7c 100644 --- a/perception_setup/CMakeLists.txt +++ b/perception_setup/CMakeLists.txt @@ -1,8 +1,57 @@ 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_undistort.py diff --git a/perception_setup/config/cameras/cameras.yaml b/perception_setup/config/cameras/cameras.yaml index 5a52fbd..8cd909e 100644 --- a/perception_setup/config/cameras/cameras.yaml +++ b/perception_setup/config/cameras/cameras.yaml @@ -1,12 +1,7 @@ # Single source of truth for camera topics and image properties. # Both camera launch files and YOLO launch files read from here. -# -# raw_*: Topics published directly by the camera driver -# output_*: Processed topics published by the camera pipeline (undistort/crop) -# — these are what downstream nodes (YOLO, etc.) subscribe to blackfly_s: - # No image processing — driver output IS the final output image_topic: "/blackfly_s/image_raw" camera_info_topic: "/blackfly_s/camera_info" image_width: 720 @@ -15,17 +10,16 @@ blackfly_s: realsense_d555: # Raw topics from the RealSense driver - raw_image_topic: "/camera/camera/color/image_raw" - raw_camera_info_topic: "/camera/camera/color/camera_info" - raw_depth_topic: "/camera/camera/depth/image_rect_raw" + raw_color_image_topic: "/camera/camera/color/image_raw" + raw_color_camera_info_topic: "/camera/camera/color/camera_info" + raw_depth_image_topic: "/camera/camera/depth/image_rect_raw" raw_depth_camera_info_topic: "/camera/camera/depth/camera_info" - calibration_camera_info_topic: "/camera/camera/color_raw/camera_info" - # Processed output topics (after undistort/crop) - image_topic: "/realsense_d555/color/image" + # Final output topics (downstream nodes subscribe to these) + image_topic: "/realsense_d555/color/image_rect" camera_info_topic: "/realsense_d555/color/camera_info" - depth_image_topic: "/realsense_d555/depth/image" - depth_camera_info_topic: "/realsense_d555/depth/camera_info" + depth_image_topic: "/camera/camera/depth/image_rect_raw" + depth_camera_info_topic: "/camera/camera/depth/camera_info" image_width: 896 image_height: 504 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..f95bcfd --- /dev/null +++ b/perception_setup/include/perception_setup/image_undistort.hpp @@ -0,0 +1,33 @@ +#pragma once + +#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/realsense_d555.launch.py b/perception_setup/launch/cameras/realsense_d555.launch.py index 52c35b3..e807db3 100644 --- a/perception_setup/launch/cameras/realsense_d555.launch.py +++ b/perception_setup/launch/cameras/realsense_d555.launch.py @@ -2,9 +2,7 @@ Starts: - RealSense camera driver node - - camera_info_publisher (publishes calibration CameraInfo) - - image_undistort (undistorts color image) - - image_crop (crops depth image) + - image_undistort (undistorts color image using color_realsense_d555_calib.yaml) Topics are read from cameras.yaml (single source of truth). """ @@ -14,8 +12,10 @@ import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch_ros.actions import Node - +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") @@ -32,12 +32,12 @@ def generate_launch_description(): realsense_node = Node( package="realsense2_camera", executable="realsense2_camera_node", - name="front_camera", + name="camera", namespace="camera", parameters=[ { "enable_color": True, - "rgb_camera.color_profile": "896,504,15", + "rgb_camera.color_profile": "896,504,15", # When updating the image size, make sure to also update the calibration file "rgb_camera.color_format": "RGB8", "rgb_camera.enable_auto_exposure": True, "enable_depth": True, @@ -54,66 +54,76 @@ def generate_launch_description(): "enable_sync": False, } ], + remappings=[ + # Color image is NOT remapped here — image_undistort reads the raw + # topic and publishes to cam["image_topic"] (image_rect). + (cam["raw_depth_image_topic"], cam["depth_image_topic"]), + (cam["raw_depth_camera_info_topic"], cam["depth_camera_info_topic"]), + ], output="screen", arguments=["--ros-args", "--log-level", "info"], emulate_tty=True, ) - camera_info_publisher_node = Node( - package="perception_setup", - executable="camera_info_publisher.py", - name="camera_info_publisher", - parameters=[ - { - "camera_info_file": calib_file, - "camera_info_topic": cam["calibration_camera_info_topic"], - } - ], - output="screen", - ) - - image_undistort_node = Node( - package="perception_setup", - executable="image_undistort.py", - name="image_undistort", - parameters=[ - { - "image_topic": cam["raw_image_topic"], - "camera_info_topic": cam["calibration_camera_info_topic"], - "raw_camera_info_topic": cam["raw_camera_info_topic"], - "output_image_topic": cam["image_topic"], - "output_camera_info_topic": cam["camera_info_topic"], - "enable_undistort": True, - } + # Undistorts the color image using the calibration YAML. + # In undistort mode : loads K+D from calib file at startup, publishes + # rectified image + zero-distortion camera_info. + # In passthrough mode: relays raw image and driver camera_info as-is. + 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": cam["raw_color_image_topic"], + "camera_info_file": calib_file, + "raw_camera_info_topic": cam["raw_color_camera_info_topic"], + "output_image_topic": cam["image_topic"], + "output_camera_info_topic": cam["camera_info_topic"], + "enable_undistort": LaunchConfiguration("enable_undistort"), + "image_qos": "reliable", + } + ], + ), ], output="screen", ) - image_crop_node = Node( - package="perception_setup", - executable="image_crop.py", - name="image_crop", - parameters=[ - { - "image_topic": cam["raw_depth_topic"], - "camera_info_topic": cam["raw_depth_camera_info_topic"], - "output_image_topic": cam["depth_image_topic"], - "output_camera_info_topic": cam["depth_camera_info_topic"], - "enable_crop": True, - "crop.x_offset": 260, - "crop.y_offset": 190, - "crop.width": 485, - "crop.height": 245, - } - ], - output="screen", - ) + # TODO: Valve detection worked better without this, figure out why. + # image_crop_node = Node( + # package="perception_setup", + # executable="image_crop.py", + # name="image_crop", + # parameters=[ + # { + # "image_topic": cam["raw_depth_image_topic"], + # "camera_info_topic": cam["raw_depth_camera_info_topic"], + # "output_image_topic": cam["depth_image_topic"], + # "output_camera_info_topic": cam["depth_camera_info_topic"], + # "enable_crop": False, + # "crop.x_offset": 260, + # "crop.y_offset": 190, + # "crop.width": 485, + # "crop.height": 245, + # } + # ], + # output="screen", + # ) return LaunchDescription( [ + DeclareLaunchArgument( + "enable_undistort", + default_value="true", + description="Undistort color image before publishing to image_topic", + ), realsense_node, - camera_info_publisher_node, - image_undistort_node, - image_crop_node, + image_undistort_container, ] ) diff --git a/perception_setup/launch/valve_intervention.launch.py b/perception_setup/launch/valve_intervention.launch.py new file mode 100644 index 0000000..a3f7ced --- /dev/null +++ b/perception_setup/launch/valve_intervention.launch.py @@ -0,0 +1,292 @@ +"""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 + + +CONVERTED_IMAGE_TOPIC = '/yolo_obb/internal/converted_image' +ENCODER_RESIZE_TOPIC = '/yolo_obb_encoder/internal/resize/image' +TENSOR_OUTPUT_TOPIC = '/yolo_obb/tensor_pub' +TENSOR_INPUT_TOPIC = '/yolo_obb/tensor_sub' +DNN_IMAGE_ENCODER_NAMESPACE = 'yolo_obb_encoder/internal' + + +def _launch_setup(context, *args, **kwargs): + """Setup launch description with all nodes.""" + pkg_dir = get_package_share_directory('perception_setup') + + # Load configurations + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras['realsense_d555'] + + yolo_config_path = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_obb.yaml') + with open(yolo_config_path) as f: + yolo_cfg = yaml.safe_load(f) + + calib_file = os.path.join( + pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml' + ) + models_dir = os.path.join(pkg_dir, 'models') + model_file_path = os.path.join(models_dir, str(yolo_cfg['model_file_path'])) + engine_file_path = os.path.join(models_dir, str(yolo_cfg['engine_file_path'])) + + input_image_width = int(cam['image_width']) + input_image_height = int(cam['image_height']) + + # RealSense Camera + 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=[ + # Color image is NOT remapped here — image_undistort reads the raw + # topic and publishes to cam["image_topic"] (image_rect). + (cam['raw_depth_image_topic'], cam['depth_image_topic']), + (cam['raw_depth_camera_info_topic'], cam['depth_camera_info_topic']), + ], + ) + + # Undistorts the color image using the calibration YAML. + # In undistort mode : loads K+D from calib file at startup, publishes + # rectified image + zero-distortion camera_info. + # In passthrough mode: relays raw image and driver camera_info as-is. + image_undistort_node = ComposableNode( + package='perception_setup', + plugin='perception_setup::ImageUndistort', + name='color_image_undistort', + parameters=[ + { + 'image_topic': cam['raw_color_image_topic'], + 'camera_info_file': calib_file, + 'raw_camera_info_topic': cam['raw_color_camera_info_topic'], + 'output_image_topic': cam['image_topic'], + 'output_camera_info_topic': cam['camera_info_topic'], + 'enable_undistort': LaunchConfiguration('enable_undistort'), + 'image_qos': 'reliable', # Isaac ros only works with reliable QoS + } + ], + ) + + # YOLO OBB Pipeline + image_format_converter = ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_converter', + parameters=[ + { + 'encoding_desired': yolo_cfg['encoding_desired'], + 'image_width': input_image_width, + 'image_height': input_image_height, + 'input_qos': 'sensor_data', + } + ], + remappings=[ + ('image_raw', cam['image_topic']), + ('image', CONVERTED_IMAGE_TOPIC), + ], + ) + + 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': bool(yolo_cfg['verbose']), + 'force_engine_update': bool(yolo_cfg['force_engine_update']), + 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + } + ], + remappings=[ + ('tensor_pub', TENSOR_OUTPUT_TOPIC), + ('tensor_sub', TENSOR_INPUT_TOPIC), + ], + ) + + 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': TENSOR_INPUT_TOPIC, + 'confidence_threshold': float(yolo_cfg['confidence_threshold']), + 'num_detections': int(yolo_cfg['num_detections']), + 'detections_topic': str(yolo_cfg['detection_topic']), + } + ], + ) + + 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(input_image_width), + 'input_image_height': str(input_image_height), + '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': DNN_IMAGE_ENCODER_NAMESPACE, + 'image_input_topic': CONVERTED_IMAGE_TOPIC, + 'camera_info_input_topic': cam['camera_info_topic'], + 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + }.items(), + ) + + yolo_obb_visualizer = Node( + package='isaac_ros_yolov26_obb', + executable='isaac_ros_yolov26_obb_visualizer.py', + name='yolo_obb_visualizer', + parameters=[ + { + 'detections_topic': yolo_cfg['detection_topic'], + 'image_topic': ENCODER_RESIZE_TOPIC, + 'output_image_topic': yolo_cfg['visualized_image_topic'], + 'class_names_yaml': str(yolo_cfg['class_names']), + } + ], + ) if bool(yolo_cfg['enable_visualizer']) else None + + # Valve Detection + valve_detection_config = 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_config, + { + 'depth_image_sub_topic': cam['depth_image_topic'], + 'detections_sub_topic': yolo_cfg['detection_topic'], + 'depth_image_info_topic': cam['depth_camera_info_topic'], + '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'), + }, + ], + ) + ], + output='screen', + ) + + # Collect all actions + actions = [ + tensor_rt_container, + dnn_image_encoder_launch, + valve_detection_container, + ] + + if yolo_obb_visualizer: + actions.append(yolo_obb_visualizer) + + return actions + + +def generate_launch_description(): + """Generate the launch description.""" + return launch.LaunchDescription( + [ + DeclareLaunchArgument( + 'enable_undistort', + default_value='true', + description='Undistort color image before passing to YOLO', + ), + # Valve Detection Options + DeclareLaunchArgument( + 'drone', + default_value='nautilus', + description='Robot name, prepended to TF frame IDs', + ), + DeclareLaunchArgument( + 'undistort_detections', + default_value='false', # If this is enabled then enable_undistort must be disabled to avoid double undistortion. TODO: I had problems getting this working, work around is to just use enable_undistort + description='Undistort detections using color camera distortion', + ), + DeclareLaunchArgument( + 'debug_visualize', + default_value='true', + description='Enable debug visualization topics', + ), + 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..e8b223b --- /dev/null +++ b/perception_setup/launch/visual_inspection.launch.py @@ -0,0 +1,131 @@ +"""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, visualizaion and writes logs +""" + +import os + +import yaml +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 +from launch_ros.descriptions import ComposableNode + +FILTERED_IMAGE_TOPIC = '/visual_inspection/filtered_image' + + +def generate_launch_description(): + pkg_dir = get_package_share_directory('perception_setup') + + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras['realsense_d555'] + + calib_file = os.path.join( + pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml' + ) + + 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': cam['raw_color_image_topic'], + 'camera_info_file': calib_file, + 'raw_camera_info_topic': cam['raw_color_camera_info_topic'], + 'output_image_topic': cam['image_topic'], + 'output_camera_info_topic': cam['camera_info_topic'], + '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': cam['image_topic'], + 'pub_topic': FILTERED_IMAGE_TOPIC, + 'input_encoding': cam['encoding'], + 'output_encoding': cam['encoding'], + 'filter_params.filter_type': 'no_filter', + }, + ], + ), + 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': FILTERED_IMAGE_TOPIC, + 'subs.camera_info_topic': cam['camera_info_topic'], + 'detect_board': False, + 'visualize': True, + 'log_markers': True, + 'publish_detections': True, + 'publish_landmarks': False, + }, + ], + ), + ], + output='screen', + arguments=['--ros-args', '--log-level', 'info'], + ) + + return LaunchDescription([ + DeclareLaunchArgument( + 'enable_undistort', + default_value='true', + description='Undistort color image before publishing to image_topic', + ), + visual_inspection_container, + ]) diff --git a/perception_setup/package.xml b/perception_setup/package.xml index 63a6471..a06c2d4 100644 --- a/perception_setup/package.xml +++ b/perception_setup/package.xml @@ -10,6 +10,12 @@ camera_info_manager vortex_utils_ros + rclcpp + rclcpp_components + sensor_msgs + cv_bridge + yaml-cpp + ament_cmake ament_lint_auto @@ -26,6 +32,9 @@ isaac_ros_image_proc realsense2_camera spinnaker_camera_driver + valve_detection + image_filtering + aruco_detector ament_cmake diff --git a/perception_setup/scripts/camera_info_publisher.py b/perception_setup/scripts/camera_info_publisher.py index b918091..32fa7ca 100755 --- a/perception_setup/scripts/camera_info_publisher.py +++ b/perception_setup/scripts/camera_info_publisher.py @@ -4,7 +4,7 @@ import yaml from rclpy.node import Node from rclpy.parameter import Parameter -from sensor_msgs.msg import CameraInfo +from sensor_msgs.msg import CameraInfo, Image from vortex_utils_ros.qos_profiles import reliable_profile @@ -14,9 +14,11 @@ def __init__(self): 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") @@ -24,6 +26,9 @@ def __init__(self): 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) @@ -39,17 +44,18 @@ def __init__(self): msg.p = data["projection_matrix"]["data"] self.publisher = self.create_publisher( - CameraInfo, topic_name, reliable_profile(1) + CameraInfo, topic_name, reliable_profile(10) ) self.msg = msg - self.create_timer(1.0, self.publish_camera_info) + self.create_subscription(Image, image_topic, self._image_callback, reliable_profile(1)) - self.get_logger().info(f"Publishing CameraInfo on {topic_name} at 1 Hz") + self.get_logger().info(f"Publishing CameraInfo on {topic_name} synced to {image_topic}") self.get_logger().info(f"Loaded calibration: {file_path}") - def publish_camera_info(self): - self.msg.header.stamp = self.get_clock().now().to_msg() + 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) diff --git a/perception_setup/scripts/image_undistort.py b/perception_setup/scripts/image_undistort.py index 09989c4..efa67d0 100755 --- a/perception_setup/scripts/image_undistort.py +++ b/perception_setup/scripts/image_undistort.py @@ -3,6 +3,7 @@ import cv2 import numpy as np import rclpy +import yaml from cv_bridge import CvBridge from rclpy.node import Node from rclpy.parameter import Parameter @@ -15,20 +16,30 @@ def __init__(self): super().__init__("image_undistort") self.declare_parameter("image_topic", Parameter.Type.STRING) - self.declare_parameter("camera_info_topic", Parameter.Type.STRING) + self.declare_parameter("camera_info_topic", "") + self.declare_parameter("camera_info_file", "") self.declare_parameter("raw_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("enable_undistort", Parameter.Type.BOOL) + self.declare_parameter("image_qos", "sensor_data") image_topic = self.get_parameter("image_topic").value info_topic = self.get_parameter("camera_info_topic").value + camera_info_file = self.get_parameter("camera_info_file").value raw_info_topic = self.get_parameter("raw_camera_info_topic").value out_topic = self.get_parameter("output_image_topic").value out_info_topic = self.get_parameter("output_camera_info_topic").value enable_undistort = self.get_parameter("enable_undistort").value + image_qos_str = self.get_parameter("image_qos").value - self.pub = self.create_publisher(Image, out_topic, reliable_profile(10)) + image_qos = ( + reliable_profile(10) + if image_qos_str == "reliable" + else sensor_data_profile(10) + ) + + self.pub = self.create_publisher(Image, out_topic, image_qos) self.info_pub = self.create_publisher( CameraInfo, out_info_topic, reliable_profile(10) ) @@ -39,11 +50,18 @@ def __init__(self): self.map2 = None self.rectified_info = None - # Camera info only needs to be received once — use transient local so we - # also catch messages published before this node started (latched). - self.create_subscription( - CameraInfo, info_topic, self.info_callback, reliable_profile(1) - ) + if camera_info_file: + # Load K and D directly from the calibration YAML — maps are + # ready immediately, no camera_info topic needed. + self._init_maps_from_file(camera_info_file) + else: + # Camera info only needs to be received once — use transient + # local so we also catch messages published before this node + # started (latched). + self.create_subscription( + CameraInfo, info_topic, self.info_callback, reliable_profile(1) + ) + self.create_subscription( Image, image_topic, self.image_callback, sensor_data_profile(10) ) @@ -59,17 +77,21 @@ def __init__(self): f"image_undistort: passthrough {image_topic} -> {out_topic}" ) - def info_callback(self, msg: CameraInfo): - if self.map1 is not None: - return - k = np.array(msg.k, dtype=np.float64).reshape(3, 3) - d = np.array(msg.d, dtype=np.float64) - h, w = msg.height, msg.width + def _init_maps_from_file(self, path: str): + with open(path) as f: + data = yaml.safe_load(f) + k = np.array(data["camera_matrix"]["data"], dtype=np.float64).reshape(3, 3) + d = np.array(data["distortion_coefficients"]["data"], dtype=np.float64) + w = int(data["image_width"]) + h = int(data["image_height"]) + self._build_maps(k, d, w, h) + self.get_logger().info(f"Undistortion maps initialised from file ({w}x{h})") + + def _build_maps(self, k, d, w, h): new_k, _ = cv2.getOptimalNewCameraMatrix(k, d, (w, h), alpha=0) self.map1, self.map2 = cv2.initUndistortRectifyMap( k, d, None, new_k, (w, h), cv2.CV_16SC2 ) - # Build the rectified camera_info (zero distortion, updated K and P) self.rectified_info = CameraInfo() self.rectified_info.width = w self.rectified_info.height = h @@ -77,21 +99,19 @@ def info_callback(self, msg: CameraInfo): self.rectified_info.d = [0.0, 0.0, 0.0, 0.0, 0.0] self.rectified_info.k = new_k.flatten().tolist() self.rectified_info.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] - p = [ - new_k[0, 0], - 0.0, - new_k[0, 2], - 0.0, - 0.0, - new_k[1, 1], - new_k[1, 2], - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, + self.rectified_info.p = [ + new_k[0, 0], 0.0, new_k[0, 2], 0.0, + 0.0, new_k[1, 1], new_k[1, 2], 0.0, + 0.0, 0.0, 1.0, 0.0, ] - self.rectified_info.p = p + + def info_callback(self, msg: CameraInfo): + if self.map1 is not None: + return + k = np.array(msg.k, dtype=np.float64).reshape(3, 3) + d = np.array(msg.d, dtype=np.float64) + h, w = msg.height, msg.width + self._build_maps(k, d, w, h) self.get_logger().info(f"Undistortion maps initialised ({w}x{h})") def image_callback(self, msg: Image): diff --git a/perception_setup/src/image_undistort.cpp b/perception_setup/src/image_undistort.cpp new file mode 100644 index 0000000..3b98895 --- /dev/null +++ b/perception_setup/src/image_undistort.cpp @@ -0,0 +1,154 @@ +#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) From 036517a986c28d164b5c6bff7f8a02ea1090b7cd Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Thu, 2 Apr 2026 02:33:42 +0200 Subject: [PATCH 19/37] refactor: update README and remove deprecated image_undistort script --- README.md | 5 - perception_setup/README.md | 186 ++++++++++++-------- perception_setup/scripts/image_undistort.py | 148 ---------------- 3 files changed, 114 insertions(+), 225 deletions(-) delete mode 100755 perception_setup/scripts/image_undistort.py 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/perception_setup/README.md b/perception_setup/README.md index 59046ed..d203433 100644 --- a/perception_setup/README.md +++ b/perception_setup/README.md @@ -1,104 +1,146 @@ -# 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. Contains camera drivers, image preprocessing (undistortion), YOLO inference pipelines, and mission-specific launch files. All camera topics and image dimensions are defined in a single config file (`cameras.yaml`) to keep launch files in sync. -## Prerequisites +## Package structure -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` +``` +perception_setup/ + config/ + cameras/ + cameras.yaml # Topic names and image dimensions (single source of truth) + 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 model config (valve detection) + yolo_detect.yaml # YOLO detection model config + yolo_seg.yaml # YOLO segmentation model config + yolo_cls.yaml # YOLO classification model config + launch/ + cameras/ + realsense_d555.launch.py # RealSense D555 + image_undistort + blackfly_s.launch.py # Blackfly S camera driver + 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 + 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 +``` -## Launch File Description +## Helper nodes -The main launch file provided is `perception.launch.py`, which includes several arguments to control the behavior of the nodes being launched. +This package provides one C++ composable node: -### Launch Arguments +| 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`. | -- `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. +### Python scripts (not currently used in any launch file) -### Configuration Files +| 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. | -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. +### ImageUndistort parameters -These files should be located in the `config` directory of the `perception_setup` package. +| 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) | -## Usage +## Launch files -To use the launch file, follow these steps: +### valve_intervention.launch.py -1. **Navigate to your ROS 2 workspace**: - ```sh - cd ~/ - ``` +Full valve detection pipeline: RealSense D555 -> image undistortion -> YOLO OBB -> valve pose estimation. -2. **Source your workspace**: - ```sh - source install/setup.bash - ``` +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 + +```sh +ros2 launch perception_setup valve_intervention.launch.py +``` -3. **Run the launch file with default arguments**: - ```sh - ros2 launch perception_setup perception.launch.py - ``` +| 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 | -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 +### visual_inspection.launch.py -The launch file can run nodes as either composable nodes or as separate nodes based on the `enable_composable_nodes` argument. +ArUco marker detection pipeline: RealSense D555 -> image undistortion -> image filtering -> ArUco detector. -### Composable Nodes +All nodes run in a single composable container (`visual_inspection_container`). -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 visual_inspection.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 processing | -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`. +### cameras/realsense_d555.launch.py -### Standalone Nodes +Standalone RealSense D555 camera driver with image undistortion. Publishes both color (undistorted) and depth streams. -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. +```sh +ros2 launch perception_setup realsense_d555.launch.py +``` -## Calibration Files +| Argument | Default | Description | +|---|---|---| +| `enable_undistort` | `true` | Undistort color image before publishing | -The camera calibration files are located in the `config` directory of the `perception_setup` package: -- `gripper_camera_calib.yaml` -- `front_camera_calib.yaml` +## Configuration -These files are referenced in the launch file to provide calibration data for the cameras. +### cameras.yaml -## Example Commands +Single source of truth for all camera topic names and image dimensions. Both camera launch files and YOLO launch files read from this file. Example entry: -**Launch with all nodes enabled (default):** -```sh -ros2 launch perception_setup perception.launch.py +```yaml +realsense_d555: + raw_color_image_topic: "/camera/camera/color/image_raw" + raw_color_camera_info_topic: "/camera/camera/color/camera_info" + image_topic: "/realsense_d555/color/image_rect" # downstream nodes subscribe here + camera_info_topic: "/realsense_d555/color/camera_info" + image_width: 896 + image_height: 504 + encoding: "rgb8" ``` -**Launch with only the gripper camera and image filtering enabled:** + +### YOLO config files + +Each YOLO variant (`yolo_obb.yaml`, `yolo_detect.yaml`, etc.) specifies model paths, network input dimensions, confidence thresholds, and output topic names. These are read by the corresponding launch files. + +## 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/scripts/image_undistort.py b/perception_setup/scripts/image_undistort.py deleted file mode 100755 index efa67d0..0000000 --- a/perception_setup/scripts/image_undistort.py +++ /dev/null @@ -1,148 +0,0 @@ -#!/usr/bin/env python3 - -import cv2 -import numpy as np -import rclpy -import yaml -from cv_bridge import CvBridge -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, sensor_data_profile - - -class ImageUndistort(Node): - def __init__(self): - super().__init__("image_undistort") - - self.declare_parameter("image_topic", Parameter.Type.STRING) - self.declare_parameter("camera_info_topic", "") - self.declare_parameter("camera_info_file", "") - self.declare_parameter("raw_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("enable_undistort", Parameter.Type.BOOL) - self.declare_parameter("image_qos", "sensor_data") - - image_topic = self.get_parameter("image_topic").value - info_topic = self.get_parameter("camera_info_topic").value - camera_info_file = self.get_parameter("camera_info_file").value - raw_info_topic = self.get_parameter("raw_camera_info_topic").value - out_topic = self.get_parameter("output_image_topic").value - out_info_topic = self.get_parameter("output_camera_info_topic").value - enable_undistort = self.get_parameter("enable_undistort").value - image_qos_str = self.get_parameter("image_qos").value - - image_qos = ( - reliable_profile(10) - if image_qos_str == "reliable" - else sensor_data_profile(10) - ) - - self.pub = self.create_publisher(Image, out_topic, image_qos) - self.info_pub = self.create_publisher( - CameraInfo, out_info_topic, reliable_profile(10) - ) - - if enable_undistort: - self.bridge = CvBridge() - self.map1 = None - self.map2 = None - self.rectified_info = None - - if camera_info_file: - # Load K and D directly from the calibration YAML — maps are - # ready immediately, no camera_info topic needed. - self._init_maps_from_file(camera_info_file) - else: - # Camera info only needs to be received once — use transient - # local so we also catch messages published before this node - # started (latched). - self.create_subscription( - CameraInfo, info_topic, self.info_callback, reliable_profile(1) - ) - - self.create_subscription( - Image, image_topic, self.image_callback, sensor_data_profile(10) - ) - self.get_logger().info(f"image_undistort: {image_topic} -> {out_topic}") - else: - self.create_subscription( - Image, image_topic, self.relay_image, sensor_data_profile(10) - ) - self.create_subscription( - CameraInfo, raw_info_topic, self.relay_camera_info, reliable_profile(1) - ) - self.get_logger().info( - f"image_undistort: passthrough {image_topic} -> {out_topic}" - ) - - def _init_maps_from_file(self, path: str): - with open(path) as f: - data = yaml.safe_load(f) - k = np.array(data["camera_matrix"]["data"], dtype=np.float64).reshape(3, 3) - d = np.array(data["distortion_coefficients"]["data"], dtype=np.float64) - w = int(data["image_width"]) - h = int(data["image_height"]) - self._build_maps(k, d, w, h) - self.get_logger().info(f"Undistortion maps initialised from file ({w}x{h})") - - def _build_maps(self, k, d, w, h): - new_k, _ = cv2.getOptimalNewCameraMatrix(k, d, (w, h), alpha=0) - self.map1, self.map2 = cv2.initUndistortRectifyMap( - k, d, None, new_k, (w, h), cv2.CV_16SC2 - ) - self.rectified_info = CameraInfo() - self.rectified_info.width = w - self.rectified_info.height = h - self.rectified_info.distortion_model = "plumb_bob" - self.rectified_info.d = [0.0, 0.0, 0.0, 0.0, 0.0] - self.rectified_info.k = new_k.flatten().tolist() - self.rectified_info.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] - self.rectified_info.p = [ - new_k[0, 0], 0.0, new_k[0, 2], 0.0, - 0.0, new_k[1, 1], new_k[1, 2], 0.0, - 0.0, 0.0, 1.0, 0.0, - ] - - def info_callback(self, msg: CameraInfo): - if self.map1 is not None: - return - k = np.array(msg.k, dtype=np.float64).reshape(3, 3) - d = np.array(msg.d, dtype=np.float64) - h, w = msg.height, msg.width - self._build_maps(k, d, w, h) - self.get_logger().info(f"Undistortion maps initialised ({w}x{h})") - - def image_callback(self, msg: Image): - if self.map1 is None: - self.get_logger().warn( - "Waiting for camera_info...", throttle_duration_sec=5.0 - ) - return - - cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") - undistorted = cv2.remap(cv_image, self.map1, self.map2, cv2.INTER_LINEAR) - - out_msg = self.bridge.cv2_to_imgmsg(undistorted, encoding=msg.encoding) - out_msg.header = msg.header - self.pub.publish(out_msg) - - self.rectified_info.header = msg.header - self.info_pub.publish(self.rectified_info) - - def relay_image(self, msg: Image): - self.pub.publish(msg) - - def relay_camera_info(self, msg: CameraInfo): - self.info_pub.publish(msg) - - -def main(): - rclpy.init() - node = ImageUndistort() - rclpy.spin(node) - - -if __name__ == "__main__": - main() From 8e9274ecd0a48f131519118aabf854a473ce30d4 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Thu, 2 Apr 2026 02:35:25 +0200 Subject: [PATCH 20/37] refactor: improve code formatting and readability across multiple launch and source files --- .../perception_setup/image_undistort.hpp | 1 + .../launch/cameras/blackfly_s.launch.py | 8 +- .../launch/cameras/realsense_d555.launch.py | 3 +- .../launch/valve_intervention.launch.py | 45 ++++++----- .../launch/visual_inspection.launch.py | 20 ++--- .../scripts/camera_info_publisher.py | 8 +- perception_setup/src/image_undistort.cpp | 77 +++++++++++++------ 7 files changed, 104 insertions(+), 58 deletions(-) diff --git a/perception_setup/include/perception_setup/image_undistort.hpp b/perception_setup/include/perception_setup/image_undistort.hpp index f95bcfd..98beaa9 100644 --- a/perception_setup/include/perception_setup/image_undistort.hpp +++ b/perception_setup/include/perception_setup/image_undistort.hpp @@ -4,6 +4,7 @@ #include #include #include +#include namespace perception_setup { diff --git a/perception_setup/launch/cameras/blackfly_s.launch.py b/perception_setup/launch/cameras/blackfly_s.launch.py index ef98252..6803f36 100644 --- a/perception_setup/launch/cameras/blackfly_s.launch.py +++ b/perception_setup/launch/cameras/blackfly_s.launch.py @@ -21,12 +21,8 @@ def generate_launch_description(): 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" - ) + 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") enable_camera_arg = DeclareLaunchArgument( "enable_camera", diff --git a/perception_setup/launch/cameras/realsense_d555.launch.py b/perception_setup/launch/cameras/realsense_d555.launch.py index e807db3..066a271 100644 --- a/perception_setup/launch/cameras/realsense_d555.launch.py +++ b/perception_setup/launch/cameras/realsense_d555.launch.py @@ -17,6 +17,7 @@ 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") @@ -37,7 +38,7 @@ def generate_launch_description(): parameters=[ { "enable_color": True, - "rgb_camera.color_profile": "896,504,15", # When updating the image size, make sure to also update the calibration file + "rgb_camera.color_profile": "896,504,15", # When updating the image size, make sure to also update the calibration file "rgb_camera.color_format": "RGB8", "rgb_camera.enable_auto_exposure": True, "enable_depth": True, diff --git a/perception_setup/launch/valve_intervention.launch.py b/perception_setup/launch/valve_intervention.launch.py index a3f7ced..740f178 100644 --- a/perception_setup/launch/valve_intervention.launch.py +++ b/perception_setup/launch/valve_intervention.launch.py @@ -12,13 +12,16 @@ import launch import yaml from ament_index_python.packages import get_package_share_directory -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +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 - CONVERTED_IMAGE_TOPIC = '/yolo_obb/internal/converted_image' ENCODER_RESIZE_TOPIC = '/yolo_obb_encoder/internal/resize/image' TENSOR_OUTPUT_TOPIC = '/yolo_obb/tensor_pub' @@ -100,7 +103,7 @@ def _launch_setup(context, *args, **kwargs): 'output_image_topic': cam['image_topic'], 'output_camera_info_topic': cam['camera_info_topic'], 'enable_undistort': LaunchConfiguration('enable_undistort'), - 'image_qos': 'reliable', # Isaac ros only works with reliable QoS + 'image_qos': 'reliable', # Isaac ros only works with reliable QoS } ], ) @@ -198,19 +201,23 @@ def _launch_setup(context, *args, **kwargs): }.items(), ) - yolo_obb_visualizer = Node( - package='isaac_ros_yolov26_obb', - executable='isaac_ros_yolov26_obb_visualizer.py', - name='yolo_obb_visualizer', - parameters=[ - { - 'detections_topic': yolo_cfg['detection_topic'], - 'image_topic': ENCODER_RESIZE_TOPIC, - 'output_image_topic': yolo_cfg['visualized_image_topic'], - 'class_names_yaml': str(yolo_cfg['class_names']), - } - ], - ) if bool(yolo_cfg['enable_visualizer']) else None + yolo_obb_visualizer = ( + Node( + package='isaac_ros_yolov26_obb', + executable='isaac_ros_yolov26_obb_visualizer.py', + name='yolo_obb_visualizer', + parameters=[ + { + 'detections_topic': yolo_cfg['detection_topic'], + 'image_topic': ENCODER_RESIZE_TOPIC, + 'output_image_topic': yolo_cfg['visualized_image_topic'], + 'class_names_yaml': str(yolo_cfg['class_names']), + } + ], + ) + if bool(yolo_cfg['enable_visualizer']) + else None + ) # Valve Detection valve_detection_config = os.path.join( @@ -240,7 +247,9 @@ def _launch_setup(context, *args, **kwargs): 'landmarks_pub_topic': '/valve_landmarks', 'output_frame_id': 'front_camera_depth_optical', 'drone': LaunchConfiguration('drone'), - 'undistort_detections': LaunchConfiguration('undistort_detections'), + 'undistort_detections': LaunchConfiguration( + 'undistort_detections' + ), 'debug_visualize': LaunchConfiguration('debug_visualize'), }, ], @@ -279,7 +288,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( 'undistort_detections', - default_value='false', # If this is enabled then enable_undistort must be disabled to avoid double undistortion. TODO: I had problems getting this working, work around is to just use enable_undistort + default_value='false', # If this is enabled then enable_undistort must be disabled to avoid double undistortion. TODO: I had problems getting this working, work around is to just use enable_undistort description='Undistort detections using color camera distortion', ), DeclareLaunchArgument( diff --git a/perception_setup/launch/visual_inspection.launch.py b/perception_setup/launch/visual_inspection.launch.py index e8b223b..c2aa775 100644 --- a/perception_setup/launch/visual_inspection.launch.py +++ b/perception_setup/launch/visual_inspection.launch.py @@ -4,7 +4,7 @@ 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, visualizaion and writes logs +4. aruco_detector runs on the filtered image, publishes detections, visualization and writes logs """ import os @@ -121,11 +121,13 @@ def generate_launch_description(): arguments=['--ros-args', '--log-level', 'info'], ) - return LaunchDescription([ - DeclareLaunchArgument( - 'enable_undistort', - default_value='true', - description='Undistort color image before publishing to image_topic', - ), - visual_inspection_container, - ]) + return LaunchDescription( + [ + DeclareLaunchArgument( + 'enable_undistort', + default_value='true', + description='Undistort color image before publishing to image_topic', + ), + visual_inspection_container, + ] + ) diff --git a/perception_setup/scripts/camera_info_publisher.py b/perception_setup/scripts/camera_info_publisher.py index 32fa7ca..2b6a132 100755 --- a/perception_setup/scripts/camera_info_publisher.py +++ b/perception_setup/scripts/camera_info_publisher.py @@ -48,9 +48,13 @@ def __init__(self): ) self.msg = msg - self.create_subscription(Image, image_topic, self._image_callback, reliable_profile(1)) + 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"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): diff --git a/perception_setup/src/image_undistort.cpp b/perception_setup/src/image_undistort.cpp index 3b98895..dd09978 100644 --- a/perception_setup/src/image_undistort.cpp +++ b/perception_setup/src/image_undistort.cpp @@ -11,13 +11,18 @@ 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 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 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_str = + declare_parameter("image_qos", "sensor_data"); const auto image_qos = (image_qos_str == "reliable") ? rclcpp::QoS(10).reliable() @@ -25,8 +30,10 @@ ImageUndistort::ImageUndistort(const rclcpp::NodeOptions& options) 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); + 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()) { @@ -69,7 +76,8 @@ 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 auto d_vec = + data["distortion_coefficients"]["data"].as>(); const int w = data["image_width"].as(); const int h = data["image_height"].as(); @@ -78,10 +86,14 @@ void ImageUndistort::init_maps_from_file(const std::string& path) { const_cast(d_vec.data())); build_maps(k, d, w, h); - RCLCPP_INFO(get_logger(), "Undistortion maps initialised from file (%dx%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) { +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_); @@ -91,32 +103,51 @@ void ImageUndistort::build_maps(const cv::Mat& k, const cv::Mat& d, int w, int h 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_.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}; + 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; +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)); + 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) { +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..."); @@ -134,7 +165,8 @@ void ImageUndistort::image_callback(const sensor_msgs::msg::Image::SharedPtr msg 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(); + auto out_msg = cv_bridge::CvImage(msg->header, msg->encoding, undistorted) + .toImageMsg(); image_pub_->publish(*out_msg); rectified_info_.header = msg->header; @@ -145,7 +177,8 @@ 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) { +void ImageUndistort::relay_camera_info( + const sensor_msgs::msg::CameraInfo::SharedPtr msg) { info_pub_->publish(*msg); } From eb2a14e9c14a307c2d5e80beb3231d069c913916 Mon Sep 17 00:00:00 2001 From: vortexuser Date: Thu, 2 Apr 2026 14:53:22 +0200 Subject: [PATCH 21/37] chore: remove deprecated python script from cmake --- perception_setup/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/perception_setup/CMakeLists.txt b/perception_setup/CMakeLists.txt index b22bb7c..824a750 100644 --- a/perception_setup/CMakeLists.txt +++ b/perception_setup/CMakeLists.txt @@ -54,7 +54,6 @@ install(DIRECTORY include/ # Python scripts (kept for reference) install(PROGRAMS scripts/camera_info_publisher.py - scripts/image_undistort.py scripts/image_crop.py DESTINATION lib/${PROJECT_NAME} ) From 9cbc1950b0928328600d7e581c93916b652ae4ef Mon Sep 17 00:00:00 2001 From: vortexuser Date: Thu, 2 Apr 2026 16:45:26 +0200 Subject: [PATCH 22/37] hello gamers --- .../launch/cameras/blackfly_s.launch.py | 2 +- .../launch/visual_inspection.launch.py | 27 +++++++++++++++++-- perception_setup/package.xml | 1 + scripts/set_MTU_nic.sh | 1 - 4 files changed, 27 insertions(+), 4 deletions(-) diff --git a/perception_setup/launch/cameras/blackfly_s.launch.py b/perception_setup/launch/cameras/blackfly_s.launch.py index 6803f36..a066f71 100644 --- a/perception_setup/launch/cameras/blackfly_s.launch.py +++ b/perception_setup/launch/cameras/blackfly_s.launch.py @@ -44,7 +44,7 @@ def generate_launch_description(): driver_params, { "parameter_file": spinnaker_map, - "serial_number": "", + "serial_number": '23494258', "camerainfo_url": f"file://{calib_path}", }, ], diff --git a/perception_setup/launch/visual_inspection.launch.py b/perception_setup/launch/visual_inspection.launch.py index c2aa775..79981c5 100644 --- a/perception_setup/launch/visual_inspection.launch.py +++ b/perception_setup/launch/visual_inspection.launch.py @@ -14,7 +14,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode FILTERED_IMAGE_TOPIC = '/visual_inspection/filtered_image' @@ -91,7 +91,7 @@ def generate_launch_description(): 'pub_topic': FILTERED_IMAGE_TOPIC, 'input_encoding': cam['encoding'], 'output_encoding': cam['encoding'], - 'filter_params.filter_type': 'no_filter', + 'filter_params.filter_type': 'remove_grid', }, ], ), @@ -121,6 +121,28 @@ def generate_launch_description(): 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': 5001, + 'bitrate': 500000, + 'framerate': 15, + 'preset_level': 1, + 'iframe_interval': 15, + 'control_rate': 1, + 'pt': 96, + 'config_interval': 1, + } + ], + output='screen', + ) + return LaunchDescription( [ DeclareLaunchArgument( @@ -129,5 +151,6 @@ def generate_launch_description(): description='Undistort color image before publishing to image_topic', ), visual_inspection_container, + image_to_gstreamer_node, ] ) diff --git a/perception_setup/package.xml b/perception_setup/package.xml index a06c2d4..fbc8a11 100644 --- a/perception_setup/package.xml +++ b/perception_setup/package.xml @@ -35,6 +35,7 @@ valve_detection image_filtering aruco_detector + image_to_gstreamer ament_cmake diff --git a/scripts/set_MTU_nic.sh b/scripts/set_MTU_nic.sh index cb99732..98a03da 100755 --- a/scripts/set_MTU_nic.sh +++ b/scripts/set_MTU_nic.sh @@ -1,6 +1,5 @@ sudo ip link set enP5p1s0f3 down sudo ip link set enP5p1s0f3 mtu 9000 sudo ip link set enP5p1s0f3 up -sudo nmcli connection modify "realsense_d555" 802-3-ethernet.mtu 9000 sudo systemctl restart NetworkManager ip link show enP5p1s0f3 From cf32ce57f13f1ec10f57ede47c0ebe500a22b936 Mon Sep 17 00:00:00 2001 From: vortexuser Date: Sat, 4 Apr 2026 09:33:28 +0200 Subject: [PATCH 23/37] temp files --- .../launch/cameras/blackfly_s.launch.py | 76 ++++++++++++++++++- .../launch/cameras/realsense_d555.launch.py | 69 +++++++++++++++++ scripts/set_fastdds_eno1.sh | 35 +++++++++ scripts/unset_fastdds.sh | 20 +++++ 4 files changed, 199 insertions(+), 1 deletion(-) create mode 100644 scripts/set_fastdds_eno1.sh create mode 100644 scripts/unset_fastdds.sh diff --git a/perception_setup/launch/cameras/blackfly_s.launch.py b/perception_setup/launch/cameras/blackfly_s.launch.py index a066f71..16f8248 100644 --- a/perception_setup/launch/cameras/blackfly_s.launch.py +++ b/perception_setup/launch/cameras/blackfly_s.launch.py @@ -11,7 +11,7 @@ from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode @@ -56,9 +56,83 @@ def generate_launch_description(): 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": LaunchConfiguration("gst_input_topic"), + "host": LaunchConfiguration("gst_host"), + "port": LaunchConfiguration("gst_port"), + "bitrate": LaunchConfiguration("gst_bitrate"), + "framerate": LaunchConfiguration("gst_framerate"), + "preset_level": LaunchConfiguration("gst_preset_level"), + "iframe_interval": LaunchConfiguration("gst_iframe_interval"), + "control_rate": LaunchConfiguration("gst_control_rate"), + "pt": LaunchConfiguration("gst_pt"), + "config_interval": LaunchConfiguration("gst_config_interval"), + "format": "BGR8", + } + ], + output="screen", + ) + return LaunchDescription( [ enable_camera_arg, + DeclareLaunchArgument( + "gst_input_topic", + default_value="/blackfly_s/image_raw", + description="Image topic to stream via GStreamer", + ), + DeclareLaunchArgument( + "gst_host", + default_value="10.0.0.154", + description="GStreamer stream destination host", + ), + DeclareLaunchArgument( + "gst_port", + default_value="5001", + description="GStreamer stream destination port", + ), + DeclareLaunchArgument( + "gst_bitrate", + default_value="500000", + description="GStreamer encoder bitrate (bps)", + ), + DeclareLaunchArgument( + "gst_framerate", + default_value="15", + description="GStreamer stream framerate", + ), + DeclareLaunchArgument( + "gst_preset_level", + default_value="1", + description="GStreamer encoder preset level", + ), + DeclareLaunchArgument( + "gst_iframe_interval", + default_value="15", + description="GStreamer I-frame interval", + ), + DeclareLaunchArgument( + "gst_control_rate", + default_value="1", + description="GStreamer control rate", + ), + DeclareLaunchArgument( + "gst_pt", + default_value="96", + description="GStreamer RTP payload type", + ), + DeclareLaunchArgument( + "gst_config_interval", + default_value="1", + description="GStreamer config interval", + ), 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 index 066a271..60a18a2 100644 --- a/perception_setup/launch/cameras/realsense_d555.launch.py +++ b/perception_setup/launch/cameras/realsense_d555.launch.py @@ -117,6 +117,29 @@ def generate_launch_description(): # 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": cam["image_topic"], + "host": LaunchConfiguration("gst_host"), + "port": LaunchConfiguration("gst_port"), + "bitrate": LaunchConfiguration("gst_bitrate"), + "framerate": LaunchConfiguration("gst_framerate"), + "preset_level": LaunchConfiguration("gst_preset_level"), + "iframe_interval": LaunchConfiguration("gst_iframe_interval"), + "control_rate": LaunchConfiguration("gst_control_rate"), + "pt": LaunchConfiguration("gst_pt"), + "config_interval": LaunchConfiguration("gst_config_interval"), + "format": "RGB", + } + ], + output="screen", + ) + return LaunchDescription( [ DeclareLaunchArgument( @@ -124,7 +147,53 @@ def generate_launch_description(): default_value="true", description="Undistort color image before publishing to image_topic", ), + DeclareLaunchArgument( + "gst_host", + default_value="10.0.0.154", + description="GStreamer stream destination host", + ), + DeclareLaunchArgument( + "gst_port", + default_value="5000", + description="GStreamer stream destination port", + ), + DeclareLaunchArgument( + "gst_bitrate", + default_value="500000", + description="GStreamer encoder bitrate (bps)", + ), + DeclareLaunchArgument( + "gst_framerate", + default_value="15", + description="GStreamer stream framerate", + ), + DeclareLaunchArgument( + "gst_preset_level", + default_value="1", + description="GStreamer encoder preset level", + ), + DeclareLaunchArgument( + "gst_iframe_interval", + default_value="15", + description="GStreamer I-frame interval", + ), + DeclareLaunchArgument( + "gst_control_rate", + default_value="1", + description="GStreamer control rate", + ), + DeclareLaunchArgument( + "gst_pt", + default_value="96", + description="GStreamer RTP payload type", + ), + DeclareLaunchArgument( + "gst_config_interval", + default_value="1", + description="GStreamer config interval", + ), realsense_node, image_undistort_container, + image_to_gstreamer_node, ] ) diff --git a/scripts/set_fastdds_eno1.sh b/scripts/set_fastdds_eno1.sh new file mode 100644 index 0000000..811779a --- /dev/null +++ b/scripts/set_fastdds_eno1.sh @@ -0,0 +1,35 @@ +#!/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 + +ROS_DOMAIN="0" +FASTDDS_IFACE="eno1" + +# Optional check only: make sure eno1 exists, but do not require `ip` +if [[ ! -e "/sys/class/net/${FASTDDS_IFACE}" ]]; then + echo "[Fast DDS] Warning: interface '${FASTDDS_IFACE}' not found. Skipping setup." + return 0 +fi + +unset FASTDDS_DEFAULT_PROFILES_FILE +export SKIP_DEFAULT_XML=1 +export FASTDDS_BUILTIN_TRANSPORTS=UDPv4 +export RMW_IMPLEMENTATION=rmw_fastrtps_cpp +export ROS_DOMAIN_ID="${ROS_DOMAIN}" +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] Enabled UDPv4-only Fast DDS, ROS_DOMAIN_ID=${ROS_DOMAIN}" +echo "[Fast DDS] FASTDDS_DEFAULT_PROFILES_FILE unset" +echo "[Fast DDS] SKIP_DEFAULT_XML=1" +echo "[Fast DDS] FASTDDS_BUILTIN_TRANSPORTS=UDPv4" +echo "[Fast DDS] RMW_IMPLEMENTATION=${RMW_IMPLEMENTATION}" diff --git a/scripts/unset_fastdds.sh b/scripts/unset_fastdds.sh new file mode 100644 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." From a37db638ac64dc24e5225eb690095c3d1d86d4bd Mon Sep 17 00:00:00 2001 From: vortexuser Date: Sat, 4 Apr 2026 22:20:51 +0200 Subject: [PATCH 24/37] hellow world --- perception_setup/config/yolo/yolo_obb.yaml | 4 +- .../launch/cameras/blackfly_s.launch.py | 4 +- .../launch/cameras/realsense_d555.launch.py | 4 +- .../launch/cameras/sonar.launch.py | 116 ++++++ .../launch/docking_blackfly_s.launch.py | 117 ++++++ .../launch/docking_realsense_d555.launch.py | 137 +++++++ perception_setup/launch/pipeline.launch.py | 383 ++++++++++++++++++ .../launch/valve_intervention.launch.py | 24 ++ .../launch/visual_inspection.launch.py | 5 +- scripts/fastdds_profile.xml | 32 ++ scripts/set_fastdds_eno1.sh | 56 +-- scripts/set_fastdds_fixed.sh | 0 scripts/tmux_nautilus.sh | 9 + 13 files changed, 855 insertions(+), 36 deletions(-) create mode 100644 perception_setup/launch/cameras/sonar.launch.py create mode 100644 perception_setup/launch/docking_blackfly_s.launch.py create mode 100644 perception_setup/launch/docking_realsense_d555.launch.py create mode 100644 perception_setup/launch/pipeline.launch.py create mode 100644 scripts/fastdds_profile.xml create mode 100755 scripts/set_fastdds_fixed.sh create mode 100755 scripts/tmux_nautilus.sh diff --git a/perception_setup/config/yolo/yolo_obb.yaml b/perception_setup/config/yolo/yolo_obb.yaml index e7ce647..f581d1f 100644 --- a/perception_setup/config/yolo/yolo_obb.yaml +++ b/perception_setup/config/yolo/yolo_obb.yaml @@ -3,8 +3,8 @@ visualized_image_topic: "/yolo_obb_processed_image" detection_topic: "/obb_detections_output" # Model and engine file paths (relative to perception_setup/models/) -model_file_path: "obb_best_simulator.onnx" -engine_file_path: "obb_best_simulator.engine" +model_file_path: "obb_best.onnx" +engine_file_path: "obb_best.engine" # Visualizer enable_visualizer: true diff --git a/perception_setup/launch/cameras/blackfly_s.launch.py b/perception_setup/launch/cameras/blackfly_s.launch.py index 16f8248..7cd6644 100644 --- a/perception_setup/launch/cameras/blackfly_s.launch.py +++ b/perception_setup/launch/cameras/blackfly_s.launch.py @@ -73,7 +73,7 @@ def generate_launch_description(): "control_rate": LaunchConfiguration("gst_control_rate"), "pt": LaunchConfiguration("gst_pt"), "config_interval": LaunchConfiguration("gst_config_interval"), - "format": "BGR8", + "format": "BGR", } ], output="screen", @@ -89,7 +89,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "gst_host", - default_value="10.0.0.154", + default_value="10.0.0.68", description="GStreamer stream destination host", ), DeclareLaunchArgument( diff --git a/perception_setup/launch/cameras/realsense_d555.launch.py b/perception_setup/launch/cameras/realsense_d555.launch.py index 60a18a2..d05f8c6 100644 --- a/perception_setup/launch/cameras/realsense_d555.launch.py +++ b/perception_setup/launch/cameras/realsense_d555.launch.py @@ -134,7 +134,7 @@ def generate_launch_description(): "control_rate": LaunchConfiguration("gst_control_rate"), "pt": LaunchConfiguration("gst_pt"), "config_interval": LaunchConfiguration("gst_config_interval"), - "format": "RGB", + "format": "BGR", } ], output="screen", @@ -149,7 +149,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "gst_host", - default_value="10.0.0.154", + default_value="10.0.0.68", description="GStreamer stream destination host", ), DeclareLaunchArgument( diff --git a/perception_setup/launch/cameras/sonar.launch.py b/perception_setup/launch/cameras/sonar.launch.py new file mode 100644 index 0000000..ec788e1 --- /dev/null +++ b/perception_setup/launch/cameras/sonar.launch.py @@ -0,0 +1,116 @@ +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": LaunchConfiguration("gst_host"), + "port": LaunchConfiguration("gst_port"), + "bitrate": LaunchConfiguration("gst_bitrate"), + "framerate": LaunchConfiguration("gst_framerate"), + "preset_level": LaunchConfiguration("gst_preset_level"), + "iframe_interval": LaunchConfiguration("gst_iframe_interval"), + "control_rate": LaunchConfiguration("gst_control_rate"), + "pt": LaunchConfiguration("gst_pt"), + "config_interval": LaunchConfiguration("gst_config_interval"), + "format": "GRAY8", + } + ], + output="screen", + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + 'namespace', + default_value='', + description='Namespace for the norbit_fls_ros_interface node', + ), + DeclareLaunchArgument( + "gst_host", + default_value="10.0.0.68", + description="GStreamer stream destination host", + ), + DeclareLaunchArgument( + "gst_port", + default_value="5002", + description="GStreamer stream destination port", + ), + DeclareLaunchArgument( + "gst_bitrate", + default_value="500000", + description="GStreamer encoder bitrate (bps)", + ), + DeclareLaunchArgument( + "gst_framerate", + default_value="15", + description="GStreamer stream framerate", + ), + DeclareLaunchArgument( + "gst_preset_level", + default_value="1", + description="GStreamer encoder preset level", + ), + DeclareLaunchArgument( + "gst_iframe_interval", + default_value="15", + description="GStreamer I-frame interval", + ), + DeclareLaunchArgument( + "gst_control_rate", + default_value="1", + description="GStreamer control rate", + ), + DeclareLaunchArgument( + "gst_pt", + default_value="96", + description="GStreamer RTP payload type", + ), + DeclareLaunchArgument( + "gst_config_interval", + default_value="1", + description="GStreamer config interval", + ), + 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..8cf35ba --- /dev/null +++ b/perception_setup/launch/docking_blackfly_s.launch.py @@ -0,0 +1,117 @@ +"""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 + +import yaml +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') + + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras['blackfly_s'] + + 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') + + 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')], + 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': cam['image_topic'], + 'subs.camera_info_topic': cam['camera_info_topic'], + 'pubs.aruco_image': '/down_cam/aruco_detector/image', + 'detect_board': True, + 'visualize': True, + 'log_markers': False, + 'publish_detections': True, + 'publish_landmarks': True, + }, + ], + ), + ], + 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( + '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..6dc1d51 --- /dev/null +++ b/perception_setup/launch/docking_realsense_d555.launch.py @@ -0,0 +1,137 @@ +"""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 + +import yaml +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') + + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras['realsense_d555'] + + calib_file = os.path.join( + pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml' + ) + + 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': cam['raw_color_image_topic'], + 'camera_info_file': calib_file, + 'raw_camera_info_topic': cam['raw_color_camera_info_topic'], + 'output_image_topic': cam['image_topic'], + 'output_camera_info_topic': cam['camera_info_topic'], + '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': cam['image_topic'], + 'subs.camera_info_topic': cam['camera_info_topic'], + 'pubs.aruco_image': '/forward_cam/aruco_detector/image', + 'detect_board': True, + 'visualize': True, + 'log_markers': False, + 'publish_detections': True, + 'publish_landmarks': True, + }, + ], + ), + ], + 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': '/forward_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( + 'enable_undistort', + default_value='true', + description='Undistort color image before ArUco detection', + ), + docking_container, + image_to_gstreamer_node, + ] + ) diff --git a/perception_setup/launch/pipeline.launch.py b/perception_setup/launch/pipeline.launch.py new file mode 100644 index 0000000..0c100e0 --- /dev/null +++ b/perception_setup/launch/pipeline.launch.py @@ -0,0 +1,383 @@ +"""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 + +# Segmentation pipeline internal topics +SEG_CONVERTED_IMAGE_TOPIC = '/yolo_seg/internal/converted_image' +SEG_TENSOR_OUTPUT_TOPIC = '/yolo_seg/tensor_pub' +SEG_TENSOR_INPUT_TOPIC = '/yolo_seg/tensor_sub' +SEG_ENCODER_NAMESPACE = 'yolo_seg_encoder/internal' + +# Classification pipeline internal topics +CLS_CONVERTED_IMAGE_TOPIC = '/yolo_cls/internal/converted_image' +CLS_TENSOR_OUTPUT_TOPIC = '/yolo_cls/tensor_pub' +CLS_TENSOR_INPUT_TOPIC = '/yolo_cls/tensor_sub' +CLS_ENCODER_NAMESPACE = 'yolo_cls_encoder/internal' + + +def _launch_setup(context, *args, **kwargs): + pkg_dir = get_package_share_directory('perception_setup') + + # Load camera config + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras['blackfly_s'] + + # Load model configs + 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) + + models_dir = os.path.join(pkg_dir, 'models') + + encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder') + + # ------------------------------------------------------------------------- + # 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='pipeline_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}', + }, + ], + remappings=[('~/control', '/exposure_control/control')], + extra_arguments=[{'use_intra_process_comms': True}], + condition=IfCondition(LaunchConfiguration('enable_camera')), + ), + ], + output='screen', + arguments=['--ros-args', '--log-level', 'info'], + ) + + # ------------------------------------------------------------------------- + # YOLO Segmentation + # Reads /blackfly_s/image_raw, outputs /yolo_seg_mask + /yolo_seg_processed_image + # ------------------------------------------------------------------------- + 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': seg_cfg['encoding_desired'], + 'image_width': int(cam['image_width']), + 'image_height': int(cam['image_height']), + 'input_qos': 'sensor_data', + } + ], + remappings=[ + ('image_raw', cam['image_topic']), + ('image', SEG_CONVERTED_IMAGE_TOPIC), + ], + ) + + 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_cfg['model_file_path']), + 'engine_file_path': os.path.join( + models_dir, seg_cfg['engine_file_path'] + ), + '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': bool(seg_cfg['verbose']), + 'force_engine_update': bool(seg_cfg['force_engine_update']), + 'tensor_output_topic': SEG_TENSOR_OUTPUT_TOPIC, + } + ], + remappings=[ + ('tensor_pub', SEG_TENSOR_OUTPUT_TOPIC), + ('tensor_sub', SEG_TENSOR_INPUT_TOPIC), + ], + ) + + seg_decoder_node = ComposableNode( + name='seg_decoder_node', + package='isaac_ros_yolov26_seg', + plugin='nvidia::isaac_ros::yolov26_seg::YoloV26SegDecoderNode', + parameters=[ + { + 'tensor_input_topic': SEG_TENSOR_INPUT_TOPIC, + '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': int(seg_cfg['output_mask_width']), + 'output_mask_height': int(seg_cfg['output_mask_height']), + 'detections_topic': seg_cfg['detection_topic'], + 'mask_topic': seg_cfg['mask_topic'], + } + ], + ) + + seg_container = ComposableNodeContainer( + name='seg_tensor_rt_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(cam['image_width']), + 'input_image_height': str(cam['image_height']), + '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': 'seg_tensor_rt_container', + 'dnn_image_encoder_namespace': SEG_ENCODER_NAMESPACE, + 'image_input_topic': SEG_CONVERTED_IMAGE_TOPIC, + 'camera_info_input_topic': cam['camera_info_topic'], + 'tensor_output_topic': SEG_TENSOR_OUTPUT_TOPIC, + }.items(), + ) + + seg_visualizer = ( + Node( + package='isaac_ros_yolov26_seg', + executable='isaac_ros_yolov26_seg_visualizer.py', + name='seg_visualizer', + parameters=[ + { + 'detections_topic': seg_cfg['detection_topic'], + 'image_topic': f'/{SEG_ENCODER_NAMESPACE}/resize/image', + 'mask_topic': seg_cfg['mask_topic'], + 'output_image_topic': seg_cfg['visualized_image_topic'], + 'class_names_yaml': str(seg_cfg['class_names']), + } + ], + ) + if bool(seg_cfg['enable_visualizer']) + else None + ) + + # ------------------------------------------------------------------------- + # YOLO Classification + # Reads /yolo_seg_mask (mask_topic from seg), outputs /classification_output + # ------------------------------------------------------------------------- + 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': cls_cfg['encoding_desired'], + 'image_width': int(cls_cfg['input_image_width']), + 'image_height': int(cls_cfg['input_image_height']), + 'input_qos': 'sensor_data', + } + ], + remappings=[ + ('image_raw', cls_cfg['image_input_topic']), + ('image', CLS_CONVERTED_IMAGE_TOPIC), + ], + ) + + 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, cls_cfg['model_file_path']), + 'engine_file_path': os.path.join( + models_dir, cls_cfg['engine_file_path'] + ), + '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': bool(cls_cfg['verbose']), + 'force_engine_update': bool(cls_cfg['force_engine_update']), + 'tensor_output_topic': CLS_TENSOR_OUTPUT_TOPIC, + } + ], + remappings=[ + ('tensor_pub', CLS_TENSOR_OUTPUT_TOPIC), + ('tensor_sub', CLS_TENSOR_INPUT_TOPIC), + ], + ) + + cls_decoder_node = ComposableNode( + name='cls_decoder_node', + package='isaac_ros_yolov26_cls', + plugin='nvidia::isaac_ros::yolov26_cls::YoloV26ClsDecoderNode', + parameters=[ + { + 'tensor_input_topic': CLS_TENSOR_INPUT_TOPIC, + 'confidence_threshold': float(cls_cfg['confidence_threshold']), + 'num_classes': int(cls_cfg['num_classes']), + 'class_topic': cls_cfg['class_topic'], + } + ], + ) + + cls_container = ComposableNodeContainer( + name='cls_tensor_rt_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(cls_cfg['input_image_width']), + 'input_image_height': str(cls_cfg['input_image_height']), + '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': 'cls_tensor_rt_container', + 'dnn_image_encoder_namespace': CLS_ENCODER_NAMESPACE, + 'image_input_topic': CLS_CONVERTED_IMAGE_TOPIC, + 'camera_info_input_topic': cls_cfg['camera_info_input_topic'], + 'tensor_output_topic': CLS_TENSOR_OUTPUT_TOPIC, + }.items(), + ) + + cls_visualizer = ( + Node( + package='isaac_ros_yolov26_cls', + executable='isaac_ros_yolov26_cls_visualizer.py', + name='cls_visualizer', + parameters=[ + { + 'class_topic': cls_cfg['class_topic'], + 'image_topic': f'/{CLS_ENCODER_NAMESPACE}/resize/image', + 'output_image_topic': cls_cfg['visualized_image_topic'], + 'class_names_yaml': str(cls_cfg['class_names']), + } + ], + ) + if bool(cls_cfg['enable_visualizer']) + else None + ) + + # ------------------------------------------------------------------------- + # 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': seg_cfg['visualized_image_topic'], + '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', + ) + + actions = [ + camera_container, + seg_container, + seg_encoder_launch, + cls_container, + cls_encoder_launch, + image_to_gstreamer_node, + ] + + if seg_visualizer: + actions.append(seg_visualizer) + if cls_visualizer: + actions.append(cls_visualizer) + + return actions + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + 'enable_camera', + default_value='true', + description='Enable FLIR Blackfly S camera component', + ), + OpaqueFunction(function=_launch_setup), + ] + ) diff --git a/perception_setup/launch/valve_intervention.launch.py b/perception_setup/launch/valve_intervention.launch.py index 740f178..151484a 100644 --- a/perception_setup/launch/valve_intervention.launch.py +++ b/perception_setup/launch/valve_intervention.launch.py @@ -258,11 +258,35 @@ def _launch_setup(context, *args, **kwargs): 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_cfg['visualized_image_topic'], + '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', + ) + # Collect all actions actions = [ tensor_rt_container, dnn_image_encoder_launch, valve_detection_container, + image_to_gstreamer_node, ] if yolo_obb_visualizer: diff --git a/perception_setup/launch/visual_inspection.launch.py b/perception_setup/launch/visual_inspection.launch.py index 79981c5..8d12dfc 100644 --- a/perception_setup/launch/visual_inspection.launch.py +++ b/perception_setup/launch/visual_inspection.launch.py @@ -129,8 +129,8 @@ def generate_launch_description(): parameters=[ { 'input_topic': '/aruco_detector/image', - 'host': '10.0.0.154', - 'port': 5001, + 'host': '10.0.0.68', + 'port': 5000, 'bitrate': 500000, 'framerate': 15, 'preset_level': 1, @@ -138,6 +138,7 @@ def generate_launch_description(): 'control_rate': 1, 'pt': 96, 'config_interval': 1, + 'format': 'RGB', } ], output='screen', diff --git a/scripts/fastdds_profile.xml b/scripts/fastdds_profile.xml new file mode 100644 index 0000000..964dc01 --- /dev/null +++ b/scripts/fastdds_profile.xml @@ -0,0 +1,32 @@ +cat > /home/vortex/workspaces/isaac_ros-dev/src/perception-auv/scripts/fastdds_profile.xml << 'EOF' + + + + + + udp_eno1 + UDPv4 + +
eno1
+ + + + udp_realsense + UDPv4 + +
enP5p1s0f3
+ + + + + + + udp_eno1 + udp_realsense + + false + + + + +EOF diff --git a/scripts/set_fastdds_eno1.sh b/scripts/set_fastdds_eno1.sh index 811779a..1271186 100644 --- a/scripts/set_fastdds_eno1.sh +++ b/scripts/set_fastdds_eno1.sh @@ -1,35 +1,35 @@ -#!/usr/bin/env bash + #!/usr/bin/env bash -# Source this file, do not execute it. + # 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 + if [[ "${BASH_SOURCE[0]}" == "${0}" ]]; then + echo "[Fast DDS] Please source this script:" + echo " source ${0}" + exit 1 + fi -ROS_DOMAIN="0" -FASTDDS_IFACE="eno1" + ROS_DOMAIN="0" + FASTDDS_IFACE="eno1" -# Optional check only: make sure eno1 exists, but do not require `ip` -if [[ ! -e "/sys/class/net/${FASTDDS_IFACE}" ]]; then - echo "[Fast DDS] Warning: interface '${FASTDDS_IFACE}' not found. Skipping setup." - return 0 -fi + # Optional check only: make sure eno1 exists, but do not require `ip` + if [[ ! -e "/sys/class/net/${FASTDDS_IFACE}" ]]; then + echo "[Fast DDS] Warning: interface '${FASTDDS_IFACE}' not found. Skipping setup." + return 0 + fi -unset FASTDDS_DEFAULT_PROFILES_FILE -export SKIP_DEFAULT_XML=1 -export FASTDDS_BUILTIN_TRANSPORTS=UDPv4 -export RMW_IMPLEMENTATION=rmw_fastrtps_cpp -export ROS_DOMAIN_ID="${ROS_DOMAIN}" -unset ROS_LOCALHOST_ONLY + unset FASTDDS_DEFAULT_PROFILES_FILE + export SKIP_DEFAULT_XML=1 + export FASTDDS_BUILTIN_TRANSPORTS=UDPv4 + export RMW_IMPLEMENTATION=rmw_fastrtps_cpp + export ROS_DOMAIN_ID="${ROS_DOMAIN}" + unset ROS_LOCALHOST_ONLY -if command -v ros2 >/dev/null 2>&1; then - ros2 daemon stop >/dev/null 2>&1 || true -fi + if command -v ros2 >/dev/null 2>&1; then + ros2 daemon stop >/dev/null 2>&1 || true + fi -echo "[Fast DDS] Enabled UDPv4-only Fast DDS, ROS_DOMAIN_ID=${ROS_DOMAIN}" -echo "[Fast DDS] FASTDDS_DEFAULT_PROFILES_FILE unset" -echo "[Fast DDS] SKIP_DEFAULT_XML=1" -echo "[Fast DDS] FASTDDS_BUILTIN_TRANSPORTS=UDPv4" -echo "[Fast DDS] RMW_IMPLEMENTATION=${RMW_IMPLEMENTATION}" + echo "[Fast DDS] Enabled UDPv4-only Fast DDS, ROS_DOMAIN_ID=${ROS_DOMAIN}" + echo "[Fast DDS] FASTDDS_DEFAULT_PROFILES_FILE unset" + echo "[Fast DDS] SKIP_DEFAULT_XML=1" + echo "[Fast DDS] FASTDDS_BUILTIN_TRANSPORTS=UDPv4" + echo "[Fast DDS] RMW_IMPLEMENTATION=${RMW_IMPLEMENTATION}" diff --git a/scripts/set_fastdds_fixed.sh b/scripts/set_fastdds_fixed.sh new file mode 100755 index 0000000..e69de29 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" From 5059cbbfa13e215ec365126d2c94e049d4c3b918 Mon Sep 17 00:00:00 2001 From: vortexuser Date: Sun, 5 Apr 2026 03:17:30 +0200 Subject: [PATCH 25/37] feat: add remaning launch files --- perception_setup/config/yolo/yolo_seg.yaml | 4 +- .../launch/cameras/blackfly_s.launch.py | 2 +- .../launch/cameras/realsense_d555.launch.py | 4 +- .../launch/docking_realsense_d555.launch.py | 2 +- ...aunch.py => pipeline_blackfly_s.launch.py} | 67 ++-- .../launch/pipeline_realsense_d555.launch.py | 285 ++++++++++++++++++ scripts/unset_fastdds.sh | 0 7 files changed, 330 insertions(+), 34 deletions(-) rename perception_setup/launch/{pipeline.launch.py => pipeline_blackfly_s.launch.py} (86%) create mode 100644 perception_setup/launch/pipeline_realsense_d555.launch.py mode change 100644 => 100755 scripts/unset_fastdds.sh diff --git a/perception_setup/config/yolo/yolo_seg.yaml b/perception_setup/config/yolo/yolo_seg.yaml index dcb7492..38b4b5b 100644 --- a/perception_setup/config/yolo/yolo_seg.yaml +++ b/perception_setup/config/yolo/yolo_seg.yaml @@ -4,8 +4,8 @@ detection_topic: "/seg_detections_output" mask_topic: "/yolo_seg_mask" # Model and engine file paths (relative to perception_setup/models/) -model_file_path: "down_cam_seg.onnx" -engine_file_path: "down_cam_seg.engine" +model_file_path: "seg_pipe_real.onnx" +engine_file_path: "seg_pipe_real.engine" # Visualizer enable_visualizer: true diff --git a/perception_setup/launch/cameras/blackfly_s.launch.py b/perception_setup/launch/cameras/blackfly_s.launch.py index 7cd6644..825ed81 100644 --- a/perception_setup/launch/cameras/blackfly_s.launch.py +++ b/perception_setup/launch/cameras/blackfly_s.launch.py @@ -73,7 +73,7 @@ def generate_launch_description(): "control_rate": LaunchConfiguration("gst_control_rate"), "pt": LaunchConfiguration("gst_pt"), "config_interval": LaunchConfiguration("gst_config_interval"), - "format": "BGR", + "format": "RGB", } ], output="screen", diff --git a/perception_setup/launch/cameras/realsense_d555.launch.py b/perception_setup/launch/cameras/realsense_d555.launch.py index d05f8c6..ed5c917 100644 --- a/perception_setup/launch/cameras/realsense_d555.launch.py +++ b/perception_setup/launch/cameras/realsense_d555.launch.py @@ -39,7 +39,7 @@ def generate_launch_description(): { "enable_color": True, "rgb_camera.color_profile": "896,504,15", # When updating the image size, make sure to also update the calibration file - "rgb_camera.color_format": "RGB8", + "rgb_camera.color_format": "BGR8", "rgb_camera.enable_auto_exposure": True, "enable_depth": True, "depth_module.depth_profile": "896,504,15", @@ -134,7 +134,7 @@ def generate_launch_description(): "control_rate": LaunchConfiguration("gst_control_rate"), "pt": LaunchConfiguration("gst_pt"), "config_interval": LaunchConfiguration("gst_config_interval"), - "format": "BGR", + "format": "RGB", } ], output="screen", diff --git a/perception_setup/launch/docking_realsense_d555.launch.py b/perception_setup/launch/docking_realsense_d555.launch.py index 6dc1d51..52d5719 100644 --- a/perception_setup/launch/docking_realsense_d555.launch.py +++ b/perception_setup/launch/docking_realsense_d555.launch.py @@ -118,7 +118,7 @@ def generate_launch_description(): 'control_rate': 1, 'pt': 96, 'config_interval': 1, - 'format': 'BGR', + 'format': 'RGB', } ], output='screen', diff --git a/perception_setup/launch/pipeline.launch.py b/perception_setup/launch/pipeline_blackfly_s.launch.py similarity index 86% rename from perception_setup/launch/pipeline.launch.py rename to perception_setup/launch/pipeline_blackfly_s.launch.py index 0c100e0..e74d7d5 100644 --- a/perception_setup/launch/pipeline.launch.py +++ b/perception_setup/launch/pipeline_blackfly_s.launch.py @@ -24,17 +24,21 @@ from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode +# All pipeline-internal and output topics are prefixed to avoid collisions +# when running alongside other camera pipelines. +CAM_PREFIX = '/down_cam' + # Segmentation pipeline internal topics -SEG_CONVERTED_IMAGE_TOPIC = '/yolo_seg/internal/converted_image' -SEG_TENSOR_OUTPUT_TOPIC = '/yolo_seg/tensor_pub' -SEG_TENSOR_INPUT_TOPIC = '/yolo_seg/tensor_sub' -SEG_ENCODER_NAMESPACE = 'yolo_seg_encoder/internal' +SEG_CONVERTED_IMAGE_TOPIC = f'{CAM_PREFIX}/yolo_seg/internal/converted_image' +SEG_TENSOR_OUTPUT_TOPIC = f'{CAM_PREFIX}/yolo_seg/tensor_pub' +SEG_TENSOR_INPUT_TOPIC = f'{CAM_PREFIX}/yolo_seg/tensor_sub' +SEG_ENCODER_NAMESPACE = 'down_cam/yolo_seg_encoder/internal' # Classification pipeline internal topics -CLS_CONVERTED_IMAGE_TOPIC = '/yolo_cls/internal/converted_image' -CLS_TENSOR_OUTPUT_TOPIC = '/yolo_cls/tensor_pub' -CLS_TENSOR_INPUT_TOPIC = '/yolo_cls/tensor_sub' -CLS_ENCODER_NAMESPACE = 'yolo_cls_encoder/internal' +CLS_CONVERTED_IMAGE_TOPIC = f'{CAM_PREFIX}/yolo_cls/internal/converted_image' +CLS_TENSOR_OUTPUT_TOPIC = f'{CAM_PREFIX}/yolo_cls/tensor_pub' +CLS_TENSOR_INPUT_TOPIC = f'{CAM_PREFIX}/yolo_cls/tensor_sub' +CLS_ENCODER_NAMESPACE = 'down_cam/yolo_cls_encoder/internal' def _launch_setup(context, *args, **kwargs): @@ -56,6 +60,14 @@ def _launch_setup(context, *args, **kwargs): encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder') + # Prefix yaml-sourced topics + seg_detection_topic = f'{CAM_PREFIX}{seg_cfg["detection_topic"]}' + seg_mask_topic = f'{CAM_PREFIX}{seg_cfg["mask_topic"]}' + seg_visualized_topic = f'{CAM_PREFIX}{seg_cfg["visualized_image_topic"]}' + cls_image_input_topic = f'{CAM_PREFIX}{cls_cfg["image_input_topic"]}' + cls_class_topic = f'{CAM_PREFIX}{cls_cfg["class_topic"]}' + cls_visualized_topic = f'{CAM_PREFIX}{cls_cfg["visualized_image_topic"]}' + # ------------------------------------------------------------------------- # Camera # ------------------------------------------------------------------------- @@ -66,7 +78,7 @@ def _launch_setup(context, *args, **kwargs): calib_path = os.path.join(pkg_dir, 'config', 'cameras', 'blackfly_s_calib.yaml') camera_container = ComposableNodeContainer( - name='pipeline_camera_container', + name='down_cam_camera_container', namespace='', package='rclcpp_components', executable='component_container_mt', @@ -81,6 +93,7 @@ def _launch_setup(context, *args, **kwargs): 'parameter_file': spinnaker_map, 'serial_number': '23494258', 'camerainfo_url': f'file://{calib_path}', + 'reliable_qos': True, }, ], remappings=[('~/control', '/exposure_control/control')], @@ -94,7 +107,6 @@ def _launch_setup(context, *args, **kwargs): # ------------------------------------------------------------------------- # YOLO Segmentation - # Reads /blackfly_s/image_raw, outputs /yolo_seg_mask + /yolo_seg_processed_image # ------------------------------------------------------------------------- seg_image_format_converter = ComposableNode( package='isaac_ros_image_proc', @@ -155,14 +167,14 @@ def _launch_setup(context, *args, **kwargs): 'network_image_height': int(seg_cfg['network_image_height']), 'output_mask_width': int(seg_cfg['output_mask_width']), 'output_mask_height': int(seg_cfg['output_mask_height']), - 'detections_topic': seg_cfg['detection_topic'], - 'mask_topic': seg_cfg['mask_topic'], + 'detections_topic': seg_detection_topic, + 'mask_topic': seg_mask_topic, } ], ) seg_container = ComposableNodeContainer( - name='seg_tensor_rt_container', + name='down_cam_seg_container', namespace='', package='rclcpp_components', executable='component_container_mt', @@ -187,7 +199,7 @@ def _launch_setup(context, *args, **kwargs): 'image_mean': str(seg_cfg['image_mean']), 'image_stddev': str(seg_cfg['image_stddev']), 'attach_to_shared_component_container': 'True', - 'component_container_name': 'seg_tensor_rt_container', + 'component_container_name': 'down_cam_seg_container', 'dnn_image_encoder_namespace': SEG_ENCODER_NAMESPACE, 'image_input_topic': SEG_CONVERTED_IMAGE_TOPIC, 'camera_info_input_topic': cam['camera_info_topic'], @@ -202,10 +214,10 @@ def _launch_setup(context, *args, **kwargs): name='seg_visualizer', parameters=[ { - 'detections_topic': seg_cfg['detection_topic'], + 'detections_topic': seg_detection_topic, 'image_topic': f'/{SEG_ENCODER_NAMESPACE}/resize/image', - 'mask_topic': seg_cfg['mask_topic'], - 'output_image_topic': seg_cfg['visualized_image_topic'], + 'mask_topic': seg_mask_topic, + 'output_image_topic': seg_visualized_topic, 'class_names_yaml': str(seg_cfg['class_names']), } ], @@ -216,7 +228,6 @@ def _launch_setup(context, *args, **kwargs): # ------------------------------------------------------------------------- # YOLO Classification - # Reads /yolo_seg_mask (mask_topic from seg), outputs /classification_output # ------------------------------------------------------------------------- cls_image_format_converter = ComposableNode( package='isaac_ros_image_proc', @@ -231,7 +242,7 @@ def _launch_setup(context, *args, **kwargs): } ], remappings=[ - ('image_raw', cls_cfg['image_input_topic']), + ('image_raw', cls_image_input_topic), ('image', CLS_CONVERTED_IMAGE_TOPIC), ], ) @@ -270,13 +281,13 @@ def _launch_setup(context, *args, **kwargs): 'tensor_input_topic': CLS_TENSOR_INPUT_TOPIC, 'confidence_threshold': float(cls_cfg['confidence_threshold']), 'num_classes': int(cls_cfg['num_classes']), - 'class_topic': cls_cfg['class_topic'], + 'class_topic': cls_class_topic, } ], ) cls_container = ComposableNodeContainer( - name='cls_tensor_rt_container', + name='down_cam_cls_container', namespace='', package='rclcpp_components', executable='component_container_mt', @@ -301,7 +312,7 @@ def _launch_setup(context, *args, **kwargs): 'image_mean': str(cls_cfg['image_mean']), 'image_stddev': str(cls_cfg['image_stddev']), 'attach_to_shared_component_container': 'True', - 'component_container_name': 'cls_tensor_rt_container', + 'component_container_name': 'down_cam_cls_container', 'dnn_image_encoder_namespace': CLS_ENCODER_NAMESPACE, 'image_input_topic': CLS_CONVERTED_IMAGE_TOPIC, 'camera_info_input_topic': cls_cfg['camera_info_input_topic'], @@ -316,9 +327,9 @@ def _launch_setup(context, *args, **kwargs): name='cls_visualizer', parameters=[ { - 'class_topic': cls_cfg['class_topic'], + 'class_topic': cls_class_topic, 'image_topic': f'/{CLS_ENCODER_NAMESPACE}/resize/image', - 'output_image_topic': cls_cfg['visualized_image_topic'], + 'output_image_topic': cls_visualized_topic, 'class_names_yaml': str(cls_cfg['class_names']), } ], @@ -328,7 +339,7 @@ def _launch_setup(context, *args, **kwargs): ) # ------------------------------------------------------------------------- - # GStreamer — streams the segmentation visualization (rgb8) + # GStreamer — streams the segmentation visualization (bgr8) # ------------------------------------------------------------------------- image_to_gstreamer_node = Node( package='image_to_gstreamer', @@ -337,8 +348,8 @@ def _launch_setup(context, *args, **kwargs): additional_env={'EGL_PLATFORM': 'surfaceless'}, parameters=[ { - 'input_topic': seg_cfg['visualized_image_topic'], - 'host': '10.0.0.169', + 'input_topic': seg_visualized_topic, + 'host': '10.0.0.68', 'port': 5001, 'bitrate': 500000, 'framerate': 15, @@ -347,7 +358,7 @@ def _launch_setup(context, *args, **kwargs): 'control_rate': 1, 'pt': 96, 'config_interval': 1, - 'format': 'RGB', + 'format': 'BGR', } ], output='screen', 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..7a865da --- /dev/null +++ b/perception_setup/launch/pipeline_realsense_d555.launch.py @@ -0,0 +1,285 @@ +"""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 + +# All pipeline-internal and output topics are prefixed to avoid collisions +# when running alongside other camera pipelines. +CAM_PREFIX = '/front_cam' + +# Segmentation pipeline internal topics +SEG_CONVERTED_IMAGE_TOPIC = f'{CAM_PREFIX}/yolo_seg/internal/converted_image' +SEG_TENSOR_OUTPUT_TOPIC = f'{CAM_PREFIX}/yolo_seg/tensor_pub' +SEG_TENSOR_INPUT_TOPIC = f'{CAM_PREFIX}/yolo_seg/tensor_sub' +SEG_ENCODER_NAMESPACE = 'front_cam/yolo_seg_encoder/internal' + + +def _launch_setup(context, *args, **kwargs): + pkg_dir = get_package_share_directory('perception_setup') + + # Load camera config + cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') + with open(cameras_path) as f: + cameras = yaml.safe_load(f) + cam = cameras['realsense_d555'] + + # Load segmentation model config + with open(os.path.join(pkg_dir, 'config', 'yolo', 'yolo_seg.yaml')) as f: + seg_cfg = yaml.safe_load(f) + + models_dir = os.path.join(pkg_dir, 'models') + + encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder') + + calib_file = os.path.join( + pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml' + ) + + # Prefix yaml-sourced topics + seg_detection_topic = f'{CAM_PREFIX}{seg_cfg["detection_topic"]}' + seg_mask_topic = f'{CAM_PREFIX}{seg_cfg["mask_topic"]}' + seg_visualized_topic = f'{CAM_PREFIX}{seg_cfg["visualized_image_topic"]}' + + # ------------------------------------------------------------------------- + # 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': cam['raw_color_image_topic'], + 'camera_info_file': calib_file, + 'raw_camera_info_topic': cam['raw_color_camera_info_topic'], + 'output_image_topic': cam['image_topic'], + 'output_camera_info_topic': cam['camera_info_topic'], + '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': seg_cfg['encoding_desired'], + 'image_width': int(cam['image_width']), + 'image_height': int(cam['image_height']), + 'input_qos': 'sensor_data', + } + ], + remappings=[ + ('image_raw', cam['image_topic']), + ('image', SEG_CONVERTED_IMAGE_TOPIC), + ], + ) + + 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_cfg['model_file_path']), + 'engine_file_path': os.path.join( + models_dir, seg_cfg['engine_file_path'] + ), + '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': bool(seg_cfg['verbose']), + 'force_engine_update': bool(seg_cfg['force_engine_update']), + 'tensor_output_topic': SEG_TENSOR_OUTPUT_TOPIC, + } + ], + remappings=[ + ('tensor_pub', SEG_TENSOR_OUTPUT_TOPIC), + ('tensor_sub', SEG_TENSOR_INPUT_TOPIC), + ], + ) + + seg_decoder_node = ComposableNode( + name='seg_decoder_node', + package='isaac_ros_yolov26_seg', + plugin='nvidia::isaac_ros::yolov26_seg::YoloV26SegDecoderNode', + parameters=[ + { + 'tensor_input_topic': SEG_TENSOR_INPUT_TOPIC, + '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': int(seg_cfg['output_mask_width']), + 'output_mask_height': int(seg_cfg['output_mask_height']), + 'detections_topic': seg_detection_topic, + 'mask_topic': seg_mask_topic, + } + ], + ) + + 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(cam['image_width']), + 'input_image_height': str(cam['image_height']), + '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': SEG_ENCODER_NAMESPACE, + 'image_input_topic': SEG_CONVERTED_IMAGE_TOPIC, + 'camera_info_input_topic': cam['camera_info_topic'], + 'tensor_output_topic': SEG_TENSOR_OUTPUT_TOPIC, + }.items(), + ) + + seg_visualizer = ( + Node( + package='isaac_ros_yolov26_seg', + executable='isaac_ros_yolov26_seg_visualizer.py', + name='seg_visualizer', + parameters=[ + { + 'detections_topic': seg_detection_topic, + 'image_topic': f'/{SEG_ENCODER_NAMESPACE}/resize/image', + 'mask_topic': seg_mask_topic, + 'output_image_topic': seg_visualized_topic, + 'class_names_yaml': str(seg_cfg['class_names']), + } + ], + ) + if bool(seg_cfg['enable_visualizer']) + else None + ) + + # ------------------------------------------------------------------------- + # 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': seg_visualized_topic, + '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, + ] + + if seg_visualizer: + actions.append(seg_visualizer) + + return actions + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + 'enable_undistort', + default_value='true', + description='Undistort color image before segmentation', + ), + OpaqueFunction(function=_launch_setup), + ] + ) diff --git a/scripts/unset_fastdds.sh b/scripts/unset_fastdds.sh old mode 100644 new mode 100755 From 11abcfdde850b81a81674b25fefe5cc34d328497 Mon Sep 17 00:00:00 2001 From: vortexuser Date: Sun, 5 Apr 2026 10:58:36 +0200 Subject: [PATCH 26/37] dds config --- config/fastdds_multi_interface.xml | 27 ++++++++++++++++ scripts/fastdds_profile.xml | 10 ------ scripts/set_fastdds_eno1.sh | 51 ++++++++++++++---------------- scripts/set_fastdds_fixed.sh | 0 4 files changed, 50 insertions(+), 38 deletions(-) create mode 100644 config/fastdds_multi_interface.xml delete mode 100755 scripts/set_fastdds_fixed.sh 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/scripts/fastdds_profile.xml b/scripts/fastdds_profile.xml index 964dc01..11b32e0 100644 --- a/scripts/fastdds_profile.xml +++ b/scripts/fastdds_profile.xml @@ -1,4 +1,3 @@ -cat > /home/vortex/workspaces/isaac_ros-dev/src/perception-auv/scripts/fastdds_profile.xml << 'EOF' @@ -10,23 +9,14 @@ cat > /home/vortex/workspaces/isaac_ros-dev/src/perception-auv/scripts/fastdds_p
eno1
- - udp_realsense - UDPv4 - -
enP5p1s0f3
- - udp_eno1 - udp_realsense false -EOF diff --git a/scripts/set_fastdds_eno1.sh b/scripts/set_fastdds_eno1.sh index 1271186..04d77b1 100644 --- a/scripts/set_fastdds_eno1.sh +++ b/scripts/set_fastdds_eno1.sh @@ -1,35 +1,30 @@ - #!/usr/bin/env bash +#!/usr/bin/env bash - # Source this file, do not execute it. +# 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 +if [[ "${BASH_SOURCE[0]}" == "${0}" ]]; then + echo "[Fast DDS] Please source this script:" + echo " source ${0}" + exit 1 +fi - ROS_DOMAIN="0" - FASTDDS_IFACE="eno1" +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +PROFILE_FILE="${SCRIPT_DIR}/../config/fastdds_multi_interface.xml" - # Optional check only: make sure eno1 exists, but do not require `ip` - if [[ ! -e "/sys/class/net/${FASTDDS_IFACE}" ]]; then - echo "[Fast DDS] Warning: interface '${FASTDDS_IFACE}' not found. Skipping setup." - return 0 - fi +if [[ ! -f "${PROFILE_FILE}" ]]; then + echo "[Fast DDS] Error: profile not found at ${PROFILE_FILE}" + return 1 +fi - unset FASTDDS_DEFAULT_PROFILES_FILE - export SKIP_DEFAULT_XML=1 - export FASTDDS_BUILTIN_TRANSPORTS=UDPv4 - export RMW_IMPLEMENTATION=rmw_fastrtps_cpp - export ROS_DOMAIN_ID="${ROS_DOMAIN}" - unset ROS_LOCALHOST_ONLY +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 +if command -v ros2 >/dev/null 2>&1; then + ros2 daemon stop >/dev/null 2>&1 || true +fi - echo "[Fast DDS] Enabled UDPv4-only Fast DDS, ROS_DOMAIN_ID=${ROS_DOMAIN}" - echo "[Fast DDS] FASTDDS_DEFAULT_PROFILES_FILE unset" - echo "[Fast DDS] SKIP_DEFAULT_XML=1" - echo "[Fast DDS] FASTDDS_BUILTIN_TRANSPORTS=UDPv4" - echo "[Fast DDS] RMW_IMPLEMENTATION=${RMW_IMPLEMENTATION}" +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/set_fastdds_fixed.sh b/scripts/set_fastdds_fixed.sh deleted file mode 100755 index e69de29..0000000 From e8d2eb23f07a87271718cb7ed7d9dd6bd780f89f Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 5 Apr 2026 14:40:40 +0200 Subject: [PATCH 27/37] add explicit tf2 aruco frame for docking launch --- perception_setup/launch/docking_blackfly_s.launch.py | 1 + perception_setup/launch/docking_realsense_d555.launch.py | 1 + 2 files changed, 2 insertions(+) diff --git a/perception_setup/launch/docking_blackfly_s.launch.py b/perception_setup/launch/docking_blackfly_s.launch.py index 8cf35ba..d9726c1 100644 --- a/perception_setup/launch/docking_blackfly_s.launch.py +++ b/perception_setup/launch/docking_blackfly_s.launch.py @@ -68,6 +68,7 @@ def generate_launch_description(): 'subs.image_topic': cam['image_topic'], 'subs.camera_info_topic': cam['camera_info_topic'], 'pubs.aruco_image': '/down_cam/aruco_detector/image', + 'out_tf_frame': 'nautilus/downwards_camera_optical', 'detect_board': True, 'visualize': True, 'log_markers': False, diff --git a/perception_setup/launch/docking_realsense_d555.launch.py b/perception_setup/launch/docking_realsense_d555.launch.py index 52d5719..4fb8f2e 100644 --- a/perception_setup/launch/docking_realsense_d555.launch.py +++ b/perception_setup/launch/docking_realsense_d555.launch.py @@ -88,6 +88,7 @@ def generate_launch_description(): 'subs.image_topic': cam['image_topic'], 'subs.camera_info_topic': cam['camera_info_topic'], 'pubs.aruco_image': '/forward_cam/aruco_detector/image', + 'out_tf_frame': 'nautilus/front_camera_optical', 'detect_board': True, 'visualize': True, 'log_markers': False, From b9b2bb17aa72f6865e87305fb50de5d754886f61 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gard=20Eltvik=20Gr=C3=B8nner=C3=B8d?= Date: Wed, 8 Apr 2026 19:28:37 +0200 Subject: [PATCH 28/37] seg_yolo_line.launch added --- .../launch/seg_yolo_line.launch.py | 75 +++++++++++++++++++ 1 file changed, 75 insertions(+) create mode 100644 perception_setup/launch/seg_yolo_line.launch.py diff --git a/perception_setup/launch/seg_yolo_line.launch.py b/perception_setup/launch/seg_yolo_line.launch.py new file mode 100644 index 0000000..0ff9ada --- /dev/null +++ b/perception_setup/launch/seg_yolo_line.launch.py @@ -0,0 +1,75 @@ + +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('vortex_image_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='vortex_image_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': '/home/gard/ros2_ws/src/vortex-deep-learning-pipelines/runs/classify/results/classify-20260304-154252/weights/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', + } + ], + ), + ]) \ No newline at end of file From 035551b527a0135a40724dd6eb76e6874287ba74 Mon Sep 17 00:00:00 2001 From: vortexuser Date: Thu, 16 Apr 2026 09:09:21 +0000 Subject: [PATCH 29/37] feat: add YOLO and IRLS line fitting launch files --- ...tralytics_pipeline_line_fitting.launch.py} | 0 .../ultralytics_valve_detection.launch.py | 184 ++++++++++++++++++ perception_setup/package.xml | 4 +- 3 files changed, 186 insertions(+), 2 deletions(-) rename perception_setup/launch/{seg_yolo_line.launch.py => ultralytics_pipeline_line_fitting.launch.py} (100%) create mode 100644 perception_setup/launch/ultralytics_valve_detection.launch.py diff --git a/perception_setup/launch/seg_yolo_line.launch.py b/perception_setup/launch/ultralytics_pipeline_line_fitting.launch.py similarity index 100% rename from perception_setup/launch/seg_yolo_line.launch.py rename to perception_setup/launch/ultralytics_pipeline_line_fitting.launch.py diff --git a/perception_setup/launch/ultralytics_valve_detection.launch.py b/perception_setup/launch/ultralytics_valve_detection.launch.py new file mode 100644 index 0000000..93bc195 --- /dev/null +++ b/perception_setup/launch/ultralytics_valve_detection.launch.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python3 +import os + +from ament_index_python.packages 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): + 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'." + ) + + +DETECTIONS_TOPIC = '/ultralytics_valve_detection/detections' + + +def launch_setup(context, *args, **kwargs): + device = LaunchConfiguration('device').perform(context) + validate_device(device) + + yolo_params = os.path.join( + get_package_share_directory('yolo_obb_object_detection'), + 'config/yolo_obb_object_detection_params.yaml', + ) + valve_params = os.path.join( + get_package_share_directory('valve_detection'), + 'config', + 'valve_detection_params.yaml', + ) + + # --- YOLO OBB detection node --- + yolo_node = Node( + package='yolo_obb_object_detection', + executable='yolo_obb_object_detection_node.py', + name='yolo_obb_object_detection', + output='screen', + parameters=[ + yolo_params, + { + 'device': device, + 'yolo_model': LaunchConfiguration('yolo_model'), + 'model_conf': LaunchConfiguration('model_conf'), + 'color_image_sub_topic': LaunchConfiguration('color_image_sub_topic'), + 'yolo_detections_pub_topic': DETECTIONS_TOPIC, + }, + ], + ) + + # --- Valve detection composable node --- + 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=[ + valve_params, + { + 'detections_sub_topic': DETECTIONS_TOPIC, + 'yolo_img_width': 1920, + 'yolo_img_height': 1080, + 'depth_image_sub_topic': LaunchConfiguration( + 'depth_image_sub_topic' + ), + 'depth_image_info_topic': LaunchConfiguration( + 'depth_image_info_topic' + ), + 'color_image_info_topic': LaunchConfiguration( + 'color_image_info_topic' + ), + 'depth_frame_id': LaunchConfiguration('depth_frame_id'), + 'color_frame_id': LaunchConfiguration('color_frame_id'), + 'landmarks_pub_topic': LaunchConfiguration( + 'landmarks_pub_topic' + ), + 'output_frame_id': LaunchConfiguration('output_frame_id'), + 'drone': LaunchConfiguration('drone'), + 'undistort_detections': LaunchConfiguration( + 'undistort_detections' + ), + 'debug_visualize': LaunchConfiguration('debug_visualize'), + }, + ], + ) + ], + output='screen', + ) + + return [yolo_node, valve_container] + + +def generate_launch_description(): + return LaunchDescription( + [ + # --- YOLO arguments --- + DeclareLaunchArgument( + 'device', + default_value='0', + description="YOLO device: 'cpu', GPU index, 'cuda', 'cuda:N', or 'mps'", + ), + DeclareLaunchArgument( + 'yolo_model', + default_value='/home/vortex/workspaces/isaac_ros-dev/src/perception-auv/perception_setup/models/obb_best_simulator.pt', + description='YOLO model file name', + ), + DeclareLaunchArgument( + 'model_conf', + default_value='0.4', + description='YOLO confidence threshold', + ), + DeclareLaunchArgument( + 'color_image_sub_topic', + default_value='/nautilus/front_camera/image_color', + description='Input color image topic for YOLO', + ), + # --- Valve detection arguments --- + DeclareLaunchArgument( + 'depth_image_sub_topic', + default_value='/nautilus/depth_camera/image_depth', + description='Depth image topic', + ), + DeclareLaunchArgument( + 'depth_image_info_topic', + default_value='/nautilus/depth_camera/camera_info', + description='Depth camera info topic', + ), + DeclareLaunchArgument( + 'color_image_info_topic', + default_value='/nautilus/front_camera/camera_info', + description='Color camera info topic', + ), + DeclareLaunchArgument( + 'depth_frame_id', + default_value='front_camera_depth_optical', + description='Depth camera optical frame ID (without drone prefix)', + ), + DeclareLaunchArgument( + 'color_frame_id', + default_value='front_camera_color_optical', + description='Color camera optical frame ID (without drone prefix)', + ), + DeclareLaunchArgument( + 'landmarks_pub_topic', + default_value='/valve_landmarks', + description='Output valve landmarks topic', + ), + DeclareLaunchArgument( + 'output_frame_id', + default_value='front_camera_depth_optical', + description='Output frame ID for published poses (without drone prefix)', + ), + DeclareLaunchArgument( + 'drone', + default_value='nautilus', + description='Robot name, prepended to TF frame IDs', + ), + DeclareLaunchArgument( + 'undistort_detections', + default_value='false', + description='Undistort bounding-box detections using color camera distortion', + ), + DeclareLaunchArgument( + 'debug_visualize', + default_value='true', + description='Enable valve detection debug visualization topics', + ), + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/perception_setup/package.xml b/perception_setup/package.xml index fbc8a11..9f6fc68 100644 --- a/perception_setup/package.xml +++ b/perception_setup/package.xml @@ -23,7 +23,7 @@ ros2launch launch_ros - isaac_ros_yolov8 + valve_detection image_filtering aruco_detector From cdb989f2299eb30d5bba74359f0fbf0001157699 Mon Sep 17 00:00:00 2001 From: vortexuser Date: Thu, 16 Apr 2026 11:24:20 +0000 Subject: [PATCH 30/37] feat: add launch arguments for valve detection configuration --- .../ultralytics_valve_detection.launch.py | 34 ++++++++++++++++++- 1 file changed, 33 insertions(+), 1 deletion(-) diff --git a/perception_setup/launch/ultralytics_valve_detection.launch.py b/perception_setup/launch/ultralytics_valve_detection.launch.py index 93bc195..fe6c017 100644 --- a/perception_setup/launch/ultralytics_valve_detection.launch.py +++ b/perception_setup/launch/ultralytics_valve_detection.launch.py @@ -94,6 +94,13 @@ def launch_setup(context, *args, **kwargs): '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'), }, ], ) @@ -156,7 +163,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( 'landmarks_pub_topic', - default_value='/valve_landmarks', + default_value='/nautilus/landmarks', description='Output valve landmarks topic', ), DeclareLaunchArgument( @@ -179,6 +186,31 @@ def generate_launch_description(): default_value='true', description='Enable valve detection 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), ] ) From f254cdc67bf54c6dc3249df66d3009b07a8f82eb Mon Sep 17 00:00:00 2001 From: vortexuser Date: Thu, 16 Apr 2026 20:31:45 +0200 Subject: [PATCH 31/37] feat: update launch files for simulator and valve detection; adjust host and port settings --- .gitignore | 2 + .../launch/cameras/realsense_d555.launch.py | 2 +- .../simulator_valve_intervention.launch.py | 295 ++++++++++++++++++ .../launch/valve_intervention.launch.py | 2 +- .../launch/visual_inspection.launch.py | 4 +- 5 files changed, 301 insertions(+), 4 deletions(-) create mode 100644 perception_setup/launch/simulator_valve_intervention.launch.py diff --git a/.gitignore b/.gitignore index 808f144..194edec 100644 --- a/.gitignore +++ b/.gitignore @@ -260,3 +260,5 @@ CATKIN_IGNORE # Models should not be pushed to git perception_setup/models/ + +.claude/ diff --git a/perception_setup/launch/cameras/realsense_d555.launch.py b/perception_setup/launch/cameras/realsense_d555.launch.py index ed5c917..a0f2b12 100644 --- a/perception_setup/launch/cameras/realsense_d555.launch.py +++ b/perception_setup/launch/cameras/realsense_d555.launch.py @@ -149,7 +149,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "gst_host", - default_value="10.0.0.68", + default_value="10.0.0.169", description="GStreamer stream destination host", ), DeclareLaunchArgument( diff --git a/perception_setup/launch/simulator_valve_intervention.launch.py b/perception_setup/launch/simulator_valve_intervention.launch.py new file mode 100644 index 0000000..ff2adf8 --- /dev/null +++ b/perception_setup/launch/simulator_valve_intervention.launch.py @@ -0,0 +1,295 @@ +"""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 — allowing it to subscribe to topics published by the +simulator (or any other upstream source). + +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 ( + 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 + +CONVERTED_IMAGE_TOPIC = '/yolo_obb/internal/converted_image' +ENCODER_RESIZE_TOPIC = '/yolo_obb_encoder/internal/resize/image' +TENSOR_OUTPUT_TOPIC = '/yolo_obb/tensor_pub' +TENSOR_INPUT_TOPIC = '/yolo_obb/tensor_sub' +DNN_IMAGE_ENCODER_NAMESPACE = 'yolo_obb_encoder/internal' + + +def _launch_setup(context, *args, **kwargs): + """Setup launch description with all nodes.""" + pkg_dir = get_package_share_directory('perception_setup') + + yolo_config_path = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_obb.yaml') + with open(yolo_config_path) as f: + yolo_cfg = yaml.safe_load(f) + + models_dir = os.path.join(pkg_dir, 'models') + model_file_path = os.path.join(models_dir, 'obb_best_simulator.onnx') + engine_file_path = os.path.join(models_dir, 'obb_best_simulator.engine') + + # Resolve launch args + color_image_topic = context.launch_configurations['color_image_topic'] + camera_info_topic = context.launch_configurations['camera_info_topic'] + depth_image_topic = context.launch_configurations['depth_image_topic'] + depth_camera_info_topic = context.launch_configurations['depth_camera_info_topic'] + input_image_width = int(context.launch_configurations['image_width']) + input_image_height = int(context.launch_configurations['image_height']) + + # YOLO OBB Pipeline + image_format_converter = ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_converter', + parameters=[ + { + 'encoding_desired': yolo_cfg['encoding_desired'], + 'image_width': input_image_width, + 'image_height': input_image_height, + 'input_qos': 'sensor_data', + } + ], + remappings=[ + ('image_raw', color_image_topic), + ('image', CONVERTED_IMAGE_TOPIC), + ], + ) + + 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': bool(yolo_cfg['verbose']), + 'force_engine_update': bool(yolo_cfg['force_engine_update']), + 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + } + ], + remappings=[ + ('tensor_pub', TENSOR_OUTPUT_TOPIC), + ('tensor_sub', TENSOR_INPUT_TOPIC), + ], + ) + + 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': TENSOR_INPUT_TOPIC, + 'confidence_threshold': float(yolo_cfg['confidence_threshold']), + 'num_detections': int(yolo_cfg['num_detections']), + 'detections_topic': str(yolo_cfg['detection_topic']), + } + ], + ) + + 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(input_image_width), + 'input_image_height': str(input_image_height), + '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': DNN_IMAGE_ENCODER_NAMESPACE, + 'image_input_topic': CONVERTED_IMAGE_TOPIC, + 'camera_info_input_topic': camera_info_topic, + 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + }.items(), + ) + + yolo_obb_visualizer = ( + Node( + package='isaac_ros_yolov26_obb', + executable='isaac_ros_yolov26_obb_visualizer.py', + name='yolo_obb_visualizer', + parameters=[ + { + 'detections_topic': yolo_cfg['detection_topic'], + 'image_topic': ENCODER_RESIZE_TOPIC, + 'output_image_topic': yolo_cfg['visualized_image_topic'], + 'class_names_yaml': str(yolo_cfg['class_names']), + } + ], + ) + if bool(yolo_cfg['enable_visualizer']) + else None + ) + + # Valve Detection + valve_detection_config = 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_config, + { + 'depth_image_sub_topic': depth_image_topic, + 'detections_sub_topic': yolo_cfg['detection_topic'], + 'depth_image_info_topic': depth_camera_info_topic, + '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'), + }, + ], + ) + ], + 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_cfg['visualized_image_topic'], + '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', + ) + + # Collect all actions + actions = [ + tensor_rt_container, + dnn_image_encoder_launch, + valve_detection_container, + image_to_gstreamer_node, + ] + + if yolo_obb_visualizer: + actions.append(yolo_obb_visualizer) + + return actions + + +def generate_launch_description(): + """Generate the launch description.""" + return launch.LaunchDescription( + [ + # Simulator camera topic/size inputs + DeclareLaunchArgument( + 'color_image_topic', + default_value='/nautilus/front_camera/image_color', + description='Color image topic published by the simulator', + ), + DeclareLaunchArgument( + 'camera_info_topic', + default_value='/nautilus/front_camera/camera_info', + description='Camera info topic for the color image', + ), + DeclareLaunchArgument( + 'depth_image_topic', + default_value='/nautilus/depth_camera/image_depth', + description='Depth image topic published by the simulator', + ), + DeclareLaunchArgument( + 'depth_camera_info_topic', + default_value='/nautilus/depth_camera/camera_info', + description='Camera info topic for the depth image', + ), + DeclareLaunchArgument( + 'image_width', + default_value='1920', + description='Input color image width in pixels', + ), + DeclareLaunchArgument( + 'image_height', + default_value='1080', + description='Input color image height in pixels', + ), + # Valve Detection Options + DeclareLaunchArgument( + 'drone', + default_value='nautilus', + description='Robot name, prepended to TF frame IDs', + ), + DeclareLaunchArgument( + 'undistort_detections', + default_value='false', + description='Undistort detections using color camera distortion', + ), + DeclareLaunchArgument( + 'debug_visualize', + default_value='true', + description='Enable debug visualization topics', + ), + OpaqueFunction(function=_launch_setup), + ] + ) diff --git a/perception_setup/launch/valve_intervention.launch.py b/perception_setup/launch/valve_intervention.launch.py index 151484a..caa3519 100644 --- a/perception_setup/launch/valve_intervention.launch.py +++ b/perception_setup/launch/valve_intervention.launch.py @@ -267,7 +267,7 @@ def _launch_setup(context, *args, **kwargs): { 'input_topic': yolo_cfg['visualized_image_topic'], 'host': '10.0.0.169', - 'port': 5001, + 'port': 5000, 'bitrate': 500000, 'framerate': 15, 'preset_level': 1, diff --git a/perception_setup/launch/visual_inspection.launch.py b/perception_setup/launch/visual_inspection.launch.py index 8d12dfc..6e0e1b7 100644 --- a/perception_setup/launch/visual_inspection.launch.py +++ b/perception_setup/launch/visual_inspection.launch.py @@ -91,7 +91,7 @@ def generate_launch_description(): 'pub_topic': FILTERED_IMAGE_TOPIC, 'input_encoding': cam['encoding'], 'output_encoding': cam['encoding'], - 'filter_params.filter_type': 'remove_grid', + 'filter_params.filter_type': 'no_filter', }, ], ), @@ -129,7 +129,7 @@ def generate_launch_description(): parameters=[ { 'input_topic': '/aruco_detector/image', - 'host': '10.0.0.68', + 'host': '10.0.0.154', 'port': 5000, 'bitrate': 500000, 'framerate': 15, From e222cca277b95ff6aa7a3ca554e498ec8659a958 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Fri, 17 Apr 2026 17:24:05 +0200 Subject: [PATCH 32/37] Refactor YOLO launch files for improved configuration and topic management - Updated visual_inspection.launch.py to remove hardcoded camera configurations and use default topics. - Refactored yolo_cls.launch.py to streamline model loading and parameter handling, including default values for image processing. - Simplified yolo_detect.launch.py by consolidating camera topic management and enhancing model file paths. - Enhanced yolo_obb.launch.py to improve clarity in tensor handling and topic remapping. - Revised yolo_seg.launch.py to optimize image processing parameters and ensure consistent topic usage across the pipeline. - Removed unnecessary launch arguments and improved overall readability of launch files. --- perception_setup/README.md | 30 +- perception_setup/config/cameras/cameras.yaml | 44 --- perception_setup/config/yolo/yolo_cls.yaml | 33 +- perception_setup/config/yolo/yolo_detect.yaml | 25 +- perception_setup/config/yolo/yolo_obb.yaml | 25 +- perception_setup/config/yolo/yolo_seg.yaml | 31 +- .../launch/cameras/blackfly_s.launch.py | 127 +++----- .../launch/cameras/realsense_d555.launch.py | 205 ++++-------- .../launch/cameras/sonar.launch.py | 78 +---- .../launch/docking_blackfly_s.launch.py | 15 +- .../launch/docking_realsense_d555.launch.py | 23 +- .../isaac_ros_valve_intervention.launch.py | 219 +++++++++++++ .../launch/pipeline_blackfly_s.launch.py | 193 +++++------- .../launch/pipeline_realsense_d555.launch.py | 119 +++---- .../simulator_valve_intervention.launch.py | 295 ------------------ ...ltralytics_pipeline_line_fitting.launch.py | 0 .../ultralytics_valve_detection.launch.py | 105 +++++++ .../ultralytics_valve_detection.launch.py | 216 ------------- .../launch/valve_intervention.launch.py | 163 +++++----- .../launch/visual_inspection.launch.py | 38 +-- .../launch/yolo/yolo_cls.launch.py | 174 ++++------- .../launch/yolo/yolo_detect.launch.py | 194 ++++-------- .../launch/yolo/yolo_obb.launch.py | 186 ++++------- .../launch/yolo/yolo_seg.launch.py | 217 ++++--------- 24 files changed, 928 insertions(+), 1827 deletions(-) delete mode 100644 perception_setup/config/cameras/cameras.yaml create mode 100644 perception_setup/launch/isaac_ros/isaac_ros_valve_intervention.launch.py delete mode 100644 perception_setup/launch/simulator_valve_intervention.launch.py rename perception_setup/launch/{ => ultralytics}/ultralytics_pipeline_line_fitting.launch.py (100%) create mode 100644 perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py delete mode 100644 perception_setup/launch/ultralytics_valve_detection.launch.py diff --git a/perception_setup/README.md b/perception_setup/README.md index d203433..9503f32 100644 --- a/perception_setup/README.md +++ b/perception_setup/README.md @@ -1,6 +1,8 @@ # perception_setup -Launch and configuration package for the perception pipeline. Contains camera drivers, image preprocessing (undistortion), YOLO inference pipelines, and mission-specific launch files. All camera topics and image dimensions are defined in a single config file (`cameras.yaml`) to keep launch files in sync. +Launch and configuration package for the perception pipeline. Contains camera drivers, image preprocessing (undistortion), YOLO inference pipelines, and mission-specific launch files. + +**Convention:** launch files own all topics, TF frames, and image dimensions (as module-level `UPPER_CASE` constants at the top of each file). Config YAMLs hold only model/algorithm tuning — thresholds, tensor names, model paths, class names, etc. This avoids having the same value declared in two places. ## Package structure @@ -8,16 +10,15 @@ Launch and configuration package for the perception pipeline. Contains camera dr perception_setup/ config/ cameras/ - cameras.yaml # Topic names and image dimensions (single source of truth) 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 model config (valve detection) - yolo_detect.yaml # YOLO detection model config - yolo_seg.yaml # YOLO segmentation model config - yolo_cls.yaml # YOLO classification model config + 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 @@ -118,24 +119,13 @@ ros2 launch perception_setup realsense_d555.launch.py ## Configuration -### cameras.yaml - -Single source of truth for all camera topic names and image dimensions. Both camera launch files and YOLO launch files read from this file. Example entry: +### Topics and frames -```yaml -realsense_d555: - raw_color_image_topic: "/camera/camera/color/image_raw" - raw_color_camera_info_topic: "/camera/camera/color/camera_info" - image_topic: "/realsense_d555/color/image_rect" # downstream nodes subscribe here - camera_info_topic: "/realsense_d555/color/camera_info" - image_width: 896 - image_height: 504 - encoding: "rgb8" -``` +Topics and TF frame names are defined as `UPPER_CASE` constants at the top of each launch file. The standalone YOLO launch files (`yolo/yolo_*.launch.py`) expose `image_input_topic`, `camera_info_input_topic`, `input_image_width` and `input_image_height` as launch arguments so they can be wired to any upstream source. ### YOLO config files -Each YOLO variant (`yolo_obb.yaml`, `yolo_detect.yaml`, etc.) specifies model paths, network input dimensions, confidence thresholds, and output topic names. These are read by the corresponding launch 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. ## Building diff --git a/perception_setup/config/cameras/cameras.yaml b/perception_setup/config/cameras/cameras.yaml deleted file mode 100644 index 8cd909e..0000000 --- a/perception_setup/config/cameras/cameras.yaml +++ /dev/null @@ -1,44 +0,0 @@ -# Single source of truth for camera topics and image properties. -# Both camera launch files and YOLO launch files read from here. - -blackfly_s: - image_topic: "/blackfly_s/image_raw" - camera_info_topic: "/blackfly_s/camera_info" - image_width: 720 - image_height: 540 - encoding: "bgr8" - -realsense_d555: - # Raw topics from the RealSense driver - raw_color_image_topic: "/camera/camera/color/image_raw" - raw_color_camera_info_topic: "/camera/camera/color/camera_info" - raw_depth_image_topic: "/camera/camera/depth/image_rect_raw" - raw_depth_camera_info_topic: "/camera/camera/depth/camera_info" - - # Final output topics (downstream nodes subscribe to these) - image_topic: "/realsense_d555/color/image_rect" - camera_info_topic: "/realsense_d555/color/camera_info" - depth_image_topic: "/camera/camera/depth/image_rect_raw" - depth_camera_info_topic: "/camera/camera/depth/camera_info" - - image_width: 896 - image_height: 504 - encoding: "rgb8" - -# Simulator cameras - -sim_front_camera: - image_topic: "/nautilus/front_camera/image_color" - camera_info_topic: "/nautilus/front_camera/camera_info" - depth_image_topic: "/nautilus/depth_camera/image_depth" - depth_camera_info_topic: "/nautilus/depth_camera/camera_info" - image_width: 1920 - image_height: 1080 - encoding: "rgb8" - -sim_down_camera: - image_topic: "/nautilus/down_camera/image_color" - camera_info_topic: "/nautilus/down_camera/camera_info" - image_width: 1440 - image_height: 1080 - encoding: "rgb8" diff --git a/perception_setup/config/yolo/yolo_cls.yaml b/perception_setup/config/yolo/yolo_cls.yaml index 4fa15b2..3a0047d 100644 --- a/perception_setup/config/yolo/yolo_cls.yaml +++ b/perception_setup/config/yolo/yolo_cls.yaml @@ -1,33 +1,6 @@ -# Topics -image_input_topic: "/yolo_seg_mask" -camera_info_input_topic: "/camera/camera/color/camera_info_rect" -visualized_image_topic: "/yolo_cls_processed_image" -class_topic: "/classification_output" - -# Model and engine file paths (relative to perception_setup/models/) -model_file_path: "end-of-pipeline-yolov26.onnx" -engine_file_path: "end-of-pipeline-yolov26.engine" - -# Visualizer -enable_visualizer: true -# Map class indices to human-readable names. -# Update these to match your classification model's classes. -class_names: - 0: "continue" - 1: "end" - -# Input image dimensions (from the subscribed topic) -input_image_width: 640 -input_image_height: 640 -encoding_desired: "rgb8" - -# TensorRT -verbose: true -force_engine_update: false - -# ============================================================================================================================================================== -# NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model -# ============================================================================================================================================================== +# 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] diff --git a/perception_setup/config/yolo/yolo_detect.yaml b/perception_setup/config/yolo/yolo_detect.yaml index 32f1514..b6671d9 100644 --- a/perception_setup/config/yolo/yolo_detect.yaml +++ b/perception_setup/config/yolo/yolo_detect.yaml @@ -1,25 +1,6 @@ -# Output topics -visualized_image_topic: "/yolov8_processed_image" -detection_topic: "/yolov8_detections_output" - -# Model and engine file paths -model_file_path: "best.onnx" # The output dimensions must be [1, 5, 8400] (see yolov8_encoder_node.cpp for details) -engine_file_path: "best.engine" - -# Visualizer and class names for the visualizer overlay. -enable_visualizer: true -class_names: - 0: "valve" - -encoding_desired: "rgb8" - -# TensorRT -verbose: true -force_engine_update: false - -# ============================================================================================================================================================== -# NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model -# ============================================================================================================================================================== +# 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"] diff --git a/perception_setup/config/yolo/yolo_obb.yaml b/perception_setup/config/yolo/yolo_obb.yaml index f581d1f..50a3bc9 100644 --- a/perception_setup/config/yolo/yolo_obb.yaml +++ b/perception_setup/config/yolo/yolo_obb.yaml @@ -1,25 +1,6 @@ -# Output topics -visualized_image_topic: "/yolo_obb_processed_image" -detection_topic: "/obb_detections_output" - -# Model and engine file paths (relative to perception_setup/models/) -model_file_path: "obb_best.onnx" -engine_file_path: "obb_best.engine" - -# Visualizer -enable_visualizer: true -class_names: - 0: "valve" - -encoding_desired: "rgb8" - -# TensorRT -verbose: true -force_engine_update: false - -# ============================================================================================================================================================== -# NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model -# ============================================================================================================================================================== +# 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] diff --git a/perception_setup/config/yolo/yolo_seg.yaml b/perception_setup/config/yolo/yolo_seg.yaml index 38b4b5b..c2aacba 100644 --- a/perception_setup/config/yolo/yolo_seg.yaml +++ b/perception_setup/config/yolo/yolo_seg.yaml @@ -1,30 +1,7 @@ -# Output topics -visualized_image_topic: "/yolo_seg_processed_image" -detection_topic: "/seg_detections_output" -mask_topic: "/yolo_seg_mask" - -# Model and engine file paths (relative to perception_setup/models/) -model_file_path: "seg_pipe_real.onnx" -engine_file_path: "seg_pipe_real.engine" - -# Visualizer -enable_visualizer: true -class_names: - 0: "pipeline" - -encoding_desired: "rgb8" - -# Output mask resolution (upscaled from 160x160 via bilinear interpolation) -output_mask_width: 640 # Set to 640 to match the network input size, which allows you to use its camera info directly -output_mask_height: 640 # Set to 640 to match the network input size, which allows you to use its camera info directly - -# TensorRT -verbose: true -force_engine_update: false - -# ============================================================================================================================================================== -# NOTE: Below won't change unless you change the model architecture or input/output tensor names, so no need to update these if you just swap in a different model -# ============================================================================================================================================================== +# 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: diff --git a/perception_setup/launch/cameras/blackfly_s.launch.py b/perception_setup/launch/cameras/blackfly_s.launch.py index 825ed81..ff04841 100644 --- a/perception_setup/launch/cameras/blackfly_s.launch.py +++ b/perception_setup/launch/cameras/blackfly_s.launch.py @@ -2,6 +2,7 @@ Starts: - Spinnaker camera driver component in a dedicated container + - image_to_gstreamer to stream the raw image over RTP/H.265 """ import os @@ -16,121 +17,69 @@ def generate_launch_description(): - pkg_dir = get_package_share_directory("perception_setup") + 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") - - enable_camera_arg = DeclareLaunchArgument( - "enable_camera", - default_value="true", - description="Enable FLIR Blackfly S camera component", + 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') flir_container = ComposableNodeContainer( - name="blackfly_s_container", - namespace="", - package="rclcpp_components", - executable="component_container_mt", + 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", + 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}", + 'parameter_file': spinnaker_map, + 'serial_number': '23494258', + 'camerainfo_url': f'file://{calib_path}', }, ], - remappings=[("~/control", "/exposure_control/control")], - extra_arguments=[{"use_intra_process_comms": True}], - condition=IfCondition(LaunchConfiguration("enable_camera")), + remappings=[('~/control', '/exposure_control/control')], + extra_arguments=[{'use_intra_process_comms': True}], + condition=IfCondition(LaunchConfiguration('enable_camera')), ) ], - output="screen", + 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"}, + package='image_to_gstreamer', + executable='image_to_gstreamer_node', + name='image_to_gstreamer_node', + additional_env={'EGL_PLATFORM': 'surfaceless'}, parameters=[ { - "input_topic": LaunchConfiguration("gst_input_topic"), - "host": LaunchConfiguration("gst_host"), - "port": LaunchConfiguration("gst_port"), - "bitrate": LaunchConfiguration("gst_bitrate"), - "framerate": LaunchConfiguration("gst_framerate"), - "preset_level": LaunchConfiguration("gst_preset_level"), - "iframe_interval": LaunchConfiguration("gst_iframe_interval"), - "control_rate": LaunchConfiguration("gst_control_rate"), - "pt": LaunchConfiguration("gst_pt"), - "config_interval": LaunchConfiguration("gst_config_interval"), - "format": "RGB", + 'input_topic': '/blackfly_s/image_raw', + '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", + output='screen', ) return LaunchDescription( [ - enable_camera_arg, - DeclareLaunchArgument( - "gst_input_topic", - default_value="/blackfly_s/image_raw", - description="Image topic to stream via GStreamer", - ), - DeclareLaunchArgument( - "gst_host", - default_value="10.0.0.68", - description="GStreamer stream destination host", - ), - DeclareLaunchArgument( - "gst_port", - default_value="5001", - description="GStreamer stream destination port", - ), - DeclareLaunchArgument( - "gst_bitrate", - default_value="500000", - description="GStreamer encoder bitrate (bps)", - ), - DeclareLaunchArgument( - "gst_framerate", - default_value="15", - description="GStreamer stream framerate", - ), - DeclareLaunchArgument( - "gst_preset_level", - default_value="1", - description="GStreamer encoder preset level", - ), - DeclareLaunchArgument( - "gst_iframe_interval", - default_value="15", - description="GStreamer I-frame interval", - ), - DeclareLaunchArgument( - "gst_control_rate", - default_value="1", - description="GStreamer control rate", - ), - DeclareLaunchArgument( - "gst_pt", - default_value="96", - description="GStreamer RTP payload type", - ), DeclareLaunchArgument( - "gst_config_interval", - default_value="1", - description="GStreamer config interval", + '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 index a0f2b12..795e07d 100644 --- a/perception_setup/launch/cameras/realsense_d555.launch.py +++ b/perception_setup/launch/cameras/realsense_d555.launch.py @@ -1,15 +1,13 @@ """RealSense D555 camera launch file. Starts: - - RealSense camera driver node + - RealSense camera driver node (raw color + depth) - image_undistort (undistorts color image using color_realsense_d555_calib.yaml) - -Topics are read from cameras.yaml (single source of truth). + - image_to_gstreamer to stream the undistorted color image """ import os -import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument @@ -17,180 +15,103 @@ 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") - - cameras_path = os.path.join(pkg_dir, "config", "cameras", "cameras.yaml") - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras["realsense_d555"] + pkg_dir = get_package_share_directory('perception_setup') calib_file = os.path.join( - pkg_dir, "config", "cameras", "color_realsense_d555_calib.yaml" + pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml' ) realsense_node = Node( - package="realsense2_camera", - executable="realsense2_camera_node", - name="camera", - namespace="camera", + package='realsense2_camera', + executable='realsense2_camera_node', + name='camera', + namespace='camera', parameters=[ { - "enable_color": True, - "rgb_camera.color_profile": "896,504,15", # When updating the image size, make sure to also update the calibration file - "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, + '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=[ - # Color image is NOT remapped here — image_undistort reads the raw - # topic and publishes to cam["image_topic"] (image_rect). - (cam["raw_depth_image_topic"], cam["depth_image_topic"]), - (cam["raw_depth_camera_info_topic"], cam["depth_camera_info_topic"]), + ('/camera/camera/depth/image_rect_raw', '/camera/camera/depth/image_rect_raw'), + ('/camera/camera/depth/camera_info', '/camera/camera/depth/camera_info'), ], - output="screen", - arguments=["--ros-args", "--log-level", "info"], + output='screen', + arguments=['--ros-args', '--log-level', 'info'], emulate_tty=True, ) - # Undistorts the color image using the calibration YAML. - # In undistort mode : loads K+D from calib file at startup, publishes - # rectified image + zero-distortion camera_info. - # In passthrough mode: relays raw image and driver camera_info as-is. image_undistort_container = ComposableNodeContainer( - name="image_undistort_container", - namespace="", - package="rclcpp_components", - executable="component_container_mt", + 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", + package='perception_setup', + plugin='perception_setup::ImageUndistort', + name='color_image_undistort', parameters=[ { - "image_topic": cam["raw_color_image_topic"], - "camera_info_file": calib_file, - "raw_camera_info_topic": cam["raw_color_camera_info_topic"], - "output_image_topic": cam["image_topic"], - "output_camera_info_topic": cam["camera_info_topic"], - "enable_undistort": LaunchConfiguration("enable_undistort"), - "image_qos": "reliable", + 'image_topic': '/camera/camera/color/image_raw', + 'camera_info_file': calib_file, + 'raw_camera_info_topic': '/camera/camera/color/camera_info', + 'output_image_topic': '/realsense_d555/color/image_rect', + 'output_camera_info_topic': '/realsense_d555/color/camera_info', + 'enable_undistort': LaunchConfiguration('enable_undistort'), + 'image_qos': 'reliable', } ], ), ], - output="screen", + output='screen', ) - # TODO: Valve detection worked better without this, figure out why. - # image_crop_node = Node( - # package="perception_setup", - # executable="image_crop.py", - # name="image_crop", - # parameters=[ - # { - # "image_topic": cam["raw_depth_image_topic"], - # "camera_info_topic": cam["raw_depth_camera_info_topic"], - # "output_image_topic": cam["depth_image_topic"], - # "output_camera_info_topic": cam["depth_camera_info_topic"], - # "enable_crop": False, - # "crop.x_offset": 260, - # "crop.y_offset": 190, - # "crop.width": 485, - # "crop.height": 245, - # } - # ], - # 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"}, + package='image_to_gstreamer', + executable='image_to_gstreamer_node', + name='image_to_gstreamer_node', + additional_env={'EGL_PLATFORM': 'surfaceless'}, parameters=[ { - "input_topic": cam["image_topic"], - "host": LaunchConfiguration("gst_host"), - "port": LaunchConfiguration("gst_port"), - "bitrate": LaunchConfiguration("gst_bitrate"), - "framerate": LaunchConfiguration("gst_framerate"), - "preset_level": LaunchConfiguration("gst_preset_level"), - "iframe_interval": LaunchConfiguration("gst_iframe_interval"), - "control_rate": LaunchConfiguration("gst_control_rate"), - "pt": LaunchConfiguration("gst_pt"), - "config_interval": LaunchConfiguration("gst_config_interval"), - "format": "RGB", + 'input_topic': '/realsense_d555/color/image_rect', + '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", + output='screen', ) return LaunchDescription( [ DeclareLaunchArgument( - "enable_undistort", - default_value="true", - description="Undistort color image before publishing to image_topic", - ), - DeclareLaunchArgument( - "gst_host", - default_value="10.0.0.169", - description="GStreamer stream destination host", - ), - DeclareLaunchArgument( - "gst_port", - default_value="5000", - description="GStreamer stream destination port", - ), - DeclareLaunchArgument( - "gst_bitrate", - default_value="500000", - description="GStreamer encoder bitrate (bps)", - ), - DeclareLaunchArgument( - "gst_framerate", - default_value="15", - description="GStreamer stream framerate", - ), - DeclareLaunchArgument( - "gst_preset_level", - default_value="1", - description="GStreamer encoder preset level", - ), - DeclareLaunchArgument( - "gst_iframe_interval", - default_value="15", - description="GStreamer I-frame interval", - ), - DeclareLaunchArgument( - "gst_control_rate", - default_value="1", - description="GStreamer control rate", - ), - DeclareLaunchArgument( - "gst_pt", - default_value="96", - description="GStreamer RTP payload type", - ), - DeclareLaunchArgument( - "gst_config_interval", - default_value="1", - description="GStreamer config interval", + 'enable_undistort', + default_value='true', + description='Undistort color image before publishing to image_topic', ), realsense_node, image_undistort_container, diff --git a/perception_setup/launch/cameras/sonar.launch.py b/perception_setup/launch/cameras/sonar.launch.py index ec788e1..53bce44 100644 --- a/perception_setup/launch/cameras/sonar.launch.py +++ b/perception_setup/launch/cameras/sonar.launch.py @@ -6,7 +6,6 @@ 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'), @@ -35,26 +34,26 @@ def generate_launch_description(): ) image_to_gstreamer_node = Node( - package="image_to_gstreamer", - executable="image_to_gstreamer_node", - name="image_to_gstreamer_node", - additional_env={"EGL_PLATFORM": "surfaceless"}, + 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": LaunchConfiguration("gst_host"), - "port": LaunchConfiguration("gst_port"), - "bitrate": LaunchConfiguration("gst_bitrate"), - "framerate": LaunchConfiguration("gst_framerate"), - "preset_level": LaunchConfiguration("gst_preset_level"), - "iframe_interval": LaunchConfiguration("gst_iframe_interval"), - "control_rate": LaunchConfiguration("gst_control_rate"), - "pt": LaunchConfiguration("gst_pt"), - "config_interval": LaunchConfiguration("gst_config_interval"), - "format": "GRAY8", + '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", + output='screen', ) return LaunchDescription( @@ -64,51 +63,6 @@ def generate_launch_description(): default_value='', description='Namespace for the norbit_fls_ros_interface node', ), - DeclareLaunchArgument( - "gst_host", - default_value="10.0.0.68", - description="GStreamer stream destination host", - ), - DeclareLaunchArgument( - "gst_port", - default_value="5002", - description="GStreamer stream destination port", - ), - DeclareLaunchArgument( - "gst_bitrate", - default_value="500000", - description="GStreamer encoder bitrate (bps)", - ), - DeclareLaunchArgument( - "gst_framerate", - default_value="15", - description="GStreamer stream framerate", - ), - DeclareLaunchArgument( - "gst_preset_level", - default_value="1", - description="GStreamer encoder preset level", - ), - DeclareLaunchArgument( - "gst_iframe_interval", - default_value="15", - description="GStreamer I-frame interval", - ), - DeclareLaunchArgument( - "gst_control_rate", - default_value="1", - description="GStreamer control rate", - ), - DeclareLaunchArgument( - "gst_pt", - default_value="96", - description="GStreamer RTP payload type", - ), - DeclareLaunchArgument( - "gst_config_interval", - default_value="1", - description="GStreamer config interval", - ), 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 index d9726c1..9403806 100644 --- a/perception_setup/launch/docking_blackfly_s.launch.py +++ b/perception_setup/launch/docking_blackfly_s.launch.py @@ -8,7 +8,6 @@ import os -import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument @@ -21,11 +20,6 @@ def generate_launch_description(): pkg_dir = get_package_share_directory('perception_setup') - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras['blackfly_s'] - driver_params = os.path.join( pkg_dir, 'config', 'cameras', 'blackfly_s_driver_params.yaml' ) @@ -65,15 +59,10 @@ def generate_launch_description(): 'aruco_detector_params.yaml', ), { - 'subs.image_topic': cam['image_topic'], - 'subs.camera_info_topic': cam['camera_info_topic'], + 'subs.image_topic': '/blackfly_s/image_raw', + 'subs.camera_info_topic': '/blackfly_s/camera_info', 'pubs.aruco_image': '/down_cam/aruco_detector/image', 'out_tf_frame': 'nautilus/downwards_camera_optical', - 'detect_board': True, - 'visualize': True, - 'log_markers': False, - 'publish_detections': True, - 'publish_landmarks': True, }, ], ), diff --git a/perception_setup/launch/docking_realsense_d555.launch.py b/perception_setup/launch/docking_realsense_d555.launch.py index 4fb8f2e..c551d3a 100644 --- a/perception_setup/launch/docking_realsense_d555.launch.py +++ b/perception_setup/launch/docking_realsense_d555.launch.py @@ -9,7 +9,6 @@ import os -import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument @@ -21,11 +20,6 @@ def generate_launch_description(): pkg_dir = get_package_share_directory('perception_setup') - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras['realsense_d555'] - calib_file = os.path.join( pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml' ) @@ -64,11 +58,11 @@ def generate_launch_description(): name='color_image_undistort', parameters=[ { - 'image_topic': cam['raw_color_image_topic'], + 'image_topic': '/camera/camera/color/image_raw', 'camera_info_file': calib_file, - 'raw_camera_info_topic': cam['raw_color_camera_info_topic'], - 'output_image_topic': cam['image_topic'], - 'output_camera_info_topic': cam['camera_info_topic'], + 'raw_camera_info_topic': '/camera/camera/color/camera_info', + 'output_image_topic': '/realsense_d555/color/image_rect', + 'output_camera_info_topic': '/realsense_d555/color/camera_info', 'enable_undistort': LaunchConfiguration('enable_undistort'), 'image_qos': 'sensor_data', } @@ -85,15 +79,10 @@ def generate_launch_description(): 'aruco_detector_params.yaml', ), { - 'subs.image_topic': cam['image_topic'], - 'subs.camera_info_topic': cam['camera_info_topic'], + 'subs.image_topic': '/realsense_d555/color/image_rect', + 'subs.camera_info_topic': '/realsense_d555/color/camera_info', 'pubs.aruco_image': '/forward_cam/aruco_detector/image', 'out_tf_frame': 'nautilus/front_camera_optical', - 'detect_board': True, - 'visualize': True, - 'log_markers': False, - 'publish_detections': True, - 'publish_landmarks': True, }, ], ), 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..9883f6d --- /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/pipeline_blackfly_s.launch.py b/perception_setup/launch/pipeline_blackfly_s.launch.py index e74d7d5..6bef8ab 100644 --- a/perception_setup/launch/pipeline_blackfly_s.launch.py +++ b/perception_setup/launch/pipeline_blackfly_s.launch.py @@ -24,50 +24,17 @@ from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode -# All pipeline-internal and output topics are prefixed to avoid collisions -# when running alongside other camera pipelines. -CAM_PREFIX = '/down_cam' - -# Segmentation pipeline internal topics -SEG_CONVERTED_IMAGE_TOPIC = f'{CAM_PREFIX}/yolo_seg/internal/converted_image' -SEG_TENSOR_OUTPUT_TOPIC = f'{CAM_PREFIX}/yolo_seg/tensor_pub' -SEG_TENSOR_INPUT_TOPIC = f'{CAM_PREFIX}/yolo_seg/tensor_sub' -SEG_ENCODER_NAMESPACE = 'down_cam/yolo_seg_encoder/internal' - -# Classification pipeline internal topics -CLS_CONVERTED_IMAGE_TOPIC = f'{CAM_PREFIX}/yolo_cls/internal/converted_image' -CLS_TENSOR_OUTPUT_TOPIC = f'{CAM_PREFIX}/yolo_cls/tensor_pub' -CLS_TENSOR_INPUT_TOPIC = f'{CAM_PREFIX}/yolo_cls/tensor_sub' -CLS_ENCODER_NAMESPACE = 'down_cam/yolo_cls_encoder/internal' - 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') - # Load camera config - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras['blackfly_s'] - - # Load model configs 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) - models_dir = os.path.join(pkg_dir, 'models') - - encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder') - - # Prefix yaml-sourced topics - seg_detection_topic = f'{CAM_PREFIX}{seg_cfg["detection_topic"]}' - seg_mask_topic = f'{CAM_PREFIX}{seg_cfg["mask_topic"]}' - seg_visualized_topic = f'{CAM_PREFIX}{seg_cfg["visualized_image_topic"]}' - cls_image_input_topic = f'{CAM_PREFIX}{cls_cfg["image_input_topic"]}' - cls_class_topic = f'{CAM_PREFIX}{cls_cfg["class_topic"]}' - cls_visualized_topic = f'{CAM_PREFIX}{cls_cfg["visualized_image_topic"]}' - # ------------------------------------------------------------------------- # Camera # ------------------------------------------------------------------------- @@ -114,15 +81,15 @@ def _launch_setup(context, *args, **kwargs): name='seg_image_format_converter', parameters=[ { - 'encoding_desired': seg_cfg['encoding_desired'], - 'image_width': int(cam['image_width']), - 'image_height': int(cam['image_height']), + 'encoding_desired': 'rgb8', + 'image_width': 720, + 'image_height': 540, 'input_qos': 'sensor_data', } ], remappings=[ - ('image_raw', cam['image_topic']), - ('image', SEG_CONVERTED_IMAGE_TOPIC), + ('image_raw', '/blackfly_s/image_raw'), + ('image', '/down_cam/yolo_seg/internal/converted_image'), ], ) @@ -132,22 +99,20 @@ def _launch_setup(context, *args, **kwargs): plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode', parameters=[ { - 'model_file_path': os.path.join(models_dir, seg_cfg['model_file_path']), - 'engine_file_path': os.path.join( - models_dir, seg_cfg['engine_file_path'] - ), + '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': bool(seg_cfg['verbose']), - 'force_engine_update': bool(seg_cfg['force_engine_update']), - 'tensor_output_topic': SEG_TENSOR_OUTPUT_TOPIC, + 'verbose': True, + 'force_engine_update': False, + 'tensor_output_topic': '/down_cam/yolo_seg/tensor_pub', } ], remappings=[ - ('tensor_pub', SEG_TENSOR_OUTPUT_TOPIC), - ('tensor_sub', SEG_TENSOR_INPUT_TOPIC), + ('tensor_pub', '/down_cam/yolo_seg/tensor_pub'), + ('tensor_sub', '/down_cam/yolo_seg/tensor_sub'), ], ) @@ -157,7 +122,7 @@ def _launch_setup(context, *args, **kwargs): plugin='nvidia::isaac_ros::yolov26_seg::YoloV26SegDecoderNode', parameters=[ { - 'tensor_input_topic': SEG_TENSOR_INPUT_TOPIC, + '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']), @@ -165,10 +130,10 @@ def _launch_setup(context, *args, **kwargs): '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': int(seg_cfg['output_mask_width']), - 'output_mask_height': int(seg_cfg['output_mask_height']), - 'detections_topic': seg_detection_topic, - 'mask_topic': seg_mask_topic, + 'output_mask_width': 640, + 'output_mask_height': 640, + 'detections_topic': '/down_cam/seg_detections_output', + 'mask_topic': '/down_cam/yolo_seg_mask', } ], ) @@ -192,38 +157,34 @@ def _launch_setup(context, *args, **kwargs): os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py') ), launch_arguments={ - 'input_image_width': str(cam['image_width']), - 'input_image_height': str(cam['image_height']), + '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': SEG_ENCODER_NAMESPACE, - 'image_input_topic': SEG_CONVERTED_IMAGE_TOPIC, - 'camera_info_input_topic': cam['camera_info_topic'], - 'tensor_output_topic': SEG_TENSOR_OUTPUT_TOPIC, + 'dnn_image_encoder_namespace': 'down_cam/yolo_seg_encoder/internal', + 'image_input_topic': '/down_cam/yolo_seg/internal/converted_image', + 'camera_info_input_topic': '/blackfly_s/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': seg_detection_topic, - 'image_topic': f'/{SEG_ENCODER_NAMESPACE}/resize/image', - 'mask_topic': seg_mask_topic, - 'output_image_topic': seg_visualized_topic, - 'class_names_yaml': str(seg_cfg['class_names']), - } - ], - ) - if bool(seg_cfg['enable_visualizer']) - else None + 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'}), + } + ], ) # ------------------------------------------------------------------------- @@ -235,15 +196,15 @@ def _launch_setup(context, *args, **kwargs): name='cls_image_format_converter', parameters=[ { - 'encoding_desired': cls_cfg['encoding_desired'], - 'image_width': int(cls_cfg['input_image_width']), - 'image_height': int(cls_cfg['input_image_height']), + 'encoding_desired': 'rgb8', + 'image_width': 640, + 'image_height': 640, 'input_qos': 'sensor_data', } ], remappings=[ - ('image_raw', cls_image_input_topic), - ('image', CLS_CONVERTED_IMAGE_TOPIC), + ('image_raw', '/down_cam/yolo_seg_mask'), + ('image', '/down_cam/yolo_cls/internal/converted_image'), ], ) @@ -253,22 +214,20 @@ def _launch_setup(context, *args, **kwargs): plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode', parameters=[ { - 'model_file_path': os.path.join(models_dir, cls_cfg['model_file_path']), - 'engine_file_path': os.path.join( - models_dir, cls_cfg['engine_file_path'] - ), + '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': bool(cls_cfg['verbose']), - 'force_engine_update': bool(cls_cfg['force_engine_update']), - 'tensor_output_topic': CLS_TENSOR_OUTPUT_TOPIC, + 'verbose': True, + 'force_engine_update': False, + 'tensor_output_topic': '/down_cam/yolo_cls/tensor_pub', } ], remappings=[ - ('tensor_pub', CLS_TENSOR_OUTPUT_TOPIC), - ('tensor_sub', CLS_TENSOR_INPUT_TOPIC), + ('tensor_pub', '/down_cam/yolo_cls/tensor_pub'), + ('tensor_sub', '/down_cam/yolo_cls/tensor_sub'), ], ) @@ -278,10 +237,10 @@ def _launch_setup(context, *args, **kwargs): plugin='nvidia::isaac_ros::yolov26_cls::YoloV26ClsDecoderNode', parameters=[ { - 'tensor_input_topic': CLS_TENSOR_INPUT_TOPIC, + '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': cls_class_topic, + 'class_topic': '/down_cam/classification_output', } ], ) @@ -305,37 +264,33 @@ def _launch_setup(context, *args, **kwargs): os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py') ), launch_arguments={ - 'input_image_width': str(cls_cfg['input_image_width']), - 'input_image_height': str(cls_cfg['input_image_height']), + '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': CLS_ENCODER_NAMESPACE, - 'image_input_topic': CLS_CONVERTED_IMAGE_TOPIC, - 'camera_info_input_topic': cls_cfg['camera_info_input_topic'], - 'tensor_output_topic': CLS_TENSOR_OUTPUT_TOPIC, + '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': cls_class_topic, - 'image_topic': f'/{CLS_ENCODER_NAMESPACE}/resize/image', - 'output_image_topic': cls_visualized_topic, - 'class_names_yaml': str(cls_cfg['class_names']), - } - ], - ) - if bool(cls_cfg['enable_visualizer']) - else None + 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'}), + } + ], ) # ------------------------------------------------------------------------- @@ -348,7 +303,7 @@ def _launch_setup(context, *args, **kwargs): additional_env={'EGL_PLATFORM': 'surfaceless'}, parameters=[ { - 'input_topic': seg_visualized_topic, + 'input_topic': '/down_cam/yolo_seg_processed_image', 'host': '10.0.0.68', 'port': 5001, 'bitrate': 500000, @@ -373,10 +328,8 @@ def _launch_setup(context, *args, **kwargs): image_to_gstreamer_node, ] - if seg_visualizer: - actions.append(seg_visualizer) - if cls_visualizer: - actions.append(cls_visualizer) + actions.append(seg_visualizer) + actions.append(cls_visualizer) return actions diff --git a/perception_setup/launch/pipeline_realsense_d555.launch.py b/perception_setup/launch/pipeline_realsense_d555.launch.py index 7a865da..e2098b4 100644 --- a/perception_setup/launch/pipeline_realsense_d555.launch.py +++ b/perception_setup/launch/pipeline_realsense_d555.launch.py @@ -23,43 +23,19 @@ from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode -# All pipeline-internal and output topics are prefixed to avoid collisions -# when running alongside other camera pipelines. -CAM_PREFIX = '/front_cam' - -# Segmentation pipeline internal topics -SEG_CONVERTED_IMAGE_TOPIC = f'{CAM_PREFIX}/yolo_seg/internal/converted_image' -SEG_TENSOR_OUTPUT_TOPIC = f'{CAM_PREFIX}/yolo_seg/tensor_pub' -SEG_TENSOR_INPUT_TOPIC = f'{CAM_PREFIX}/yolo_seg/tensor_sub' -SEG_ENCODER_NAMESPACE = 'front_cam/yolo_seg_encoder/internal' - 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') - # Load camera config - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras['realsense_d555'] - - # Load segmentation model config with open(os.path.join(pkg_dir, 'config', 'yolo', 'yolo_seg.yaml')) as f: seg_cfg = yaml.safe_load(f) - models_dir = os.path.join(pkg_dir, 'models') - - encoder_dir = get_package_share_directory('isaac_ros_dnn_image_encoder') - calib_file = os.path.join( pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml' ) - # Prefix yaml-sourced topics - seg_detection_topic = f'{CAM_PREFIX}{seg_cfg["detection_topic"]}' - seg_mask_topic = f'{CAM_PREFIX}{seg_cfg["mask_topic"]}' - seg_visualized_topic = f'{CAM_PREFIX}{seg_cfg["visualized_image_topic"]}' - # ------------------------------------------------------------------------- # Camera + Undistort # ------------------------------------------------------------------------- @@ -97,11 +73,11 @@ def _launch_setup(context, *args, **kwargs): name='color_image_undistort', parameters=[ { - 'image_topic': cam['raw_color_image_topic'], + 'image_topic': '/camera/camera/color/image_raw', 'camera_info_file': calib_file, - 'raw_camera_info_topic': cam['raw_color_camera_info_topic'], - 'output_image_topic': cam['image_topic'], - 'output_camera_info_topic': cam['camera_info_topic'], + 'raw_camera_info_topic': '/camera/camera/color/camera_info', + 'output_image_topic': '/realsense_d555/color/image_rect', + 'output_camera_info_topic': '/realsense_d555/color/camera_info', 'enable_undistort': LaunchConfiguration('enable_undistort'), 'image_qos': 'sensor_data', } @@ -121,15 +97,15 @@ def _launch_setup(context, *args, **kwargs): name='seg_image_format_converter', parameters=[ { - 'encoding_desired': seg_cfg['encoding_desired'], - 'image_width': int(cam['image_width']), - 'image_height': int(cam['image_height']), + 'encoding_desired': 'rgb8', + 'image_width': 896, + 'image_height': 504, 'input_qos': 'sensor_data', } ], remappings=[ - ('image_raw', cam['image_topic']), - ('image', SEG_CONVERTED_IMAGE_TOPIC), + ('image_raw', '/realsense_d555/color/image_rect'), + ('image', '/front_cam/yolo_seg/internal/converted_image'), ], ) @@ -139,22 +115,20 @@ def _launch_setup(context, *args, **kwargs): plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode', parameters=[ { - 'model_file_path': os.path.join(models_dir, seg_cfg['model_file_path']), - 'engine_file_path': os.path.join( - models_dir, seg_cfg['engine_file_path'] - ), + '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': bool(seg_cfg['verbose']), - 'force_engine_update': bool(seg_cfg['force_engine_update']), - 'tensor_output_topic': SEG_TENSOR_OUTPUT_TOPIC, + 'verbose': True, + 'force_engine_update': False, + 'tensor_output_topic': '/front_cam/yolo_seg/tensor_pub', } ], remappings=[ - ('tensor_pub', SEG_TENSOR_OUTPUT_TOPIC), - ('tensor_sub', SEG_TENSOR_INPUT_TOPIC), + ('tensor_pub', '/front_cam/yolo_seg/tensor_pub'), + ('tensor_sub', '/front_cam/yolo_seg/tensor_sub'), ], ) @@ -164,7 +138,7 @@ def _launch_setup(context, *args, **kwargs): plugin='nvidia::isaac_ros::yolov26_seg::YoloV26SegDecoderNode', parameters=[ { - 'tensor_input_topic': SEG_TENSOR_INPUT_TOPIC, + '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']), @@ -172,10 +146,10 @@ def _launch_setup(context, *args, **kwargs): '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': int(seg_cfg['output_mask_width']), - 'output_mask_height': int(seg_cfg['output_mask_height']), - 'detections_topic': seg_detection_topic, - 'mask_topic': seg_mask_topic, + 'output_mask_width': 640, + 'output_mask_height': 640, + 'detections_topic': '/front_cam/seg_detections_output', + 'mask_topic': '/front_cam/yolo_seg_mask', } ], ) @@ -199,38 +173,34 @@ def _launch_setup(context, *args, **kwargs): os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py') ), launch_arguments={ - 'input_image_width': str(cam['image_width']), - 'input_image_height': str(cam['image_height']), + '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': SEG_ENCODER_NAMESPACE, - 'image_input_topic': SEG_CONVERTED_IMAGE_TOPIC, - 'camera_info_input_topic': cam['camera_info_topic'], - 'tensor_output_topic': SEG_TENSOR_OUTPUT_TOPIC, + 'dnn_image_encoder_namespace': 'front_cam/yolo_seg_encoder/internal', + 'image_input_topic': '/front_cam/yolo_seg/internal/converted_image', + 'camera_info_input_topic': '/realsense_d555/color/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': seg_detection_topic, - 'image_topic': f'/{SEG_ENCODER_NAMESPACE}/resize/image', - 'mask_topic': seg_mask_topic, - 'output_image_topic': seg_visualized_topic, - 'class_names_yaml': str(seg_cfg['class_names']), - } - ], - ) - if bool(seg_cfg['enable_visualizer']) - else None + 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'}), + } + ], ) # ------------------------------------------------------------------------- @@ -243,7 +213,7 @@ def _launch_setup(context, *args, **kwargs): additional_env={'EGL_PLATFORM': 'surfaceless'}, parameters=[ { - 'input_topic': seg_visualized_topic, + 'input_topic': '/front_cam/yolo_seg_processed_image', 'host': '10.0.0.68', 'port': 5000, 'bitrate': 500000, @@ -266,8 +236,7 @@ def _launch_setup(context, *args, **kwargs): image_to_gstreamer_node, ] - if seg_visualizer: - actions.append(seg_visualizer) + actions.append(seg_visualizer) return actions diff --git a/perception_setup/launch/simulator_valve_intervention.launch.py b/perception_setup/launch/simulator_valve_intervention.launch.py deleted file mode 100644 index ff2adf8..0000000 --- a/perception_setup/launch/simulator_valve_intervention.launch.py +++ /dev/null @@ -1,295 +0,0 @@ -"""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 — allowing it to subscribe to topics published by the -simulator (or any other upstream source). - -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 ( - 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 - -CONVERTED_IMAGE_TOPIC = '/yolo_obb/internal/converted_image' -ENCODER_RESIZE_TOPIC = '/yolo_obb_encoder/internal/resize/image' -TENSOR_OUTPUT_TOPIC = '/yolo_obb/tensor_pub' -TENSOR_INPUT_TOPIC = '/yolo_obb/tensor_sub' -DNN_IMAGE_ENCODER_NAMESPACE = 'yolo_obb_encoder/internal' - - -def _launch_setup(context, *args, **kwargs): - """Setup launch description with all nodes.""" - pkg_dir = get_package_share_directory('perception_setup') - - yolo_config_path = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_obb.yaml') - with open(yolo_config_path) as f: - yolo_cfg = yaml.safe_load(f) - - models_dir = os.path.join(pkg_dir, 'models') - model_file_path = os.path.join(models_dir, 'obb_best_simulator.onnx') - engine_file_path = os.path.join(models_dir, 'obb_best_simulator.engine') - - # Resolve launch args - color_image_topic = context.launch_configurations['color_image_topic'] - camera_info_topic = context.launch_configurations['camera_info_topic'] - depth_image_topic = context.launch_configurations['depth_image_topic'] - depth_camera_info_topic = context.launch_configurations['depth_camera_info_topic'] - input_image_width = int(context.launch_configurations['image_width']) - input_image_height = int(context.launch_configurations['image_height']) - - # YOLO OBB Pipeline - image_format_converter = ComposableNode( - package='isaac_ros_image_proc', - plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', - name='image_format_converter', - parameters=[ - { - 'encoding_desired': yolo_cfg['encoding_desired'], - 'image_width': input_image_width, - 'image_height': input_image_height, - 'input_qos': 'sensor_data', - } - ], - remappings=[ - ('image_raw', color_image_topic), - ('image', CONVERTED_IMAGE_TOPIC), - ], - ) - - 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': bool(yolo_cfg['verbose']), - 'force_engine_update': bool(yolo_cfg['force_engine_update']), - 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, - } - ], - remappings=[ - ('tensor_pub', TENSOR_OUTPUT_TOPIC), - ('tensor_sub', TENSOR_INPUT_TOPIC), - ], - ) - - 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': TENSOR_INPUT_TOPIC, - 'confidence_threshold': float(yolo_cfg['confidence_threshold']), - 'num_detections': int(yolo_cfg['num_detections']), - 'detections_topic': str(yolo_cfg['detection_topic']), - } - ], - ) - - 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(input_image_width), - 'input_image_height': str(input_image_height), - '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': DNN_IMAGE_ENCODER_NAMESPACE, - 'image_input_topic': CONVERTED_IMAGE_TOPIC, - 'camera_info_input_topic': camera_info_topic, - 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, - }.items(), - ) - - yolo_obb_visualizer = ( - Node( - package='isaac_ros_yolov26_obb', - executable='isaac_ros_yolov26_obb_visualizer.py', - name='yolo_obb_visualizer', - parameters=[ - { - 'detections_topic': yolo_cfg['detection_topic'], - 'image_topic': ENCODER_RESIZE_TOPIC, - 'output_image_topic': yolo_cfg['visualized_image_topic'], - 'class_names_yaml': str(yolo_cfg['class_names']), - } - ], - ) - if bool(yolo_cfg['enable_visualizer']) - else None - ) - - # Valve Detection - valve_detection_config = 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_config, - { - 'depth_image_sub_topic': depth_image_topic, - 'detections_sub_topic': yolo_cfg['detection_topic'], - 'depth_image_info_topic': depth_camera_info_topic, - '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'), - }, - ], - ) - ], - 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_cfg['visualized_image_topic'], - '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', - ) - - # Collect all actions - actions = [ - tensor_rt_container, - dnn_image_encoder_launch, - valve_detection_container, - image_to_gstreamer_node, - ] - - if yolo_obb_visualizer: - actions.append(yolo_obb_visualizer) - - return actions - - -def generate_launch_description(): - """Generate the launch description.""" - return launch.LaunchDescription( - [ - # Simulator camera topic/size inputs - DeclareLaunchArgument( - 'color_image_topic', - default_value='/nautilus/front_camera/image_color', - description='Color image topic published by the simulator', - ), - DeclareLaunchArgument( - 'camera_info_topic', - default_value='/nautilus/front_camera/camera_info', - description='Camera info topic for the color image', - ), - DeclareLaunchArgument( - 'depth_image_topic', - default_value='/nautilus/depth_camera/image_depth', - description='Depth image topic published by the simulator', - ), - DeclareLaunchArgument( - 'depth_camera_info_topic', - default_value='/nautilus/depth_camera/camera_info', - description='Camera info topic for the depth image', - ), - DeclareLaunchArgument( - 'image_width', - default_value='1920', - description='Input color image width in pixels', - ), - DeclareLaunchArgument( - 'image_height', - default_value='1080', - description='Input color image height in pixels', - ), - # Valve Detection Options - DeclareLaunchArgument( - 'drone', - default_value='nautilus', - description='Robot name, prepended to TF frame IDs', - ), - DeclareLaunchArgument( - 'undistort_detections', - default_value='false', - description='Undistort detections using color camera distortion', - ), - DeclareLaunchArgument( - 'debug_visualize', - default_value='true', - description='Enable debug visualization topics', - ), - OpaqueFunction(function=_launch_setup), - ] - ) diff --git a/perception_setup/launch/ultralytics_pipeline_line_fitting.launch.py b/perception_setup/launch/ultralytics/ultralytics_pipeline_line_fitting.launch.py similarity index 100% rename from perception_setup/launch/ultralytics_pipeline_line_fitting.launch.py rename to perception_setup/launch/ultralytics/ultralytics_pipeline_line_fitting.launch.py 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..d80a196 --- /dev/null +++ b/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +"""Simulator -> Ultralytics YOLO OBB (Python) -> Valve Detection. + +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, 'obb_best_simulator.pt') + device = LaunchConfiguration('device').perform(context) + _validate_device(device) + + yolo_node = Node( + package='yolo_obb_object_detection', + executable='yolo_obb_object_detection_node.py', + name='yolo_obb_object_detection', + output='screen', + parameters=[ + { + 'device': device, + 'yolo_model': model_file_path, + 'model_conf': 0.4, + 'color_image_sub_topic': '/nautilus/front_camera/image_color', + 'yolo_detections_pub_topic': '/ultralytics_valve_detection/detections', + }, + ], + ) + + 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=[ + { + 'detections_sub_topic': '/ultralytics_valve_detection/detections', + 'yolo_img_width': 1920, + 'yolo_img_height': 1080, + 'depth_image_sub_topic': '/nautilus/depth_camera/image_depth', + 'depth_image_info_topic': '/nautilus/depth_camera/camera_info', + 'color_image_info_topic': '/nautilus/front_camera/camera_info', + 'depth_frame_id': 'front_camera_depth_optical', + 'color_frame_id': 'front_camera_color_optical', + 'landmarks_pub_topic': '/nautilus/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, + }, + ], + ) + ], + output='screen', + ) + + 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'", + ), + OpaqueFunction(function=_launch_setup), + ] + ) diff --git a/perception_setup/launch/ultralytics_valve_detection.launch.py b/perception_setup/launch/ultralytics_valve_detection.launch.py deleted file mode 100644 index fe6c017..0000000 --- a/perception_setup/launch/ultralytics_valve_detection.launch.py +++ /dev/null @@ -1,216 +0,0 @@ -#!/usr/bin/env python3 -import os - -from ament_index_python.packages 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): - 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'." - ) - - -DETECTIONS_TOPIC = '/ultralytics_valve_detection/detections' - - -def launch_setup(context, *args, **kwargs): - device = LaunchConfiguration('device').perform(context) - validate_device(device) - - yolo_params = os.path.join( - get_package_share_directory('yolo_obb_object_detection'), - 'config/yolo_obb_object_detection_params.yaml', - ) - valve_params = os.path.join( - get_package_share_directory('valve_detection'), - 'config', - 'valve_detection_params.yaml', - ) - - # --- YOLO OBB detection node --- - yolo_node = Node( - package='yolo_obb_object_detection', - executable='yolo_obb_object_detection_node.py', - name='yolo_obb_object_detection', - output='screen', - parameters=[ - yolo_params, - { - 'device': device, - 'yolo_model': LaunchConfiguration('yolo_model'), - 'model_conf': LaunchConfiguration('model_conf'), - 'color_image_sub_topic': LaunchConfiguration('color_image_sub_topic'), - 'yolo_detections_pub_topic': DETECTIONS_TOPIC, - }, - ], - ) - - # --- Valve detection composable node --- - 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=[ - valve_params, - { - 'detections_sub_topic': DETECTIONS_TOPIC, - 'yolo_img_width': 1920, - 'yolo_img_height': 1080, - 'depth_image_sub_topic': LaunchConfiguration( - 'depth_image_sub_topic' - ), - 'depth_image_info_topic': LaunchConfiguration( - 'depth_image_info_topic' - ), - 'color_image_info_topic': LaunchConfiguration( - 'color_image_info_topic' - ), - 'depth_frame_id': LaunchConfiguration('depth_frame_id'), - 'color_frame_id': LaunchConfiguration('color_frame_id'), - 'landmarks_pub_topic': LaunchConfiguration( - 'landmarks_pub_topic' - ), - 'output_frame_id': LaunchConfiguration('output_frame_id'), - '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', - ) - - return [yolo_node, valve_container] - - -def generate_launch_description(): - return LaunchDescription( - [ - # --- YOLO arguments --- - DeclareLaunchArgument( - 'device', - default_value='0', - description="YOLO device: 'cpu', GPU index, 'cuda', 'cuda:N', or 'mps'", - ), - DeclareLaunchArgument( - 'yolo_model', - default_value='/home/vortex/workspaces/isaac_ros-dev/src/perception-auv/perception_setup/models/obb_best_simulator.pt', - description='YOLO model file name', - ), - DeclareLaunchArgument( - 'model_conf', - default_value='0.4', - description='YOLO confidence threshold', - ), - DeclareLaunchArgument( - 'color_image_sub_topic', - default_value='/nautilus/front_camera/image_color', - description='Input color image topic for YOLO', - ), - # --- Valve detection arguments --- - DeclareLaunchArgument( - 'depth_image_sub_topic', - default_value='/nautilus/depth_camera/image_depth', - description='Depth image topic', - ), - DeclareLaunchArgument( - 'depth_image_info_topic', - default_value='/nautilus/depth_camera/camera_info', - description='Depth camera info topic', - ), - DeclareLaunchArgument( - 'color_image_info_topic', - default_value='/nautilus/front_camera/camera_info', - description='Color camera info topic', - ), - DeclareLaunchArgument( - 'depth_frame_id', - default_value='front_camera_depth_optical', - description='Depth camera optical frame ID (without drone prefix)', - ), - DeclareLaunchArgument( - 'color_frame_id', - default_value='front_camera_color_optical', - description='Color camera optical frame ID (without drone prefix)', - ), - DeclareLaunchArgument( - 'landmarks_pub_topic', - default_value='/nautilus/landmarks', - description='Output valve landmarks topic', - ), - DeclareLaunchArgument( - 'output_frame_id', - default_value='front_camera_depth_optical', - description='Output frame ID for published poses (without drone prefix)', - ), - DeclareLaunchArgument( - 'drone', - default_value='nautilus', - description='Robot name, prepended to TF frame IDs', - ), - DeclareLaunchArgument( - 'undistort_detections', - default_value='false', - description='Undistort bounding-box detections using color camera distortion', - ), - DeclareLaunchArgument( - 'debug_visualize', - default_value='true', - description='Enable valve detection 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/valve_intervention.launch.py b/perception_setup/launch/valve_intervention.launch.py index caa3519..331af49 100644 --- a/perception_setup/launch/valve_intervention.launch.py +++ b/perception_setup/launch/valve_intervention.launch.py @@ -22,38 +22,20 @@ from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode -CONVERTED_IMAGE_TOPIC = '/yolo_obb/internal/converted_image' -ENCODER_RESIZE_TOPIC = '/yolo_obb_encoder/internal/resize/image' -TENSOR_OUTPUT_TOPIC = '/yolo_obb/tensor_pub' -TENSOR_INPUT_TOPIC = '/yolo_obb/tensor_sub' -DNN_IMAGE_ENCODER_NAMESPACE = 'yolo_obb_encoder/internal' - def _launch_setup(context, *args, **kwargs): - """Setup launch description with all nodes.""" pkg_dir = get_package_share_directory('perception_setup') + models_dir = os.path.join(pkg_dir, 'models') - # Load configurations - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras['realsense_d555'] - - yolo_config_path = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_obb.yaml') - with open(yolo_config_path) as f: + 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' ) - models_dir = os.path.join(pkg_dir, 'models') - model_file_path = os.path.join(models_dir, str(yolo_cfg['model_file_path'])) - engine_file_path = os.path.join(models_dir, str(yolo_cfg['engine_file_path'])) + model_file_path = os.path.join(models_dir, 'obb_best.onnx') + engine_file_path = os.path.join(models_dir, 'obb_best.engine') - input_image_width = int(cam['image_width']) - input_image_height = int(cam['image_height']) - - # RealSense Camera realsense_node = ComposableNode( package='realsense2_camera', plugin='realsense2_camera::RealSenseNodeFactory', @@ -79,51 +61,40 @@ def _launch_setup(context, *args, **kwargs): 'enable_sync': False, } ], - remappings=[ - # Color image is NOT remapped here — image_undistort reads the raw - # topic and publishes to cam["image_topic"] (image_rect). - (cam['raw_depth_image_topic'], cam['depth_image_topic']), - (cam['raw_depth_camera_info_topic'], cam['depth_camera_info_topic']), - ], ) - # Undistorts the color image using the calibration YAML. - # In undistort mode : loads K+D from calib file at startup, publishes - # rectified image + zero-distortion camera_info. - # In passthrough mode: relays raw image and driver camera_info as-is. image_undistort_node = ComposableNode( package='perception_setup', plugin='perception_setup::ImageUndistort', name='color_image_undistort', parameters=[ { - 'image_topic': cam['raw_color_image_topic'], + 'image_topic': '/camera/camera/color/image_raw', 'camera_info_file': calib_file, - 'raw_camera_info_topic': cam['raw_color_camera_info_topic'], - 'output_image_topic': cam['image_topic'], - 'output_camera_info_topic': cam['camera_info_topic'], + 'raw_camera_info_topic': '/camera/camera/color/camera_info', + 'output_image_topic': '/realsense_d555/color/image_rect', + 'output_camera_info_topic': '/realsense_d555/color/camera_info', 'enable_undistort': LaunchConfiguration('enable_undistort'), 'image_qos': 'reliable', # Isaac ros only works with reliable QoS } ], ) - # YOLO OBB Pipeline image_format_converter = ComposableNode( package='isaac_ros_image_proc', plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', name='image_format_converter', parameters=[ { - 'encoding_desired': yolo_cfg['encoding_desired'], - 'image_width': input_image_width, - 'image_height': input_image_height, + 'encoding_desired': 'rgb8', + 'image_width': 896, + 'image_height': 504, 'input_qos': 'sensor_data', } ], remappings=[ - ('image_raw', cam['image_topic']), - ('image', CONVERTED_IMAGE_TOPIC), + ('image_raw', '/realsense_d555/color/image_rect'), + ('image', '/yolo_obb/internal/converted_image'), ], ) @@ -139,14 +110,14 @@ def _launch_setup(context, *args, **kwargs): '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': bool(yolo_cfg['verbose']), - 'force_engine_update': bool(yolo_cfg['force_engine_update']), - 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + 'verbose': True, + 'force_engine_update': False, + 'tensor_output_topic': '/yolo_obb/tensor_pub', } ], remappings=[ - ('tensor_pub', TENSOR_OUTPUT_TOPIC), - ('tensor_sub', TENSOR_INPUT_TOPIC), + ('tensor_pub', '/yolo_obb/tensor_pub'), + ('tensor_sub', '/yolo_obb/tensor_sub'), ], ) @@ -156,10 +127,10 @@ def _launch_setup(context, *args, **kwargs): plugin='nvidia::isaac_ros::yolov26_obb::YoloV26OBBDecoderNode', parameters=[ { - 'tensor_input_topic': TENSOR_INPUT_TOPIC, + 'tensor_input_topic': '/yolo_obb/tensor_sub', 'confidence_threshold': float(yolo_cfg['confidence_threshold']), 'num_detections': int(yolo_cfg['num_detections']), - 'detections_topic': str(yolo_cfg['detection_topic']), + 'detections_topic': '/obb_detections_output', } ], ) @@ -186,41 +157,36 @@ def _launch_setup(context, *args, **kwargs): os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py') ), launch_arguments={ - 'input_image_width': str(input_image_width), - 'input_image_height': str(input_image_height), + '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': DNN_IMAGE_ENCODER_NAMESPACE, - 'image_input_topic': CONVERTED_IMAGE_TOPIC, - 'camera_info_input_topic': cam['camera_info_topic'], - 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + '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(), ) - yolo_obb_visualizer = ( - Node( - package='isaac_ros_yolov26_obb', - executable='isaac_ros_yolov26_obb_visualizer.py', - name='yolo_obb_visualizer', - parameters=[ - { - 'detections_topic': yolo_cfg['detection_topic'], - 'image_topic': ENCODER_RESIZE_TOPIC, - 'output_image_topic': yolo_cfg['visualized_image_topic'], - 'class_names_yaml': str(yolo_cfg['class_names']), - } - ], - ) - if bool(yolo_cfg['enable_visualizer']) - else None + 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 - valve_detection_config = os.path.join( + valve_detection_tuning = os.path.join( get_package_share_directory('valve_detection'), 'config', 'valve_detection_params.yaml', @@ -237,11 +203,11 @@ def _launch_setup(context, *args, **kwargs): plugin='valve_detection::ValvePoseNode', name='valve_pose_node', parameters=[ - valve_detection_config, + valve_detection_tuning, { - 'depth_image_sub_topic': cam['depth_image_topic'], - 'detections_sub_topic': yolo_cfg['detection_topic'], - 'depth_image_info_topic': cam['depth_camera_info_topic'], + 'depth_image_sub_topic': '/camera/camera/depth/image_rect_raw', + 'detections_sub_topic': '/obb_detections_output', + 'depth_image_info_topic': '/camera/camera/depth/camera_info', 'depth_frame_id': 'front_camera_depth_optical', 'color_frame_id': 'front_camera_color_optical', 'landmarks_pub_topic': '/valve_landmarks', @@ -251,6 +217,13 @@ def _launch_setup(context, *args, **kwargs): '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'), }, ], ) @@ -265,7 +238,7 @@ def _launch_setup(context, *args, **kwargs): additional_env={'EGL_PLATFORM': 'surfaceless'}, parameters=[ { - 'input_topic': yolo_cfg['visualized_image_topic'], + 'input_topic': '/yolo_obb_processed_image', 'host': '10.0.0.169', 'port': 5000, 'bitrate': 500000, @@ -281,7 +254,6 @@ def _launch_setup(context, *args, **kwargs): output='screen', ) - # Collect all actions actions = [ tensor_rt_container, dnn_image_encoder_launch, @@ -289,14 +261,12 @@ def _launch_setup(context, *args, **kwargs): image_to_gstreamer_node, ] - if yolo_obb_visualizer: - actions.append(yolo_obb_visualizer) + actions.append(yolo_obb_visualizer) return actions def generate_launch_description(): - """Generate the launch description.""" return launch.LaunchDescription( [ DeclareLaunchArgument( @@ -304,7 +274,6 @@ def generate_launch_description(): default_value='true', description='Undistort color image before passing to YOLO', ), - # Valve Detection Options DeclareLaunchArgument( 'drone', default_value='nautilus', @@ -312,7 +281,8 @@ def generate_launch_description(): ), DeclareLaunchArgument( 'undistort_detections', - default_value='false', # If this is enabled then enable_undistort must be disabled to avoid double undistortion. TODO: I had problems getting this working, work around is to just use enable_undistort + # If enabled, disable enable_undistort to avoid double-undistortion. + default_value='false', description='Undistort detections using color camera distortion', ), DeclareLaunchArgument( @@ -320,6 +290,31 @@ def generate_launch_description(): 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 index 6e0e1b7..268f6a9 100644 --- a/perception_setup/launch/visual_inspection.launch.py +++ b/perception_setup/launch/visual_inspection.launch.py @@ -9,7 +9,6 @@ import os -import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument @@ -17,17 +16,10 @@ from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode -FILTERED_IMAGE_TOPIC = '/visual_inspection/filtered_image' - def generate_launch_description(): pkg_dir = get_package_share_directory('perception_setup') - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras['realsense_d555'] - calib_file = os.path.join( pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml' ) @@ -66,11 +58,11 @@ def generate_launch_description(): name='color_image_undistort', parameters=[ { - 'image_topic': cam['raw_color_image_topic'], + 'image_topic': '/camera/camera/color/image_raw', 'camera_info_file': calib_file, - 'raw_camera_info_topic': cam['raw_color_camera_info_topic'], - 'output_image_topic': cam['image_topic'], - 'output_camera_info_topic': cam['camera_info_topic'], + 'raw_camera_info_topic': '/camera/camera/color/camera_info', + 'output_image_topic': '/realsense_d555/color/image_rect', + 'output_camera_info_topic': '/realsense_d555/color/camera_info', 'enable_undistort': LaunchConfiguration('enable_undistort'), 'image_qos': 'sensor_data', } @@ -87,11 +79,11 @@ def generate_launch_description(): 'image_filtering_params.yaml', ), { - 'sub_topic': cam['image_topic'], - 'pub_topic': FILTERED_IMAGE_TOPIC, - 'input_encoding': cam['encoding'], - 'output_encoding': cam['encoding'], - 'filter_params.filter_type': 'no_filter', + 'sub_topic': '/realsense_d555/color/image_rect', + 'pub_topic': '/visual_inspection/filtered_image', + 'input_encoding': 'rgb8', + 'output_encoding': 'rgb8', + 'filter_params.filter_type': LaunchConfiguration('filter_type'), }, ], ), @@ -106,12 +98,9 @@ def generate_launch_description(): 'aruco_detector_params.yaml', ), { - 'subs.image_topic': FILTERED_IMAGE_TOPIC, - 'subs.camera_info_topic': cam['camera_info_topic'], + 'subs.image_topic': '/visual_inspection/filtered_image', + 'subs.camera_info_topic': '/realsense_d555/color/camera_info', 'detect_board': False, - 'visualize': True, - 'log_markers': True, - 'publish_detections': True, 'publish_landmarks': False, }, ], @@ -151,6 +140,11 @@ def generate_launch_description(): 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 index ef40f1a..2cd7008 100644 --- a/perception_setup/launch/yolo/yolo_cls.launch.py +++ b/perception_setup/launch/yolo/yolo_cls.launch.py @@ -3,25 +3,21 @@ """Isaac ROS YOLO classification TensorRT inference pipeline. 1. ImageFormatConverterNode - Input: image_input_topic - Output: /yolo_cls/internal/converted_image - Purpose: Convert camera image encoding (e.g. bgra8 -> rgb8) + Input: IMAGE_INPUT_TOPIC (launch arg) + Output: CONVERTED_IMAGE_TOPIC 2. DNNImageEncoderNode - Input: /yolo_cls/internal/converted_image - Output: /tensor_pub - Purpose: Resize + normalize image and convert to network tensor + Input: CONVERTED_IMAGE_TOPIC + Output: TENSOR_OUTPUT_TOPIC 3. TensorRTNode - Input: /tensor_pub - Output: /tensor_sub - Purpose: Run TensorRT inference + Input: TENSOR_OUTPUT_TOPIC + Output: TENSOR_INPUT_TOPIC Expected output tensor shape: [1, num_classes] 4. YoloV26ClsDecoderNode - Input: /tensor_sub - Output: class_topic (UInt8 with predicted class index) - Purpose: Argmax over class scores and publish class ID + Input: TENSOR_INPUT_TOPIC + Output: CLASS_TOPIC (UInt8 with predicted class index) """ import os @@ -30,61 +26,23 @@ 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 -CONVERTED_IMAGE_TOPIC = '/yolo_cls/internal/converted_image' -ENCODER_RESIZE_TOPIC = '/yolo_cls_encoder/internal/resize/image' -TENSOR_OUTPUT_TOPIC = '/yolo_cls/tensor_pub' -TENSOR_INPUT_TOPIC = '/yolo_cls/tensor_sub' -DNN_IMAGE_ENCODER_NAMESPACE = 'yolo_cls_encoder/internal' - - def _launch_setup(context, *args, **kwargs): - config_path = LaunchConfiguration('config_file').perform(context) - - with open(config_path) as f: - cfg = yaml.safe_load(f) - pkg_dir = get_package_share_directory('perception_setup') models_dir = os.path.join(pkg_dir, 'models') - model_file_path = os.path.join(models_dir, str(cfg['model_file_path'])) - engine_file_path = os.path.join(models_dir, str(cfg['engine_file_path'])) - - input_tensor_names = cfg['input_tensor_names'] - input_binding_names = cfg['input_binding_names'] - output_tensor_names = cfg['output_tensor_names'] - output_binding_names = cfg['output_binding_names'] - - verbose = bool(cfg['verbose']) - force_engine_update = bool(cfg['force_engine_update']) - - input_image_width = int(cfg['input_image_width']) - input_image_height = int(cfg['input_image_height']) - encoding_desired = str(cfg['encoding_desired']) + 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') - network_image_width = int(cfg['network_image_width']) - network_image_height = int(cfg['network_image_height']) - image_mean = cfg['image_mean'] - image_stddev = cfg['image_stddev'] - - confidence_threshold = float(cfg['confidence_threshold']) - num_classes = int(cfg['num_classes']) - class_topic = str(cfg['class_topic']) - - image_input_topic = str(cfg['image_input_topic']) - camera_info_input_topic = str(cfg['camera_info_input_topic']) - - enable_visualizer = bool(cfg['enable_visualizer']) - visualized_image_topic = str(cfg['visualized_image_topic']) - class_names = cfg['class_names'] + 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', @@ -92,15 +50,15 @@ def _launch_setup(context, *args, **kwargs): name='image_format_converter', parameters=[ { - 'encoding_desired': encoding_desired, - 'image_width': input_image_width, - 'image_height': input_image_height, + 'encoding_desired': 'rgb8', + 'image_width': 640, + 'image_height': 640, 'input_qos': 'sensor_data', } ], remappings=[ - ('image_raw', image_input_topic), - ('image', CONVERTED_IMAGE_TOPIC), + ('image_raw', '/yolo_seg_mask'), + ('image', '/yolo_cls/internal/converted_image'), ], ) @@ -112,18 +70,18 @@ def _launch_setup(context, *args, **kwargs): { '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, - 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + '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', TENSOR_OUTPUT_TOPIC), - ('tensor_sub', TENSOR_INPUT_TOPIC), + ('tensor_pub', '/yolo_cls/tensor_pub'), + ('tensor_sub', '/yolo_cls/tensor_sub'), ], ) @@ -133,10 +91,10 @@ def _launch_setup(context, *args, **kwargs): plugin='nvidia::isaac_ros::yolov26_cls::YoloV26ClsDecoderNode', parameters=[ { - 'tensor_input_topic': TENSOR_INPUT_TOPIC, - 'confidence_threshold': confidence_threshold, - 'num_classes': num_classes, - 'class_topic': class_topic, + 'tensor_input_topic': '/yolo_cls/tensor_sub', + 'confidence_threshold': float(cfg['confidence_threshold']), + 'num_classes': int(cfg['num_classes']), + 'class_topic': '/classification_output', } ], ) @@ -156,64 +114,46 @@ def _launch_setup(context, *args, **kwargs): ) 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', - ) + os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py') ), launch_arguments={ - 'input_image_width': str(input_image_width), - 'input_image_height': str(input_image_height), - 'network_image_width': str(network_image_width), - 'network_image_height': str(network_image_height), - 'image_mean': str(image_mean), - 'image_stddev': str(image_stddev), + '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': DNN_IMAGE_ENCODER_NAMESPACE, - 'image_input_topic': CONVERTED_IMAGE_TOPIC, - 'camera_info_input_topic': camera_info_input_topic, - 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + '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] - if enable_visualizer: - actions.append( - Node( - package='isaac_ros_yolov26_cls', - executable='isaac_ros_yolov26_cls_visualizer.py', - name='yolo_cls_visualizer', - parameters=[ - { - 'class_topic': class_topic, - 'image_topic': ENCODER_RESIZE_TOPIC, - 'output_image_topic': visualized_image_topic, - 'class_names_yaml': str(class_names), - } - ], - ) + 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(): - pkg_dir = get_package_share_directory('perception_setup') - default_config = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_cls.yaml') - - return launch.LaunchDescription( - [ - DeclareLaunchArgument( - 'config_file', - default_value=default_config, - description='Path to YOLO classification pipeline config YAML', - ), - OpaqueFunction(function=_launch_setup), - ] - ) + 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 index f4ec5f6..34bff69 100644 --- a/perception_setup/launch/yolo/yolo_detect.launch.py +++ b/perception_setup/launch/yolo/yolo_detect.launch.py @@ -3,28 +3,28 @@ """Isaac ROS YOLOv8 TensorRT inference pipeline. 1. ImageFormatConverterNode - Input: image_input_topic - Output: /yolov8/internal/converted_image + Input: IMAGE_INPUT_TOPIC (launch arg) + Output: CONVERTED_IMAGE_TOPIC Purpose: Convert camera image to desired encoding (e.g. bgra8 -> rgb8) 2. DNNImageEncoderNode - Input: /yolov8/internal/converted_image - Output: /tensor_pub + Input: CONVERTED_IMAGE_TOPIC + Output: TENSOR_OUTPUT_TOPIC Purpose: Resize + normalize image and convert to network tensor 3. TensorRTNode - Input: /tensor_pub - Output: /tensor_sub + Input: TENSOR_OUTPUT_TOPIC + Output: TENSOR_INPUT_TOPIC Purpose: Run TensorRT inference on encoded tensor 4. YoloV8DecoderNode - Input: /tensor_sub - Output: /yolov8/internal/detection_topic + Input: TENSOR_INPUT_TOPIC + Output: DETECTION_TOPIC Purpose: Convert network output tensor -> Detection2DArray 5. (Optional) YOLOv8Visualizer - Input: /yolov8/internal/detection_topic + /yolov8/internal/converted_image - Output: overlay visualization + Input: DETECTION_TOPIC + encoder resize image + Output: VISUALIZED_IMAGE_TOPIC Purpose: Draw bounding boxes on image """ @@ -34,67 +34,23 @@ 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 -# Internal pipeline topics -CONVERTED_IMAGE_TOPIC = '/yolov8/internal/converted_image' -ENCODER_RESIZE_TOPIC = '/yolov8_encoder/internal/resize/image' -TENSOR_OUTPUT_TOPIC = '/yolov8/tensor_pub' -TENSOR_INPUT_TOPIC = '/yolov8/tensor_sub' -DNN_IMAGE_ENCODER_NAMESPACE = 'yolov8_encoder/internal' - def _launch_setup(context, *args, **kwargs): - config_path = LaunchConfiguration('config_file').perform(context) - + 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) - pkg_dir = get_package_share_directory('perception_setup') - - # Resolve camera topics from cameras.yaml (single source of truth) - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras[LaunchConfiguration('camera').perform(context)] - image_input_topic = cam['image_topic'] - camera_info_input_topic = cam['camera_info_topic'] - input_image_width = int(cam['image_width']) - input_image_height = int(cam['image_height']) - - model_file_path = str(cfg['model_file_path']) - engine_file_path = str(cfg['engine_file_path']) - - input_tensor_names = cfg['input_tensor_names'] - input_binding_names = cfg['input_binding_names'] - output_tensor_names = cfg['output_tensor_names'] - output_binding_names = cfg['output_binding_names'] - - verbose = bool(cfg['verbose']) - force_engine_update = bool(cfg['force_engine_update']) - - encoding_desired = str(cfg['encoding_desired']) - - network_image_width = int(cfg['network_image_width']) - network_image_height = int(cfg['network_image_height']) - image_mean = cfg['image_mean'] - image_stddev = cfg['image_stddev'] - - confidence_threshold = float(cfg['confidence_threshold']) - nms_threshold = float(cfg['nms_threshold']) - num_classes = int(cfg['num_classes']) - detection_topic = str(cfg['detection_topic']) - - enable_visualizer = bool(cfg['enable_visualizer']) - visualized_image_topic = str(cfg['visualized_image_topic']) - class_names = cfg['class_names'] + 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', @@ -102,15 +58,15 @@ def _launch_setup(context, *args, **kwargs): name='image_format_converter', parameters=[ { - 'encoding_desired': encoding_desired, - 'image_width': input_image_width, - 'image_height': input_image_height, + 'encoding_desired': 'rgb8', + 'image_width': 896, + 'image_height': 504, 'input_qos': 'sensor_data', } ], remappings=[ - ('image_raw', image_input_topic), - ('image', CONVERTED_IMAGE_TOPIC), + ('image_raw', '/realsense_d555/color/image_rect'), + ('image', '/yolov8/internal/converted_image'), ], ) @@ -122,18 +78,18 @@ def _launch_setup(context, *args, **kwargs): { '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, - 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + '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', TENSOR_OUTPUT_TOPIC), - ('tensor_sub', TENSOR_INPUT_TOPIC), + ('tensor_pub', '/yolov8/tensor_pub'), + ('tensor_sub', '/yolov8/tensor_sub'), ], ) @@ -143,11 +99,11 @@ def _launch_setup(context, *args, **kwargs): plugin='nvidia::isaac_ros::yolov8::YoloV8DecoderNode', parameters=[ { - 'tensor_input_topic': TENSOR_INPUT_TOPIC, - 'confidence_threshold': confidence_threshold, - 'nms_threshold': nms_threshold, - 'num_classes': num_classes, - 'detections_topic': detection_topic, + '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', } ], ) @@ -167,72 +123,46 @@ def _launch_setup(context, *args, **kwargs): ) 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', - ) + os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py') ), launch_arguments={ - 'input_image_width': str(input_image_width), - 'input_image_height': str(input_image_height), - 'network_image_width': str(network_image_width), - 'network_image_height': str(network_image_height), - 'image_mean': str(image_mean), - 'image_stddev': str(image_stddev), + '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': DNN_IMAGE_ENCODER_NAMESPACE, - 'image_input_topic': CONVERTED_IMAGE_TOPIC, - 'camera_info_input_topic': camera_info_input_topic, - 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + '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, - ] - - if enable_visualizer: - actions.append( - Node( - package='isaac_ros_yolov8', - executable='isaac_ros_yolov8_visualizer.py', - name='yolov8_visualizer', - parameters=[ - { - 'detections_topic': detection_topic, - 'image_topic': ENCODER_RESIZE_TOPIC, - 'output_image_topic': visualized_image_topic, - 'class_names_yaml': str(class_names), - } - ], - ) + 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(): - pkg_dir = get_package_share_directory('perception_setup') - default_config = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_detect.yaml') - - return launch.LaunchDescription( - [ - DeclareLaunchArgument( - 'config_file', - default_value=default_config, - description='Path to YOLO pipeline config YAML', - ), - DeclareLaunchArgument( - 'camera', - default_value='realsense_d555', - description='Camera key in cameras.yaml', - ), - OpaqueFunction(function=_launch_setup), - ] - ) + 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 index e3dcc07..0af1ac7 100644 --- a/perception_setup/launch/yolo/yolo_obb.launch.py +++ b/perception_setup/launch/yolo/yolo_obb.launch.py @@ -3,26 +3,21 @@ """Isaac ROS YOLO-OBB TensorRT inference pipeline. 1. ImageFormatConverterNode - Input: image_input_topic - Output: /yolo_obb/internal/converted_image - Purpose: Convert camera image encoding (e.g. bgra8 -> rgb8) + Input: IMAGE_INPUT_TOPIC (launch arg) + Output: CONVERTED_IMAGE_TOPIC 2. DNNImageEncoderNode - Input: /yolo_obb/internal/converted_image - Output: /tensor_pub - Purpose: Resize + normalize image and convert to network tensor + Input: CONVERTED_IMAGE_TOPIC + Output: TENSOR_OUTPUT_TOPIC 3. TensorRTNode - Input: /tensor_pub - Output: /tensor_sub - Purpose: Run TensorRT inference + Input: TENSOR_OUTPUT_TOPIC + Output: TENSOR_INPUT_TOPIC Expected output tensor shape: [1, num_detections, 7] 4. YoloV26OBBDecoderNode - Input: /tensor_sub - Output: detection_topic (Detection2DArray with BoundingBox2D.center.theta = OBB angle) - Purpose: Filter by confidence and publish OBB detections - Note: NMS is embedded in the model - no NMS is applied here. + Input: TENSOR_INPUT_TOPIC + Output: DETECTION_TOPIC (Detection2DArray with BoundingBox2D.center.theta = OBB angle) """ import os @@ -31,67 +26,23 @@ 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 -CONVERTED_IMAGE_TOPIC = '/yolo_obb/internal/converted_image' -ENCODER_RESIZE_TOPIC = '/yolo_obb_encoder/internal/resize/image' -TENSOR_OUTPUT_TOPIC = '/yolo_obb/tensor_pub' -TENSOR_INPUT_TOPIC = '/yolo_obb/tensor_sub' -DNN_IMAGE_ENCODER_NAMESPACE = 'yolo_obb_encoder/internal' - def _launch_setup(context, *args, **kwargs): - config_path = LaunchConfiguration('config_file').perform(context) - - with open(config_path) as f: - cfg = yaml.safe_load(f) - pkg_dir = get_package_share_directory('perception_setup') - - # Resolve camera topics from cameras.yaml (single source of truth) - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras[LaunchConfiguration('camera').perform(context)] - image_input_topic = cam['image_topic'] - camera_info_input_topic = cam['camera_info_topic'] - input_image_width = int(cam['image_width']) - input_image_height = int(cam['image_height']) - 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, str(cfg['model_file_path'])) - engine_file_path = os.path.join(models_dir, str(cfg['engine_file_path'])) - - input_tensor_names = cfg['input_tensor_names'] - input_binding_names = cfg['input_binding_names'] - output_tensor_names = cfg['output_tensor_names'] - output_binding_names = cfg['output_binding_names'] - - verbose = bool(cfg['verbose']) - force_engine_update = bool(cfg['force_engine_update']) - - encoding_desired = str(cfg['encoding_desired']) - - network_image_width = int(cfg['network_image_width']) - network_image_height = int(cfg['network_image_height']) - image_mean = cfg['image_mean'] - image_stddev = cfg['image_stddev'] - - confidence_threshold = float(cfg['confidence_threshold']) - num_detections = int(cfg['num_detections']) - detection_topic = str(cfg['detection_topic']) - - enable_visualizer = bool(cfg['enable_visualizer']) - visualized_image_topic = str(cfg['visualized_image_topic']) - class_names = cfg['class_names'] + 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', @@ -99,15 +50,15 @@ def _launch_setup(context, *args, **kwargs): name='image_format_converter', parameters=[ { - 'encoding_desired': encoding_desired, - 'image_width': input_image_width, - 'image_height': input_image_height, + 'encoding_desired': 'rgb8', + 'image_width': 896, + 'image_height': 504, 'input_qos': 'sensor_data', } ], remappings=[ - ('image_raw', image_input_topic), - ('image', CONVERTED_IMAGE_TOPIC), + ('image_raw', '/realsense_d555/color/image_rect'), + ('image', '/yolo_obb/internal/converted_image'), ], ) @@ -119,18 +70,18 @@ def _launch_setup(context, *args, **kwargs): { '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, - 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + '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', TENSOR_OUTPUT_TOPIC), - ('tensor_sub', TENSOR_INPUT_TOPIC), + ('tensor_pub', '/yolo_obb/tensor_pub'), + ('tensor_sub', '/yolo_obb/tensor_sub'), ], ) @@ -140,10 +91,10 @@ def _launch_setup(context, *args, **kwargs): plugin='nvidia::isaac_ros::yolov26_obb::YoloV26OBBDecoderNode', parameters=[ { - 'tensor_input_topic': TENSOR_INPUT_TOPIC, - 'confidence_threshold': confidence_threshold, - 'num_detections': num_detections, - 'detections_topic': detection_topic, + 'tensor_input_topic': '/yolo_obb/tensor_sub', + 'confidence_threshold': float(cfg['confidence_threshold']), + 'num_detections': int(cfg['num_detections']), + 'detections_topic': '/obb_detections_output', } ], ) @@ -163,69 +114,46 @@ def _launch_setup(context, *args, **kwargs): ) 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', - ) + os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py') ), launch_arguments={ - 'input_image_width': str(input_image_width), - 'input_image_height': str(input_image_height), - 'network_image_width': str(network_image_width), - 'network_image_height': str(network_image_height), - 'image_mean': str(image_mean), - 'image_stddev': str(image_stddev), + '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': DNN_IMAGE_ENCODER_NAMESPACE, - 'image_input_topic': CONVERTED_IMAGE_TOPIC, - 'camera_info_input_topic': camera_info_input_topic, - 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + '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] - if enable_visualizer: - actions.append( - Node( - package='isaac_ros_yolov26_obb', - executable='isaac_ros_yolov26_obb_visualizer.py', - name='yolo_obb_visualizer', - parameters=[ - { - 'detections_topic': detection_topic, - 'image_topic': ENCODER_RESIZE_TOPIC, - 'output_image_topic': visualized_image_topic, - 'class_names_yaml': str(class_names), - } - ], - ) + 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(): - pkg_dir = get_package_share_directory('perception_setup') - default_config = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_obb.yaml') - - return launch.LaunchDescription( - [ - DeclareLaunchArgument( - 'config_file', - default_value=default_config, - description='Path to YOLO OBB pipeline config YAML', - ), - DeclareLaunchArgument( - 'camera', - default_value='realsense_d555', - description='Camera key in cameras.yaml', - ), - OpaqueFunction(function=_launch_setup), - ] - ) + 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 index c80b0b3..2fea4b3 100644 --- a/perception_setup/launch/yolo/yolo_seg.launch.py +++ b/perception_setup/launch/yolo/yolo_seg.launch.py @@ -3,33 +3,25 @@ """Isaac ROS YOLO Instance-Segmentation TensorRT inference pipeline. 1. ImageFormatConverterNode - Input: image_input_topic - Output: /yolo_seg/internal/converted_image - Purpose: Convert camera image encoding (e.g. bgra8 -> rgb8) + Input: IMAGE_INPUT_TOPIC (launch arg) + Output: CONVERTED_IMAGE_TOPIC 2. DNNImageEncoderNode - Input: /yolo_seg/internal/converted_image - Output: /tensor_pub - Purpose: Resize + normalize image and convert to network tensor + Input: CONVERTED_IMAGE_TOPIC + Output: TENSOR_OUTPUT_TOPIC 3. TensorRTNode - Input: /tensor_pub - Output: /tensor_sub - Purpose: Run TensorRT inference + 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_sub (NitrosTensorList with both output tensors) - Output: detection_topic (Detection2DArray) - mask_topic (mono8 Image - combined binary mask) - Purpose: Decode detections and compute instance segmentation mask - -5. (Optional) YoloSegVisualizer - Input: detection_topic + encoder resize image + mask_topic - Output: overlay visualization - Purpose: Draw mask overlay and bounding boxes on image + Input: TENSOR_INPUT_TOPIC + Output: DETECTION_TOPIC (Detection2DArray), MASK_TOPIC (mono8 Image) + +5. (Optional) YoloSegVisualizer -> VISUALIZED_IMAGE_TOPIC """ import os @@ -38,73 +30,23 @@ 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 -CONVERTED_IMAGE_TOPIC = '/yolo_seg/internal/converted_image' -ENCODER_RESIZE_TOPIC = '/yolo_seg_encoder/internal/resize/image' -TENSOR_OUTPUT_TOPIC = '/yolo_seg/tensor_pub' -TENSOR_INPUT_TOPIC = '/yolo_seg/tensor_sub' -DNN_IMAGE_ENCODER_NAMESPACE = 'yolo_seg_encoder/internal' - def _launch_setup(context, *args, **kwargs): - config_path = LaunchConfiguration('config_file').perform(context) - - with open(config_path) as f: - cfg = yaml.safe_load(f) - pkg_dir = get_package_share_directory('perception_setup') - - # Resolve camera topics from cameras.yaml (single source of truth) - cameras_path = os.path.join(pkg_dir, 'config', 'cameras', 'cameras.yaml') - with open(cameras_path) as f: - cameras = yaml.safe_load(f) - cam = cameras[LaunchConfiguration('camera').perform(context)] - image_input_topic = cam['image_topic'] - camera_info_input_topic = cam['camera_info_topic'] - input_image_width = int(cam['image_width']) - input_image_height = int(cam['image_height']) - 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, str(cfg['model_file_path'])) - engine_file_path = os.path.join(models_dir, str(cfg['engine_file_path'])) - - input_tensor_names = cfg['input_tensor_names'] - input_binding_names = cfg['input_binding_names'] - output_tensor_names = cfg['output_tensor_names'] - output_binding_names = cfg['output_binding_names'] - - verbose = bool(cfg['verbose']) - force_engine_update = bool(cfg['force_engine_update']) - - encoding_desired = str(cfg['encoding_desired']) - - network_image_width = int(cfg['network_image_width']) - network_image_height = int(cfg['network_image_height']) - image_mean = cfg['image_mean'] - image_stddev = cfg['image_stddev'] - - 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']) - output_mask_width = int(cfg['output_mask_width']) - output_mask_height = int(cfg['output_mask_height']) - detection_topic = str(cfg['detection_topic']) - mask_topic = str(cfg['mask_topic']) - - enable_visualizer = bool(cfg['enable_visualizer']) - visualized_image_topic = str(cfg['visualized_image_topic']) - class_names = cfg['class_names'] + 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', @@ -112,15 +54,15 @@ def _launch_setup(context, *args, **kwargs): name='image_format_converter', parameters=[ { - 'encoding_desired': encoding_desired, - 'image_width': input_image_width, - 'image_height': input_image_height, + 'encoding_desired': 'rgb8', + 'image_width': 720, + 'image_height': 540, 'input_qos': 'sensor_data', } ], remappings=[ - ('image_raw', image_input_topic), - ('image', CONVERTED_IMAGE_TOPIC), + ('image_raw', '/blackfly_s/image_raw'), + ('image', '/yolo_seg/internal/converted_image'), ], ) @@ -132,18 +74,18 @@ def _launch_setup(context, *args, **kwargs): { '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, - 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + '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', TENSOR_OUTPUT_TOPIC), - ('tensor_sub', TENSOR_INPUT_TOPIC), + ('tensor_pub', '/yolo_seg/tensor_pub'), + ('tensor_sub', '/yolo_seg/tensor_sub'), ], ) @@ -153,18 +95,18 @@ def _launch_setup(context, *args, **kwargs): plugin='nvidia::isaac_ros::yolov26_seg::YoloV26SegDecoderNode', parameters=[ { - 'tensor_input_topic': TENSOR_INPUT_TOPIC, - 'confidence_threshold': confidence_threshold, - 'num_detections': num_detections, - 'mask_width': mask_width, - 'mask_height': mask_height, - 'num_protos': num_protos, - 'network_image_width': network_image_width, - 'network_image_height': network_image_height, - 'output_mask_width': output_mask_width, - 'output_mask_height': output_mask_height, - 'detections_topic': detection_topic, - 'mask_topic': mask_topic, + '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', } ], ) @@ -184,70 +126,47 @@ def _launch_setup(context, *args, **kwargs): ) 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', - ) + os.path.join(encoder_dir, 'launch', 'dnn_image_encoder.launch.py') ), launch_arguments={ - 'input_image_width': str(input_image_width), - 'input_image_height': str(input_image_height), - 'network_image_width': str(network_image_width), - 'network_image_height': str(network_image_height), - 'image_mean': str(image_mean), - 'image_stddev': str(image_stddev), + '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': DNN_IMAGE_ENCODER_NAMESPACE, - 'image_input_topic': CONVERTED_IMAGE_TOPIC, - 'camera_info_input_topic': camera_info_input_topic, - 'tensor_output_topic': TENSOR_OUTPUT_TOPIC, + '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] - if enable_visualizer: - actions.append( - Node( - package='isaac_ros_yolov26_seg', - executable='isaac_ros_yolov26_seg_visualizer.py', - name='yolo_seg_visualizer', - parameters=[ - { - 'detections_topic': detection_topic, - 'image_topic': ENCODER_RESIZE_TOPIC, - 'mask_topic': mask_topic, - 'output_image_topic': visualized_image_topic, - 'class_names_yaml': str(class_names), - } - ], - ) + 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(): - pkg_dir = get_package_share_directory('perception_setup') - default_config = os.path.join(pkg_dir, 'config', 'yolo', 'yolo_seg.yaml') - - return launch.LaunchDescription( - [ - DeclareLaunchArgument( - 'config_file', - default_value=default_config, - description='Path to YOLO segmentation pipeline config YAML', - ), - DeclareLaunchArgument( - 'camera', - default_value='blackfly_s', - description='Camera key in cameras.yaml', - ), - OpaqueFunction(function=_launch_setup), - ] - ) + return launch.LaunchDescription([OpaqueFunction(function=_launch_setup)]) From 88b8facfdb23d05323086c928df0e5c25086712c Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Fri, 17 Apr 2026 17:27:20 +0200 Subject: [PATCH 33/37] docs: update perception_setup readme --- perception_setup/README.md | 28 +++++++++++++++++++++------- 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/perception_setup/README.md b/perception_setup/README.md index 9503f32..d13c4cf 100644 --- a/perception_setup/README.md +++ b/perception_setup/README.md @@ -1,8 +1,14 @@ # perception_setup -Launch and configuration package for the perception pipeline. Contains camera drivers, image preprocessing (undistortion), YOLO inference pipelines, and mission-specific launch files. +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. -**Convention:** launch files own all topics, TF frames, and image dimensions (as module-level `UPPER_CASE` constants at the top of each file). Config YAMLs hold only model/algorithm tuning — thresholds, tensor names, model paths, class names, etc. This avoids having the same value declared in two places. +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. + +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. ## Package structure @@ -23,11 +29,16 @@ perception_setup/ 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 @@ -74,7 +85,7 @@ This package provides one C++ composable node: ### valve_intervention.launch.py -Full valve detection pipeline: RealSense D555 -> image undistortion -> YOLO OBB -> valve pose estimation. +Full valve detection pipeline used in missions: RealSense D555 -> image undistortion -> YOLO OBB -> valve pose estimation. 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 @@ -119,14 +130,17 @@ ros2 launch perception_setup realsense_d555.launch.py ## Configuration -### Topics and frames - -Topics and TF frame names are defined as `UPPER_CASE` constants at the top of each launch file. The standalone YOLO launch files (`yolo/yolo_*.launch.py`) expose `image_input_topic`, `camera_info_input_topic`, `input_image_width` and `input_image_height` as launch arguments so they can be wired to any upstream source. - ### 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 From 3485462eab79bcb7066a8f9cbff450fd6753cdc3 Mon Sep 17 00:00:00 2001 From: vortexuser Date: Wed, 22 Apr 2026 14:10:41 +0000 Subject: [PATCH 34/37] add stuff --- ...ltralytics_pipeline_line_fitting.launch.py | 6 +- .../ultralytics_valve_detection.launch.py | 81 ++++++++++++++----- 2 files changed, 66 insertions(+), 21 deletions(-) diff --git a/perception_setup/launch/ultralytics/ultralytics_pipeline_line_fitting.launch.py b/perception_setup/launch/ultralytics/ultralytics_pipeline_line_fitting.launch.py index 0ff9ada..8c21eb8 100644 --- a/perception_setup/launch/ultralytics/ultralytics_pipeline_line_fitting.launch.py +++ b/perception_setup/launch/ultralytics/ultralytics_pipeline_line_fitting.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): yolo_params = os.path.join( - get_package_share_directory('vortex_image_segmentation'), + get_package_share_directory('yolo_segmentation'), 'params', 'yolo_params.yaml', ) @@ -22,7 +22,7 @@ def generate_launch_description(): return LaunchDescription([ # YOLO segmentation node Node( - package='vortex_image_segmentation', + package='yolo_segmentation', executable='yolo_seg_node', name='yolo_segmentation_node', output='screen', @@ -37,7 +37,7 @@ def generate_launch_description(): output='screen', parameters=[ { - 'model_path': '/home/gard/ros2_ws/src/vortex-deep-learning-pipelines/runs/classify/results/classify-20260304-154252/weights/best.pt' + 'model_path': 'best.pt' } ], ), diff --git a/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py b/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py index d80a196..4a38f54 100644 --- a/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py +++ b/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Simulator -> Ultralytics YOLO OBB (Python) -> Valve Detection. +"""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 @@ -15,6 +15,7 @@ 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 @@ -32,22 +33,30 @@ 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, 'obb_best_simulator.pt') + model_file_path = os.path.join(models_dir, 'simulator_valve_handle_large.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 = '/valve_landmarks' + typed_landmarks_topic = '/valve_landmarks_typed' + yolo_node = Node( package='yolo_obb_object_detection', - executable='yolo_obb_object_detection_node.py', + executable='yolo_obb_object_detection_node', name='yolo_obb_object_detection', output='screen', parameters=[ { 'device': device, - 'yolo_model': model_file_path, - 'model_conf': 0.4, - 'color_image_sub_topic': '/nautilus/front_camera/image_color', - 'yolo_detections_pub_topic': '/ultralytics_valve_detection/detections', + 'model_path': model_file_path, + 'confidence_threshold': 0.4, + 'input_topic': '/nautilus/front_camera/image_color', + 'output_detections_topic': detections_topic, + 'output_annotated_topic': '/ultralytics_valve_detection/annotated_image', }, ], ) @@ -63,25 +72,28 @@ def _launch_setup(context, *args, **kwargs): 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': '/ultralytics_valve_detection/detections', - 'yolo_img_width': 1920, - 'yolo_img_height': 1080, + 'detections_sub_topic': detections_topic, 'depth_image_sub_topic': '/nautilus/depth_camera/image_depth', 'depth_image_info_topic': '/nautilus/depth_camera/camera_info', 'color_image_info_topic': '/nautilus/front_camera/camera_info', 'depth_frame_id': 'front_camera_depth_optical', 'color_frame_id': 'front_camera_color_optical', - 'landmarks_pub_topic': '/nautilus/landmarks', + 'landmarks_pub_topic': raw_landmarks_topic, 'output_frame_id': 'front_camera_depth_optical', - 'drone': 'nautilus', + 'drone': drone, 'undistort_detections': False, + # yolo_obb_object_detection publishes detections in + # original image coordinates, not letterbox space. + 'detections_letterboxed': False, 'debug_visualize': True, - 'clamp_rotation': True, - 'use_hardcoded_extrinsic': True, - 'extrinsic_tx': -0.059, - 'extrinsic_ty': 0.0, - 'extrinsic_tz': 0.0, + 'use_hardcoded_extrinsic': False, + 'clamp_yaw': True, }, ], ) @@ -89,7 +101,27 @@ def _launch_setup(context, *args, **kwargs): output='screen', ) - return [yolo_node, valve_container] + 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, subtype_resolver_node] def generate_launch_description(): @@ -100,6 +132,19 @@ def generate_launch_description(): 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), ] ) From 1f4e2c281ffcb355e1478e19c3b79ba3d567ebea Mon Sep 17 00:00:00 2001 From: vortexuser Date: Wed, 22 Apr 2026 20:18:57 +0000 Subject: [PATCH 35/37] chore: update threshold values in ultralytics valve detection launch file --- .../ultralytics/ultralytics_valve_detection.launch.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py b/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py index 4a38f54..dfdbb21 100644 --- a/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py +++ b/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py @@ -41,8 +41,8 @@ def _launch_setup(context, *args, **kwargs): clamp_yaw = LaunchConfiguration('clamp_yaw') detections_topic = '/ultralytics_valve_detection/detections' - raw_landmarks_topic = '/valve_landmarks' - typed_landmarks_topic = '/valve_landmarks_typed' + raw_landmarks_topic = '/nautilus/landmarks' + typed_landmarks_topic = '/nautilus/landmarks_test' yolo_node = Node( package='yolo_obb_object_detection', @@ -53,7 +53,7 @@ def _launch_setup(context, *args, **kwargs): { 'device': device, 'model_path': model_file_path, - 'confidence_threshold': 0.4, + 'confidence_threshold': 0.6, 'input_topic': '/nautilus/front_camera/image_color', 'output_detections_topic': detections_topic, 'output_annotated_topic': '/ultralytics_valve_detection/annotated_image', From 3b17c7294558010c4348b7e96ba4405ee64c880f Mon Sep 17 00:00:00 2001 From: vortexuser Date: Fri, 24 Apr 2026 10:23:09 +0000 Subject: [PATCH 36/37] feat: add drone configuration to launch files and update topic remappings for camera nodes --- .../launch/cameras/blackfly_s.launch.py | 15 ++++- .../launch/cameras/realsense_d555.launch.py | 17 ++++-- .../launch/docking_blackfly_s.launch.py | 17 +++++- .../launch/docking_realsense_d555.launch.py | 21 ++++--- .../launch/pipeline_blackfly_s.launch.py | 16 ++++- .../launch/pipeline_realsense_d555.launch.py | 14 +++-- .../ultralytics_valve_detection.launch.py | 60 +++++++++---------- .../launch/valve_intervention.launch.py | 17 ++++-- .../launch/visual_inspection.launch.py | 15 +++-- 9 files changed, 128 insertions(+), 64 deletions(-) diff --git a/perception_setup/launch/cameras/blackfly_s.launch.py b/perception_setup/launch/cameras/blackfly_s.launch.py index ff04841..55b1a5a 100644 --- a/perception_setup/launch/cameras/blackfly_s.launch.py +++ b/perception_setup/launch/cameras/blackfly_s.launch.py @@ -25,6 +25,8 @@ def generate_launch_description(): 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='', @@ -43,7 +45,11 @@ def generate_launch_description(): 'camerainfo_url': f'file://{calib_path}', }, ], - remappings=[('~/control', '/exposure_control/control')], + 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')), ) @@ -58,7 +64,7 @@ def generate_launch_description(): additional_env={'EGL_PLATFORM': 'surfaceless'}, parameters=[ { - 'input_topic': '/blackfly_s/image_raw', + 'input_topic': ['/', drone, '/down_camera/image_color'], 'host': '10.0.0.68', 'port': 5001, 'bitrate': 500000, @@ -76,6 +82,11 @@ 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', diff --git a/perception_setup/launch/cameras/realsense_d555.launch.py b/perception_setup/launch/cameras/realsense_d555.launch.py index 795e07d..061fd9c 100644 --- a/perception_setup/launch/cameras/realsense_d555.launch.py +++ b/perception_setup/launch/cameras/realsense_d555.launch.py @@ -22,6 +22,8 @@ def generate_launch_description(): pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml' ) + drone = LaunchConfiguration('drone') + realsense_node = Node( package='realsense2_camera', executable='realsense2_camera_node', @@ -49,8 +51,8 @@ def generate_launch_description(): } ], remappings=[ - ('/camera/camera/depth/image_rect_raw', '/camera/camera/depth/image_rect_raw'), - ('/camera/camera/depth/camera_info', '/camera/camera/depth/camera_info'), + ('/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'], @@ -72,8 +74,8 @@ def generate_launch_description(): 'image_topic': '/camera/camera/color/image_raw', 'camera_info_file': calib_file, 'raw_camera_info_topic': '/camera/camera/color/camera_info', - 'output_image_topic': '/realsense_d555/color/image_rect', - 'output_camera_info_topic': '/realsense_d555/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', } @@ -90,7 +92,7 @@ def generate_launch_description(): additional_env={'EGL_PLATFORM': 'surfaceless'}, parameters=[ { - 'input_topic': '/realsense_d555/color/image_rect', + 'input_topic': ['/', drone, '/front_camera/image_color'], 'host': '10.0.0.169', 'port': 5000, 'bitrate': 500000, @@ -108,6 +110,11 @@ 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', diff --git a/perception_setup/launch/docking_blackfly_s.launch.py b/perception_setup/launch/docking_blackfly_s.launch.py index 9403806..fdee423 100644 --- a/perception_setup/launch/docking_blackfly_s.launch.py +++ b/perception_setup/launch/docking_blackfly_s.launch.py @@ -26,6 +26,8 @@ def generate_launch_description(): 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='', @@ -44,7 +46,11 @@ def generate_launch_description(): 'camerainfo_url': f'file://{calib_path}', }, ], - remappings=[('~/control', '/exposure_control/control')], + 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')), ), @@ -59,8 +65,8 @@ def generate_launch_description(): 'aruco_detector_params.yaml', ), { - 'subs.image_topic': '/blackfly_s/image_raw', - 'subs.camera_info_topic': '/blackfly_s/camera_info', + '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', }, @@ -96,6 +102,11 @@ 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', diff --git a/perception_setup/launch/docking_realsense_d555.launch.py b/perception_setup/launch/docking_realsense_d555.launch.py index c551d3a..9999f22 100644 --- a/perception_setup/launch/docking_realsense_d555.launch.py +++ b/perception_setup/launch/docking_realsense_d555.launch.py @@ -24,6 +24,8 @@ def generate_launch_description(): pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml' ) + drone = LaunchConfiguration('drone') + docking_container = ComposableNodeContainer( name='docking_realsense_d555_container', namespace='', @@ -61,8 +63,8 @@ def generate_launch_description(): 'image_topic': '/camera/camera/color/image_raw', 'camera_info_file': calib_file, 'raw_camera_info_topic': '/camera/camera/color/camera_info', - 'output_image_topic': '/realsense_d555/color/image_rect', - 'output_camera_info_topic': '/realsense_d555/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', } @@ -79,10 +81,10 @@ def generate_launch_description(): 'aruco_detector_params.yaml', ), { - 'subs.image_topic': '/realsense_d555/color/image_rect', - 'subs.camera_info_topic': '/realsense_d555/color/camera_info', - 'pubs.aruco_image': '/forward_cam/aruco_detector/image', - 'out_tf_frame': 'nautilus/front_camera_optical', + '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'], }, ], ), @@ -98,7 +100,7 @@ def generate_launch_description(): additional_env={'EGL_PLATFORM': 'surfaceless'}, parameters=[ { - 'input_topic': '/forward_cam/aruco_detector/image', + 'input_topic': '/front_camera/aruco_detector/image', 'host': '10.0.0.169', 'port': 5001, 'bitrate': 500000, @@ -116,6 +118,11 @@ 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', diff --git a/perception_setup/launch/pipeline_blackfly_s.launch.py b/perception_setup/launch/pipeline_blackfly_s.launch.py index 6bef8ab..543ef6b 100644 --- a/perception_setup/launch/pipeline_blackfly_s.launch.py +++ b/perception_setup/launch/pipeline_blackfly_s.launch.py @@ -29,6 +29,7 @@ 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) @@ -63,7 +64,11 @@ def _launch_setup(context, *args, **kwargs): 'reliable_qos': True, }, ], - remappings=[('~/control', '/exposure_control/control')], + 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')), ), @@ -88,7 +93,7 @@ def _launch_setup(context, *args, **kwargs): } ], remappings=[ - ('image_raw', '/blackfly_s/image_raw'), + ('image_raw', f'/{drone}/down_camera/image_color'), ('image', '/down_cam/yolo_seg/internal/converted_image'), ], ) @@ -167,7 +172,7 @@ def _launch_setup(context, *args, **kwargs): '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': '/blackfly_s/camera_info', + 'camera_info_input_topic': f'/{drone}/down_camera/camera_info', 'tensor_output_topic': '/down_cam/yolo_seg/tensor_pub', }.items(), ) @@ -337,6 +342,11 @@ def _launch_setup(context, *args, **kwargs): 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', diff --git a/perception_setup/launch/pipeline_realsense_d555.launch.py b/perception_setup/launch/pipeline_realsense_d555.launch.py index e2098b4..9bbe8a1 100644 --- a/perception_setup/launch/pipeline_realsense_d555.launch.py +++ b/perception_setup/launch/pipeline_realsense_d555.launch.py @@ -28,6 +28,7 @@ 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) @@ -76,8 +77,8 @@ def _launch_setup(context, *args, **kwargs): 'image_topic': '/camera/camera/color/image_raw', 'camera_info_file': calib_file, 'raw_camera_info_topic': '/camera/camera/color/camera_info', - 'output_image_topic': '/realsense_d555/color/image_rect', - 'output_camera_info_topic': '/realsense_d555/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', } @@ -104,7 +105,7 @@ def _launch_setup(context, *args, **kwargs): } ], remappings=[ - ('image_raw', '/realsense_d555/color/image_rect'), + ('image_raw', f'/{drone}/front_camera/image_color'), ('image', '/front_cam/yolo_seg/internal/converted_image'), ], ) @@ -183,7 +184,7 @@ def _launch_setup(context, *args, **kwargs): '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': '/realsense_d555/color/camera_info', + 'camera_info_input_topic': f'/{drone}/front_camera/camera_info', 'tensor_output_topic': '/front_cam/yolo_seg/tensor_pub', }.items(), ) @@ -244,6 +245,11 @@ def _launch_setup(context, *args, **kwargs): 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', diff --git a/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py b/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py index dfdbb21..50fe922 100644 --- a/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py +++ b/perception_setup/launch/ultralytics/ultralytics_valve_detection.launch.py @@ -33,7 +33,7 @@ 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, 'simulator_valve_handle_large.pt') + model_file_path = os.path.join(models_dir, 'best_valve_handle_real_and_sim.pt') device = LaunchConfiguration('device').perform(context) _validate_device(device) @@ -53,8 +53,8 @@ def _launch_setup(context, *args, **kwargs): { 'device': device, 'model_path': model_file_path, - 'confidence_threshold': 0.6, - 'input_topic': '/nautilus/front_camera/image_color', + 'confidence_threshold': 0.7, + 'input_topic': '/camera/camera/color/image_raw', 'output_detections_topic': detections_topic, 'output_annotated_topic': '/ultralytics_valve_detection/annotated_image', }, @@ -79,21 +79,21 @@ def _launch_setup(context, *args, **kwargs): ), { 'detections_sub_topic': detections_topic, - 'depth_image_sub_topic': '/nautilus/depth_camera/image_depth', - 'depth_image_info_topic': '/nautilus/depth_camera/camera_info', - 'color_image_info_topic': '/nautilus/front_camera/camera_info', + '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': False, + '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': False, - 'clamp_yaw': True, + 'use_hardcoded_extrinsic': True, + 'clamp_yaw': False, }, ], ) @@ -101,27 +101,27 @@ def _launch_setup(context, *args, **kwargs): 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, subtype_resolver_node] + # 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(): diff --git a/perception_setup/launch/valve_intervention.launch.py b/perception_setup/launch/valve_intervention.launch.py index 331af49..d7a7fe6 100644 --- a/perception_setup/launch/valve_intervention.launch.py +++ b/perception_setup/launch/valve_intervention.launch.py @@ -26,6 +26,7 @@ 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) @@ -61,6 +62,10 @@ def _launch_setup(context, *args, **kwargs): '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( @@ -72,8 +77,8 @@ def _launch_setup(context, *args, **kwargs): 'image_topic': '/camera/camera/color/image_raw', 'camera_info_file': calib_file, 'raw_camera_info_topic': '/camera/camera/color/camera_info', - 'output_image_topic': '/realsense_d555/color/image_rect', - 'output_camera_info_topic': '/realsense_d555/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 } @@ -93,7 +98,7 @@ def _launch_setup(context, *args, **kwargs): } ], remappings=[ - ('image_raw', '/realsense_d555/color/image_rect'), + ('image_raw', f'/{drone}/front_camera/image_color'), ('image', '/yolo_obb/internal/converted_image'), ], ) @@ -167,7 +172,7 @@ def _launch_setup(context, *args, **kwargs): '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', + 'camera_info_input_topic': f'/{drone}/front_camera/camera_info', 'tensor_output_topic': '/yolo_obb/tensor_pub', }.items(), ) @@ -205,9 +210,9 @@ def _launch_setup(context, *args, **kwargs): parameters=[ valve_detection_tuning, { - 'depth_image_sub_topic': '/camera/camera/depth/image_rect_raw', + 'depth_image_sub_topic': f'/{drone}/depth_camera/image_depth', 'detections_sub_topic': '/obb_detections_output', - 'depth_image_info_topic': '/camera/camera/depth/camera_info', + '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', diff --git a/perception_setup/launch/visual_inspection.launch.py b/perception_setup/launch/visual_inspection.launch.py index 268f6a9..eb95686 100644 --- a/perception_setup/launch/visual_inspection.launch.py +++ b/perception_setup/launch/visual_inspection.launch.py @@ -24,6 +24,8 @@ def generate_launch_description(): pkg_dir, 'config', 'cameras', 'color_realsense_d555_calib.yaml' ) + drone = LaunchConfiguration('drone') + visual_inspection_container = ComposableNodeContainer( name='visual_inspection_container', namespace='', @@ -61,8 +63,8 @@ def generate_launch_description(): 'image_topic': '/camera/camera/color/image_raw', 'camera_info_file': calib_file, 'raw_camera_info_topic': '/camera/camera/color/camera_info', - 'output_image_topic': '/realsense_d555/color/image_rect', - 'output_camera_info_topic': '/realsense_d555/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', } @@ -79,7 +81,7 @@ def generate_launch_description(): 'image_filtering_params.yaml', ), { - 'sub_topic': '/realsense_d555/color/image_rect', + 'sub_topic': ['/', drone, '/front_camera/image_color'], 'pub_topic': '/visual_inspection/filtered_image', 'input_encoding': 'rgb8', 'output_encoding': 'rgb8', @@ -99,7 +101,7 @@ def generate_launch_description(): ), { 'subs.image_topic': '/visual_inspection/filtered_image', - 'subs.camera_info_topic': '/realsense_d555/color/camera_info', + 'subs.camera_info_topic': ['/', drone, '/front_camera/camera_info'], 'detect_board': False, 'publish_landmarks': False, }, @@ -135,6 +137,11 @@ 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', From d58d92b6149d4b496ecfe06eab8861ed56bf096f Mon Sep 17 00:00:00 2001 From: vortexuser Date: Fri, 24 Apr 2026 10:24:18 +0000 Subject: [PATCH 37/37] refactor: apply pre-commit changes --- .../launch/cameras/blackfly_s.launch.py | 7 +- .../launch/cameras/realsense_d555.launch.py | 17 ++- .../launch/cameras/sonar.launch.py | 1 + .../launch/docking_blackfly_s.launch.py | 13 +- .../launch/docking_realsense_d555.launch.py | 12 +- .../isaac_ros_valve_intervention.launch.py | 2 +- .../launch/pipeline_blackfly_s.launch.py | 10 +- ...ltralytics_pipeline_line_fitting.launch.py | 116 ++++++++---------- .../launch/valve_intervention.launch.py | 7 +- .../launch/visual_inspection.launch.py | 12 +- .../launch/yolo/yolo_cls.launch.py | 1 + 11 files changed, 118 insertions(+), 80 deletions(-) diff --git a/perception_setup/launch/cameras/blackfly_s.launch.py b/perception_setup/launch/cameras/blackfly_s.launch.py index 55b1a5a..a515e0f 100644 --- a/perception_setup/launch/cameras/blackfly_s.launch.py +++ b/perception_setup/launch/cameras/blackfly_s.launch.py @@ -47,8 +47,11 @@ def generate_launch_description(): ], remappings=[ ('~/control', '/exposure_control/control'), - ('/blackfly_s/image_raw', ['/', drone, '/down_camera/image_color']), - ('/blackfly_s/camera_info', ['/', drone, '/down_camera/camera_info']), + ('/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')), diff --git a/perception_setup/launch/cameras/realsense_d555.launch.py b/perception_setup/launch/cameras/realsense_d555.launch.py index 061fd9c..88e0305 100644 --- a/perception_setup/launch/cameras/realsense_d555.launch.py +++ b/perception_setup/launch/cameras/realsense_d555.launch.py @@ -15,6 +15,7 @@ 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') @@ -51,8 +52,14 @@ def generate_launch_description(): } ], remappings=[ - ('/camera/camera/depth/image_rect_raw', ['/', drone, '/depth_camera/image_depth']), - ('/camera/camera/depth/camera_info', ['/', drone, '/depth_camera/camera_info']), + ( + '/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'], @@ -75,7 +82,11 @@ def generate_launch_description(): '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'], + 'output_camera_info_topic': [ + '/', + drone, + '/front_camera/camera_info', + ], 'enable_undistort': LaunchConfiguration('enable_undistort'), 'image_qos': 'reliable', } diff --git a/perception_setup/launch/cameras/sonar.launch.py b/perception_setup/launch/cameras/sonar.launch.py index 53bce44..f82cdca 100644 --- a/perception_setup/launch/cameras/sonar.launch.py +++ b/perception_setup/launch/cameras/sonar.launch.py @@ -6,6 +6,7 @@ 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'), diff --git a/perception_setup/launch/docking_blackfly_s.launch.py b/perception_setup/launch/docking_blackfly_s.launch.py index fdee423..975b4bc 100644 --- a/perception_setup/launch/docking_blackfly_s.launch.py +++ b/perception_setup/launch/docking_blackfly_s.launch.py @@ -48,8 +48,11 @@ def generate_launch_description(): ], remappings=[ ('~/control', '/exposure_control/control'), - ('/blackfly_s/image_raw', ['/', drone, '/down_camera/image_color']), - ('/blackfly_s/camera_info', ['/', drone, '/down_camera/camera_info']), + ('/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')), @@ -66,7 +69,11 @@ def generate_launch_description(): ), { 'subs.image_topic': ['/', drone, '/down_camera/image_color'], - 'subs.camera_info_topic': ['/', drone, '/down_camera/camera_info'], + 'subs.camera_info_topic': [ + '/', + drone, + '/down_camera/camera_info', + ], 'pubs.aruco_image': '/down_cam/aruco_detector/image', 'out_tf_frame': 'nautilus/downwards_camera_optical', }, diff --git a/perception_setup/launch/docking_realsense_d555.launch.py b/perception_setup/launch/docking_realsense_d555.launch.py index 9999f22..8e97d94 100644 --- a/perception_setup/launch/docking_realsense_d555.launch.py +++ b/perception_setup/launch/docking_realsense_d555.launch.py @@ -64,7 +64,11 @@ def generate_launch_description(): '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'], + 'output_camera_info_topic': [ + '/', + drone, + '/front_camera/camera_info', + ], 'enable_undistort': LaunchConfiguration('enable_undistort'), 'image_qos': 'sensor_data', } @@ -82,7 +86,11 @@ def generate_launch_description(): ), { 'subs.image_topic': ['/', drone, '/front_camera/image_color'], - 'subs.camera_info_topic': ['/', drone, '/front_camera/camera_info'], + 'subs.camera_info_topic': [ + '/', + drone, + '/front_camera/camera_info', + ], 'pubs.aruco_image': '/front_camera/aruco_detector/image', 'out_tf_frame': ['/', drone, '/front_camera_optical'], }, 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 index 9883f6d..4c73367 100644 --- a/perception_setup/launch/isaac_ros/isaac_ros_valve_intervention.launch.py +++ b/perception_setup/launch/isaac_ros/isaac_ros_valve_intervention.launch.py @@ -210,7 +210,7 @@ def _launch_setup(context, *args, **kwargs): dnn_image_encoder_launch, valve_detection_container, image_to_gstreamer_node, - yolo_obb_visualizer + yolo_obb_visualizer, ] return actions diff --git a/perception_setup/launch/pipeline_blackfly_s.launch.py b/perception_setup/launch/pipeline_blackfly_s.launch.py index 543ef6b..14166cc 100644 --- a/perception_setup/launch/pipeline_blackfly_s.launch.py +++ b/perception_setup/launch/pipeline_blackfly_s.launch.py @@ -66,7 +66,7 @@ def _launch_setup(context, *args, **kwargs): ], remappings=[ ('~/control', '/exposure_control/control'), - ('/blackfly_s/image_raw', f'/{drone}/down_camera/image_color'), + ('/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}], @@ -219,8 +219,12 @@ def _launch_setup(context, *args, **kwargs): 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'), + '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'], diff --git a/perception_setup/launch/ultralytics/ultralytics_pipeline_line_fitting.launch.py b/perception_setup/launch/ultralytics/ultralytics_pipeline_line_fitting.launch.py index 8c21eb8..6105e35 100644 --- a/perception_setup/launch/ultralytics/ultralytics_pipeline_line_fitting.launch.py +++ b/perception_setup/launch/ultralytics/ultralytics_pipeline_line_fitting.launch.py @@ -1,11 +1,10 @@ - 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'), @@ -14,62 +13,55 @@ def generate_launch_description(): ) irls_config = os.path.join( - get_package_share_directory('irls_line_fitter_2x'), - 'config', - 'irls_line.yaml' + 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', + } + ], + ), + ] ) - - 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', - } - ], - ), - ]) \ No newline at end of file diff --git a/perception_setup/launch/valve_intervention.launch.py b/perception_setup/launch/valve_intervention.launch.py index d7a7fe6..7eaa2bc 100644 --- a/perception_setup/launch/valve_intervention.launch.py +++ b/perception_setup/launch/valve_intervention.launch.py @@ -63,8 +63,11 @@ def _launch_setup(context, *args, **kwargs): } ], remappings=[ - ('/camera/camera/depth/image_rect_raw', f'/{drone}/depth_camera/image_depth'), - ('/camera/camera/depth/camera_info', f'/{drone}/depth_camera/camera_info'), + ( + '/camera/camera/depth/image_rect_raw', + f'/{drone}/depth_camera/image_depth', + ), + ('/camera/camera/depth/camera_info', f'/{drone}/depth_camera/camera_info'), ], ) diff --git a/perception_setup/launch/visual_inspection.launch.py b/perception_setup/launch/visual_inspection.launch.py index eb95686..6954f0d 100644 --- a/perception_setup/launch/visual_inspection.launch.py +++ b/perception_setup/launch/visual_inspection.launch.py @@ -64,7 +64,11 @@ def generate_launch_description(): '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'], + 'output_camera_info_topic': [ + '/', + drone, + '/front_camera/camera_info', + ], 'enable_undistort': LaunchConfiguration('enable_undistort'), 'image_qos': 'sensor_data', } @@ -101,7 +105,11 @@ def generate_launch_description(): ), { 'subs.image_topic': '/visual_inspection/filtered_image', - 'subs.camera_info_topic': ['/', drone, '/front_camera/camera_info'], + 'subs.camera_info_topic': [ + '/', + drone, + '/front_camera/camera_info', + ], 'detect_board': False, 'publish_landmarks': False, }, diff --git a/perception_setup/launch/yolo/yolo_cls.launch.py b/perception_setup/launch/yolo/yolo_cls.launch.py index 2cd7008..6c8c93d 100644 --- a/perception_setup/launch/yolo/yolo_cls.launch.py +++ b/perception_setup/launch/yolo/yolo_cls.launch.py @@ -33,6 +33,7 @@ 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')