Merge branch 'main' into feat/add_enable_all_modules_auto_mode_behavi…

…or_velocity
kyoichi-sugahara · Jan 17, 2024 · 0d9a1e4 · 0d9a1e4
2 parents da2e7f1 + f78ea59
commit 0d9a1e4
Show file tree

Hide file tree

Showing 56 changed files with 736 additions and 126 deletions.
diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml
@@ -8,6 +8,9 @@
   <maintainer email="koji.minoda@tier4.jp">Koji Minoda</maintainer>
   <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
   <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
   <license>Apache License 2.0</license>
   <author email="yamato.ando@tier4.jp">Yamato Ando</author>
 

diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml
@@ -9,6 +9,11 @@
   <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
   <maintainer email="takeshi.ishita@tier4.jp">Takeshi Ishita</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
   <author email="takamasa.horibe@tier4.jp">Takamasa Horibe</author>
 

diff --git a/localization/geo_pose_projector/package.xml b/localization/geo_pose_projector/package.xml
@@ -6,6 +6,12 @@
   <description>The geo_pose_projector package</description>
   <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
   <maintainer email="koji.minoda@tier4.jp">Koji Minoda</maintainer>
+  <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
   <author email="koji.minoda@tier4.jp">Koji Minoda</author>
 

diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml
@@ -6,6 +6,11 @@
   <description>The gyro_odometer package as a ROS 2 node</description>
   <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
   <author email="yamato.ando@tier4.jp">Yamato Ando</author>
 

diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml
@@ -8,6 +8,9 @@
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
   <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
   <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

diff --git a/localization/landmark_based_localizer/landmark_manager/package.xml b/localization/landmark_based_localizer/landmark_manager/package.xml
@@ -8,6 +8,9 @@
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
   <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
   <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml
@@ -7,6 +7,11 @@
   <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
   <maintainer email="koji.minoda@tier4.jp">Koji Minoda</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="taichi.higashide@tier4.jp">Taichi Higashide</author>

diff --git a/localization/localization_util/package.xml b/localization/localization_util/package.xml
@@ -8,6 +8,9 @@
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
   <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
   <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
   <author email="yamato.ando@tier4.jp">Yamato Ando</author>
 

diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml
@@ -8,6 +8,10 @@
   <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
   <maintainer email="koji.minoda@tier4.jp">Koji Minoda</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
   <author email="yamato.ando@tier4.jp">Yamato Ando</author>
 

diff --git a/localization/pose2twist/package.xml b/localization/pose2twist/package.xml
@@ -6,6 +6,11 @@
   <description>The pose2twist package</description>
   <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
   <author email="yamato.ando@tier4.jp">Yamato Ando</author>
 

diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml
@@ -7,6 +7,11 @@
   <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
   <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
   <author email="yamato.ando@tier4.jp">Yamato Ando</author>
   <author email="isamu.takagi@tier4.jp">Takagi, Isamu</author>

diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml
@@ -10,6 +10,7 @@
   <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
   <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
   <license>Apache License 2.0</license>
   <author email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</author>
 

diff --git a/localization/stop_filter/package.xml b/localization/stop_filter/package.xml
@@ -7,6 +7,11 @@
   <maintainer email="koji.minoda@tier4.jp">Koji Minoda</maintainer>
   <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
   <author email="koji.minoda@tier4.jp">Koji Minoda</author>
 

diff --git a/localization/tree_structured_parzen_estimator/package.xml b/localization/tree_structured_parzen_estimator/package.xml
@@ -8,6 +8,9 @@
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
   <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
   <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
   <author email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</author>
 

diff --git a/localization/twist2accel/package.xml b/localization/twist2accel/package.xml
@@ -7,6 +7,11 @@
   <maintainer email="koji.minoda@tier4.jp">Koji Minoda</maintainer>
   <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
   <author email="koji.minoda@tier4.jp">Koji Minoda</author>
 

diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml
@@ -5,6 +5,11 @@
   <description>YabLoc common library</description>
   <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>

diff --git a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml
@@ -1,7 +1,7 @@
 <launch>
   <arg name="src_image" default="/sensing/camera/traffic_light/image_raw"/>
   <arg name="src_info" default="/sensing/camera/traffic_light/camera_info"/>
