From ddf5c5419cd0f4e2b1f6dc7da69b14e9d6f6e18e Mon Sep 17 00:00:00 2001
From: Yukihiro Saito <yukky.saito@gmail.com>
Date: Fri, 8 Jul 2022 13:13:17 +0900
Subject: [PATCH] feat(tier4_perception_launch): revert #1263 (#1285)

---
 ...ra_lidar_fusion_based_detection.launch.xml |  3 +-
 ...ar_radar_fusion_based_detection.launch.xml | 68 -------------------
 .../detection/detection.launch.xml            | 57 ++--------------
 .../lidar_based_detection.launch.xml          |  3 +-
 .../lidar_radar_based_detection.launch.xml    | 32 ---------
 .../radar_based_detection.launch.xml          | 12 ----
 .../ground_segmentation.launch.py             | 16 +----
 .../launch/perception.launch.xml              | 13 ++--
 8 files changed, 17 insertions(+), 187 deletions(-)
 delete mode 100644 launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
 delete mode 100644 launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
 delete mode 100644 launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml

diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
index 81e7b2e097551..46ddadad521cd 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -2,7 +2,6 @@
 <launch>
   <arg name="input/pointcloud"/>
   <arg name="input/obstacle_segmentation/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
-  <arg name="output/objects" default="objects"/>
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
   <arg name="camera_info0" default="/camera_info" description="camera info topic name"/>
   <arg name="image_raw1" default="/image_raw1"/>
@@ -185,7 +184,7 @@
   <group>
     <include file="$(find-pkg-share detected_object_validation)/launch/object_lanelet_filter.launch.xml">
       <arg name="input/object" value="objects_before_filter"/>
-      <arg name="output/object" value="$(var output/objects)"/>
+      <arg name="output/object" value="objects"/>
     </include>
   </group>
 </launch>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
deleted file mode 100644
index 95cdaae066674..0000000000000
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
+++ /dev/null
@@ -1,68 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-  <!-- lidar param -->
-  <arg name="input/pointcloud"/>
-  <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
-  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
-
-  <!-- camera param -->
-  <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
-  <arg name="camera_info0" default="/camera_info" description="camera info topic name"/>
-  <arg name="image_raw1" default="/image_raw1"/>
-  <arg name="camera_info1" default="/camera_info1"/>
-  <arg name="image_raw2" default="/image_raw2"/>
-  <arg name="camera_info2" default="/camera_info2"/>
-  <arg name="image_raw3" default="/image_raw3"/>
-  <arg name="camera_info3" default="/camera_info3"/>
-  <arg name="image_raw4" default="/image_raw4"/>
-  <arg name="camera_info4" default="/camera_info4"/>
-  <arg name="image_raw5" default="/image_raw5"/>
-  <arg name="camera_info5" default="/camera_info5"/>
-  <arg name="image_raw6" default="/image_raw6"/>
-  <arg name="camera_info6" default="/camera_info6"/>
-  <arg name="image_raw7" default="/image_raw7"/>
-  <arg name="camera_info7" default="/camera_info7"/>
-  <arg name="image_number" default="1" description="choose image raw number(0-7)"/>
-
-  <!-- camera lidar fusion based detection-->
-  <group>
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml">
-      <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
-      <arg name="output/objects" value="camera_lidar_fusion/objects"/>
-      <arg name="image_raw0" value="$(var image_raw0)"/>
-      <arg name="camera_info0" value="$(var camera_info0)"/>
-      <arg name="image_raw1" value="$(var image_raw1)"/>
-      <arg name="camera_info1" value="$(var camera_info1)"/>
-      <arg name="image_raw2" value="$(var image_raw2)"/>
-      <arg name="camera_info2" value="$(var camera_info2)"/>
-      <arg name="image_raw3" value="$(var image_raw3)"/>
-      <arg name="camera_info3" value="$(var camera_info3)"/>
-      <arg name="image_raw4" value="$(var image_raw4)"/>
-      <arg name="camera_info4" value="$(var camera_info4)"/>
-      <arg name="image_raw5" value="$(var image_raw5)"/>
-      <arg name="camera_info5" value="$(var camera_info5)"/>
-      <arg name="image_raw6" value="$(var image_raw6)"/>
-      <arg name="camera_info6" value="$(var camera_info6)"/>
-      <arg name="image_raw7" value="$(var image_raw7)"/>
-      <arg name="camera_info7" value="$(var camera_info7)"/>
-      <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)"/>
-    </include>
-  </group>
-
-  <!-- radar detection-->
-  <group>
-    <push-ros-namespace namespace="radar"/>
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/radar_based_detection.launch.xml">
-      <arg name="output/objects" value="front_center/objects"/>
-    </include>
-  </group>
-
-  <!-- camera lidar radar fusion-->
-  <include file="$(find-pkg-share radar_fusion_to_detected_object)/launch/radar_object_fusion_to_detected_object.launch.xml">
-    <arg name="input/objects" value="camera_lidar_fusion/objects"/>
-    <arg name="input/radars" value="radar/objects"/>
-    <arg name="output/objects" value="objects"/>
-  </include>
-</launch>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
index 19afdf22d9cbd..6a975a2874627 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
@@ -1,13 +1,8 @@
 <?xml version="1.0"?>
 <launch>
