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.

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

feat: remove use_pointcloud_container #6115

Merged
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@
<arg name="input/pointcloud"/>
<arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
<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="pointcloud_container_name" default="pointcloud_container"/>

<!-- Camera parameters -->
Expand Down Expand Up @@ -56,7 +55,6 @@
<arg name="image_number" value="$(var image_number)"/>
<arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
<arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="pointcloud_container_name" value="$(var pointcloud_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)"/>
Expand Down Expand Up @@ -130,7 +128,6 @@
<arg name="image_number" value="$(var image_number)"/>
<arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
<arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="pointcloud_container_name" value="$(var pointcloud_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)"/>
Expand Down Expand Up @@ -190,7 +187,6 @@
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml">
<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_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
</include>
<!-- Lidar object merger -->
Expand Down Expand Up @@ -219,7 +215,6 @@
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml">
<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_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
</include>
<!-- Lidar object merger -->
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
<arg name="input/pointcloud"/>
<arg name="input/obstacle_segmentation/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
<arg name="pointcloud_container_name" default="pointcloud_container"/>
<arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>

<!-- Lidar + Camera detector parameters -->
<arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
Expand Down Expand Up @@ -86,7 +85,7 @@
<arg name="model_param_path" value="$(var lidar_model_param_path)/$(var lidar_detection_model).param.yaml"/>
<arg name="class_remapper_param_path" value="$(var lidar_model_param_path)/detection_class_remapper.param.yaml"/>

<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="use_pointcloud_container" value="true"/>
<arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
</include>
</group>
Expand All @@ -98,7 +97,6 @@
<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="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
<arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
</include>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
<arg name="model_param_path" value="$(var lidar_model_param_path)/$(var centerpoint_model_name).param.yaml"/>
<arg name="class_remapper_param_path" value="$(var lidar_model_param_path)/detection_class_remapper.param.yaml"/>

<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="use_pointcloud_container" value="true"/>
<arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
</include>
</group>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
<!-- Lidar parameters -->
<arg name="input/obstacle_segmentation/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
<arg name="pointcloud_container_name" default="pointcloud_container"/>
<arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>

<!-- Pointcloud filter -->
<group>
Expand All @@ -12,7 +11,6 @@
<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="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
<arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
</include>
Expand All @@ -27,7 +25,7 @@
<arg name="output_clusters" value="clusters"/>
<arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>

<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="use_pointcloud_container" value="true"/>
<arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
</include>
</group>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@
from launch.conditions import IfCondition
from launch.conditions import UnlessCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
import yaml
Expand Down Expand Up @@ -140,21 +139,11 @@ def launch_setup(context, *args, **kwargs):
pipeline = PointcloudMapFilterPipeline(context)
components = []
components.extend(pipeline.create_pipeline())
individual_container = ComposableNodeContainer(
name=LaunchConfiguration("individual_container_name"),
namespace="",
package="rclcpp_components",
executable=LaunchConfiguration("container_executable"),
composable_node_descriptions=components,
condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
output="screen",
)
pointcloud_container_loader = LoadComposableNodes(
composable_node_descriptions=components,
target_container=LaunchConfiguration("pointcloud_container_name"),
condition=IfCondition(LaunchConfiguration("use_pointcloud_container")),
)
return [individual_container, pointcloud_container_loader]
return [pointcloud_container_loader]


def generate_launch_description():
Expand All @@ -167,9 +156,7 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("output_topic", "")
add_launch_arg("use_multithread", "False")
add_launch_arg("use_intra_process", "True")
add_launch_arg("use_pointcloud_container", "False")
add_launch_arg("pointcloud_container_name", "pointcloud_container")
add_launch_arg("individual_container_name", "pointcloud_map_filter_container")
add_launch_arg("use_pointcloud_map", "true")
set_container_executable = SetLaunchConfiguration(
"container_executable",
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,6 @@
from launch.conditions import UnlessCondition
from launch.substitutions import LaunchConfiguration
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
from launch_ros.substitutions import FindPackageShare
Expand Down Expand Up @@ -504,21 +503,11 @@ def launch_setup(context, *args, **kwargs):
output_topic=pipeline.output_topic,
)
)
individual_container = ComposableNodeContainer(
name=LaunchConfiguration("individual_container_name"),
namespace="",
package="rclcpp_components",
executable=LaunchConfiguration("container_executable"),
composable_node_descriptions=components,
condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
output="screen",
)
pointcloud_container_loader = LoadComposableNodes(
composable_node_descriptions=components,
target_container=LaunchConfiguration("pointcloud_container_name"),
condition=IfCondition(LaunchConfiguration("use_pointcloud_container")),
)
return [individual_container, pointcloud_container_loader]
return [pointcloud_container_loader]