-  <arg name="resized_image" default="/sensing/camera/undistorted/image_raw/relay"/>
+  <arg name="resized_image" default="/sensing/camera/undistorted/image_raw"/>
   <arg name="resized_info" default="/sensing/camera/undistorted/camera_info"/>
 
   <!-- undistort -->

diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml
@@ -6,6 +6,11 @@
   <description>YabLoc image processing package</description>
   <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

diff --git a/localization/yabloc/yabloc_monitor/package.xml b/localization/yabloc/yabloc_monitor/package.xml
@@ -7,6 +7,11 @@
   <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
   <maintainer email="koji.minoda@tier4.jp">Koji Minoda</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>

diff --git a/localization/yabloc/yabloc_particle_filter/README.md b/localization/yabloc/yabloc_particle_filter/README.md
@@ -46,6 +46,12 @@ This package contains some executable nodes related to particle filter.
 | `prediction_rate`             | double           | frequency of forecast updates, in Hz                              |
 | `cov_xx_yy`                   | vector\<double\> | the covariance of initial pose                                    |
 
+### Services
+
+| Name                 | Type                     | Description                                      |
+| -------------------- | ------------------------ | ------------------------------------------------ |
+| `yabloc_trigger_srv` | `std_srvs::srv::SetBool` | activation and deactivation of yabloc estimation |
+
 ## gnss_particle_corrector
 
 ### Purpose

diff --git a/...ion/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp b/...ion/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp
@@ -27,6 +27,7 @@
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include <std_msgs/msg/float32.hpp>
+#include <std_srvs/srv/set_bool.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
 #include <tf2_ros/transform_broadcaster.h>
@@ -46,6 +47,7 @@ class Predictor : public rclcpp::Node
   using TwistCovStamped = geometry_msgs::msg::TwistWithCovarianceStamped;
   using TwistStamped = geometry_msgs::msg::TwistStamped;
   using Marker = visualization_msgs::msg::Marker;
+  using SetBool = std_srvs::srv::SetBool;
 
   Predictor();
 
@@ -62,6 +64,7 @@ class Predictor : public rclcpp::Node
   const std::vector<double> cov_xx_yy_;
 
   // Subscriber
+  rclcpp::Subscription<PoseCovStamped>::SharedPtr ekf_pose_sub_;
   rclcpp::Subscription<PoseCovStamped>::SharedPtr initialpose_sub_;
   rclcpp::Subscription<TwistCovStamped>::SharedPtr twist_cov_sub_;
   rclcpp::Subscription<ParticleArray>::SharedPtr particles_sub_;
@@ -73,11 +76,15 @@ class Predictor : public rclcpp::Node
   rclcpp::Publisher<PoseCovStamped>::SharedPtr pose_cov_pub_;
   rclcpp::Publisher<Marker>::SharedPtr marker_pub_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf2_broadcaster_;
+  // Server
+  rclcpp::Service<SetBool>::SharedPtr yabloc_trigger_server_;
 
   // Timer callback
   rclcpp::TimerBase::SharedPtr timer_;
 
   float ground_height_{0};
+  bool yabloc_activated_{true};
+  PoseCovStamped::ConstSharedPtr latest_ekf_pose_ptr_{nullptr};
 
   std::optional<ParticleArray> particle_array_opt_{std::nullopt};
   std::optional<TwistCovStamped> latest_twist_opt_{std::nullopt};
@@ -92,6 +99,8 @@ class Predictor : public rclcpp::Node
   void on_twist_cov(const TwistCovStamped::ConstSharedPtr twist);
   void on_weighted_particles(const ParticleArray::ConstSharedPtr weighted_particles);
   void on_timer();
+  void on_trigger_service(
+    SetBool::Request::ConstSharedPtr request, SetBool::Response::SharedPtr response);
 
   //
   void initialize_particles(const PoseCovStamped & initialpose);

diff --git a/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml b/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml
@@ -16,6 +16,9 @@
     <remap from="~/output/predicted_particles" to="predicted_particles"/>
     <remap from="~/debug/init_marker" to="init_maker"/>
     <remap from="~/debug/particles_marker_array" to="$(var output_particles_marker_array)"/>
