Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

refactor(perception): rearrange clustering pipeline #4999

Merged
merged 8 commits into from
Sep 19, 2023
Merged

refactor(perception): rearrange clustering pipeline #4999

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
265ab4e
fix: change downsample filter
badai-nguyen Sep 14, 2023
bbc05cd
fix: remove downsamle after compare map
badai-nguyen Sep 14, 2023
1db7712
fix: add low range cropbox
badai-nguyen Sep 14, 2023
01500a7
refactor: use_pointcloud_map
badai-nguyen Sep 14, 2023
c66eacc
chore: refactor
badai-nguyen Sep 14, 2023
3d3aa4f
fix: add roi based clustering option
badai-nguyen Sep 14, 2023
9639f8a
chore: change node name
badai-nguyen Sep 15, 2023
a0b6c7b
fix: launch argument pasrer
badai-nguyen Sep 16, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 11 additions & 12 deletions ...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,13 +11,12 @@
<arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
<arg name="score_threshold" default="0.35"/>

<arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
miursh marked this conversation as resolved.
Show resolved Hide resolved
<arg name="use_downsample_pointcloud" default="true" description="use downsample pointcloud in perception"/>
<arg name="use_object_filter" default="true" description="use object filter"/>
<arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
<arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
<arg name="remove_unknown" default="true"/>
<arg name="trust_distance" default="30.0"/>
<arg name="use_roi_based_cluster" default="false"/>

<!-- Camera param -->
<arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
Expand Down Expand Up @@ -59,35 +58,35 @@
<arg name="image_number" value="$(var image_number)"/>
</include> -->

<!-- Pointcloud map filter -->
<!-- Pointcloud filter -->
<group>
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py" if="$(var use_pointcloud_map)">
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py">
<arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
<arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
<arg name="use_intra_process" value="true"/>
<arg name="use_multithread" value="true"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="container_name" value="$(var container_name)"/>
<arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
</include>
</group>

<!-- Clustering -->
<group>
<push-ros-namespace namespace="clustering"/>
<group>
<let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
<let name="clustering/input/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
<let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
<let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
<include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
<arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
<arg name="output_clusters" value="euclidean_cluster/clusters"/>
<arg name="use_pointcloud_map" value="false"/>
<arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
<arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
<arg name="output_clusters" value="$(var euclidean_cluster_output)"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
</include>
</group>

<!-- roi based clustering -->
<group>
<include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml">
<include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml" if="$(var use_roi_based_cluster)">
<arg name="input/camera_info0" value="$(var camera_info0)"/>
<arg name="input/rois0" value="$(var detection_rois0)"/>
<arg name="input/camera_info1" value="$(var camera_info1)"/>
Expand Down Expand Up @@ -120,7 +119,7 @@

<!-- simple_cluster_merger -->
<group>
<include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml">
<include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml" if="$(var use_roi_based_cluster)">
<arg name="input/cluster0" value="euclidean_cluster/clusters"/>
<arg name="input/cluster1" value="roi_cluster/clusters"/>
<arg name="output/clusters" value="clusters"/>
Expand Down
22 changes: 10 additions & 12 deletions .../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,6 @@
<arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
<arg name="score_threshold" default="0.35"/>

<arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
<arg name="use_downsample_pointcloud" default="true" description="use downsample pointcloud in perception"/>
<arg name="use_object_filter" default="true" description="use object filter"/>
<arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
<arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
Expand Down Expand Up @@ -78,29 +76,29 @@
</include>
</group>

<!-- Pointcloud map filter -->
<!-- Pointcloud filter -->
<group>
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py" if="$(var use_pointcloud_map)">
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py">
<arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
<arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
<arg name="use_intra_process" value="true"/>
<arg name="use_multithread" value="true"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="container_name" value="$(var container_name)"/>
<arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
</include>
</group>

<!-- Clustering -->
<group>
<push-ros-namespace namespace="clustering"/>
<group>
<let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
<let name="clustering/input/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
<let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
<let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
<include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
<arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
<arg name="output_clusters" value="clusters"/>
<arg name="use_pointcloud_map" value="false"/>
<arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
<arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
<arg name="output_clusters" value="$(var euclidean_cluster_output)"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
</include>
</group>