def generate_launch_description():
Expand All @@ -530,9 +519,7 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("base_frame", "base_link")
add_launch_arg("use_multithread", "False")
add_launch_arg("use_intra_process", "True")
add_launch_arg("use_pointcloud_container", "False")
add_launch_arg("pointcloud_container_name", "pointcloud_container")
add_launch_arg("individual_container_name", "ground_segmentation_container")
add_launch_arg("input/pointcloud", "/sensing/lidar/concatenated/pointcloud")

set_container_executable = SetLaunchConfiguration(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@
<arg name="output" default="/perception/occupancy_grid_map/map"/>
<arg name="use_intra_process" default="false"/>
<arg name="use_multithread" default="false"/>
<arg name="use_pointcloud_container" default="false"/>
<arg name="pointcloud_container_name" default="pointcloud_container"/>
<arg name="occupancy_grid_map_method" description="options: pointcloud_based_occupancy_grid_map, laserscan_based_occupancy_grid_map, multi_lidar_pointcloud_based_occupancy_grid_map"/>
<arg name="occupancy_grid_map_param_path"/>
Expand All @@ -23,7 +22,7 @@
<arg name="output" value="$(var output)"/>
<arg name="use_intra_process" value="$(var use_intra_process)"/>
<arg name="use_multithread" value="$(var use_multithread)"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="use_pointcloud_container" value="true"/>
<arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
<arg name="param_file" value="$(var occupancy_grid_map_param_path)"/>
<arg name="updater_type" value="$(var occupancy_grid_map_updater)"/>
Expand All @@ -39,7 +38,7 @@
<arg name="output" value="$(var output)"/>
<arg name="use_intra_process" value="$(var use_intra_process)"/>
<arg name="use_multithread" value="$(var use_multithread)"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="use_pointcloud_container" value="true"/>
<arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
<arg name="param_file" value="$(var occupancy_grid_map_param_path)"/>
<arg name="updater_type" value="$(var occupancy_grid_map_updater)"/>
Expand All @@ -55,7 +54,7 @@
<arg name="output" value="/perception/occupancy_grid_map/map"/>
<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="use_pointcloud_container" value="true"/>
<arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
<arg name="multi_lidar_fusion_config_file" value="$(var occupancy_grid_map_param_path)"/>
<arg name="updater_type" value="$(var occupancy_grid_map_updater)"/>
Expand Down
4 changes: 0 additions & 4 deletions launch/tier4_perception_launch/launch/perception.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,6 @@
default="false"
description="if use_empty_dynamic_object_publisher:=true, /perception/object_recognition/objects topic has an empty DynamicObjectArray"
/>
<arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
<arg name="pointcloud_container_name" default="pointcloud_container"/>
<arg name="objects_filter_method" default="lanelet_filter"/>
<arg name="objects_validation_method" default="obstacle_pointcloud"/>
Expand Down Expand Up @@ -131,7 +130,6 @@
<arg name="base_frame" value="base_link"/>
<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="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
<arg name="input/pointcloud" value="$(var input/pointcloud)"/>
</include>
Expand All @@ -146,7 +144,6 @@
<arg name="output" value="/perception/occupancy_grid_map/map"/>
<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="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
<arg name="occupancy_grid_map_method" value="$(var occupancy_grid_map_method)"/>
<arg name="occupancy_grid_map_param_path" value="$(var occupancy_grid_map_param_path)"/>
Expand Down Expand Up @@ -197,7 +194,6 @@
<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="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
<arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
</include>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -269,59 +269,28 @@
</node_container>

<group if="$(var launch_compare_map_pipeline)">
<group if="$(var use_pointcloud_container)">
<!-- use pointcloud container -->
<load_composable_node target="$(var pointcloud_container_name)">
<composable_node pkg="compare_map_segmentation" plugin="compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent" name="voxel_distance_based_compare_map_filter_node" namespace="">
<!-- topic remap -->
<remap from="map" to="$(var input_pointcloud_map_topic_name)"/>
<remap from="kinematic_state" to="/localization/kinematic_state"/>
<remap from="map_loader_service" to="/map/get_differential_pointcloud_map"/>
<remap from="input" to="/perception/obstacle_segmentation/pointcloud"/>
<remap from="output" to="compare_map_filtered/pointcloud"/>
<!-- params -->
<param from="$(var compare_map_filter_param_path)"/>
<extra_arg name="use_intra_process_comms" value="false"/>
</composable_node>
<!-- use pointcloud container -->
<load_composable_node target="$(var pointcloud_container_name)">
<composable_node pkg="compare_map_segmentation" plugin="compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent" name="voxel_distance_based_compare_map_filter_node" namespace="">
<!-- topic remap -->
<remap from="map" to="$(var input_pointcloud_map_topic_name)"/>
<remap from="kinematic_state" to="/localization/kinematic_state"/>
<remap from="map_loader_service" to="/map/get_differential_pointcloud_map"/>
<remap from="input" to="/perception/obstacle_segmentation/pointcloud"/>
<remap from="output" to="compare_map_filtered/pointcloud"/>
<!-- params -->
<param from="$(var compare_map_filter_param_path)"/>
<extra_arg name="use_intra_process_comms" value="false"/>
</composable_node>

<composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::VectorMapInsideAreaFilterComponent" name="vector_map_inside_area_filter_node" namespace="">
<!-- topic remap -->
<remap from="input/vector_map" to="$(var input_vector_map_topic_name)"/>
<remap from="input" to="compare_map_filtered/pointcloud"/>
<remap from="output" to="vector_map_inside_area_filtered/pointcloud"/>
<param name="polygon_type" value="no_obstacle_segmentation_area_for_run_out"/>
<extra_arg name="use_intra_process_comms" value="false"/>
</composable_node>
</load_composable_node>
</group>

<group unless="$(var use_pointcloud_container)">
<!-- launch new container -->
<node_container pkg="rclcpp_components" exec="$(var container_type)" name="compare_map_container" namespace="" args="">
<composable_node pkg="compare_map_segmentation" plugin="compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent" name="voxel_distance_based_compare_map_filter_node" namespace="">
<!-- topic remap -->
<remap from="map" to="$(var input_pointcloud_map_topic_name)"/>
<remap from="kinematic_state" to="/localization/kinematic_state"/>
<remap from="map_loader_service" to="/map/get_differential_pointcloud_map"/>
<remap from="input" to="/perception/obstacle_segmentation/pointcloud"/>
<remap from="output" to="compare_map_filtered/pointcloud"/>
<!-- params -->
<param from="$(var compare_map_filter_param_path)"/>
<extra_arg name="use_intra_process_comms" value="false"/>
</composable_node>

<composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::VectorMapInsideAreaFilterComponent" name="vector_map_inside_area_filter_node" namespace="">
<!-- topic remap -->
<remap from="input/vector_map" to="$(var input_vector_map_topic_name)"/>
<remap from="input" to="compare_map_filtered/pointcloud"/>
<remap from="output" to="vector_map_inside_area_filtered/pointcloud"/>
<!-- params -->
<param name="polygon_type" value="no_obstacle_segmentation_area_for_run_out"/>
<extra_arg name="use_intra_process_comms" value="false"/>
</composable_node>

<composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
</node_container>
</group>
<composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::VectorMapInsideAreaFilterComponent" name="vector_map_inside_area_filter_node" namespace="">
<!-- topic remap -->
<remap from="input/vector_map" to="$(var input_vector_map_topic_name)"/>
<remap from="input" to="compare_map_filtered/pointcloud"/>
<remap from="output" to="vector_map_inside_area_filtered/pointcloud"/>
<param name="polygon_type" value="no_obstacle_segmentation_area_for_run_out"/>
<extra_arg name="use_intra_process_comms" value="false"/>
</composable_node>
</load_composable_node>
</group>
</launch>
Loading
Loading