From f357bd31ab78d213fc11c0fadac28cf7ff599746 Mon Sep 17 00:00:00 2001
From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Date: Tue, 7 Nov 2023 17:29:56 +0900
Subject: [PATCH] refactor(tier4_planning_launch): use xml style launch (#5502)

* refactor(tier4_planning_launch): use xml style launch

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* refactor(tier4_planning_launch): remove python style launch

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(tier4_planning_launch): enable console output

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../behavior_planning.launch.xml              |   2 +-
 .../motion_planning.launch.xml                |   2 +-
 .../scenario_planning/parking.launch.py       | 160 ------------------
 .../scenario_planning/parking.launch.xml      |  58 ++++---
 .../scenario_planning.launch.xml              |   4 +-
 5 files changed, 36 insertions(+), 190 deletions(-)
 delete mode 100644 launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py

diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
index 4df5152e2ebce..085dc92982663 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -5,7 +5,7 @@
   <arg name="input_vector_map_topic_name" default="/map/vector_map"/>
   <arg name="input_pointcloud_map_topic_name" default="/map/pointcloud_map"/>
 
-  <node_container pkg="rclcpp_components" exec="$(var container_type)" name="behavior_planning_container" namespace="" args="">
+  <node_container pkg="rclcpp_components" exec="$(var container_type)" name="behavior_planning_container" namespace="" args="" output="screen">
     <composable_node pkg="behavior_path_planner" plugin="behavior_path_planner::BehaviorPathPlannerNode" name="behavior_path_planner" namespace="">
       <!-- topic remap -->
       <remap from="~/input/route" to="$(var input_route_topic_name)"/>
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
index 45fa85f0d77fb..577b0e00d133c 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
@@ -2,7 +2,7 @@
   <arg name="interface_input_topic" default="/planning/scenario_planning/lane_driving/behavior_planning/path"/>
   <arg name="interface_output_topic" default="/planning/scenario_planning/lane_driving/trajectory"/>
 
-  <node_container pkg="rclcpp_components" exec="$(var container_type)" name="motion_planning_container" namespace="" args="">
+  <node_container pkg="rclcpp_components" exec="$(var container_type)" name="motion_planning_container" namespace="" args="" output="screen">
     <composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
   </node_container>
 
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py
deleted file mode 100644
index 7dfcbb0307bf0..0000000000000
--- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py
+++ /dev/null
@@ -1,160 +0,0 @@
-# Copyright 2021 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.
-
-import launch
-from launch.actions import DeclareLaunchArgument
-from launch.actions import GroupAction
-from launch.actions import OpaqueFunction
-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 PushRosNamespace
-from launch_ros.descriptions import ComposableNode
-from launch_ros.substitutions import FindPackageShare
-import yaml
-
-
-def launch_setup(context, *args, **kwargs):
-    vehicle_info_param_path = LaunchConfiguration("vehicle_param_file").perform(context)
-    with open(vehicle_info_param_path, "r") as f:
-        vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-
-    with open(LaunchConfiguration("freespace_planner_param_path").perform(context), "r") as f:
-        freespace_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-
-    container = ComposableNodeContainer(
-        name="parking_container",
-        namespace="",
-        package="rclcpp_components",
-        executable=LaunchConfiguration("container_executable"),
-        composable_node_descriptions=[
-            ComposableNode(
-                package="costmap_generator",
-                plugin="CostmapGenerator",
-                name="costmap_generator",
-                remappings=[
-                    ("~/input/objects", "/perception/object_recognition/objects"),
-                    (
-                        "~/input/points_no_ground",
-                        "/perception/obstacle_segmentation/pointcloud",
-                    ),
-                    ("~/input/vector_map", "/map/vector_map"),
-                    ("~/input/scenario", "/planning/scenario_planning/scenario"),
-                    ("~/output/grid_map", "costmap_generator/grid_map"),
-                    ("~/output/occupancy_grid", "costmap_generator/occupancy_grid"),
-                ],
-                parameters=[
-                    {
-                        "costmap_frame": "map",
-                        "vehicle_frame": "base_link",
-                        "map_frame": "map",
-                        "update_rate": 10.0,
-                        "activate_by_scenario": False,
-                        "grid_min_value": 0.0,
-                        "grid_max_value": 1.0,
-                        "grid_resolution": 0.2,
-                        "grid_length_x": 70.0,
-                        "grid_length_y": 70.0,
-                        "grid_position_x": 0.0,
-                        "grid_position_y": 0.0,
-                        "maximum_lidar_height_thres": 0.3,
-                        "minimum_lidar_height_thres": -2.2,
-                        "use_wayarea": True,
-                        "use_parkinglot": True,
-                        "use_objects": True,
-                        "use_points": True,
-                        "expand_polygon_size": 1.0,
-                        "size_of_expansion_kernel": 9,
-                    },
-                    vehicle_info_param,
-                ],
-                extra_arguments=[
-                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
-                ],
-            ),
-            ComposableNode(
-                package="freespace_planner",
-                plugin="freespace_planner::FreespacePlannerNode",
-                name="freespace_planner",
-                remappings=[
-                    ("~/input/route", "/planning/mission_planning/route"),
-                    ("~/input/occupancy_grid", "costmap_generator/occupancy_grid"),
-                    ("~/input/scenario", "/planning/scenario_planning/scenario"),
-                    ("~/input/odometry", "/localization/kinematic_state"),
-                    ("~/output/trajectory", "/planning/scenario_planning/parking/trajectory"),
-                    ("is_completed", "/planning/scenario_planning/parking/is_completed"),
-                ],
-                parameters=[
-                    freespace_planner_param,
-                    vehicle_info_param,
-                ],
-                extra_arguments=[
-                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
-                ],
-            ),
-        ],
-    )
-
-    group = GroupAction(
-        [
-            PushRosNamespace("parking"),
-            container,
-        ]
-    )
-
-    return [group]
-
-
-def generate_launch_description():
-    launch_arguments = []
-
-    def add_launch_arg(name: str, default_value=None, description=None):
-        launch_arguments.append(
-            DeclareLaunchArgument(name, default_value=default_value, description=description)
-        )
-
-    add_launch_arg(
-        "vehicle_param_file",
-        [
-            FindPackageShare("vehicle_info_util"),
-            "/config/vehicle_info.param.yaml",
-        ],
-        "path to the parameter file of vehicle information",
-    )
-
-    # component
-    add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication")
-    add_launch_arg("use_multithread", "false", "use multithread")
-
-    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")),
-    )
-
-    return launch.LaunchDescription(
-        launch_arguments
-        + [
-            set_container_executable,
-            set_container_mt_executable,
-        ]
-        + [OpaqueFunction(function=launch_setup)]
-    )
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml
index 741df95937c42..98315919b540a 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml
@@ -1,32 +1,38 @@
 <launch>