-  <arg name="mode" default="lidar" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
-
-  <!-- lidar param -->
   <arg name="input/pointcloud"/>
-  <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
-  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
-
-  <!-- camera param -->
+  <arg name="mode" description="options: `camera_lidar_fusion`, `lidar` or `camera`"/>
+  <arg name="lidar_detection_model" default="apollo" description="options: `centerpoint`, `apollo`"/>
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
   <arg name="camera_info0" default="/camera_info" description="camera info topic name"/>
   <arg name="image_raw1" default=""/>
@@ -25,34 +20,7 @@
   <arg name="image_raw7" default=""/>
   <arg name="camera_info7" default=""/>
   <arg name="image_number" default="1" description="choose image raw number(0-7)"/>
-
-  <!-- radar param -->
-
-  <!-- camera lidar radar fusion based detection-->
-  <group if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_radar_fusion&quot;')">
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml">
-      <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
-      <arg name="image_raw0" value="$(var image_raw0)"/>
-      <arg name="camera_info0" value="$(var camera_info0)"/>
-      <arg name="image_raw1" value="$(var image_raw1)"/>
-      <arg name="camera_info1" value="$(var camera_info1)"/>
-      <arg name="image_raw2" value="$(var image_raw2)"/>
-      <arg name="camera_info2" value="$(var camera_info2)"/>
-      <arg name="image_raw3" value="$(var image_raw3)"/>
-      <arg name="camera_info3" value="$(var camera_info3)"/>
-      <arg name="image_raw4" value="$(var image_raw4)"/>
-      <arg name="camera_info4" value="$(var camera_info4)"/>
-      <arg name="image_raw5" value="$(var image_raw5)"/>
-      <arg name="camera_info5" value="$(var camera_info5)"/>
-      <arg name="image_raw6" value="$(var image_raw6)"/>
-      <arg name="camera_info6" value="$(var camera_info6)"/>
-      <arg name="image_raw7" value="$(var image_raw7)"/>
-      <arg name="camera_info7" value="$(var camera_info7)"/>
-      <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)"/>
-    </include>
-  </group>
+  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
 
   <!-- camera lidar fusion based detection-->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_fusion&quot;')">
@@ -79,16 +47,6 @@
       <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
     </include>
   </group>
-
-  <!-- lidar radar based detection-->
-  <group if="$(eval '&quot;$(var mode)&quot;==&quot;lidar_radar_fusion&quot;')">
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml">
-      <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
-      <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
-      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
-    </include>
-  </group>
-
   <!-- lidar based detection-->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;lidar&quot;')">
     <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/lidar_based_detection.launch.xml">
@@ -97,11 +55,6 @@
       <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
     </include>
   </group>
-
-  <!-- radar based detection-->
-  <group if="$(eval '&quot;$(var mode)&quot;==&quot;radar&quot;')">
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/radar_based_detection.launch.xml">
-      <arg name="output/objects" value="objects"/>
-    </include>
-  </group>
+  <!-- camera based detection-->
+  <group if="$(eval '&quot;$(var mode)&quot;==&quot;camera&quot;')"/>
 </launch>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
