feat(pose_initilizer): support gnss/imu pose estimator (autowarefound…

…ation#2904)

* Support GNSS/IMU pose estimator

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* style(pre-commit): autofix

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Revert gnss/imu support

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Support GNSS/IMU pose estimator

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* style(pre-commit): autofix

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Separate EKF and NDT trigger modules

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Integrate activate and deactivate into sendRequest

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* style(pre-commit): autofix

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Change sendRequest function arguments

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* style(pre-commit): autofix

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Remove unused conditional branches

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Fix command name

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Change to snake_case

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Fix typos

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Update localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp

Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Update localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp

Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Update copyright year

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Set the copyright year of ekf_localization_module to 2022

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Delete unnecessary conditional branches

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Add ekf_enabled parameter

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

* Add #include <string>

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>

---------

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>

launch/tier4_localization_launch/launch/util/util.launch.xml

@@ -16,6 +16,7 @@
     <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="ekf_enabled" value="true"/>
       <arg name="stop_check_enabled" value="true"/>
     </include>
   </group>

launch/tier4_simulator_launch/launch/simulator.launch.xml

@@ -99,6 +99,7 @@
     <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="ekf_enabled" value="false"/>
       <arg name="stop_check_enabled" value="false"/>
     </include>
   </group>

localization/pose_initializer/CMakeLists.txt

@@ -10,7 +10,8 @@ ament_auto_add_executable(pose_initializer_node
   src/pose_initializer/gnss_module.cpp
   src/pose_initializer/ndt_module.cpp
   src/pose_initializer/stop_check_module.cpp
-  src/pose_initializer/localization_trigger_module.cpp
+  src/pose_initializer/ekf_localization_trigger_module.cpp
+  src/pose_initializer/ndt_localization_trigger_module.cpp
 )
 
 if(BUILD_TESTING)

localization/pose_initializer/README.md

@@ -15,6 +15,7 @@ This node depends on the map height fitter library.
 
 | Name                  | Type | Description                                                                              |
 | --------------------- | ---- | ---------------------------------------------------------------------------------------- |
+| `ekf_enabled`         | bool | If true, EKF localizar is activated.                                                     |
 | `ndt_enabled`         | bool | If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through. |
 | `stop_check_enabled`  | bool | If true, initialization is accepted only when the vehicle is stopped.                    |
 | `stop_check_duration` | bool | The duration used for the stop check above.                                              |

localization/pose_initializer/launch/pose_initializer.launch.xml

@@ -2,10 +2,12 @@
   <arg name="config_file" default="$(find-pkg-share pose_initializer)/config/pose_initializer.param.yaml"/>
   <arg name="gnss_enabled"/>
   <arg name="ndt_enabled"/>
+  <arg name="ekf_enabled"/>
   <arg name="stop_check_enabled"/>
 
   <node pkg="pose_initializer" exec="pose_initializer_node" name="pose_initializer_node">
     <param from="$(var config_file)"/>
+    <param name="ekf_enabled" value="$(var ekf_enabled)"/>
     <param name="ndt_enabled" value="$(var ndt_enabled)"/>
     <remap from="ndt_align" to="/localization/pose_estimator/ndt_align_srv"/>
     <param name="stop_check_enabled" value="$(var stop_check_enabled)"/>

localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp

@@ -0,0 +1,56 @@
+// Copyright 2022 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ekf_localization_trigger_module.hpp"
+
+#include <component_interface_specs/localization.hpp>
+#include <component_interface_utils/rclcpp/exceptions.hpp>
+
+#include <memory>
+#include <string>
+
+using ServiceException = component_interface_utils::ServiceException;
+using Initialize = localization_interface::Initialize;
+
+EkfLocalizationTriggerModule::EkfLocalizationTriggerModule(rclcpp::Node * node)
+: logger_(node->get_logger())
+{
+  client_ekf_trigger_ = node->create_client<SetBool>("ekf_trigger_node");
+}
+
+void EkfLocalizationTriggerModule::send_request(bool flag) const
+{
+  const auto req = std::make_shared<SetBool::Request>();
+  std::string command_name;
+  req->data = flag;
+  if (flag) {
+    command_name = "Activation";
+  } else {
+    command_name = "Deactivation";
+  }
+
+  if (!client_ekf_trigger_->service_is_ready()) {
+    throw component_interface_utils::ServiceUnready("EKF triggering service is not ready");
+  }
+
+  auto future_ekf = client_ekf_trigger_->async_send_request(req);
+
+  if (future_ekf.get()->success) {
+    RCLCPP_INFO(logger_, "EKF %s succeeded", command_name.c_str());
+  } else {
+    RCLCPP_INFO(logger_, "EKF %s failed", command_name.c_str());
+    throw ServiceException(
+      Initialize::Service::Response::ERROR_ESTIMATION, "EKF " + command_name + " failed");
+  }
+}

localization/pose_initializer/src/pose_initializer/localization_trigger_module.hpp → localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp

@@ -12,27 +12,25 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef POSE_INITIALIZER__LOCALIZATION_TRIGGER_MODULE_HPP_
-#define POSE_INITIALIZER__LOCALIZATION_TRIGGER_MODULE_HPP_
+#ifndef POSE_INITIALIZER__EKF_LOCALIZATION_TRIGGER_MODULE_HPP_
+#define POSE_INITIALIZER__EKF_LOCALIZATION_TRIGGER_MODULE_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 
 #include <std_srvs/srv/set_bool.hpp>
 
-class LocalizationTriggerModule
+class EkfLocalizationTriggerModule
 {
 private:
   using SetBool = std_srvs::srv::SetBool;
 
 public:
-  explicit LocalizationTriggerModule(rclcpp::Node * node);
-  void deactivate() const;
-  void activate() const;
+  explicit EkfLocalizationTriggerModule(rclcpp::Node * node);
+  void send_request(bool flag) const;
 
 private:
   rclcpp::Logger logger_;
   rclcpp::Client<SetBool>::SharedPtr client_ekf_trigger_;
-  rclcpp::Client<SetBool>::SharedPtr client_ndt_trigger_;
 };
 
-#endif  // POSE_INITIALIZER__LOCALIZATION_TRIGGER_MODULE_HPP_
+#endif  // POSE_INITIALIZER__EKF_LOCALIZATION_TRIGGER_MODULE_HPP_

localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp

@@ -0,0 +1,56 @@
+// Copyright 2023 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ndt_localization_trigger_module.hpp"
+
+#include <component_interface_specs/localization.hpp>
+#include <component_interface_utils/rclcpp/exceptions.hpp>
+
+#include <memory>
+#include <string>
+
+using ServiceException = component_interface_utils::ServiceException;
+using Initialize = localization_interface::Initialize;
+
+NdtLocalizationTriggerModule::NdtLocalizationTriggerModule(rclcpp::Node * node)
+: logger_(node->get_logger())
+{
+  client_ndt_trigger_ = node->create_client<SetBool>("ndt_trigger_node");
+}
+
+void NdtLocalizationTriggerModule::send_request(bool flag) const
+{
+  const auto req = std::make_shared<SetBool::Request>();
+  std::string command_name;
+  req->data = flag;
+  if (flag) {
+    command_name = "Activation";
+  } else {
+    command_name = "Deactivation";
+  }
+
+  if (!client_ndt_trigger_->service_is_ready()) {
+    throw component_interface_utils::ServiceUnready("NDT triggering service is not ready");
+  }
+
+  auto future_ndt = client_ndt_trigger_->async_send_request(req);
+
+  if (future_ndt.get()->success) {
+    RCLCPP_INFO(logger_, "NDT %s succeeded", command_name.c_str());
+  } else {
+    RCLCPP_INFO(logger_, "NDT %s failed", command_name.c_str());
+    throw ServiceException(
+      Initialize::Service::Response::ERROR_ESTIMATION, "NDT " + command_name + " failed");
+  }
+}

localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp

@@ -0,0 +1,36 @@
+// Copyright 2023 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef POSE_INITIALIZER__NDT_LOCALIZATION_TRIGGER_MODULE_HPP_
+#define POSE_INITIALIZER__NDT_LOCALIZATION_TRIGGER_MODULE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <std_srvs/srv/set_bool.hpp>
+
+class NdtLocalizationTriggerModule
+{
+private:
+  using SetBool = std_srvs::srv::SetBool;
+
+public:
+  explicit NdtLocalizationTriggerModule(rclcpp::Node * node);
+  void send_request(bool flag) const;
+
+private:
+  rclcpp::Logger logger_;
+  rclcpp::Client<SetBool>::SharedPtr client_ndt_trigger_;
+};
+
+#endif  // POSE_INITIALIZER__NDT_LOCALIZATION_TRIGGER_MODULE_HPP_

localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp

@@ -15,8 +15,9 @@
 #include "pose_initializer_core.hpp"
 
 #include "copy_vector_to_array.hpp"
+#include "ekf_localization_trigger_module.hpp"
 #include "gnss_module.hpp"
-#include "localization_trigger_module.hpp"
+#include "ndt_localization_trigger_module.hpp"
 #include "ndt_module.hpp"
 #include "stop_check_module.hpp"
 
@@ -34,12 +35,15 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer")
   output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance");
   gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance");
 
+  if (declare_parameter<bool>("ekf_enabled")) {
+    ekf_localization_trigger_ = std::make_unique<EkfLocalizationTriggerModule>(this);
+  }
   if (declare_parameter<bool>("gnss_enabled")) {
     gnss_ = std::make_unique<GnssModule>(this);
   }
   if (declare_parameter<bool>("ndt_enabled")) {
     ndt_ = std::make_unique<NdtModule>(this);
-    localization_trigger_ = std::make_unique<LocalizationTriggerModule>(this);
+    ndt_localization_trigger_ = std::make_unique<NdtLocalizationTriggerModule>(this);
   }
   if (declare_parameter<bool>("stop_check_enabled")) {
     // Add 1.0 sec margin for twist buffer.
@@ -73,17 +77,23 @@ void PoseInitializer::on_initialize(
   }
   try {
     change_state(State::Message::INITIALIZING);
-    if (localization_trigger_) {
-      localization_trigger_->deactivate();
+    if (ekf_localization_trigger_) {
+      ekf_localization_trigger_->send_request(false);
+    }
+    if (ndt_localization_trigger_) {
+      ndt_localization_trigger_->send_request(false);
     }
     auto pose = req->pose.empty() ? get_gnss_pose() : req->pose.front();
     if (ndt_) {
       pose = ndt_->align_pose(pose);
     }
     pose.pose.covariance = output_pose_covariance_;
     pub_reset_->publish(pose);
-    if (localization_trigger_) {
-      localization_trigger_->activate();
+    if (ekf_localization_trigger_) {
+      ekf_localization_trigger_->send_request(true);
+    }
+    if (ndt_localization_trigger_) {
+      ndt_localization_trigger_->send_request(true);
     }
     res->status.success = true;
     change_state(State::Message::INITIALIZED);

localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp

@@ -26,7 +26,8 @@
 class StopCheckModule;
 class NdtModule;
 class GnssModule;
-class LocalizationTriggerModule;
+class EkfLocalizationTriggerModule;
+class NdtLocalizationTriggerModule;
 
 class PoseInitializer : public rclcpp::Node
 {
@@ -50,7 +51,8 @@ class PoseInitializer : public rclcpp::Node
   std::unique_ptr<GnssModule> gnss_;
   std::unique_ptr<NdtModule> ndt_;
   std::unique_ptr<StopCheckModule> stop_check_;
-  std::unique_ptr<LocalizationTriggerModule> localization_trigger_;
+  std::unique_ptr<EkfLocalizationTriggerModule> ekf_localization_trigger_;
+  std::unique_ptr<NdtLocalizationTriggerModule> ndt_localization_trigger_;
   double stop_check_duration_;
   void change_state(State::Message::_state_type state);
   void on_initialize(