feat(pose_estimator_arbiter): add pose_estimator_arbiter with simple …

…switching rule (autowarefoundation#6144) * implement pose_estimator_manager pkg Signed-off-by: Kento Yabuuchi <[email protected]> * tmp Signed-off-by: Kento Yabuuchi <[email protected]> * swap ndt & yabloc Signed-off-by: Kento Yabuuchi <[email protected]> * add suspension service in yabloc particle filter Signed-off-by: Kento Yabuuchi <[email protected]> * add pluginlib for switching rule Signed-off-by: Kento Yabuuchi <[email protected]> * implement switch rule using pluginlib Signed-off-by: Kento Yabuuchi <[email protected]> * WIP: implement simple_switch_rule Signed-off-by: Kento Yabuuchi <[email protected]> * implement pcd_occupancy_rule based switcher Signed-off-by: Kento Yabuuchi <[email protected]> * resolve conflicts occured by rebase Signed-off-by: Kento Yabuuchi <[email protected]> * sub_manager_node is not necessary Signed-off-by: Kento Yabuuchi <[email protected]> * add ndt_yabloc_eagleye Signed-off-by: Kento Yabuuchi <[email protected]> * fix bug Signed-off-by: Kento Yabuuchi <[email protected]> * intuitive_multi_pose_estimator_launch Signed-off-by: Kento Yabuuchi <[email protected]> * yabloc_pf shoulbe be activated at the first Signed-off-by: Kento Yabuuchi <[email protected]> * merge swith_rule_plugin as inheritance Signed-off-by: Kento Yabuuchi <[email protected]> * fix launch bug Signed-off-by: Kento Yabuuchi <[email protected]> * add eagleye_area rule Signed-off-by: Kento Yabuuchi <[email protected]> * implement strict switching rule Signed-off-by: Kento Yabuuchi <[email protected]> * refine message Signed-off-by: Kento Yabuuchi <[email protected]> * fix merge conflict Signed-off-by: Kento Yabuuchi <[email protected]> * use hysteresis threshold for pcd occupancy criteria Signed-off-by: Kento Yabuuchi <[email protected]> * fix merge conflict Signed-off-by: Kento Yabuuchi <[email protected]> * add gtest Signed-off-by: Kento Yabuuchi <[email protected]> * add component test Signed-off-by: Kento Yabuuchi <[email protected]> * add artag submanager Signed-off-by: Kento Yabuuchi <[email protected]> * add ar_tag_position to get ar-tag position Signed-off-by: Kento Yabuuchi <[email protected]> * check distance to nearest ar marker Signed-off-by: Kento Yabuuchi <[email protected]> * switch ARTAG localizer if ar marker locates around ego Signed-off-by: Kento Yabuuchi <[email protected]> * improve ar_tag_position.[hc]pp Signed-off-by: Kento Yabuuchi <[email protected]> * split update() from map_base_rule.cpp Signed-off-by: Kento Yabuuchi <[email protected]> * apply pre-commit Signed-off-by: Kento Yabuuchi <[email protected]> * add license description Signed-off-by: Kento Yabuuchi <[email protected]> * update include guard Signed-off-by: Kento Yabuuchi <[email protected]> * reflected all pre-commit's points Signed-off-by: Kento Yabuuchi <[email protected]> * use magic_enum Signed-off-by: Kento Yabuuchi <[email protected]> * add pcd_occupancy helper Signed-off-by: Kento Yabuuchi <[email protected]> * change directory structure Signed-off-by: Kento Yabuuchi <[email protected]> * change namespace Signed-off-by: Kento Yabuuchi <[email protected]> * remap some topics Signed-off-by: Kento Yabuuchi <[email protected]> * update test Signed-off-by: Kento Yabuuchi <[email protected]> * add shared_data to share data Signed-off-by: Kento Yabuuchi <[email protected]> * remove obsolete comments Signed-off-by: Kento Yabuuchi <[email protected]> * share subscribed data by SharedData Signed-off-by: Kento Yabuuchi <[email protected]> * remove obsolete comments and fix to pass test.py Signed-off-by: Kento Yabuuchi <[email protected]> * rename SharedData Signed-off-by: Kento Yabuuchi <[email protected]> * stream debug_msg as is Signed-off-by: Kento Yabuuchi <[email protected]> * add README.md Signed-off-by: Kento Yabuuchi <[email protected]> * Update README.md update README.md on github Signed-off-by: Kento Yabuuchi <[email protected]> * fix eagleye bug Signed-off-by: Kento Yabuuchi <[email protected]> * update README Signed-off-by: Kento Yabuuchi <[email protected]> * wip Signed-off-by: Kento Yabuuchi <[email protected]> * update README.md Signed-off-by: Kento Yabuuchi <[email protected]> * update README Signed-off-by: Kento Yabuuchi <[email protected]> * use landmark_manager Signed-off-by: Kento Yabuuchi <[email protected]> * add glog & fix rule_helper bug Signed-off-by: Kento Yabuuchi <[email protected]> * publish empty diagnostics Signed-off-by: Kento Yabuuchi <[email protected]> * fix artag arbitorator Signed-off-by: Kento Yabuuchi <[email protected]> * implement callback_involving_variable Signed-off-by: Kento Yabuuchi <[email protected]> * rename invokingVariable Signed-off-by: Kento Yabuuchi <[email protected]> * clarify log level Signed-off-by: Kento Yabuuchi <[email protected]> * update diagnostics Signed-off-by: Kento Yabuuchi <[email protected]> * adope new landmark_manager Signed-off-by: Kento Yabuuchi <[email protected]> * rename manager arbiter Signed-off-by: Kento Yabuuchi <[email protected]> * style(pre-commit): autofix * fix obsolete change Signed-off-by: Kento Yabuuchi <[email protected]> * change yabloc relayed input topic Signed-off-by: Kento Yabuuchi <[email protected]> * resolve merge conflict Signed-off-by: Kento Yabuuchi <[email protected]> * adopt ar_tag_position for new ar tag map specification Signed-off-by: Kento Yabuuchi <[email protected]> * rename sub_arbitr to stopper Signed-off-by: Kento Yabuuchi <[email protected]> * apply pre-commit Signed-off-by: Kento Yabuuchi <[email protected]> * add timeout for async parameter client Signed-off-by: Kento Yabuuchi <[email protected]> * style(pre-commit): autofix * fix typo Signed-off-by: Kento Yabuuchi <[email protected]> * refactor shared_data Signed-off-by: Kento Yabuuchi <[email protected]> * rename yabloc_suspend_service Signed-off-by: Kento Yabuuchi <[email protected]> * improve debug log Signed-off-by: Kento Yabuuchi <[email protected]> * fix integration test Signed-off-by: Kento Yabuuchi <[email protected]> * style(pre-commit): autofix * remove obsolete notation Signed-off-by: Kento Yabuuchi <[email protected]> * fix ar_tag_based_localizer.launch.xml Signed-off-by: Kento Yabuuchi <[email protected]> * again fix ar_tag_based_localizer.launch.xml Signed-off-by: Kento Yabuuchi <[email protected]> * style(pre-commit): autofix * add sample data url Signed-off-by: Kento Yabuuchi <[email protected]> * (review reflect) refactor launch Signed-off-by: Kento Yabuuchi <[email protected]> * (review reflect) refactor launch about gnss_enabled Signed-off-by: Kento Yabuuchi <[email protected]> * (review reflect) organize type alias accessibility Signed-off-by: Kento Yabuuchi <[email protected]> * (review reflect) rename PoseEstimatorName to PoseEstimatorType Signed-off-by: Kento Yabuuchi <[email protected]> * (review reflect) fix typo Signed-off-by: Kento Yabuuchi <[email protected]> * style(pre-commit): autofix * fix pedantic warning of PCL Signed-off-by: Kento Yabuuchi <[email protected]> * (review reflect) improve diag & suppress warning Signed-off-by: Kento Yabuuchi <[email protected]> * (review reflect) create sub only when the corresponding estimator is running Signed-off-by: Kento Yabuuchi <[email protected]> * rename eagleye_area to pose_estimator_area Signed-off-by: Kento Yabuuchi <[email protected]> * vectormap based rule works well Signed-off-by: Kento Yabuuchi <[email protected]> * move old rules to example_rule/ Signed-off-by: Kento Yabuuchi <[email protected]> * update README Signed-off-by: Kento Yabuuchi <[email protected]> * improve some features Signed-off-by: Kento Yabuuchi <[email protected]> * style(pre-commit): autofix * style(pre-commit): autofix * move some rules into example_rule & add new simple rule Signed-off-by: Kento Yabuuchi <[email protected]> * apply pre-commit & update README Signed-off-by: Kento Yabuuchi <[email protected]> * split CMake for example_rule Signed-off-by: Kento Yabuuchi <[email protected]> * remove ar_tag_position & simplify example switching rule Signed-off-by: Kento Yabuuchi <[email protected]> * add vector_map_based_rule test Signed-off-by: Kento Yabuuchi <[email protected]> * add pcd_map_based_rule test Signed-off-by: Kento Yabuuchi <[email protected]> * improve README Signed-off-by: Kento Yabuuchi <[email protected]> * fix integration test.py Signed-off-by: Kento Yabuuchi <[email protected]> * add test Signed-off-by: Kento Yabuuchi <[email protected]> * refactor & update README Signed-off-by: Kento Yabuuchi <[email protected]> * replace obsolete video Signed-off-by: Kento Yabuuchi <[email protected]> * fix typo Signed-off-by: Kento Yabuuchi <[email protected]> * Update README.md fix markdown (add one line just after <summary>) Signed-off-by: Kento Yabuuchi <[email protected]> * use structures bindings Signed-off-by: Kento Yabuuchi <[email protected]> * add many comments Signed-off-by: Kento Yabuuchi <[email protected]> * remove obsolete include & alias Signed-off-by: Kento Yabuuchi <[email protected]> * fix miss of eagleye output relay Signed-off-by: Kento Yabuuchi <[email protected]> * fix 404 URL Signed-off-by: Kento Yabuuchi <[email protected]> * remove obsolete args Signed-off-by: Kento Yabuuchi <[email protected]> --------- Signed-off-by: Kento Yabuuchi <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
zulfaqar-azmi-t4 · Feb 6, 2024 · 51d4e64 · 51d4e64
1 parent 7d21a60
commit 51d4e64
Show file tree

Hide file tree

Showing 41 changed files with 5,508 additions and 128 deletions.
diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml
@@ -1,10 +1,10 @@
 <?xml version="1.0"?>
 <launch>
-  <!-- Parameter files -->
-  <arg name="pose_source"/>
+  <arg name="pose_source" description="A string consisting of ndt, yabloc, artag and eagleye joined by underscores no matter the order. e.g. ndt_yabloc"/>
   <arg name="twist_source"/>
   <arg name="system_run_mode"/>
 
+  <!-- Parameter files -->
   <arg name="crop_box_filter_measurement_range_param_path"/>
   <arg name="voxel_grid_downsample_filter_param_path"/>
   <arg name="random_downsample_filter_param_path"/>

diff --git a/...h/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/...h/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml
@@ -1,9 +1,11 @@
 <?xml version="1.0"?>
 <launch>
   <group>
+    <arg name="input_image"/>
+
     <include file="$(find-pkg-share ar_tag_based_localizer)/launch/ar_tag_based_localizer.launch.xml">
       <arg name="input_lanelet2_map" value="/map/vector_map"/>
-      <arg name="input_image" value="/sensing/camera/traffic_light/image_raw"/>
+      <arg name="input_image" value="$(var input_image)"/>
       <arg name="input_camera_info" value="/sensing/camera/traffic_light/camera_info"/>
       <arg name="input_ekf_pose" value="/localization/pose_twist_fusion_filter/biased_pose_with_covariance"/>
       <arg name="output_pose_with_covariance" value="/localization/pose_estimator/pose_with_covariance"/>

diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml
@@ -1,152 +1,116 @@
 <?xml version="1.0"?>
 <launch>
+  <!-- only when running with a real vehicle, the pose_initializer judges the stop -->
   <let name="stop_check_enabled" if="$(eval &quot;'$(var system_run_mode)'=='online'&quot;)" value="true"/>
   <let name="stop_check_enabled" if="$(eval &quot;'$(var system_run_mode)'=='logging_simulation'&quot;)" value="false"/>
 
+  <!-- When gnss_enabled is false, automatic_pose_initializer will not run, only manual initial position estimation is available. -->
+  <arg name="gnss_enabled" default="true" description="gnss availability for initial position estimation"/>
+
+  <!-- split string with underscores -->
+  <let name="available_args" value="[\'ndt\',\'yabloc\',\'eagleye\',\'artag\']"/>
+  <let name="split_function" value="list(set('$(var pose_source)'.split('_')).intersection($(var available_args)))"/>
+  <let name="pose_sources" value="$(eval $(var split_function))"/>
+  <let name="multi_localizer_mode" value="$(eval &quot;len($(var pose_sources))> 1&quot;)"/>
+
+  <!-- organizes flags for which nodes to activate -->
+  <let name="use_ndt_pose" value="$(eval &quot;'ndt' in $(var pose_sources)&quot;)"/>
+  <let name="use_yabloc_pose" value="$(eval &quot;'yabloc' in $(var pose_sources)&quot;)"/>
+  <let name="use_artag_pose" value="$(eval &quot;'artag' in $(var pose_sources)&quot;)"/>
+  <let name="use_eagleye_pose" value="$(eval &quot;'eagleye' in $(var pose_sources)&quot;)"/>
+  <let name="use_eagleye_twist" value="$(eval &quot;'eagleye' == '$(var twist_source)'&quot;)"/>
+  <let name="use_gyro_odom_twist" value="$(eval &quot;'gyro_odom' == '$(var twist_source)'&quot;)"/>
+
   <!-- NDT Scan Matcher Launch (as pose estimator) -->
-  <group if="$(eval &quot;'$(var pose_source)'=='ndt'&quot;)">
-    <group>
-      <push-ros-namespace namespace="pose_estimator"/>
-      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml"/>
-    </group>
-    <group>
-      <push-ros-namespace namespace="util"/>
-      <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
-        <arg name="ndt_enabled" value="true"/>
-        <arg name="gnss_enabled" value="true"/>
-        <arg name="ekf_enabled" value="true"/>
-        <arg name="yabloc_enabled" value="false"/>
-        <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
-        <arg name="config_file" value="$(var pose_initializer_param_path)"/>
-        <arg name="sub_gnss_pose_cov" value="/sensing/gnss/pose_with_covariance"/>
-      </include>
-      <group if="$(var gnss_enabled)">
-        <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
-      </group>
-      <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py">
-        <arg name="lidar_container_name" value="$(var lidar_container_name)"/>
-      </include>
-    </group>
+  <group if="$(var use_ndt_pose)">
+    <push-ros-namespace namespace="pose_estimator"/>
+    <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml"/>
   </group>
 
   <!-- YabLoc Launch (as pose estimator) -->
-  <group if="$(eval &quot;'$(var pose_source)'=='yabloc'&quot;)">
-    <group>
-      <push-ros-namespace namespace="pose_estimator"/>
-      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/yabloc.launch.xml"/>
-    </group>
-    <group>
-      <push-ros-namespace namespace="util"/>
-      <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
-        <arg name="ndt_enabled" value="false"/>
-        <arg name="gnss_enabled" value="true"/>
-        <arg name="ekf_enabled" value="true"/>
-        <arg name="yabloc_enabled" value="true"/>
-        <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
-        <arg name="config_file" value="$(var pose_initializer_param_path)"/>
-        <arg name="sub_gnss_pose_cov" value="/sensing/gnss/pose_with_covariance"/>
-      </include>
-      <group if="$(var gnss_enabled)">
-        <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
-      </group>
-    </group>
+  <group if="$(var use_yabloc_pose)">
+    <push-ros-namespace namespace="pose_estimator"/>
+    <let name="yabloc_src_image" value="/sensing/camera/traffic_light/image_raw" unless="$(var multi_localizer_mode)"/>
+    <let name="yabloc_src_image" value="/sensing/camera/traffic_light/image_raw/yabloc_relay" if="$(var multi_localizer_mode)"/>
+
+    <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/yabloc.launch.xml">
+      <arg name="src_image" value="$(var yabloc_src_image)"/>
+    </include>
   </group>
 
-  <!-- Gyro Odometer Launch (as pose estimator) -->
-  <group if="$(eval &quot;'$(var twist_source)'=='gyro_odom'&quot;)">
+  <!-- Gyro Odometer Launch (as twist estimator) -->
+  <group if="$(var use_gyro_odom_twist)">
     <push-ros-namespace namespace="twist_estimator"/>
     <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/gyro_odometer.launch.xml"/>
   </group>
 
-  <!-- Eagleye Launch (as pose & twist estimator) -->
-  <group if="$(eval &quot;'$(var pose_source)'=='eagleye' and '$(var twist_source)'=='eagleye'&quot;)">
-    <group>
-      <push-ros-namespace namespace="pose_twist_estimator"/>
-      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml">
-        <arg name="output_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
-        <arg name="output_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
-        <arg name="use_eagleye_pose" value="true"/>
-        <arg name="use_eagleye_twist" value="true"/>
-      </include>
-    </group>
-    <group>
-      <push-ros-namespace namespace="util"/>
-      <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
-        <arg name="ndt_enabled" value="false"/>
-        <arg name="gnss_enabled" value="true"/>
-        <arg name="ekf_enabled" value="true"/>
-        <arg name="yabloc_enabled" value="false"/>
-        <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
-        <arg name="config_file" value="$(var pose_initializer_param_path)"/>
-        <arg name="sub_gnss_pose_cov" value="/localization/pose_estimator/pose_with_covariance"/>
-      </include>
-      <group if="$(var gnss_enabled)">
-        <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
-      </group>
-    </group>
-  </group>
+  <!-- Eagleye Launch -->
+  <group if="$(eval &quot;$(var use_eagleye_pose) or $(var use_eagleye_twist)&quot;)">
+    <let name="eagleye_name_space" value="pose_twist_estimator"/>
+    <let name="eagleye_name_space" value="twist_estimator" unless="$(var use_eagleye_pose)"/>
+    <let name="eagleye_name_space" value="pose_estimator" unless="$(var use_eagleye_twist)"/>
+    <let name="output_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance" unless="$(var multi_localizer_mode)"/>
+    <let name="output_pose_with_cov_name" value="/localization/pose_estimator/eagleye/pose_with_covariance/to_relay" if="$(var multi_localizer_mode)"/>
 
-  <!-- Eagleye Launch (as pose estimator) -->
-  <group if="$(eval &quot;'$(var pose_source)'=='eagleye' and '$(var twist_source)'!='eagleye'&quot;)">
-    <group>
-      <push-ros-namespace namespace="pose_estimator"/>
-      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml">
-        <arg name="output_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
-        <arg name="use_eagleye_pose" value="true"/>
-        <arg name="use_eagleye_twist" value="false"/>
-      </include>
-    </group>
-    <group>
-      <push-ros-namespace namespace="util"/>
-      <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
-        <arg name="ndt_enabled" value="false"/>
-        <arg name="gnss_enabled" value="true"/>
-        <arg name="ekf_enabled" value="true"/>
-        <arg name="yabloc_enabled" value="false"/>
-        <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
-        <arg name="config_file" value="$(var pose_initializer_param_path)"/>
-        <arg name="sub_gnss_pose_cov" value="/localization/pose_estimator/pose_with_covariance"/>
-      </include>
-      <group if="$(var gnss_enabled)">
-        <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
-      </group>
-    </group>
-  </group>
-
-  <!-- Eagleye Launch (as twist estimator) -->
-  <group if="$(eval &quot;'$(var pose_source)'!='eagleye' and '$(var twist_source)'=='eagleye'&quot;)">
-    <push-ros-namespace namespace="twist_estimator"/>
+    <push-ros-namespace namespace="$(var eagleye_name_space)"/>
     <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml">
       <arg name="output_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
-      <arg name="use_eagleye_pose" value="false"/>
-      <arg name="use_eagleye_twist" value="true"/>
+      <arg name="output_pose_with_cov_name" value="$(var output_pose_with_cov_name)"/>
+      <arg name="use_eagleye_pose" value="$(var use_eagleye_pose)"/>
+      <arg name="use_eagleye_twist" value="$(var use_eagleye_twist)"/>
     </include>
   </group>
 
   <!-- AR Tag Based Localizer (as pose estimator) -->
-  <group if="$(eval &quot;'$(var pose_source)'=='artag'&quot;)">
-    <group>
-      <push-ros-namespace namespace="pose_estimator"/>
-      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml"/>
+  <group if="$(var use_artag_pose)">
+    <push-ros-namespace namespace="pose_estimator"/>
+    <let name="artag_input_image" value="/sensing/camera/traffic_light/image_raw" unless="$(var multi_localizer_mode)"/>
+    <let name="artag_input_image" value="/sensing/camera/traffic_light/image_raw/artag_relay" if="$(var multi_localizer_mode)"/>
+
+    <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml">
+      <arg name="input_image" value="$(var artag_input_image)"/>
+    </include>
+  </group>
+
+  <!-- Pose Estimator Arbiter Launch -->
+  <group if="$(var multi_localizer_mode)">
+    <include file="$(find-pkg-share pose_estimator_arbiter)/launch/pose_estimator_arbiter.launch.xml">
+      <arg name="pose_sources" value="$(var pose_sources)"/>
+      <arg name="input_pointcloud" value="$(var input_pointcloud)"/>
+    </include>
+  </group>
+
+  <!-- Util Launch -->
+  <group>
+    <push-ros-namespace namespace="util"/>
+
+    <!-- pose_initializer -->
+    <let name="sub_gnss_pose_cov" value="/sensing/gnss/pose_with_covariance"/>
+    <group if="$(var use_eagleye_pose)" scoped="false">
+      <let name="sub_gnss_pose_cov" value="/localization/pose_estimator/pose_with_covariance" unless="$(var multi_localizer_mode)"/>
     </group>
 
-    <group>
-      <push-ros-namespace namespace="util"/>
-      <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
-        <arg name="ndt_enabled" value="false"/>
-        <arg name="gnss_enabled" value="false"/>
-        <arg name="ekf_enabled" value="true"/>
-        <arg name="yabloc_enabled" value="false"/>
-        <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
-        <arg name="config_file" value="$(var pose_initializer_param_path)"/>
-        <arg name="sub_gnss_pose_cov" value="/sensing/gnss/pose_with_covariance"/>
-      </include>
-      <group if="$(var gnss_enabled)">
-        <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
-      </group>
-      <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py">
-        <arg name="lidar_container_name" value="$(var lidar_container_name)"/>
-      </include>
+    <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
+      <arg name="ndt_enabled" value="$(var use_ndt_pose)"/>
+      <arg name="yabloc_enabled" value="$(var use_yabloc_pose)"/>
+      <arg name="gnss_enabled" value="$(var gnss_enabled)"/>
+      <arg name="ekf_enabled" value="true"/>
+      <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
+      <arg name="config_file" value="$(var pose_initializer_param_path)"/>
+      <arg name="sub_gnss_pose_cov" value="$(var sub_gnss_pose_cov)"/>
+    </include>
+
+    <!-- automatic_pose_initializer -->
+    <group if="$(var gnss_enabled)">
+      <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
     </group>
+
+    <!-- pointcloud_downsampling -->
+    <let name="override_input_pointcloud" value="$(var input_pointcloud)"/>
+    <let name="override_input_pointcloud" value="$(var input_pointcloud)/relay" if="$(var multi_localizer_mode)"/>
+    <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py" if="$(var use_ndt_pose)">
+      <arg name="lidar_container_name" value="$(var lidar_container_name)"/>
+      <arg name="input_pointcloud" value="$(var override_input_pointcloud)"/>
+    </include>
   </group>
 </launch>
diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml
@@ -27,6 +27,7 @@
   <exec_depend>gyro_odometer</exec_depend>
   <exec_depend>ndt_scan_matcher</exec_depend>
   <exec_depend>pointcloud_preprocessor</exec_depend>
+  <exec_depend>pose_estimator_arbiter</exec_depend>
   <exec_depend>pose_initializer</exec_depend>
   <exec_depend>pose_instability_detector</exec_depend>
   <exec_depend>topic_tools</exec_depend>

diff --git a/localization/pose_estimator_arbiter/CMakeLists.txt b/localization/pose_estimator_arbiter/CMakeLists.txt
@@ -0,0 +1,58 @@
+cmake_minimum_required(VERSION 3.14)
+project(pose_estimator_arbiter)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(glog REQUIRED)
+
+find_package(PCL REQUIRED COMPONENTS common)
+include_directories(SYSTEM ${PCL_INCLUDE_DIRS})
+
+# ==============================
+# switch rule library
+ament_auto_add_library(switch_rule
+  SHARED
+  src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp
+)
+target_include_directories(switch_rule PUBLIC src)
+
+# ==============================
+# pose estimator arbiter node
+ament_auto_add_executable(${PROJECT_NAME}
+  src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp
+  src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp
+)
+target_include_directories(${PROJECT_NAME} PUBLIC src)
+target_link_libraries(${PROJECT_NAME} switch_rule glog::glog)
+
+# ==============================
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+
+  # define test definition macro
+  function(add_testcase filepath)
+    get_filename_component(filename ${filepath} NAME)
+    string(REGEX REPLACE ".cpp" "" test_name ${filename})
+    ament_add_gmock(${test_name} ${filepath})
+    target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src)
+    target_link_libraries(${test_name} fmt)
+    ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
+  endfunction()
+
+  add_testcase(test/test_shared_data.cpp)
+  add_ros_test(
+    test/test_pose_estimator_arbiter.py
+    TIMEOUT "30"
+  )
+endif()
+
+# ==============================
+# In practice, the example rule is not used as autoware code.
+# It exists only for user reference and is tested only.
+add_subdirectory(example_rule)
+
+# ==============================
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)