From a136f54e6787fcd1f53f3d8066f5a49b54d24d51 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Wed, 14 Aug 2024 14:05:11 +0900
Subject: [PATCH] rebase to new mode name

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../tier4_simulator_launch/launch/simulator.launch.xml | 10 +++++++---
 .../launch/simple_planning_simulator.launch.py         |  8 ++++++--
 2 files changed, 13 insertions(+), 5 deletions(-)

diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml
index 9abf8021b87ff..af7144817ecec 100644
--- a/launch/tier4_simulator_launch/launch/simulator.launch.xml
+++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml
@@ -154,7 +154,8 @@
   </group>
 
   <group if="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;pose_twist_estimator&quot;')">
-    <group> <!-- start name space localization -->
+    <group>
+      <!-- start name space localization -->
       <push-ros-namespace namespace="localization"/>
       <group>
         <push-ros-namespace namespace="util"/>
@@ -180,7 +181,8 @@
         <push-ros-namespace namespace="pose_twist_fusion_filter"/>
         <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml"/>
       </group>
-    </group> <!-- end name space localization -->
+    </group>
+    <!-- end name space localization -->
     <group>
       <push-ros-namespace namespace="sensing"/>
       <arg name="input_vehicle_velocity_topic" default="/vehicle/status/velocity_status"/>
@@ -210,12 +212,14 @@
   <!-- 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"/>
+    <let name="motion_publish_mode" value="pose_only" if="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;pose_twist_estimator&quot;')"/>
+    <let name="motion_publish_mode" value="full_motion" unless="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;pose_twist_estimator&quot;')"/>
     <include file="$(find-pkg-share simple_planning_simulator)/launch/simple_planning_simulator.launch.py">
       <arg name="vehicle_info_param_file" value="$(var vehicle_info_param_file)"/>
       <arg name="simulator_model_param_file" value="$(var simulator_model)"/>
       <arg name="initial_engage_state" value="$(var initial_engage_state)"/>
       <arg name="raw_vehicle_cmd_converter_param_path" value="$(var raw_vehicle_cmd_converter_param_path)"/>
-      <arg name="launch_localization_for_sim_vehicle" value="$(var launch_localization_for_sim_vehicle)"/>
+      <arg name="motion_publish_mode" value="$(var motion_publish_mode)"/>
     </include>
   </group>
 </launch>
diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
index 9e0f41ba8a85f..092afe782cfc6 100644
--- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
+++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
@@ -65,7 +65,7 @@ def launch_setup(context, *args, **kwargs):
     ]
 
     # Additional remappings
-    if LaunchConfiguration("launch_localization_for_sim_vehicle").perform(context) == "true":
+    if LaunchConfiguration("motion_publish_mode").perform(context) == "pose_only":
         remappings.extend(
             [
                 ("output/odometry", "/simulation/debug/localization/kinematic_state"),
@@ -73,7 +73,7 @@ def launch_setup(context, *args, **kwargs):
                 ("output/pose", "/localization/pose_estimator/pose_with_covariance"),
             ]
         )
-    else:
+    elif LaunchConfiguration("motion_publish_mode").perform(context) == "full_motion":
         remappings.extend(
             [
                 ("output/odometry", "/localization/kinematic_state"),
@@ -84,6 +84,10 @@ def launch_setup(context, *args, **kwargs):
                 ),
             ]
         )
+    else:
+        raise ValueError(
+            "Invalid motion_publish_mode specified. Please choose 'pose_only' or 'full_motion'."
+        )
 
     simple_planning_simulator_node = Node(
         package="simple_planning_simulator",