Expand Down Expand Up @@ -151,7 +149,7 @@

<!-- roi based clustering -->
<group>
<include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml">
<include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml" if="$(var use_roi_based_cluster)">
<arg name="input/camera_info0" value="$(var camera_info0)"/>
<arg name="input/rois0" value="$(var detection_rois0)"/>
<arg name="input/camera_info1" value="$(var camera_info1)"/>
Expand Down Expand Up @@ -184,7 +182,7 @@

<!-- simple_cluster_merger -->
<group>
<include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml">
<include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml" if="$(var use_roi_based_cluster)">
<arg name="input/cluster0" value="euclidean_cluster/clusters"/>
<arg name="input/cluster1" value="roi_cluster/clusters"/>
<arg name="output/clusters" value="clusters"/>
Expand Down
7 changes: 6 additions & 1 deletion launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@
<!-- lidar param -->
<arg name="input/pointcloud"/>
<arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
<arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
<arg name="use_object_filter" default="true" description="use object filter"/>
<arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
<arg name="container_name" default="pointcloud_container"/>
Expand Down Expand Up @@ -65,6 +64,8 @@
<arg name="remove_unknown" value="$(var remove_unknown)"/>
<arg name="trust_distance" value="$(var trust_distance)"/>
<arg name="input/radar" value="$(var input/radar)"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
<arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
</include>
</group>

Expand Down Expand Up @@ -94,6 +95,8 @@
<arg name="use_object_filter" value="$(var use_object_filter)"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="container_name" value="$(var container_name)"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
<arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
</include>
</group>

Expand All @@ -107,6 +110,7 @@
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="container_name" value="$(var container_name)"/>
<arg name="input/radar" value="$(var input/radar)"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
</include>
</group>

Expand All @@ -119,6 +123,7 @@
<arg name="use_object_filter" value="$(var use_object_filter)"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="container_name" value="$(var container_name)"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
</include>
</group>

Expand Down
14 changes: 5 additions & 9 deletions ...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,6 @@
<arg name="input/obstacle_segmentation/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
<arg name="output/objects" default="objects"/>
<arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `clustering`"/>
<arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
<arg name="use_downsample_pointcloud" default="true" description="use downsample pointcloud in perception"/>
<arg name="use_object_filter" default="true" description="use object filter"/>
<arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
<arg name="container_name" default="pointcloud_container"/>
Expand All @@ -19,29 +17,27 @@
<arg name="object_recognition_detection_object_merger_data_association_matrix_param_path" default="$(find-pkg-share object_merger)/config/data_association_matrix.param.yaml"/>
<arg name="object_recognition_detection_object_merger_distance_threshold_list_path" default="$(find-pkg-share object_merger)/config/overlapped_judge.param.yaml"/>

<!-- Pointcloud map filter -->
<!-- Pointcloud filter -->
<group>
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py" if="$(var use_pointcloud_map)">
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py">
<arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
<arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
<arg name="use_intra_process" value="true"/>
<arg name="use_multithread" value="true"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="container_name" value="$(var container_name)"/>
<arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
</include>
</group>

<!-- Clustering -->
<group>
<push-ros-namespace namespace="clustering"/>
<let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
<let name="clustering/input/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
<group>
<include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
<arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
<arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
<arg name="output_clusters" value="clusters"/>
<arg name="use_pointcloud_map" value="false"/>
<arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
</include>
</group>

Expand Down
2 changes: 1 addition & 1 deletion ...ception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
<!-- lidar param -->
<arg name="input/pointcloud"/>
<arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `clustering`"/>
<arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
<arg name="use_object_filter" default="true" description="use object filter"/>
<arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
<arg name="container_name" default="pointcloud_container"/>
Expand All @@ -19,6 +18,7 @@
<arg name="output/objects" value="lidar/objects"/>
<arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
<arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
<arg name="use_object_filter" value="$(var use_object_filter)"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="container_name" value="$(var container_name)"/>
Expand Down
39 changes: 16 additions & 23 deletions ...er4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"]
Expand All @@ -46,55 +45,48 @@ 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"
)
components.append(
ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
name="voxel_grid_downsample_filter",
remappings=[
("input", LaunchConfiguration("input_topic")),
Expand Down Expand Up @@ -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",
Expand Down
Loading
Loading