feat: add launch for vector map inside area filter (#459)

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -419,6 +419,19 @@ def generate_launch_description():
         condition=IfCondition(LaunchConfiguration("use_multithread")),
     )
 
+    # This condition is true if run_out module is enabled and its detection method is Points
+    launch_run_out_with_points_method = PythonExpression(
+        [
+            LaunchConfiguration(
+                "launch_run_out", default=behavior_velocity_planner_param["launch_run_out"]
+            ),
+            " and ",
+            "'",
+            run_out_param["run_out"]["detection_method"],
+            "' == 'Points'",
+        ]
+    )
+
     # load compare map for run out module
     load_compare_map = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -433,19 +446,24 @@ def generate_launch_description():
             "use_multithread": "true",
         }.items(),
         # launch compare map only when run_out module is enabled and detection method is Points
-        condition=IfCondition(
-            PythonExpression(
-                [
-                    LaunchConfiguration(
-                        "launch_run_out", default=behavior_velocity_planner_param["launch_run_out"]
-                    ),
-                    " and ",
-                    "'",
-                    run_out_param["run_out"]["detection_method"],
-                    "' == 'Points'",
-                ]
-            )
+        condition=IfCondition(launch_run_out_with_points_method),
+    )
+
+    load_vector_map_inside_area_filter = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                FindPackageShare("tier4_planning_launch"),
+                "/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py",
+            ]
         ),
+        launch_arguments={
+            "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"),
+            "container_name": LaunchConfiguration("container_name"),
+            "use_multithread": "true",
+            "polygon_type": "no_obstacle_segmentation_area_for_run_out",
+        }.items(),
+        # launch vector map filter only when run_out module is enabled and detection method is Points
+        condition=IfCondition(launch_run_out_with_points_method),
     )
 
     return launch.LaunchDescription(
@@ -460,5 +478,6 @@ def generate_launch_description():
             set_container_mt_executable,
             container,
             load_compare_map,
+            load_vector_map_inside_area_filter,
         ]
     )

planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py

@@ -45,7 +45,7 @@ def add_launch_arg(name: str, default_value=None):
             plugin="compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent",
             name="voxel_distance_based_compare_map_filter_node",
             remappings=[
-                ("input", "/perception/obstacle_segmentation/pointcloud"),
+                ("input", "vector_map_inside_area_filtered/pointcloud"),
                 ("map", "/map/pointcloud_map"),
                 ("output", "compare_map_filtered/pointcloud"),
             ],
@@ -61,7 +61,7 @@ def add_launch_arg(name: str, default_value=None):
     ]
 
     compare_map_container = ComposableNodeContainer(
-        name=LaunchConfiguration("container_name"),
+        name="compare_map_container",
         namespace="",
         package="rclcpp_components",
         executable=LaunchConfiguration("container_executable"),

planning_launch/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py

@@ -0,0 +1,88 @@
+# Copyright 2022 TIER IV, Inc. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import SetLaunchConfiguration
+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
+
+
+def generate_launch_description():
+    def add_launch_arg(name: str, default_value=None):
+        return DeclareLaunchArgument(name, default_value=default_value)
+
+    set_container_executable = SetLaunchConfiguration(
+        "container_executable",
+        "component_container",
+        condition=UnlessCondition(LaunchConfiguration("use_multithread")),
+    )
+
+    set_container_mt_executable = SetLaunchConfiguration(
+        "container_executable",
+        "component_container_mt",
+        condition=IfCondition(LaunchConfiguration("use_multithread")),
+    )
+
+    composable_nodes = [
+        ComposableNode(
+            package="pointcloud_preprocessor",
+            plugin="pointcloud_preprocessor::VectorMapInsideAreaFilterComponent",
+            name="vector_map_inside_area_filter_node",
+            remappings=[
+                ("input", "/perception/obstacle_segmentation/pointcloud"),
+                ("input/vector_map", "/map/vector_map"),
+                ("output", "vector_map_inside_area_filtered/pointcloud"),
+            ],
+            parameters=[
+                {
+                    "polygon_type": LaunchConfiguration("polygon_type"),
+                }
+            ],
+            # this node has QoS of transient local
+            extra_arguments=[{"use_intra_process_comms": False}],
+        ),
+    ]
+
+    vector_map_area_filter_container = ComposableNodeContainer(
+        name="vector_map_area_filter_container",
+        namespace="",
+        package="rclcpp_components",
+        executable=LaunchConfiguration("container_executable"),
+        composable_node_descriptions=composable_nodes,
+        condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
+        output="screen",
+    )
+
+    load_composable_nodes = LoadComposableNodes(
+        composable_node_descriptions=composable_nodes,
+        target_container=LaunchConfiguration("container_name"),
+        condition=IfCondition(LaunchConfiguration("use_pointcloud_container")),
+    )
+
+    return LaunchDescription(
+        [
+            add_launch_arg("use_multithread", "true"),
+            add_launch_arg("use_pointcloud_container", "true"),
+            add_launch_arg("container_name", "vector_map_area_filter_container"),
+            set_container_executable,
+            set_container_mt_executable,
+            vector_map_area_filter_container,
+            load_composable_nodes,
+        ]
+    )