From 913abba0d234b90a88956f2a638a2a1e2dca8de4 Mon Sep 17 00:00:00 2001
From: Kah Hooi Tan <41041286+tkhmy@users.noreply.github.com>
Date: Fri, 16 Sep 2022 15:14:09 +0900
Subject: [PATCH] feat: sync launch for init pose (#486)

* add rviz helper for ad api

Signed-off-by: khtan <tkh.my.p@gmail.com>

* sync for init pose

Signed-off-by: khtan <tkh.my.p@gmail.com>

* add missing sync

Signed-off-by: khtan <tkh.my.p@gmail.com>

* add rviz adaptor

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix launch_dummy_localization

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* add dependency

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* add dependency

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* ci(pre-commit): autofix

Signed-off-by: khtan <tkh.my.p@gmail.com>
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
Co-authored-by: Takagi, Isamu <isamu.takagi@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
---
 autoware_api_launch/launch/autoware_api.launch.xml     |  6 ++++++
 .../launch/include/internal_api_adaptor.launch.py      |  6 ------
 .../launch/include/internal_api_relay.launch.xml       |  6 ------
 autoware_api_launch/package.xml                        |  1 +
 autoware_launch/launch/autoware.launch.xml             |  6 ++----
 autoware_launch/launch/logging_simulator.launch.xml    |  6 ++----
 autoware_launch/launch/planning_simulator.launch.xml   |  9 +++++----
 autoware_launch/package.xml                            |  1 +
 localization_launch/launch/util/util.launch.xml        |  7 ++++++-
 localization_launch/package.xml                        |  1 +
 simulator_launch/launch/simulator.launch.xml           | 10 ++++++++++
 ...ervice_state_monitor.planning_simulation.param.yaml |  4 ++--
 12 files changed, 36 insertions(+), 27 deletions(-)

diff --git a/autoware_api_launch/launch/autoware_api.launch.xml b/autoware_api_launch/launch/autoware_api.launch.xml
index c3562857f..45c1f62d1 100644
--- a/autoware_api_launch/launch/autoware_api.launch.xml
+++ b/autoware_api_launch/launch/autoware_api.launch.xml
@@ -1,6 +1,12 @@
 <launch>
 
   <arg name="respawn" default="true" />
+  <arg name="launch_default_ad_api" default="true"/>
+
+  <!-- AD API -->
+  <group if="$(var launch_default_ad_api)">
+    <include file="$(find-pkg-share default_ad_api)/launch/default_ad_api.launch.py"/>
+  </group>
 
   <!-- awapi (deprecated) -->
   <group>
diff --git a/autoware_api_launch/launch/include/internal_api_adaptor.launch.py b/autoware_api_launch/launch/include/internal_api_adaptor.launch.py
index 4b8513f54..422c35abe 100644
--- a/autoware_api_launch/launch/include/internal_api_adaptor.launch.py
+++ b/autoware_api_launch/launch/include/internal_api_adaptor.launch.py
@@ -13,7 +13,6 @@
 # limitations under the License.
 
 import launch
-from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.descriptions import ComposableNode
 
@@ -29,12 +28,7 @@ def _create_api_node(node_name, class_name, **kwargs):
 
 
 def generate_launch_description():
-    param_initial_pose = {
-        "init_simulator_pose": LaunchConfiguration("init_simulator_pose"),
-        "init_localization_pose": LaunchConfiguration("init_localization_pose"),
-    }
     components = [
-        _create_api_node("initial_pose", "InitialPose", parameters=[param_initial_pose]),
         _create_api_node("iv_msgs", "IVMsgs"),
         _create_api_node("operator", "Operator"),
         _create_api_node("route", "Route"),
diff --git a/autoware_api_launch/launch/include/internal_api_relay.launch.xml b/autoware_api_launch/launch/include/internal_api_relay.launch.xml
index 810e6a566..e850fb3dc 100644
--- a/autoware_api_launch/launch/include/internal_api_relay.launch.xml
+++ b/autoware_api_launch/launch/include/internal_api_relay.launch.xml
@@ -16,11 +16,5 @@
       <param name="output_topic" value="/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/input/external_crosswalk_states"/>
       <param name="type" value="tier4_api_msgs/msg/CrosswalkStatus"/>
     </node>
-    <!-- TODO(Takagi, Isamu): workaround for topic check -->
-    <node pkg="topic_tools" exec="relay" name="initial_pose_2d">
-      <param name="input_topic" value="/initialpose"/>
-      <param name="output_topic" value="/initialpose2d"/>
-      <param name="type" value="geometry_msgs/msg/PoseWithCovarianceStamped"/>
-    </node>
   </group>
 </launch>
diff --git a/autoware_api_launch/package.xml b/autoware_api_launch/package.xml
index 577788065..71a37f44f 100644
--- a/autoware_api_launch/package.xml
+++ b/autoware_api_launch/package.xml
@@ -12,6 +12,7 @@
   <exec_depend>autoware_iv_external_api_adaptor</exec_depend>
   <exec_depend>autoware_iv_internal_api_adaptor</exec_depend>
   <exec_depend>awapi_awiv_adapter</exec_depend>
+  <exec_depend>default_ad_api</exec_depend>
   <exec_depend>path_distance_calculator</exec_depend>
   <exec_depend>topic_tools</exec_depend>
 
diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml
index d4975c646..5f2af3643 100644
--- a/autoware_launch/launch/autoware.launch.xml
+++ b/autoware_launch/launch/autoware.launch.xml
@@ -95,10 +95,7 @@
 
   <!-- Autoware API -->
   <group>
-    <include file="$(find-pkg-share autoware_api_launch)/launch/autoware_api.launch.xml">
-      <arg name="init_simulator_pose" value="false"/>
-      <arg name="init_localization_pose" value="true"/>
-    </include>
+    <include file="$(find-pkg-share autoware_api_launch)/launch/autoware_api.launch.xml"/>
   </group>
 
   <!-- Tools -->
@@ -112,6 +109,7 @@
       args="-d $(find-pkg-share autoware_launch)/rviz/autoware.rviz -s $(find-pkg-share autoware_launch)/rviz/image/autoware.png"
       if="$(var rviz)"
     />
+    <include file="$(find-pkg-share ad_api_adaptors)/launch/rviz_adaptors.launch.xml" if="$(var rviz)"/>
 
     <!-- Web Controller -->
     <include file="$(find-pkg-share web_controller)/launch/web_controller.launch.xml"/>
diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml
index c143f2c51..3a360d460 100644
--- a/autoware_launch/launch/logging_simulator.launch.xml
+++ b/autoware_launch/launch/logging_simulator.launch.xml
@@ -111,10 +111,7 @@
 
   <!-- Autoware API -->
   <group>
-    <include file="$(find-pkg-share autoware_api_launch)/launch/autoware_api.launch.xml">
-      <arg name="init_simulator_pose" value="false"/>
-      <arg name="init_localization_pose" value="true"/>
-    </include>
+    <include file="$(find-pkg-share autoware_api_launch)/launch/autoware_api.launch.xml"/>
   </group>
 
   <!-- Tools -->
@@ -128,6 +125,7 @@
       args="-d $(find-pkg-share autoware_launch)/rviz/autoware.rviz -s $(find-pkg-share autoware_launch)/rviz/image/autoware.png"
       if="$(var rviz)"
     />
+    <include file="$(find-pkg-share ad_api_adaptors)/launch/rviz_adaptors.launch.xml" if="$(var rviz)"/>
 
     <!-- Web Controller -->
     <include file="$(find-pkg-share web_controller)/launch/web_controller.launch.xml"/>
diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml
index e9cb8c5f1..b7fb373d5 100644
--- a/autoware_launch/launch/planning_simulator.launch.xml
+++ b/autoware_launch/launch/planning_simulator.launch.xml
@@ -68,10 +68,7 @@
 
   <!-- Autoware API -->
   <group>
-    <include file="$(find-pkg-share autoware_api_launch)/launch/autoware_api.launch.xml">
-      <arg name="init_simulator_pose" value="true"/>
-      <arg name="init_localization_pose" value="false"/>
-    </include>
+    <include file="$(find-pkg-share autoware_api_launch)/launch/autoware_api.launch.xml"/>
   </group>
 
   <!-- Tools -->
@@ -85,6 +82,7 @@
       args="-d $(var rviz_config) -s $(find-pkg-share autoware_launch)/rviz/image/autoware.png"
       if="$(var rviz)"
     />
+    <include file="$(find-pkg-share ad_api_adaptors)/launch/rviz_adaptors.launch.xml" if="$(var rviz)"/>
 
     <!-- Web Controller -->
     <include file="$(find-pkg-share web_controller)/launch/web_controller.launch.xml"/>
@@ -96,10 +94,13 @@
     <let name="launch_dummy_perception" value="true" unless="$(var scenario_simulation)"/>
     <let name="launch_dummy_vehicle" value="false" if="$(var scenario_simulation)"/>
     <let name="launch_dummy_vehicle" value="true" unless="$(var scenario_simulation)"/>
+    <let name="launch_dummy_localization" value="false" if="$(var scenario_simulation)"/>
+    <let name="launch_dummy_localization" value="true" unless="$(var scenario_simulation)"/>
 
     <include file="$(find-pkg-share simulator_launch)/launch/simulator.launch.xml">
       <arg name="launch_dummy_perception" value="$(var launch_dummy_perception)"/>
       <arg name="launch_dummy_vehicle" value="$(var launch_dummy_vehicle)"/>
+      <arg name="launch_dummy_localization" value="$(var launch_dummy_localization)"/>
       <arg name="perception/enable_detection_failure" value="$(var perception/enable_detection_failure)"/>
       <arg name="perception/enable_object_recognition" value="$(var perception/enable_object_recognition)"/>
       <arg name="perception/enable_traffic_light" value="$(var perception/enable_traffic_light)"/>
diff --git a/autoware_launch/package.xml b/autoware_launch/package.xml
index fa7f7aad1..037f453fa 100644
--- a/autoware_launch/package.xml
+++ b/autoware_launch/package.xml
@@ -9,6 +9,7 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
+  <exec_depend>ad_api_adaptors</exec_depend>
   <exec_depend>autoware_api_launch</exec_depend>
   <exec_depend>control_launch</exec_depend>
   <exec_depend>global_parameter_loader</exec_depend>
diff --git a/localization_launch/launch/util/util.launch.xml b/localization_launch/launch/util/util.launch.xml
index 4b80a343d..783eee8ad 100644
--- a/localization_launch/launch/util/util.launch.xml
+++ b/localization_launch/launch/util/util.launch.xml
@@ -7,7 +7,12 @@
   <arg name="use_intra_process" default="true" description="use ROS2 component container communication"/>
 
   <!-- pose_initializer -->
-  <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml" />
+  <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
+    <arg name="gnss_enabled" value="true"/>
+    <arg name="ndt_enabled" value="true"/>
+    <arg name="stop_check_enabled" value="true"/>
+  </include>
+  <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
 
   <!-- util -->
   <include file="$(find-pkg-share localization_launch)/launch/util/util.launch.py">
diff --git a/localization_launch/package.xml b/localization_launch/package.xml
index a6dbe359c..c61aaeed7 100644
--- a/localization_launch/package.xml
+++ b/localization_launch/package.xml
@@ -11,6 +11,7 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
+  <exec_depend>automatic_pose_initializer</exec_depend>
   <exec_depend>ekf_localizer</exec_depend>
   <exec_depend>gyro_odometer</exec_depend>
   <exec_depend>ndt_scan_matcher</exec_depend>
diff --git a/simulator_launch/launch/simulator.launch.xml b/simulator_launch/launch/simulator.launch.xml
index 5ebe48cd9..08d27a73b 100644
--- a/simulator_launch/launch/simulator.launch.xml
+++ b/simulator_launch/launch/simulator.launch.xml
@@ -1,6 +1,7 @@
 <launch>
   <arg name="launch_dummy_perception" />
   <arg name="launch_dummy_vehicle"/>
+  <arg name="launch_dummy_localization"/>
 
   <arg name="perception/enable_detection_failure" default="true" description="enable to simulate detection failure when using dummy perception"/>
   <arg name="perception/enable_object_recognition" default="false" description="enable object recognition"/>
@@ -74,6 +75,15 @@
     </group>
   </group>
 
+  <!-- Dummy localization -->
+  <group if="$(var launch_dummy_localization)">
+    <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
+      <arg name="gnss_enabled" value="false"/>
+      <arg name="ndt_enabled" value="false"/>
+      <arg name="stop_check_enabled" value="false"/>
+    </include>
+  </group>
+
   <!-- Simulator model -->
   <group if="$(var launch_dummy_vehicle)">
     <arg name="simulator_model" default="$(var vehicle_model_pkg)/config/simulator_model.param.yaml" description="path to the file of simulator model"/>
diff --git a/system_launch/config/ad_service_state_monitor.planning_simulation.param.yaml b/system_launch/config/ad_service_state_monitor.planning_simulation.param.yaml
index b20ffec09..51fbe99dd 100644
--- a/system_launch/config/ad_service_state_monitor.planning_simulation.param.yaml
+++ b/system_launch/config/ad_service_state_monitor.planning_simulation.param.yaml
@@ -24,7 +24,7 @@
       names: [
         "/map/vector_map",
         "/perception/object_recognition/objects",
-        "/initialpose2d",
+        "/initialpose3d",
         "/planning/mission_planning/route",
         "/planning/scenario_planning/trajectory",
         "/control/trajectory_follower/control_cmd",
@@ -48,7 +48,7 @@
           warn_rate: 5.0
           type: "autoware_auto_perception_msgs/msg/PredictedObjects"
 
-        /initialpose2d:
+        /initialpose3d:
           module: "localization"
           timeout: 0.0
           warn_rate: 0.0