+
+    <remap from="~/input/ekf_pose" to="/localization/pose_with_covariance"/>
+    <remap from="~/yabloc_trigger_srv" to="/localization/pose_estimator/yabloc/pf/yabloc_trigger_srv"/>
   </node>
 
   <!-- camera  correction -->

diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml
@@ -6,6 +6,11 @@
   <description>YabLoc particle filter package</description>
   <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>

diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp
@@ -52,27 +52,62 @@ Predictor::Predictor()
   auto on_initial = std::bind(&Predictor::on_initial_pose, this, _1);
   auto on_twist_cov = std::bind(&Predictor::on_twist_cov, this, _1);
   auto on_particle = std::bind(&Predictor::on_weighted_particles, this, _1);
-  auto on_height = [this](std_msgs::msg::Float32 m) -> void { this->ground_height_ = m.data; };
+  auto on_height = [this](std_msgs::msg::Float32::ConstSharedPtr msg) -> void {
+    this->ground_height_ = msg->data;
+  };
+  auto on_ekf_pose = [this](PoseCovStamped::ConstSharedPtr msg) -> void {
+    this->latest_ekf_pose_ptr_ = msg;
+  };
 
   initialpose_sub_ = create_subscription<PoseCovStamped>("~/input/initialpose", 1, on_initial);
   particles_sub_ =
     create_subscription<ParticleArray>("~/input/weighted_particles", 10, on_particle);
   height_sub_ = create_subscription<std_msgs::msg::Float32>("~/input/height", 10, on_height);
   twist_cov_sub_ =
     create_subscription<TwistCovStamped>("~/input/twist_with_covariance", 10, on_twist_cov);
+  ekf_pose_sub_ = create_subscription<PoseCovStamped>("~/input/ekf_pose", 10, on_ekf_pose);
 
   // Timer callback
   const double prediction_rate = declare_parameter<double>("prediction_rate");
   auto on_timer = std::bind(&Predictor::on_timer, this);
   timer_ = rclcpp::create_timer(
     this, this->get_clock(), rclcpp::Rate(prediction_rate).period(), std::move(on_timer));
 
+  // Service server
+  using std::placeholders::_2;
+  auto on_trigger_service = std::bind(&Predictor::on_trigger_service, this, _1, _2);
+  yabloc_trigger_server_ = create_service<SetBool>("~/yabloc_trigger_srv", on_trigger_service);
+
   // Optional modules
   if (declare_parameter<bool>("visualize", false)) {
     visualizer_ptr_ = std::make_unique<ParticleVisualizer>(*this);
   }
 }
 
+void Predictor::on_trigger_service(
+  SetBool::Request::ConstSharedPtr request, SetBool::Response::SharedPtr response)
+{
+  if (request->data) {
+    RCLCPP_INFO_STREAM(get_logger(), "yabloc particle filter is activated");
+  } else {
+    RCLCPP_INFO_STREAM(get_logger(), "yabloc particle filter is deactivated");
+  }
+
+  const bool before_activated_ = yabloc_activated_;
+  yabloc_activated_ = request->data;
+  response->success = true;
+
+  if (yabloc_activated_ && (!before_activated_)) {
+    RCLCPP_INFO_STREAM(get_logger(), "restart particle filter");
+    if (latest_ekf_pose_ptr_) {
+      on_initial_pose(latest_ekf_pose_ptr_);
+    } else {
+      yabloc_activated_ = false;
+      response->success = false;
+    }
+  }
+}
+
 void Predictor::on_initial_pose(const PoseCovStamped::ConstSharedPtr initialpose)
 {
   // Publish initial pose marker
@@ -181,6 +216,10 @@ void Predictor::on_timer()
 {
   // ==========================================================================
   // Pre-check section
+  // Return if yabloc is not activated
+  if (!yabloc_activated_) {
+    return;
+  }
   // Return if particle_array is not initialized yet
   if (!particle_array_opt_.has_value()) {
     return;

diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml
@@ -5,6 +5,11 @@
   <description>The pose initializer</description>
   <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>