index d36b9d6265c34..e65f8162e2d49 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -2,7 +2,6 @@
 <launch>
   <arg name="input/pointcloud"/>
   <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`"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
@@ -108,7 +107,7 @@
   <group>
     <include file="$(find-pkg-share detected_object_validation)/launch/object_lanelet_filter.launch.xml">
       <arg name="input/object" value="objects_before_filter"/>
-      <arg name="output/object" value="$(var output/objects)"/>
+      <arg name="output/object" value="objects"/>
     </include>
   </group>
 </launch>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
deleted file mode 100644
index 8b7e9a3089fa0..0000000000000
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
+++ /dev/null
@@ -1,32 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-  <!-- lidar param -->
-  <arg name="input/pointcloud"/>
-  <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
-  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
-
-  <!-- lidar based detection-->
-  <group>
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/lidar_based_detection.launch.xml">
-      <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
-      <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)"/>
-    </include>
-  </group>
-
-  <!-- radar detection-->
-  <group>
-    <push-ros-namespace namespace="radar"/>
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/radar_based_detection.launch.xml">
-      <arg name="output/objects" value="objects"/>
-    </include>
-  </group>
-
-  <!-- lidar radar fusion-->
-  <include file="$(find-pkg-share radar_fusion_to_detected_object)/launch/radar_object_fusion_to_detected_object.launch.xml">
-    <arg name="input/objects" value="lidar/objects"/>
-    <arg name="input/radars" value="radar/objects"/>
-    <arg name="output/objects" value="objects"/>
-  </include>
-</launch>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml
deleted file mode 100644
index 21de187f126e5..0000000000000
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml
+++ /dev/null
@@ -1,12 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-  <arg name="output/objects" default="objects"/>
-
-  <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
-    <arg name="input/radar_objects" value="/sensing/radar/front_center/objects_raw"/>
-    <arg name="input/odometry" value="/localization/kinematic_state"/>
-    <arg name="output/radar_objects" value="$(var output/objects)"/>
-    <arg name="update_rate_hz" value="20.0"/>
-    <arg name="use_twist_compensation" value="true"/>
-  </include>
-</launch>
diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
index e580eae17833d..654af3d9bfd0b 100644
--- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
+++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
@@ -75,10 +75,7 @@ def create_additional_pipeline(self, lidar_name):
                 plugin="pointcloud_preprocessor::CropBoxFilterComponent",
                 name=f"{lidar_name}_crop_box_filter",
                 remappings=[
-                    (
-                        "input",
-                        f"/sensing/lidar/{lidar_name}/outlier_filtered/pointcloud",
-                    ),
+                    ("input", f"/sensing/lidar/{lidar_name}/outlier_filtered/pointcloud"),
                     ("output", f"{lidar_name}/range_cropped/pointcloud"),
                 ],
                 parameters=[
@@ -299,10 +296,7 @@ def create_time_series_outlier_filter_components(input_topic, output_topic):
                 plugin="occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent",
                 name="occupancy_grid_map_outlier_filter",
                 remappings=[
-                    (
-                        "~/input/occupancy_grid_map",
-                        "/perception/occupancy_grid_map/map",
-                    ),
+                    ("~/input/occupancy_grid_map", "/perception/occupancy_grid_map/map"),
                     ("~/input/pointcloud", input_topic),
                     ("~/output/pointcloud", output_topic),
                 ],
@@ -343,11 +337,7 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic):
                             ]
                         ),
                         "elevation_map_directory": PathJoinSubstitution(
-                            [
-                                FindPackageShare("elevation_map_loader"),
-                                "data",
-                                "elevation_maps",
-                            ]
+                            [FindPackageShare("elevation_map_loader"), "data", "elevation_maps"]
                         ),
                         "use_elevation_map_cloud_publisher": False,
                     }
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index 5c634930e7fa9..51ee075f27d0e 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <launch>
+  <arg name="vehicle_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml" description="path to the file of vehicle info yaml"/>
   <!-- common parameters -->
   <arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud" description="The topic will be used in the detection module"/>
-  <arg name="mode" default="camera_lidar_fusion" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
-  <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
+  <arg name="mode" description="options: `camera_lidar_fusion`, `lidar` or `camera`"/>
   <arg name="image_raw0" default="/sensing/camera/camera0/image_rect_color" description="image raw topic name"/>
   <arg name="camera_info0" default="/sensing/camera/camera0/camera_info" description="camera info topic name"/>
   <arg name="image_raw1" default="/sensing/camera/camera1/image_rect_color"/>
@@ -40,11 +40,11 @@
       <push-ros-namespace namespace="obstacle_segmentation"/>
       <include file="$(find-pkg-share tier4_perception_launch)/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py">
         <arg name="base_frame" value="base_link"/>
+        <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
         <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 pointcloud_container_name)"/>
-        <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
       </include>
     </group>
 
@@ -53,7 +53,7 @@
       <push-ros-namespace namespace="occupancy_grid_map"/>
       <include file="$(find-pkg-share tier4_perception_launch)/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py">
         <arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/single_frame/pointcloud_raw"/>
-        <arg name="input/raw_pointcloud" value="$(var input/pointcloud)"/>
+        <arg name="input/raw_pointcloud" value="/sensing/lidar/concatenated/pointcloud"/>
         <arg name="output" value="/perception/occupancy_grid_map/map"/>
         <arg name="use_intra_process" value="true"/>
         <arg name="use_multithread" value="true"/>
@@ -69,9 +69,8 @@
       <group>
         <push-ros-namespace namespace="detection"/>
         <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detection.launch.xml">
-          <arg name="mode" value="$(var mode)"/>
           <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
-          <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
+          <arg name="mode" value="$(var mode)"/>
           <arg name="image_raw0" value="$(var image_raw0)"/>
           <arg name="camera_info0" value="$(var camera_info0)"/>
           <arg name="image_raw1" value="$(var image_raw1)"/>
@@ -90,6 +89,8 @@
           <arg name="camera_info7" value="$(var camera_info7)"/>
           <arg name="image_number" value="$(var image_number)"/>
           <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
+          <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+          <arg name="container_name" value="$(var pointcloud_container_name)"/>
         </include>
       </group>
       <!-- tracking module -->