From e9835c08015d13905f706868de6346df9ad5ae73 Mon Sep 17 00:00:00 2001
From: Yamato Ando <yamato.ando@tier4.jp>
Date: Wed, 27 Mar 2024 18:30:11 +0900
Subject: [PATCH 1/2] feat(pose_initilizer): set intial pose directly

Signed-off-by: Yamato Ando <yamato.ando@tier4.jp>
---
 .../config/localization/pose_initializer.param.yaml           | 4 ++++
 .../launch/components/tier4_localization_component.launch.xml | 2 ++
 2 files changed, 6 insertions(+)

diff --git a/autoware_launch/config/localization/pose_initializer.param.yaml b/autoware_launch/config/localization/pose_initializer.param.yaml
index 88acbfb526..5f669526b4 100644
--- a/autoware_launch/config/localization/pose_initializer.param.yaml
+++ b/autoware_launch/config/localization/pose_initializer.param.yaml
@@ -1,5 +1,9 @@
 /**:
   ros__parameters:
+    initial_pose_directly:
+      enable: $(var initial_pose_directly/enable)
+      pose: $(var initial_pose_directly/pose)
+
     gnss_pose_timeout: 3.0 # [sec]
     stop_check_duration: 3.0 # [sec]
     ekf_enabled: $(var ekf_enabled)
diff --git a/autoware_launch/launch/components/tier4_localization_component.launch.xml b/autoware_launch/launch/components/tier4_localization_component.launch.xml
index b8387c1d25..7dfe0a3dea 100644
--- a/autoware_launch/launch/components/tier4_localization_component.launch.xml
+++ b/autoware_launch/launch/components/tier4_localization_component.launch.xml
@@ -5,6 +5,7 @@
   <arg name="twist_source" default="gyro_odom" description="select twist_estimator. gyro_odom, eagleye"/>
   <arg name="input_pointcloud" default="/sensing/lidar/concatenated/pointcloud" description="The topic will be used in the localization util module"/>
   <arg name="localization_pointcloud_container_name" default="/pointcloud_container" description="The target container to which pointcloud preprocessing nodes in localization be attached"/>
+  <arg name="initial_pose" default="[]" description="initial pose (x, y, z, quat_x, quat_y, quat_z, quat_w)"/>
 
   <group>
     <include file="$(find-pkg-share tier4_localization_launch)/launch/localization.launch.xml">
@@ -13,6 +14,7 @@
       <arg name="system_run_mode" value="$(var system_run_mode)"/>
       <arg name="input_pointcloud" value="$(var input_pointcloud)"/>
       <arg name="localization_pointcloud_container_name" value="$(var localization_pointcloud_container_name)"/>
+      <arg name="initial_pose" value="$(var initial_pose)"/>
 
       <!-- parameter paths for common -->
       <arg name="localization_error_monitor_param_path" value="$(var loc_config_path)/localization_error_monitor.param.yaml"/>

From 2e6340cd48f6fe1e8292a1579896f8775789f9d6 Mon Sep 17 00:00:00 2001
From: Yamato Ando <yamato.ando@tier4.jp>
Date: Thu, 28 Mar 2024 18:07:14 +0900
Subject: [PATCH 2/2] rename params

Signed-off-by: Yamato Ando <yamato.ando@tier4.jp>
---
 .../config/localization/pose_initializer.param.yaml         | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/autoware_launch/config/localization/pose_initializer.param.yaml b/autoware_launch/config/localization/pose_initializer.param.yaml
index 5f669526b4..90ec78257e 100644
--- a/autoware_launch/config/localization/pose_initializer.param.yaml
+++ b/autoware_launch/config/localization/pose_initializer.param.yaml
@@ -1,8 +1,8 @@
 /**:
   ros__parameters:
-    initial_pose_directly:
-      enable: $(var initial_pose_directly/enable)
-      pose: $(var initial_pose_directly/pose)
+    user_defined_initial_pose:
+      enable: $(var user_defined_initial_pose/enable)
+      pose: $(var user_defined_initial_pose/pose)
 
     gnss_pose_timeout: 3.0 # [sec]
     stop_check_duration: 3.0 # [sec]