From 4d796cadc52f1b0b49551d315081b55782ff705f Mon Sep 17 00:00:00 2001 From: jorgen fjermedal Date: Sat, 11 Oct 2025 21:46:31 +0200 Subject: [PATCH 1/5] simple setup. No Resize/Downscale --- .isaac_ros_common-config | 4 +- entrypoint.sh | 6 +- perception_setup/launch/unet.launch.py | 237 +++++++++++++++++++++++++ submodules/isaac_ros_common | 2 +- 4 files changed, 243 insertions(+), 6 deletions(-) create mode 100644 perception_setup/launch/unet.launch.py diff --git a/.isaac_ros_common-config b/.isaac_ros_common-config index f76cc49..49fafce 100644 --- a/.isaac_ros_common-config +++ b/.isaac_ros_common-config @@ -1,4 +1,4 @@ -CONFIG_IMAGE_KEY=aarch64.ros2_humble.main -CONFIG_DOCKER_SEARCH_DIRS=(/home/vortex/perception_ws/src/perception-auv/submodules/isaac_ros_common/docker ../lib/src/gxf/docker) +CONFIG_IMAGE_KEY=aarch64.ros2_humble +CONFIG_DOCKER_SEARCH_DIRS=(/home/nx/ros2_ws/src/perception-auv/submodules/isaac_ros_common/docker ../lib/src/gxf/docker) CONFIG_CONTAINER_NAME_SUFFIX=gortex BASE_DOCKER_REGISTRY_NAMES=("nvcr.io/isaac/ros") diff --git a/entrypoint.sh b/entrypoint.sh index 2276658..bd653d4 100755 --- a/entrypoint.sh +++ b/entrypoint.sh @@ -24,8 +24,8 @@ done echo "Offline mode is set to: $OFFLINE" # Define paths -CONFIG_FILE_SOURCE="/home/vortex/perception_ws/src/perception-auv/.isaac_ros_common-config" -CONFIG_FILE_TARGET="/home/vortex/.isaac_ros_common-config" +CONFIG_FILE_SOURCE="/home/nx/ros2_ws/src/perception-auv/.isaac_ros_common-config" +CONFIG_FILE_TARGET="/home/nx/.isaac_ros_common-config" # Remove existing file or symlink if it exists rm -f "$CONFIG_FILE_TARGET" @@ -41,4 +41,4 @@ else fi # Run the command with the extra flag if needed -$PATH_TO_ISAAC_ROS_COMMON/scripts/run_dev.sh $EXTRA_FLAG -d ~/perception_ws/ +/home/nx/ros2_ws/src/perception-auv/submodules/isaac_ros_common/scripts/run_dev.sh $EXTRA_FLAG -d ~/ros2_ws/ \ No newline at end of file diff --git a/perception_setup/launch/unet.launch.py b/perception_setup/launch/unet.launch.py new file mode 100644 index 0000000..58a33d9 --- /dev/null +++ b/perception_setup/launch/unet.launch.py @@ -0,0 +1,237 @@ +import os +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 +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + # === Declare launch arguments === + model_file_path_arg = DeclareLaunchArgument( + 'model_file_path', + default_value=os.path.join( + get_package_share_directory('perception_setup'), + 'models', 'unet-simple-sigmoid.onnx'), + description='Path to ONNX model file', + ) + engine_file_path_arg = DeclareLaunchArgument( + 'engine_file_path', + default_value=os.path.join( + get_package_share_directory('perception_setup'), + 'models', 'unet-simple-sigmoid.engine'), + description='Path to TensorRT engine file', + ) + + network_image_width_arg = DeclareLaunchArgument('network_image_width', default_value='720') + network_image_height_arg = DeclareLaunchArgument('network_image_height', default_value='540') + use_planar_input_arg = DeclareLaunchArgument('use_planar_input', default_value='True') + encoder_image_mean_arg = DeclareLaunchArgument('encoder_image_mean', default_value='[0.485, 0.456, 0.406]') + encoder_image_stddev_arg = DeclareLaunchArgument('encoder_image_stddev', default_value='[0.229, 0.224, 0.225]') + alpha_arg = DeclareLaunchArgument('alpha', default_value='0.5') + network_output_type_arg = DeclareLaunchArgument( + 'network_output_type', + default_value='sigmoid', + choices=['argmax', 'softmax', 'sigmoid'] + ) + + # === LaunchConfigurations === + model_file_path = LaunchConfiguration('model_file_path') + engine_file_path = LaunchConfiguration('engine_file_path') + network_image_width = LaunchConfiguration('network_image_width') + network_image_height = LaunchConfiguration('network_image_height') + use_planar_input = LaunchConfiguration('use_planar_input') + encoder_image_mean = LaunchConfiguration('encoder_image_mean') + encoder_image_stddev = LaunchConfiguration('encoder_image_stddev') + alpha = LaunchConfiguration('alpha') + network_output_type = LaunchConfiguration('network_output_type') + + # === Image Processing and Encoder Nodes === + + debayer_node = ComposableNode( + package='image_proc', + plugin='image_proc::DebayerNode', + name='debayer_node', + remappings=[ + ('image_raw', '/gripper_camera/image_raw'), + ('image_color', '/debayered_image'), + ], + ) + + image_format_node = ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', + name='image_format_node', + parameters=[{'encoding_desired': 'rgb8'}], + remappings=[ + ('image_raw', '/debayered_image'), + ('image', '/image_rect'), + ], + ) + + image_to_tensor = ComposableNode( + package='isaac_ros_tensor_proc', + plugin='nvidia::isaac_ros::dnn_inference::ImageToTensorNode', + name='image_to_tensor', + parameters=[{'tensor_name': 'image', 'scale': True}], + remappings=[ + ('image', '/image_rect'), + ('tensor', '/unet_encoder/image_tensor'), + ], + ) + + normalize_node = ComposableNode( + package='isaac_ros_tensor_proc', + plugin='nvidia::isaac_ros::dnn_inference::ImageTensorNormalizeNode', + name='normalize_node', + parameters=[ + { + 'mean': LaunchConfiguration('encoder_image_mean'), + 'stddev': LaunchConfiguration('encoder_image_stddev'), + 'input_tensor_name': 'image', + 'output_tensor_name': 'image', + } + ], + remappings=[ + ('tensor', '/unet_encoder/image_tensor'), + ('normalized_tensor', '/unet_encoder/normalized_tensor') + ], + ) + + interleaved_to_planar_node = ComposableNode( + condition=IfCondition(LaunchConfiguration('use_planar_input')), + package='isaac_ros_tensor_proc', + plugin='nvidia::isaac_ros::dnn_inference::InterleavedToPlanarNode', + name='interleaved_to_planar_node', + parameters=[ + { + 'input_tensor_shape': [LaunchConfiguration('network_image_height'), + LaunchConfiguration('network_image_width'), 3], + 'num_blocks': 40, + } + ], + remappings=[ + ('interleaved_tensor', '/unet_encoder/normalized_tensor'), + ('planar_tensor', '/unet_encoder/planar_tensor'), + ], + ) + + reshape_node = ComposableNode( + package='isaac_ros_tensor_proc', + plugin='nvidia::isaac_ros::dnn_inference::ReshapeNode', + name='reshape_node', + parameters=[ + { + 'input_tensor_shape': [3, LaunchConfiguration('network_image_height'), + LaunchConfiguration('network_image_width')], + 'output_tensor_shape': [1, 3, LaunchConfiguration('network_image_height'), + LaunchConfiguration('network_image_width')], + 'output_tensor_name': 'input_tensor', + 'num_blocks': 40, + } + ], + remappings=[ + ('tensor', '/unet_encoder/planar_tensor'), + ('reshaped_tensor', '/unet_encoder/reshaped_tensor'), + ], + ) + + # === TensorRT and Decoder === + + tensor_rt_node = ComposableNode( + package='isaac_ros_tensor_rt', + plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode', + name='tensor_rt', + parameters=[ + { + 'model_file_path': LaunchConfiguration('model_file_path'), + 'engine_file_path': LaunchConfiguration('engine_file_path'), + 'input_tensor_names': ['input_tensor'], + 'input_binding_names': ['input'], + 'input_tensor_formats': ['nitros_tensor_list_nchw_rgb_f32'], + 'output_tensor_names': ['output_tensor'], + 'output_binding_names': ['output'], + 'output_tensor_formats': ['nitros_tensor_list_nchw_rgb_f32'], + 'verbose': True, + 'force_engine_update': False + } + ], + remappings=[ + ('tensor_pub', '/unet_encoder/reshaped_tensor'), + ('tensor_sub', '/unet_prediction'), + ], + ) + + unet_decoder_node = ComposableNode( + name='unet_decoder', + package='isaac_ros_unet', + plugin='nvidia::isaac_ros::unet::UNetDecoderNode', + parameters=[{ + 'network_output_type': network_output_type, + 'color_segmentation_mask_encoding': 'rgb8', + 'mask_width': network_image_width, + 'mask_height': network_image_height, + 'color_palette': [0x556B2F, 0x800000, 0x008080, 0x000080, 0x9ACD32, + 0xFF0000, 0xFF8C00, 0xFFD700, 0x00FF00, 0xBA55D3, + 0x00FA9A, 0x00FFFF, 0x0000FF, 0xF08080, 0xFF00FF, + 0x1E90FF, 0xDDA0DD, 0xFF1493, 0x87CEFA, 0xFFDEAD], + }], + remappings=[ + ('/tensor_sub', '/unet_prediction'), + ('/unet/colored_segmentation_mask', '/unet/colored_segmentation_mask'), + ('/unet/raw_segmentation_mask', '/unet/raw_segmentation_mask') + ], + ) + + alpha_blend_node = ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::AlphaBlendNode', + name='alpha_blend', + parameters=[{ + 'alpha': alpha, + 'mask_queue_size': 5, + 'image_queue_size': 5, + 'sync_queue_size': 5, + }], + remappings=[ + ('mask_input', '/unet/colored_segmentation_mask'), + ('image_input', '/image_rect'), + ('blended_image', '/segmentation_image_overlay'), + ], + ) + + # === Container === + + container = ComposableNodeContainer( + name='unet_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + debayer_node, + image_format_node, + image_to_tensor, + normalize_node, + interleaved_to_planar_node, + reshape_node, + tensor_rt_node, + unet_decoder_node, + alpha_blend_node, + ], + output='screen', + arguments=['--ros-args', '--log-level', 'INFO'], + ) + + return LaunchDescription([ + model_file_path_arg, + engine_file_path_arg, + network_image_width_arg, + network_image_height_arg, + encoder_image_mean_arg, + encoder_image_stddev_arg, + use_planar_input_arg, + alpha_arg, + network_output_type_arg, + container, + ]) diff --git a/submodules/isaac_ros_common b/submodules/isaac_ros_common index 70afe92..f8c80cd 160000 --- a/submodules/isaac_ros_common +++ b/submodules/isaac_ros_common @@ -1 +1 @@ -Subproject commit 70afe92cede737b6f65e39adfedf752229081d50 +Subproject commit f8c80cd14cee9153ea1271ffb3f5bb2a36b5305b From 0b62e6e6cfb43d49669cda34bbd4e179d8ff9b14 Mon Sep 17 00:00:00 2001 From: jorgen fjermedal Date: Sat, 11 Oct 2025 22:30:44 +0200 Subject: [PATCH 2/5] resize node + camera info pub --- perception_setup/launch/camera_info_pub.py | 80 ++++++++++++++++++++++ perception_setup/launch/unet.launch.py | 32 +++++++-- 2 files changed, 106 insertions(+), 6 deletions(-) create mode 100644 perception_setup/launch/camera_info_pub.py diff --git a/perception_setup/launch/camera_info_pub.py b/perception_setup/launch/camera_info_pub.py new file mode 100644 index 0000000..1d60b86 --- /dev/null +++ b/perception_setup/launch/camera_info_pub.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo, Image + + +class CameraInfoPublisher(Node): + def __init__(self): + super().__init__('gripper_camera_info_publisher') + + # --- Example camera intrinsics (change these for your camera) --- + self.width = 720 + self.height = 540 + fx = 500.0 + fy = 500.0 + cx = self.width / 2.0 + cy = self.height / 2.0 + + # Prepare static camera info message + self.camera_info_msg = CameraInfo() + self.camera_info_msg.header.frame_id = 'gripper_camera' + self.camera_info_msg.width = self.width + self.camera_info_msg.height = self.height + + # Intrinsic matrix K + self.camera_info_msg.k = [ + fx, 0.0, cx, + 0.0, fy, cy, + 0.0, 0.0, 1.0 + ] + + # Projection matrix P + self.camera_info_msg.p = [ + fx, 0.0, cx, 0.0, + 0.0, fy, cy, 0.0, + 0.0, 0.0, 1.0, 0.0 + ] + + # Rectification matrix (identity) + self.camera_info_msg.r = [ + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 + ] + + # Distortion (none) + self.camera_info_msg.d = [0.0, 0.0, 0.0, 0.0, 0.0] + self.camera_info_msg.distortion_model = 'plumb_bob' + + # Publisher and subscriber + self.publisher_ = self.create_publisher(CameraInfo, '/gripper_camera/camera_info', 10) + self.subscription = self.create_subscription( + Image, + '/gripper_camera/image_raw', + self.image_callback, + 10 + ) + + self.get_logger().info('Subscribing to /gripper_camera/image_raw and publishing synchronized /gripper_camera/camera_info') + + def image_callback(self, msg: Image): + """Called whenever a new image arrives""" + self.camera_info_msg.header.stamp = msg.header.stamp + self.publisher_.publish(self.camera_info_msg) + + +def main(args=None): + rclpy.init(args=args) + node = CameraInfoPublisher() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/perception_setup/launch/unet.launch.py b/perception_setup/launch/unet.launch.py index 58a33d9..96a39dc 100644 --- a/perception_setup/launch/unet.launch.py +++ b/perception_setup/launch/unet.launch.py @@ -13,19 +13,19 @@ def generate_launch_description(): 'model_file_path', default_value=os.path.join( get_package_share_directory('perception_setup'), - 'models', 'unet-simple-sigmoid.onnx'), + 'models', 'unet-simple-320-240-l-5-e10-b16.onnx'), description='Path to ONNX model file', ) engine_file_path_arg = DeclareLaunchArgument( 'engine_file_path', default_value=os.path.join( get_package_share_directory('perception_setup'), - 'models', 'unet-simple-sigmoid.engine'), + 'models', 'unet-simple-320-240-l-5-e10-b16.engine'), description='Path to TensorRT engine file', ) - network_image_width_arg = DeclareLaunchArgument('network_image_width', default_value='720') - network_image_height_arg = DeclareLaunchArgument('network_image_height', default_value='540') + network_image_width_arg = DeclareLaunchArgument('network_image_width', default_value='320') + network_image_height_arg = DeclareLaunchArgument('network_image_height', default_value='240') use_planar_input_arg = DeclareLaunchArgument('use_planar_input', default_value='True') encoder_image_mean_arg = DeclareLaunchArgument('encoder_image_mean', default_value='[0.485, 0.456, 0.406]') encoder_image_stddev_arg = DeclareLaunchArgument('encoder_image_stddev', default_value='[0.229, 0.224, 0.225]') @@ -70,13 +70,32 @@ def generate_launch_description(): ], ) + resize_node = ComposableNode( + package='isaac_ros_image_proc', + plugin='nvidia::isaac_ros::image_proc::ResizeNode', + name='resize_node', + parameters=[{ + 'output_width': LaunchConfiguration('network_image_width'), + 'output_height': LaunchConfiguration('network_image_height'), + 'keep_aspect_ratio': True, + 'disable_padding': False, + 'encoding_desired': 'rgb8', + }], + remappings=[ + ('/image', '/image_rect'), + ('/resize/image', '/image_resized'), + ('/camera_info', '/gripper_camera/camera_info'), # resize node wont publish without camera info + ], + ) + + image_to_tensor = ComposableNode( package='isaac_ros_tensor_proc', plugin='nvidia::isaac_ros::dnn_inference::ImageToTensorNode', name='image_to_tensor', parameters=[{'tensor_name': 'image', 'scale': True}], remappings=[ - ('image', '/image_rect'), + ('image', '/image_resized'), ('tensor', '/unet_encoder/image_tensor'), ], ) @@ -196,7 +215,7 @@ def generate_launch_description(): }], remappings=[ ('mask_input', '/unet/colored_segmentation_mask'), - ('image_input', '/image_rect'), + ('image_input', '/image_resized'), ('blended_image', '/segmentation_image_overlay'), ], ) @@ -211,6 +230,7 @@ def generate_launch_description(): composable_node_descriptions=[ debayer_node, image_format_node, + resize_node, image_to_tensor, normalize_node, interleaved_to_planar_node, From d5a312f3e94feb37b5c574a66eff5705d875c09f Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 11 Oct 2025 22:40:25 +0200 Subject: [PATCH 3/5] reverted dokcer changes. Current setup is broken --- .isaac_ros_common-config | 4 ++-- entrypoint.sh | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.isaac_ros_common-config b/.isaac_ros_common-config index 49fafce..f76cc49 100644 --- a/.isaac_ros_common-config +++ b/.isaac_ros_common-config @@ -1,4 +1,4 @@ -CONFIG_IMAGE_KEY=aarch64.ros2_humble -CONFIG_DOCKER_SEARCH_DIRS=(/home/nx/ros2_ws/src/perception-auv/submodules/isaac_ros_common/docker ../lib/src/gxf/docker) +CONFIG_IMAGE_KEY=aarch64.ros2_humble.main +CONFIG_DOCKER_SEARCH_DIRS=(/home/vortex/perception_ws/src/perception-auv/submodules/isaac_ros_common/docker ../lib/src/gxf/docker) CONFIG_CONTAINER_NAME_SUFFIX=gortex BASE_DOCKER_REGISTRY_NAMES=("nvcr.io/isaac/ros") diff --git a/entrypoint.sh b/entrypoint.sh index bd653d4..c6d95e7 100755 --- a/entrypoint.sh +++ b/entrypoint.sh @@ -24,8 +24,8 @@ done echo "Offline mode is set to: $OFFLINE" # Define paths -CONFIG_FILE_SOURCE="/home/nx/ros2_ws/src/perception-auv/.isaac_ros_common-config" -CONFIG_FILE_TARGET="/home/nx/.isaac_ros_common-config" +CONFIG_FILE_SOURCE="/home/vortex/perception_ws/src/perception-auv/.isaac_ros_common-config" +CONFIG_FILE_TARGET="/home/vortex/.isaac_ros_common-config" # Remove existing file or symlink if it exists rm -f "$CONFIG_FILE_TARGET" @@ -41,4 +41,4 @@ else fi # Run the command with the extra flag if needed -/home/nx/ros2_ws/src/perception-auv/submodules/isaac_ros_common/scripts/run_dev.sh $EXTRA_FLAG -d ~/ros2_ws/ \ No newline at end of file +$PATH_TO_ISAAC_ROS_COMMON/scripts/run_dev.sh $EXTRA_FLAG -d ~/perception_ws/ \ No newline at end of file From 292d592dc6a34051b11e65cc5278e370d560c09c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sat, 11 Oct 2025 20:42:27 +0000 Subject: [PATCH 4/5] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- entrypoint.sh | 2 +- perception_setup/launch/camera_info_pub.py | 40 ++--- perception_setup/launch/unet.launch.py | 164 ++++++++++++++------- 3 files changed, 131 insertions(+), 75 deletions(-) diff --git a/entrypoint.sh b/entrypoint.sh index c6d95e7..2276658 100755 --- a/entrypoint.sh +++ b/entrypoint.sh @@ -41,4 +41,4 @@ else fi # Run the command with the extra flag if needed -$PATH_TO_ISAAC_ROS_COMMON/scripts/run_dev.sh $EXTRA_FLAG -d ~/perception_ws/ \ No newline at end of file +$PATH_TO_ISAAC_ROS_COMMON/scripts/run_dev.sh $EXTRA_FLAG -d ~/perception_ws/ diff --git a/perception_setup/launch/camera_info_pub.py b/perception_setup/launch/camera_info_pub.py index 1d60b86..83ce1e6 100644 --- a/perception_setup/launch/camera_info_pub.py +++ b/perception_setup/launch/camera_info_pub.py @@ -23,40 +23,42 @@ def __init__(self): self.camera_info_msg.height = self.height # Intrinsic matrix K - self.camera_info_msg.k = [ - fx, 0.0, cx, - 0.0, fy, cy, - 0.0, 0.0, 1.0 - ] + self.camera_info_msg.k = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0] # Projection matrix P self.camera_info_msg.p = [ - fx, 0.0, cx, 0.0, - 0.0, fy, cy, 0.0, - 0.0, 0.0, 1.0, 0.0 + fx, + 0.0, + cx, + 0.0, + 0.0, + fy, + cy, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, ] # Rectification matrix (identity) - self.camera_info_msg.r = [ - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0 - ] + self.camera_info_msg.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] # Distortion (none) self.camera_info_msg.d = [0.0, 0.0, 0.0, 0.0, 0.0] self.camera_info_msg.distortion_model = 'plumb_bob' # Publisher and subscriber - self.publisher_ = self.create_publisher(CameraInfo, '/gripper_camera/camera_info', 10) + self.publisher_ = self.create_publisher( + CameraInfo, '/gripper_camera/camera_info', 10 + ) self.subscription = self.create_subscription( - Image, - '/gripper_camera/image_raw', - self.image_callback, - 10 + Image, '/gripper_camera/image_raw', self.image_callback, 10 ) - self.get_logger().info('Subscribing to /gripper_camera/image_raw and publishing synchronized /gripper_camera/camera_info') + self.get_logger().info( + 'Subscribing to /gripper_camera/image_raw and publishing synchronized /gripper_camera/camera_info' + ) def image_callback(self, msg: Image): """Called whenever a new image arrives""" diff --git a/perception_setup/launch/unet.launch.py b/perception_setup/launch/unet.launch.py index 96a39dc..2fda8ea 100644 --- a/perception_setup/launch/unet.launch.py +++ b/perception_setup/launch/unet.launch.py @@ -1,11 +1,13 @@ 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 -from ament_index_python.packages import get_package_share_directory + def generate_launch_description(): # === Declare launch arguments === @@ -13,27 +15,41 @@ def generate_launch_description(): 'model_file_path', default_value=os.path.join( get_package_share_directory('perception_setup'), - 'models', 'unet-simple-320-240-l-5-e10-b16.onnx'), + 'models', + 'unet-simple-320-240-l-5-e10-b16.onnx', + ), description='Path to ONNX model file', ) engine_file_path_arg = DeclareLaunchArgument( 'engine_file_path', default_value=os.path.join( get_package_share_directory('perception_setup'), - 'models', 'unet-simple-320-240-l-5-e10-b16.engine'), + 'models', + 'unet-simple-320-240-l-5-e10-b16.engine', + ), description='Path to TensorRT engine file', ) - network_image_width_arg = DeclareLaunchArgument('network_image_width', default_value='320') - network_image_height_arg = DeclareLaunchArgument('network_image_height', default_value='240') - use_planar_input_arg = DeclareLaunchArgument('use_planar_input', default_value='True') - encoder_image_mean_arg = DeclareLaunchArgument('encoder_image_mean', default_value='[0.485, 0.456, 0.406]') - encoder_image_stddev_arg = DeclareLaunchArgument('encoder_image_stddev', default_value='[0.229, 0.224, 0.225]') + network_image_width_arg = DeclareLaunchArgument( + 'network_image_width', default_value='320' + ) + network_image_height_arg = DeclareLaunchArgument( + 'network_image_height', default_value='240' + ) + use_planar_input_arg = DeclareLaunchArgument( + 'use_planar_input', default_value='True' + ) + encoder_image_mean_arg = DeclareLaunchArgument( + 'encoder_image_mean', default_value='[0.485, 0.456, 0.406]' + ) + encoder_image_stddev_arg = DeclareLaunchArgument( + 'encoder_image_stddev', default_value='[0.229, 0.224, 0.225]' + ) alpha_arg = DeclareLaunchArgument('alpha', default_value='0.5') network_output_type_arg = DeclareLaunchArgument( 'network_output_type', default_value='sigmoid', - choices=['argmax', 'softmax', 'sigmoid'] + choices=['argmax', 'softmax', 'sigmoid'], ) # === LaunchConfigurations === @@ -74,21 +90,25 @@ def generate_launch_description(): package='isaac_ros_image_proc', plugin='nvidia::isaac_ros::image_proc::ResizeNode', name='resize_node', - parameters=[{ - 'output_width': LaunchConfiguration('network_image_width'), - 'output_height': LaunchConfiguration('network_image_height'), - 'keep_aspect_ratio': True, - 'disable_padding': False, - 'encoding_desired': 'rgb8', - }], + parameters=[ + { + 'output_width': LaunchConfiguration('network_image_width'), + 'output_height': LaunchConfiguration('network_image_height'), + 'keep_aspect_ratio': True, + 'disable_padding': False, + 'encoding_desired': 'rgb8', + } + ], remappings=[ ('/image', '/image_rect'), ('/resize/image', '/image_resized'), - ('/camera_info', '/gripper_camera/camera_info'), # resize node wont publish without camera info + ( + '/camera_info', + '/gripper_camera/camera_info', + ), # resize node won't publish without camera info ], ) - image_to_tensor = ComposableNode( package='isaac_ros_tensor_proc', plugin='nvidia::isaac_ros::dnn_inference::ImageToTensorNode', @@ -114,7 +134,7 @@ def generate_launch_description(): ], remappings=[ ('tensor', '/unet_encoder/image_tensor'), - ('normalized_tensor', '/unet_encoder/normalized_tensor') + ('normalized_tensor', '/unet_encoder/normalized_tensor'), ], ) @@ -125,8 +145,11 @@ def generate_launch_description(): name='interleaved_to_planar_node', parameters=[ { - 'input_tensor_shape': [LaunchConfiguration('network_image_height'), - LaunchConfiguration('network_image_width'), 3], + 'input_tensor_shape': [ + LaunchConfiguration('network_image_height'), + LaunchConfiguration('network_image_width'), + 3, + ], 'num_blocks': 40, } ], @@ -142,10 +165,17 @@ def generate_launch_description(): name='reshape_node', parameters=[ { - 'input_tensor_shape': [3, LaunchConfiguration('network_image_height'), - LaunchConfiguration('network_image_width')], - 'output_tensor_shape': [1, 3, LaunchConfiguration('network_image_height'), - LaunchConfiguration('network_image_width')], + 'input_tensor_shape': [ + 3, + LaunchConfiguration('network_image_height'), + LaunchConfiguration('network_image_width'), + ], + 'output_tensor_shape': [ + 1, + 3, + LaunchConfiguration('network_image_height'), + LaunchConfiguration('network_image_width'), + ], 'output_tensor_name': 'input_tensor', 'num_blocks': 40, } @@ -173,7 +203,7 @@ def generate_launch_description(): 'output_binding_names': ['output'], 'output_tensor_formats': ['nitros_tensor_list_nchw_rgb_f32'], 'verbose': True, - 'force_engine_update': False + 'force_engine_update': False, } ], remappings=[ @@ -186,20 +216,40 @@ def generate_launch_description(): name='unet_decoder', package='isaac_ros_unet', plugin='nvidia::isaac_ros::unet::UNetDecoderNode', - parameters=[{ - 'network_output_type': network_output_type, - 'color_segmentation_mask_encoding': 'rgb8', - 'mask_width': network_image_width, - 'mask_height': network_image_height, - 'color_palette': [0x556B2F, 0x800000, 0x008080, 0x000080, 0x9ACD32, - 0xFF0000, 0xFF8C00, 0xFFD700, 0x00FF00, 0xBA55D3, - 0x00FA9A, 0x00FFFF, 0x0000FF, 0xF08080, 0xFF00FF, - 0x1E90FF, 0xDDA0DD, 0xFF1493, 0x87CEFA, 0xFFDEAD], - }], + parameters=[ + { + 'network_output_type': network_output_type, + 'color_segmentation_mask_encoding': 'rgb8', + 'mask_width': network_image_width, + 'mask_height': network_image_height, + 'color_palette': [ + 0x556B2F, + 0x800000, + 0x008080, + 0x000080, + 0x9ACD32, + 0xFF0000, + 0xFF8C00, + 0xFFD700, + 0x00FF00, + 0xBA55D3, + 0x00FA9A, + 0x00FFFF, + 0x0000FF, + 0xF08080, + 0xFF00FF, + 0x1E90FF, + 0xDDA0DD, + 0xFF1493, + 0x87CEFA, + 0xFFDEAD, + ], + } + ], remappings=[ ('/tensor_sub', '/unet_prediction'), ('/unet/colored_segmentation_mask', '/unet/colored_segmentation_mask'), - ('/unet/raw_segmentation_mask', '/unet/raw_segmentation_mask') + ('/unet/raw_segmentation_mask', '/unet/raw_segmentation_mask'), ], ) @@ -207,12 +257,14 @@ def generate_launch_description(): package='isaac_ros_image_proc', plugin='nvidia::isaac_ros::image_proc::AlphaBlendNode', name='alpha_blend', - parameters=[{ - 'alpha': alpha, - 'mask_queue_size': 5, - 'image_queue_size': 5, - 'sync_queue_size': 5, - }], + parameters=[ + { + 'alpha': alpha, + 'mask_queue_size': 5, + 'image_queue_size': 5, + 'sync_queue_size': 5, + } + ], remappings=[ ('mask_input', '/unet/colored_segmentation_mask'), ('image_input', '/image_resized'), @@ -243,15 +295,17 @@ def generate_launch_description(): arguments=['--ros-args', '--log-level', 'INFO'], ) - return LaunchDescription([ - model_file_path_arg, - engine_file_path_arg, - network_image_width_arg, - network_image_height_arg, - encoder_image_mean_arg, - encoder_image_stddev_arg, - use_planar_input_arg, - alpha_arg, - network_output_type_arg, - container, - ]) + return LaunchDescription( + [ + model_file_path_arg, + engine_file_path_arg, + network_image_width_arg, + network_image_height_arg, + encoder_image_mean_arg, + encoder_image_stddev_arg, + use_planar_input_arg, + alpha_arg, + network_output_type_arg, + container, + ] + ) From 77590177f10a575cc711953e085099237aac2422 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 18 Oct 2025 15:54:12 +0200 Subject: [PATCH 5/5] remove inline LaunchConfiguration --- perception_setup/launch/camera_info_pub.py | 2 +- perception_setup/launch/unet.launch.py | 26 +++++++++++----------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/perception_setup/launch/camera_info_pub.py b/perception_setup/launch/camera_info_pub.py index 83ce1e6..433266e 100644 --- a/perception_setup/launch/camera_info_pub.py +++ b/perception_setup/launch/camera_info_pub.py @@ -61,7 +61,7 @@ def __init__(self): ) def image_callback(self, msg: Image): - """Called whenever a new image arrives""" + """Called whenever a new image arrives.""" self.camera_info_msg.header.stamp = msg.header.stamp self.publisher_.publish(self.camera_info_msg) diff --git a/perception_setup/launch/unet.launch.py b/perception_setup/launch/unet.launch.py index 2fda8ea..82d4001 100644 --- a/perception_setup/launch/unet.launch.py +++ b/perception_setup/launch/unet.launch.py @@ -92,8 +92,8 @@ def generate_launch_description(): name='resize_node', parameters=[ { - 'output_width': LaunchConfiguration('network_image_width'), - 'output_height': LaunchConfiguration('network_image_height'), + 'output_width': network_image_width, + 'output_height': network_image_height, 'keep_aspect_ratio': True, 'disable_padding': False, 'encoding_desired': 'rgb8', @@ -126,8 +126,8 @@ def generate_launch_description(): name='normalize_node', parameters=[ { - 'mean': LaunchConfiguration('encoder_image_mean'), - 'stddev': LaunchConfiguration('encoder_image_stddev'), + 'mean': encoder_image_mean, + 'stddev': encoder_image_stddev, 'input_tensor_name': 'image', 'output_tensor_name': 'image', } @@ -139,15 +139,15 @@ def generate_launch_description(): ) interleaved_to_planar_node = ComposableNode( - condition=IfCondition(LaunchConfiguration('use_planar_input')), + condition=IfCondition(use_planar_input), package='isaac_ros_tensor_proc', plugin='nvidia::isaac_ros::dnn_inference::InterleavedToPlanarNode', name='interleaved_to_planar_node', parameters=[ { 'input_tensor_shape': [ - LaunchConfiguration('network_image_height'), - LaunchConfiguration('network_image_width'), + network_image_height, + network_image_width, 3, ], 'num_blocks': 40, @@ -167,14 +167,14 @@ def generate_launch_description(): { 'input_tensor_shape': [ 3, - LaunchConfiguration('network_image_height'), - LaunchConfiguration('network_image_width'), + network_image_height, + network_image_width, ], 'output_tensor_shape': [ 1, 3, - LaunchConfiguration('network_image_height'), - LaunchConfiguration('network_image_width'), + network_image_height, + network_image_width, ], 'output_tensor_name': 'input_tensor', 'num_blocks': 40, @@ -194,8 +194,8 @@ def generate_launch_description(): name='tensor_rt', parameters=[ { - 'model_file_path': LaunchConfiguration('model_file_path'), - 'engine_file_path': LaunchConfiguration('engine_file_path'), + 'model_file_path': model_file_path, + 'engine_file_path': engine_file_path, 'input_tensor_names': ['input_tensor'], 'input_binding_names': ['input'], 'input_tensor_formats': ['nitros_tensor_list_nchw_rgb_f32'],