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>
tier4 · Sep 20, 2022 · 913abba · 913abba
1 parent d2faafd
commit 913abba
Show file tree

Hide file tree

Showing 12 changed files with 36 additions and 27 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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