diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
index eab2a56607dee..7f2fc45396f7f 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -11,13 +11,12 @@
-
-
+
@@ -59,15 +58,16 @@
-->
-
+
-
+
+
@@ -75,19 +75,18 @@
-
-
+
+
-
-
-
-
+
+
+
-
+
@@ -120,7 +119,7 @@
-
+
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
index 463340efdecfe..2bd007b658978 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
@@ -11,8 +11,6 @@
-
-
@@ -78,15 +76,16 @@
-
+
-
+
+
@@ -94,13 +93,12 @@
-
-
+
+
-
-
-
-
+
+
+
@@ -151,7 +149,7 @@
-
+
@@ -184,7 +182,7 @@
-
+
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
index ca5d1d0f7e8bb..ab55047132482 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
@@ -5,7 +5,6 @@
-
@@ -65,6 +64,8 @@
+
+
@@ -94,6 +95,8 @@
+
+
@@ -107,6 +110,7 @@
+
@@ -119,6 +123,7 @@
+
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
index 5b6134dc45584..ccc236d9cdc94 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -4,8 +4,6 @@
-
-
@@ -19,29 +17,27 @@
-
+
-
+
+
-
-
-
+
-
-
+
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
index a47e6a86f1c7b..4756fc9338e46 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
@@ -3,7 +3,6 @@
-
@@ -19,6 +18,7 @@
+
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py
index a5d8d2113d2db..93d395ca3e466 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py
@@ -35,7 +35,6 @@ def __init__(self, context):
)
with open(pointcloud_map_filter_param_path, "r") as f:
self.pointcloud_map_filter_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"]
self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"]
self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"]
self.downsize_ratio_z_axis = self.pointcloud_map_filter_param["downsize_ratio_z_axis"]
@@ -46,47 +45,40 @@ def __init__(self, context):
]
self.map_loader_radius = self.pointcloud_map_filter_param["map_loader_radius"]
self.publish_debug_pcd = self.pointcloud_map_filter_param["publish_debug_pcd"]
+ self.use_pointcloud_map = LaunchConfiguration("use_pointcloud_map").perform(context)
def create_pipeline(self):
- if self.use_down_sample_filter:
- return self.create_down_sample_pipeline()
+ if self.use_pointcloud_map == "true":
+ return self.create_compare_map_pipeline()
else:
- return self.create_normal_pipeline()
+ return self.create_no_compare_map_pipeline()
- def create_normal_pipeline(self):
+ def create_no_compare_map_pipeline(self):
components = []
components.append(
ComposableNode(
- package="compare_map_segmentation",
- plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent",
- name="voxel_based_compare_map_filter",
+ package="pointcloud_preprocessor",
+ plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
+ name="voxel_grid_downsample_filter",
remappings=[
("input", LaunchConfiguration("input_topic")),
- ("map", "/map/pointcloud_map"),
("output", LaunchConfiguration("output_topic")),
- ("map_loader_service", "/map/get_differential_pointcloud_map"),
- ("kinematic_state", "/localization/kinematic_state"),
],
parameters=[
{
- "distance_threshold": self.distance_threshold,
- "downsize_ratio_z_axis": self.downsize_ratio_z_axis,
- "timer_interval_ms": self.timer_interval_ms,
- "use_dynamic_map_loading": self.use_dynamic_map_loading,
- "map_update_distance_threshold": self.map_update_distance_threshold,
- "map_loader_radius": self.map_loader_radius,
- "publish_debug_pcd": self.publish_debug_pcd,
- "input_frame": "map",
+ "voxel_size_x": self.voxel_size,
+ "voxel_size_y": self.voxel_size,
+ "voxel_size_z": self.voxel_size,
}
],
extra_arguments=[
- {"use_intra_process_comms": False},
+ {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
],
- )
+ ),
)
return components
- def create_down_sample_pipeline(self):
+ def create_compare_map_pipeline(self):
components = []
down_sample_topic = (
"/perception/obstacle_segmentation/pointcloud_map_filtered/downsampled/pointcloud"
@@ -94,7 +86,7 @@ def create_down_sample_pipeline(self):
components.append(
ComposableNode(
package="pointcloud_preprocessor",
- plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
+ plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
name="voxel_grid_downsample_filter",
remappings=[
("input", LaunchConfiguration("input_topic")),
@@ -177,6 +169,7 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("use_intra_process", "True")
add_launch_arg("use_pointcloud_container", "False")
add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container")
+ add_launch_arg("use_pointcloud_map", "true")
set_container_executable = SetLaunchConfiguration(
"container_executable",
"component_container",
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index c41177e51116b..a30b87a1068b5 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -1,10 +1,8 @@
-
-
@@ -59,8 +57,9 @@
-
+
+
-
-
-
+
+
diff --git a/perception/euclidean_cluster/config/compare_map.param.yaml b/perception/euclidean_cluster/config/compare_map.param.yaml
deleted file mode 100644
index 3dd303464a2c1..0000000000000
--- a/perception/euclidean_cluster/config/compare_map.param.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-/**:
- ros__parameters:
-
- # distance threshold for compare compare
- distance_threshold: 0.5
-
- # publish voxelized map pointcloud for debug
- publish_debug_pcd: False
-
- # use dynamic map loading
- use_dynamic_map_loading: True
-
- # time interval to check dynamic map loading
- timer_interval_ms: 100
-
- # distance threshold for dynamic map update
- map_update_distance_threshold: 10.0
-
- # radius map for dynamic map loading
- map_loader_radius: 150.0
diff --git a/perception/euclidean_cluster/config/outlier.param.yaml b/perception/euclidean_cluster/config/outlier.param.yaml
deleted file mode 100644
index 1962fba1f332a..0000000000000
--- a/perception/euclidean_cluster/config/outlier.param.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-/**:
- ros__parameters:
- input_frame: base_link
- output_frame: base_link
- voxel_size_x: 0.3
- voxel_size_y: 0.3
- voxel_size_z: 100.0
- voxel_points_threshold: 3
diff --git a/perception/euclidean_cluster/config/voxel_grid.param.yaml b/perception/euclidean_cluster/config/voxel_grid.param.yaml
deleted file mode 100644
index 3ff32bfbb7146..0000000000000
--- a/perception/euclidean_cluster/config/voxel_grid.param.yaml
+++ /dev/null
@@ -1,7 +0,0 @@
-/**:
- ros__parameters:
- input_frame: base_link
- output_frame: base_link
- voxel_size_x: 0.15
- voxel_size_y: 0.15
- voxel_size_z: 0.15
diff --git a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml
index 2f3de2b789eb3..26b027f0077aa 100644
--- a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml
+++ b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml
@@ -7,9 +7,11 @@
max_cluster_size: 3000
use_height: false
input_frame: "base_link"
- max_x: 70.0
- min_x: -70.0
- max_y: 70.0
- min_y: -70.0
- max_z: 4.5
- min_z: -4.5
+
+ # low height crop box filter param
+ max_x: 200.0
+ min_x: -200.0
+ max_y: 200.0
+ min_y: -200.0
+ max_z: 2.0
+ min_z: -10.0
diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py
index db86bbf80d250..66c25396a656e 100644
--- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py
+++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py
@@ -35,61 +35,35 @@ def load_composable_node_param(param_path):
ns = ""
pkg = "euclidean_cluster"
- # set voxel grid filter as a component
- voxel_grid_filter_component = ComposableNode(
+ low_height_cropbox_filter_component = ComposableNode(
package="pointcloud_preprocessor",
- plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
- name=AnonName("voxel_grid_filter"),
+ namespace=ns,
+ plugin="pointcloud_preprocessor::CropBoxFilterComponent",
+ name="low_height_crop_box_filter",
remappings=[
("input", LaunchConfiguration("input_pointcloud")),
- ("output", "voxel_grid_filtered/pointcloud"),
- ],
- parameters=[load_composable_node_param("voxel_grid_param_path")],
- )
-
- # set compare map filter as a component
- compare_map_filter_component = ComposableNode(
- package="compare_map_segmentation",
- plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent",
- name=AnonName("compare_map_filter"),
- remappings=[
- ("input", "voxel_grid_filtered/pointcloud"),
- ("map", LaunchConfiguration("input_map")),
- ("output", "compare_map_filtered/pointcloud"),
- ("map_loader_service", "/map/get_differential_pointcloud_map"),
- ("kinematic_state", "/localization/kinematic_state"),
- ],
- parameters=[
- {
- "distance_threshold": 0.5,
- "timer_interval_ms": 100,
- "use_dynamic_map_loading": True,
- "downsize_ratio_z_axis": 0.5,
- "map_update_distance_threshold": 10.0,
- "map_loader_radius": 150.0,
- "publish_debug_pcd": True,
- "input_frame": "map",
- }
+ ("output", "low_height/pointcloud"),
],
+ parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")],
)
- use_map_euclidean_cluster_component = ComposableNode(
+ use_low_height_euclidean_component = ComposableNode(
package=pkg,
plugin="euclidean_cluster::EuclideanClusterNode",
name=AnonName("euclidean_cluster"),
remappings=[
- ("input", "compare_map_filtered/pointcloud"),
+ ("input", "low_height/pointcloud"),
("output", LaunchConfiguration("output_clusters")),
],
parameters=[load_composable_node_param("euclidean_param_path")],
)
- disuse_map_euclidean_cluster_component = ComposableNode(
+ disuse_low_height_euclidean_component = ComposableNode(
package=pkg,
plugin="euclidean_cluster::EuclideanClusterNode",
name=AnonName("euclidean_cluster"),
remappings=[
- ("input", "voxel_grid_filtered/pointcloud"),
+ ("input", LaunchConfiguration("input_pointcloud")),
("output", LaunchConfiguration("output_clusters")),
],
parameters=[load_composable_node_param("euclidean_param_path")],
@@ -100,26 +74,26 @@ def load_composable_node_param(param_path):
namespace=ns,
package="rclcpp_components",
executable="component_container",
- composable_node_descriptions=[voxel_grid_filter_component],
+ composable_node_descriptions=[],
output="screen",
)
- use_map_loader = LoadComposableNodes(
+ use_low_height_pointcloud_loader = LoadComposableNodes(
composable_node_descriptions=[
- compare_map_filter_component,
- use_map_euclidean_cluster_component,
+ low_height_cropbox_filter_component,
+ use_low_height_euclidean_component,
],
target_container=container,
- condition=IfCondition(LaunchConfiguration("use_pointcloud_map")),
+ condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")),
)
- disuse_map_loader = LoadComposableNodes(
- composable_node_descriptions=[disuse_map_euclidean_cluster_component],
+ disuse_low_height_pointcloud_loader = LoadComposableNodes(
+ composable_node_descriptions=[disuse_low_height_euclidean_component],
target_container=container,
- condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")),
+ condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")),
)
- return [container, use_map_loader, disuse_map_loader]
+ return [container, use_low_height_pointcloud_loader, disuse_low_height_pointcloud_loader]
def generate_launch_description():
@@ -131,14 +105,7 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"),
add_launch_arg("input_map", "/map/pointcloud_map"),
add_launch_arg("output_clusters", "clusters"),
- add_launch_arg("use_pointcloud_map", "false"),
- add_launch_arg(
- "voxel_grid_param_path",
- [
- FindPackageShare("autoware_launch"),
- "/config/perception/object_recognition/detection/clustering/voxel_grid.param.yaml",
- ],
- ),
+ add_launch_arg("use_low_height_cropbox", "false"),
add_launch_arg(
"euclidean_param_path",
[
diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml
index 7159fb4c42793..fd1ea82befae0 100644
--- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml
+++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml
@@ -3,16 +3,14 @@
-
-
+
-
-
+
diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py
index 82ce5605b67cd..00bcd4422bd3c 100644
--- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py
+++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py
@@ -16,10 +16,8 @@
from launch.actions import DeclareLaunchArgument
from launch.actions import OpaqueFunction
from launch.conditions import IfCondition
-from launch.substitutions import AndSubstitution
-from launch.substitutions import AnonName
+from launch.conditions import UnlessCondition
from launch.substitutions import LaunchConfiguration
-from launch.substitutions import NotSubstitution
from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
@@ -36,159 +34,31 @@ def load_composable_node_param(param_path):
ns = ""
pkg = "euclidean_cluster"
- # set compare map filter as a component
- compare_map_filter_component = ComposableNode(
- package="compare_map_segmentation",
- namespace=ns,
- plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent",
- name=AnonName("compare_map_filter"),
- remappings=[
- ("input", LaunchConfiguration("input_pointcloud")),
- ("map", LaunchConfiguration("input_map")),
- ("output", "map_filter/pointcloud"),
- ("map_loader_service", "/map/get_differential_pointcloud_map"),
- ("kinematic_state", "/localization/kinematic_state"),
- ],
- parameters=[load_composable_node_param("compare_map_param_path")],
- )
-
- # separate range of pointcloud when map_filter used
- use_map_short_range_crop_box_filter_component = ComposableNode(
- package="pointcloud_preprocessor",
- namespace=ns,
- plugin="pointcloud_preprocessor::CropBoxFilterComponent",
- name="short_distance_crop_box_range",
- remappings=[
- ("input", "map_filter/pointcloud"),
- ("output", "short_range/pointcloud"),
- ],
- parameters=[
- load_composable_node_param("voxel_grid_based_euclidean_param_path"),
- {
- "negative": False,
- },
- ],
- )
-
- use_map_long_range_crop_box_filter_component = ComposableNode(
- package="pointcloud_preprocessor",
- namespace=ns,
- plugin="pointcloud_preprocessor::CropBoxFilterComponent",
- name="long_distance_crop_box_range",
- remappings=[
- ("input", LaunchConfiguration("input_pointcloud")),
- ("output", "long_range/pointcloud"),
- ],
- parameters=[
- load_composable_node_param("voxel_grid_based_euclidean_param_path"),
- {
- "negative": True,
- },
- ],
- )
-
- # disuse_map_voxel_grid_filter
- disuse_map_short_range_crop_box_filter_component = ComposableNode(
+ low_height_cropbox_filter_component = ComposableNode(
package="pointcloud_preprocessor",
namespace=ns,
plugin="pointcloud_preprocessor::CropBoxFilterComponent",
- name="short_distance_crop_box_range",
+ name="low_height_crop_box_filter",
remappings=[
("input", LaunchConfiguration("input_pointcloud")),
- ("output", "short_range/pointcloud"),
- ],
- parameters=[
- load_composable_node_param("voxel_grid_based_euclidean_param_path"),
- {
- "negative": False,
- },
- ],
- )
-
- disuse_map_long_range_crop_box_filter_component = ComposableNode(
- package="pointcloud_preprocessor",
- namespace=ns,
- plugin="pointcloud_preprocessor::CropBoxFilterComponent",
- name="long_distance_crop_box_range",
- remappings=[
- ("input", LaunchConfiguration("input_pointcloud")),
- ("output", "long_range/pointcloud"),
- ],
- parameters=[
- load_composable_node_param("voxel_grid_based_euclidean_param_path"),
- {
- "negative": True,
- },
- ],
- )
-
- # set voxel grid filter as a component
- voxel_grid_filter_component = ComposableNode(
- package="pointcloud_preprocessor",
- namespace=ns,
- plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
- name=AnonName("voxel_grid_filter"),
- remappings=[
- ("input", "short_range/pointcloud"),
- ("output", "downsampled/short_range/pointcloud"),
- ],
- parameters=[load_composable_node_param("voxel_grid_param_path")],
- )
-
- # set outlier filter as a component
- outlier_filter_component = ComposableNode(
- package="pointcloud_preprocessor",
- namespace=ns,
- plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent",
- name="outlier_filter",
- remappings=[
- ("input", "downsampled/short_range/pointcloud"),
- ("output", "outlier_filter/pointcloud"),
- ],
- parameters=[load_composable_node_param("outlier_param_path")],
- )
-
- # concat with-outlier pointcloud and without-outlier pcl
- downsample_concat_component = ComposableNode(
- package="pointcloud_preprocessor",
- namespace=ns,
- plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent",
- name="concat_downsampled_pcl",
- remappings=[("output", "downsampled/concatenated/pointcloud")],
- parameters=[
- {
- "input_topics": ["long_range/pointcloud", "outlier_filter/pointcloud"],
- "output_frame": "base_link",
- }
- ],
- extra_arguments=[{"use_intra_process_comms": True}],
- )
-
- # set euclidean cluster as a component
- use_downsample_euclidean_cluster_component = ComposableNode(
- package=pkg,
- namespace=ns,
- plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode",
- name="euclidean_cluster",
- remappings=[
- ("input", "downsampled/concatenated/pointcloud"),
- ("output", LaunchConfiguration("output_clusters")),
+ ("output", "low_height/pointcloud"),
],
parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")],
)
- use_map_disuse_downsample_euclidean_component = ComposableNode(
+ use_low_height_euclidean_component = ComposableNode(
package=pkg,
namespace=ns,
plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode",
name="euclidean_cluster",
remappings=[
- ("input", "map_filter/pointcloud"),
+ ("input", "low_height/pointcloud"),
("output", LaunchConfiguration("output_clusters")),
],
parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")],
)
- disuse_map_disuse_downsample_euclidean_component = ComposableNode(
+
+ disuse_low_height_euclidean_component = ComposableNode(
package=pkg,
namespace=ns,
plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode",
@@ -209,75 +79,24 @@ def load_composable_node_param(param_path):
output="screen",
)
- use_map_use_downsample_loader = LoadComposableNodes(
+ use_low_height_pointcloud_loader = LoadComposableNodes(
composable_node_descriptions=[
- compare_map_filter_component,
- use_map_short_range_crop_box_filter_component,
- use_map_long_range_crop_box_filter_component,
- voxel_grid_filter_component,
- outlier_filter_component,
- downsample_concat_component,
- use_downsample_euclidean_cluster_component,
+ low_height_cropbox_filter_component,
+ use_low_height_euclidean_component,
],
target_container=container,
- condition=IfCondition(
- AndSubstitution(
- LaunchConfiguration("use_pointcloud_map"),
- LaunchConfiguration("use_downsample_pointcloud"),
- )
- ),
+ condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")),
)
- disuse_map_use_downsample_loader = LoadComposableNodes(
- composable_node_descriptions=[
- disuse_map_short_range_crop_box_filter_component,
- disuse_map_long_range_crop_box_filter_component,
- voxel_grid_filter_component,
- outlier_filter_component,
- downsample_concat_component,
- use_downsample_euclidean_cluster_component,
- ],
+ disuse_low_height_pointcloud_loader = LoadComposableNodes(
+ composable_node_descriptions=[disuse_low_height_euclidean_component],
target_container=container,
- condition=IfCondition(
- AndSubstitution(
- NotSubstitution(LaunchConfiguration("use_pointcloud_map")),
- LaunchConfiguration("use_downsample_pointcloud"),
- )
- ),
- )
-
- use_map_disuse_downsample_loader = LoadComposableNodes(
- composable_node_descriptions=[
- compare_map_filter_component,
- use_map_disuse_downsample_euclidean_component,
- ],
- target_container=container,
- condition=IfCondition(
- AndSubstitution(
- (LaunchConfiguration("use_pointcloud_map")),
- NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")),
- )
- ),
- )
-
- disuse_map_disuse_downsample_loader = LoadComposableNodes(
- composable_node_descriptions=[
- disuse_map_disuse_downsample_euclidean_component,
- ],
- target_container=container,
- condition=IfCondition(
- AndSubstitution(
- NotSubstitution(LaunchConfiguration("use_pointcloud_map")),
- NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")),
- )
- ),
+ condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")),
)
return [
container,
- use_map_use_downsample_loader,
- disuse_map_use_downsample_loader,
- use_map_disuse_downsample_loader,
- disuse_map_disuse_downsample_loader,
+ use_low_height_pointcloud_loader,
+ disuse_low_height_pointcloud_loader,
]
@@ -290,29 +109,7 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"),
add_launch_arg("input_map", "/map/pointcloud_map"),
add_launch_arg("output_clusters", "clusters"),
- add_launch_arg("use_pointcloud_map", "false"),
- add_launch_arg("use_downsample_pointcloud", "false"),
- add_launch_arg(
- "voxel_grid_param_path",
- [
- FindPackageShare("autoware_launch"),
- "/config/perception/object_recognition/detection/clustering/voxel_grid.param.yaml",
- ],
- ),
- add_launch_arg(
- "outlier_param_path",
- [
- FindPackageShare("autoware_launch"),
- "/config/perception/object_recognition/detection/clustering/outlier.param.yaml",
- ],
- ),
- add_launch_arg(
- "compare_map_param_path",
- [
- FindPackageShare("autoware_launch"),
- "/config/perception/object_recognition/detection/clustering/compare_map.param.yaml",
- ],
- ),
+ add_launch_arg("use_low_height_cropbox", "false"),
add_launch_arg(
"voxel_grid_based_euclidean_param_path",
[
diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml
index 2833fed81c28e..b6a426dabfd12 100644
--- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml
+++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml
@@ -3,22 +3,14 @@
-
-
-
-
-
+
-
-
-
-
-
+