Skip to content

Commit

Permalink
merge main branch
Browse files Browse the repository at this point in the history 
Signed-off-by: scepter914 <[email protected]>
  • Loading branch information
@scepter914
scepter914 committed Sep 19, 2023
2 parents fac721b + 1506a94 commit 6be86f9
Show file tree
Hide file tree
Showing 23 changed files with 302 additions and 602 deletions.
10 changes: 4 additions & 6 deletions common/geography_utils/src/lanelet2_projector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,13 +43,11 @@ std::unique_ptr<lanelet::Projector> get_lanelet2_projector(const MapProjectorInf
lanelet::Origin origin{position};
lanelet::projection::TransverseMercatorProjector projector{origin};
return std::make_unique<lanelet::projection::TransverseMercatorProjector>(projector);

} else {
const std::string error_msg =
"Invalid map projector type: " + projector_info.projector_type +
". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator";
throw std::invalid_argument(error_msg);
}
const std::string error_msg =
"Invalid map projector type: " + projector_info.projector_type +
". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator";
throw std::invalid_argument(error_msg);
}

} // namespace geography_utils
23 changes: 11 additions & 12 deletions ...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
View file
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"/>
<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 parameters -->
<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
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="lidar_detection_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 @@ -79,29 +77,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 @@ -152,7 +150,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 @@ -185,7 +183,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
10 changes: 9 additions & 1 deletion launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@
<!-- LiDAR parameters -->
<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 @@ -66,6 +65,8 @@
<arg name="container_name" value="$(var container_name)"/>
<arg name="remove_unknown" value="$(var remove_unknown)"/>
<arg name="trust_distance" value="$(var trust_distance)"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
<arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
<arg name="input/radar" value="$(var input/radar)"/>
<arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
</include>
Expand Down Expand Up @@ -97,6 +98,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 @@ -110,7 +113,11 @@
<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)"/>
<<<<<<< HEAD
<arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
=======
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
>>>>>>> main
</include>
</group>

Expand All @@ -123,6 +130,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
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
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
<!-- LiDAR parameters -->
<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 @@ -20,6 +19,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
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

0 comments on commit 6be86f9

Please sign in to comment.