diff --git a/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml index cb5e873c..cbb78b0e 100644 --- a/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + use_cuda: false debug_mode: false has_static_tf_only: false rosbag_replay: false diff --git a/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml index f726354b..b4c1f752 100644 --- a/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + use_cuda: false debug_mode: false has_static_tf_only: false rosbag_replay: false diff --git a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml index bd1dc498..187f0f45 100644 --- a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + use_cuda: false debug_mode: false has_static_tf_only: false rosbag_replay: false diff --git a/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml index 86ef53b0..5ff81605 100644 --- a/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml +++ b/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml @@ -1,8 +1,9 @@ /**: ros__parameters: + use_cuda: true debug_mode: false has_static_tf_only: false - rosbag_replay: false + rosbag_replay: true rosbag_length: 20.0 maximum_queue_size: 5 timeout_sec: 0.2 diff --git a/aip_xx1_launch/launch/lidar.launch.xml b/aip_xx1_launch/launch/lidar.launch.xml index 3a628503..9c29f934 100644 --- a/aip_xx1_launch/launch/lidar.launch.xml +++ b/aip_xx1_launch/launch/lidar.launch.xml @@ -6,6 +6,8 @@ + + @@ -23,6 +25,9 @@ + + + @@ -45,6 +50,9 @@ + + + @@ -67,6 +75,9 @@ + + + @@ -89,6 +100,9 @@ + + + diff --git a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py index d1fa63a1..e683b523 100644 --- a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py @@ -44,9 +44,11 @@ def launch_setup(context, *args, **kwargs): remappings=[ ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), ("output", "concatenated/pointcloud"), + ("output/cuda", "concatenated/pointcloud/cuda"), ], parameters=[concatenate_and_time_sync_node_param], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + # NOTE(knzo25): when using the cuda blackboard, this setting can not be made global + # extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) # load concat or passthrough filter diff --git a/aip_xx1_launch/package.xml b/aip_xx1_launch/package.xml index 0b5b5be9..73b4b940 100644 --- a/aip_xx1_launch/package.xml +++ b/aip_xx1_launch/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto + autoware_cuda_pointcloud_preprocessor autoware_gnss_poser autoware_imu_corrector autoware_pointcloud_preprocessor diff --git a/common_sensor_launch/launch/hesai_OT128.launch.xml b/common_sensor_launch/launch/hesai_OT128.launch.xml index dcb260f4..8850abd6 100644 --- a/common_sensor_launch/launch/hesai_OT128.launch.xml +++ b/common_sensor_launch/launch/hesai_OT128.launch.xml @@ -18,6 +18,9 @@ + + + @@ -36,6 +39,9 @@ + + + diff --git a/common_sensor_launch/launch/hesai_XT32.launch.xml b/common_sensor_launch/launch/hesai_XT32.launch.xml index e7e93a1b..5d53caed 100644 --- a/common_sensor_launch/launch/hesai_XT32.launch.xml +++ b/common_sensor_launch/launch/hesai_XT32.launch.xml @@ -18,6 +18,9 @@ + + + @@ -42,5 +45,8 @@ + + + diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index 68736b64..b39d2516 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -85,13 +85,14 @@ def create_parameter_dict(*args): nodes = [] - nodes.append( - ComposableNode( - package="glog_component", - plugin="GlogComponent", - name="glog_component", + if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context): + nodes.append( + ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) ) - ) nodes.append( ComposableNode( @@ -168,94 +169,235 @@ def create_parameter_dict(*args): ) ) - cropbox_parameters = create_parameter_dict("input_frame", "output_frame") - cropbox_parameters["negative"] = True - vehicle_info = get_vehicle_info(context) - cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"] - cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"] - cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"] - cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"] - cropbox_parameters["min_z"] = vehicle_info["min_height_offset"] - cropbox_parameters["max_z"] = vehicle_info["max_height_offset"] + mirror_info = load_composable_node_param("vehicle_mirror_param_file") - nodes.append( - ComposableNode( - package="autoware_pointcloud_preprocessor", - plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter_self", - remappings=[ - ("input", "pointcloud_raw_ex"), - ("output", "self_cropped/pointcloud_ex"), - ], - parameters=[cropbox_parameters], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + if IfCondition(LaunchConfiguration("use_cuda_preprocessor")).evaluate(context): + nodes.append( + ComposableNode( + package="autoware_cuda_pointcloud_preprocessor", + plugin="autoware::cuda_organized_pointcloud_adapter::CudaOrganizedPointcloudAdapterNode", + name="cuda_organized_pointcloud_adapter_node", + remappings=[ + ("~/input/pointcloud", "pointcloud_raw_ex"), + ("~/output/pointcloud", "cuda_points"), + ("~/output/pointcloud/cuda", "cuda_points/cuda"), + ], + # The whole node can not set use_intra_process due to type negotiation internal topics + # extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) ) - ) - mirror_info = load_composable_node_param("vehicle_mirror_param_file") - cropbox_parameters["min_x"] = mirror_info["min_longitudinal_offset"] - cropbox_parameters["max_x"] = mirror_info["max_longitudinal_offset"] - cropbox_parameters["min_y"] = mirror_info["min_lateral_offset"] - cropbox_parameters["max_y"] = mirror_info["max_lateral_offset"] - cropbox_parameters["min_z"] = mirror_info["min_height_offset"] - cropbox_parameters["max_z"] = mirror_info["max_height_offset"] + preprocessor_parameters = {} + preprocessor_parameters["self_crop.min_x"] = vehicle_info["min_longitudinal_offset"] + preprocessor_parameters["self_crop.max_x"] = vehicle_info["max_longitudinal_offset"] + preprocessor_parameters["self_crop.min_y"] = vehicle_info["min_lateral_offset"] + preprocessor_parameters["self_crop.max_y"] = vehicle_info["max_lateral_offset"] + preprocessor_parameters["self_crop.min_z"] = vehicle_info["min_height_offset"] + preprocessor_parameters["self_crop.max_z"] = vehicle_info["max_height_offset"] + preprocessor_parameters["mirror_crop.min_x"] = mirror_info["min_longitudinal_offset"] + preprocessor_parameters["mirror_crop.max_x"] = mirror_info["max_longitudinal_offset"] + preprocessor_parameters["mirror_crop.min_y"] = mirror_info["min_lateral_offset"] + preprocessor_parameters["mirror_crop.max_y"] = mirror_info["max_lateral_offset"] + preprocessor_parameters["mirror_crop.min_z"] = mirror_info["min_height_offset"] + preprocessor_parameters["mirror_crop.max_z"] = mirror_info["max_height_offset"] + + ring_outlier_filter_node_param = ParameterFile( + param_file=LaunchConfiguration("ring_outlier_filter_node_param_file").perform(context), + allow_substs=True, + ) - nodes.append( - ComposableNode( - package="autoware_pointcloud_preprocessor", - plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter_mirror", - remappings=[ - ("input", "self_cropped/pointcloud_ex"), - ("output", "mirror_cropped/pointcloud_ex"), - ], - parameters=[cropbox_parameters], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + nodes.append( + ComposableNode( + package="autoware_cuda_pointcloud_preprocessor", + plugin="autoware::cuda_pointcloud_preprocessor::CudaPointcloudPreprocessorNode", + name="cuda_pointcloud_preprocessor_node", + parameters=[ + preprocessor_parameters, + load_composable_node_param("distortion_corrector_node_param_file"), + ring_outlier_filter_node_param, + ], + remappings=[ + ("~/input/pointcloud", "cuda_points"), + ("~/input/pointcloud/cuda", "cuda_points/cuda"), + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("~/input/imu", "/sensing/imu/imu_data"), + ("~/output/pointcloud", "pointcloud_before_sync"), + ("~/output/pointcloud/cuda", "pointcloud_before_sync/cuda"), + ], + # The whole node can not set use_intra_process due to type negotiation internal topics + # extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) ) - ) + else: + cropbox_parameters = create_parameter_dict("input_frame", "output_frame") + cropbox_parameters["negative"] = True - nodes.append( - ComposableNode( - package="autoware_pointcloud_preprocessor", - plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent", - name="distortion_corrector_node", - remappings=[ - ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), - ("~/input/imu", "/sensing/imu/imu_data"), - ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), - ("~/output/pointcloud", "rectified/pointcloud_ex"), - ], - parameters=[load_composable_node_param("distortion_corrector_node_param_file")], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + vehicle_info = get_vehicle_info(context) + cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"] + cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"] + cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"] + cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"] + cropbox_parameters["min_z"] = vehicle_info["min_height_offset"] + cropbox_parameters["max_z"] = vehicle_info["max_height_offset"] + + nodes.append( + ComposableNode( + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_self", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "self_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) ) - ) - ring_outlier_filter_node_param = ParameterFile( - param_file=LaunchConfiguration("ring_outlier_filter_node_param_file").perform(context), - allow_substs=True, - ) + mirror_info = load_composable_node_param("vehicle_mirror_param_file") + cropbox_parameters["min_x"] = mirror_info["min_longitudinal_offset"] + cropbox_parameters["max_x"] = mirror_info["max_longitudinal_offset"] + cropbox_parameters["min_y"] = mirror_info["min_lateral_offset"] + cropbox_parameters["max_y"] = mirror_info["max_lateral_offset"] + cropbox_parameters["min_z"] = mirror_info["min_height_offset"] + cropbox_parameters["max_z"] = mirror_info["max_height_offset"] - # Ring Outlier Filter is the last component in the pipeline, so control the output frame here - if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true": - ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")} - else: - # keep the output frame as the input frame - ring_outlier_output_frame = {"output_frame": ""} + nodes.append( + ComposableNode( + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_mirror", + remappings=[ + ("input", "self_cropped/pointcloud_ex"), + ("output", "mirror_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) - nodes.append( - ComposableNode( - package="autoware_pointcloud_preprocessor", - plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent", - name="ring_outlier_filter", - remappings=[ - ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud_before_sync"), - ], - parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + nodes.append( + ComposableNode( + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent", + name="distortion_corrector_node", + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("~/input/imu", "/sensing/imu/imu_data"), + ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), + ("~/output/pointcloud", "rectified/pointcloud_ex"), + ], + parameters=[load_composable_node_param("distortion_corrector_node_param_file")], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + ring_outlier_filter_node_param = ParameterFile( + param_file=LaunchConfiguration("ring_outlier_filter_node_param_file").perform(context), + allow_substs=True, + ) + + # Ring Outlier Filter is the last component in the pipeline, so control the output frame here + if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true": + ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")} + else: + # keep the output frame as the input frame + ring_outlier_output_frame = {"output_frame": ""} + + nodes.append( + ComposableNode( + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent", + name="ring_outlier_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "pointcloud_before_sync"), + ], + parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ) + + if launch_driver: + nodes.append( + ComposableNode( + package="nebula_ros", + plugin=sensor_make + "HwInterfaceRosWrapper", + # node is created in a global context, need to avoid name clash + name=sensor_make.lower() + "_hw_interface_ros_wrapper_node", + parameters=[ + { + "sensor_model": sensor_model, + "calibration_file": sensor_calib_fp, + **create_parameter_dict( + "sensor_ip", + "host_ip", + "scan_phase", + "return_mode", + "frame_id", + "rotation_speed", + "data_port", + "gnss_port", + "cloud_min_angle", + "cloud_max_angle", + "packet_mtu_size", + "dual_return_distance_threshold", + "setup_sensor", + "ptp_profile", + "ptp_transport_type", + "ptp_switch_type", + "ptp_domain", + "retry_hw", + ), + } + ], + ) + ) + + if IfCondition(LaunchConfiguration("enable_blockage_diag")).evaluate(context): + nodes.append( + ComposableNode( + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::BlockageDiagComponent", + name="blockage_diag", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "blockage_diag/pointcloud"), + ], + parameters=[ + { + "angle_range": [ + float( + context.perform_substitution(LaunchConfiguration("cloud_min_angle")) + ), + float( + context.perform_substitution(LaunchConfiguration("cloud_max_angle")) + ), + ], + "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"), + "vertical_bins": LaunchConfiguration("vertical_bins"), + "is_channel_order_top2down": LaunchConfiguration( + "is_channel_order_top2down" + ), + "max_distance_range": LaunchConfiguration("max_range"), + "horizontal_resolution": LaunchConfiguration("horizontal_resolution"), + } + ] + + [load_composable_node_param("blockage_diagnostics_param_file")], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) ) - ) # set container to run all required components in the same process container = ComposableNodeContainer( @@ -264,79 +406,17 @@ def create_parameter_dict(*args): package="rclcpp_components", executable=LaunchConfiguration("container_executable"), composable_node_descriptions=nodes, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), output="both", ) - driver_component = ComposableNode( - package="nebula_ros", - plugin=sensor_make + "HwInterfaceRosWrapper", - # node is created in a global context, need to avoid name clash - name=sensor_make.lower() + "_hw_interface_ros_wrapper_node", - parameters=[ - { - "sensor_model": sensor_model, - "calibration_file": sensor_calib_fp, - **create_parameter_dict( - "sensor_ip", - "host_ip", - "scan_phase", - "return_mode", - "frame_id", - "rotation_speed", - "data_port", - "gnss_port", - "cloud_min_angle", - "cloud_max_angle", - "packet_mtu_size", - "dual_return_distance_threshold", - "setup_sensor", - "ptp_profile", - "ptp_transport_type", - "ptp_switch_type", - "ptp_domain", - "retry_hw", - ), - } - ], - ) - - blockage_diag_component = ComposableNode( - package="autoware_pointcloud_preprocessor", - plugin="autoware::pointcloud_preprocessor::BlockageDiagComponent", - name="blockage_diag", - remappings=[ - ("input", "pointcloud_raw_ex"), - ("output", "blockage_diag/pointcloud"), - ], - parameters=[ - { - "angle_range": [ - float(context.perform_substitution(LaunchConfiguration("cloud_min_angle"))), - float(context.perform_substitution(LaunchConfiguration("cloud_max_angle"))), - ], - "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"), - "vertical_bins": LaunchConfiguration("vertical_bins"), - "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"), - "max_distance_range": LaunchConfiguration("max_range"), - "horizontal_resolution": LaunchConfiguration("horizontal_resolution"), - } - ] - + [load_composable_node_param("blockage_diagnostics_param_file")], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - driver_component_loader = LoadComposableNodes( - composable_node_descriptions=[driver_component], - target_container=container, - condition=IfCondition(LaunchConfiguration("launch_driver")), - ) - blockage_diag_loader = LoadComposableNodes( - composable_node_descriptions=[blockage_diag_component], - target_container=container, - condition=IfCondition(LaunchConfiguration("enable_blockage_diag")), + load_composable_nodes = LoadComposableNodes( + composable_node_descriptions=nodes, + target_container=LaunchConfiguration("pointcloud_container_name"), + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) - return [container, driver_component_loader, blockage_diag_loader] + return [container, load_composable_nodes] def generate_launch_description(): @@ -380,6 +460,9 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("use_multithread", "False", "use multithread") add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") add_launch_arg("container_name", "nebula_node_container") + add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg("use_pointcloud_container", "False") + add_launch_arg("use_cuda_preprocessor", "False") add_launch_arg("ptp_profile", "1588v2") add_launch_arg("ptp_transport_type", "L2") add_launch_arg("ptp_switch_type", "TSN") diff --git a/common_sensor_launch/launch/velodyne_VLP16.launch.xml b/common_sensor_launch/launch/velodyne_VLP16.launch.xml index 4a6cd6c1..98aa4a05 100644 --- a/common_sensor_launch/launch/velodyne_VLP16.launch.xml +++ b/common_sensor_launch/launch/velodyne_VLP16.launch.xml @@ -16,6 +16,9 @@ + + + @@ -39,6 +42,9 @@ + + + diff --git a/common_sensor_launch/launch/velodyne_VLP32C.launch.xml b/common_sensor_launch/launch/velodyne_VLP32C.launch.xml index 3544d16e..3a5f55ab 100644 --- a/common_sensor_launch/launch/velodyne_VLP32C.launch.xml +++ b/common_sensor_launch/launch/velodyne_VLP32C.launch.xml @@ -16,6 +16,9 @@ + + + @@ -39,6 +42,9 @@ + + + diff --git a/common_sensor_launch/launch/velodyne_VLS128.launch.xml b/common_sensor_launch/launch/velodyne_VLS128.launch.xml index 01c8affa..85fab387 100644 --- a/common_sensor_launch/launch/velodyne_VLS128.launch.xml +++ b/common_sensor_launch/launch/velodyne_VLS128.launch.xml @@ -16,6 +16,9 @@ + + + @@ -39,6 +42,9 @@ + + +