-  <arg name="vehicle_param_file"/>
-
   <!-- parking scenario -->
-  <group>
+  <group if="$(var launch_parking_module)">
     <push-ros-namespace namespace="parking"/>
-    <group>
-      <include file="$(find-pkg-share costmap_generator)/launch/costmap_generator.launch.xml">
-        <arg name="input_objects" value="/perception/object_recognition/objects"/>
-        <arg name="input_points_no_ground" value="/perception/obstacle_segmentation/pointcloud"/>
-        <arg name="input_lanelet_map" value="/map/vector_map"/>
-        <arg name="input_scenario" value="/planning/scenario_planning/scenario"/>
-        <arg name="output_grid_map" value="costmap_generator/grid_map"/>
-        <arg name="output_occupancy_grid" value="costmap_generator/occupancy_grid"/>
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
-      </include>
-    </group>
 
-    <group>
-      <include file="$(find-pkg-share freespace_planner)/launch/freespace_planner.launch.xml">
-        <arg name="input_route" value="/planning/mission_planning/route"/>
-        <arg name="input_occupancy_grid" value="costmap_generator/occupancy_grid"/>
-        <arg name="input_scenario" value="/planning/scenario_planning/scenario"/>
-        <arg name="input_odometry" value="/localization/kinematic_state"/>
-        <arg name="output_trajectory" value="/planning/scenario_planning/parking/trajectory"/>
-        <arg name="is_completed" value="/planning/scenario_planning/parking/is_completed"/>
-        <arg name="param_file" value="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml"/>
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
-      </include>
-    </group>
+    <node_container pkg="rclcpp_components" exec="$(var container_type)" name="parking_container" namespace="" output="screen">
+      <composable_node pkg="costmap_generator" plugin="CostmapGenerator" name="costmap_generator" namespace="">
+        <!-- topic remap -->
+        <remap from="~/input/objects" to="/perception/object_recognition/objects"/>
+        <remap from="~/input/points_no_ground" to="/perception/obstacle_segmentation/pointcloud"/>
+        <remap from="~/input/vector_map" to="/map/vector_map"/>
+        <remap from="~/input/scenario" to="/planning/scenario_planning/scenario"/>
+        <remap from="~/output/grid_map" to="costmap_generator/grid_map"/>
+        <remap from="~/output/occupancy_grid" to="costmap_generator/occupancy_grid"/>
+        <!-- params -->
+        <param from="$(var vehicle_param_file)"/>
+        <param from="$(var costmap_generator_param_path)"/>
+        <extra_arg name="use_intra_process_comms" value="false"/>
+      </composable_node>
+
+      <composable_node pkg="freespace_planner" plugin="freespace_planner::FreespacePlannerNode" name="freespace_planner" namespace="">
+        <!-- topic remap -->
+        <remap from="~/input/route" to="/planning/mission_planning/route"/>
+        <remap from="~/input/scenario" to="/planning/scenario_planning/scenario"/>
+        <remap from="~/input/occupancy_grid" to="costmap_generator/occupancy_grid"/>
+        <remap from="~/input/odometry" to="/localization/kinematic_state"/>
+        <remap from="~/output/trajectory" to="/planning/scenario_planning/parking/trajectory"/>
+        <remap from="is_completed" to="/planning/scenario_planning/parking/is_completed"/>
+        <!-- params -->
+        <param from="$(var vehicle_param_file)"/>
+        <param from="$(var freespace_planner_param_path)"/>
+        <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>
 </launch>
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
index 8c015892d06b5..66c90ef2ff833 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
@@ -54,8 +54,8 @@
     </group>
     <!-- parking -->
     <group>
-      <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/parking.launch.py">
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
+      <include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/parking.launch.xml">
+        <arg name="container_type" value="component_container_mt"/>
       </include>
     </group>
   </group>