Support GNSS/IMU pose estimator

Signed-off-by: Ryohei Sasaki <ryohei.sasaki@map4.jp>
autowarefoundation · Feb 17, 2023 · afdadb3 · afdadb3
1 parent f63d7d4
commit afdadb3
Show file tree

Hide file tree

Showing 3 changed files with 41 additions and 13 deletions.
diff --git a/localization/pose_initializer/src/pose_initializer/localization_trigger_module.cpp b/localization/pose_initializer/src/pose_initializer/localization_trigger_module.cpp
@@ -21,11 +21,13 @@
 using ServiceException = component_interface_utils::ServiceException;
 using Initialize = localization_interface::Initialize;
 
-LocalizationTriggerModule::LocalizationTriggerModule(rclcpp::Node * node)
+LocalizationTriggerModule::LocalizationTriggerModule(rclcpp::Node * node, bool ndt_enabled)
 : logger_(node->get_logger())
 {
   client_ekf_trigger_ = node->create_client<SetBool>("ekf_trigger_node");
-  client_ndt_trigger_ = node->create_client<SetBool>("ndt_trigger_node");
+  if(ndt_enabled) client_ndt_trigger_ = node->create_client<SetBool>("ndt_trigger_node");
+
+  ndt_enabled_ = ndt_enabled;
 }
 
 void LocalizationTriggerModule::deactivate() const
@@ -36,14 +38,24 @@ void LocalizationTriggerModule::deactivate() const
   if (!client_ekf_trigger_->service_is_ready()) {
     throw component_interface_utils::ServiceUnready("EKF triggering service is not ready");
   }
-  if (!client_ndt_trigger_->service_is_ready()) {
+  if (ndt_enabled_ && !client_ndt_trigger_->service_is_ready()) {
     throw component_interface_utils::ServiceUnready("NDT triggering service is not ready");
   }
 
   auto future_ekf = client_ekf_trigger_->async_send_request(req);
-  auto future_ndt = client_ndt_trigger_->async_send_request(req);
 
-  if (future_ekf.get()->success & future_ndt.get()->success) {
+  bool is_succeeded = false;
+  if(ndt_enabled_)
+  {
+    auto future_ndt = client_ndt_trigger_->async_send_request(req);
+    is_succeeded = (future_ekf.get()->success  && future_ndt.get()->success);
+  }
+  else
+  {
+    is_succeeded = future_ekf.get()->success;
+  }
+
+  if (is_succeeded) {
     RCLCPP_INFO(logger_, "Deactivation succeeded");
   } else {
     RCLCPP_INFO(logger_, "Deactivation failed");
@@ -59,14 +71,24 @@ void LocalizationTriggerModule::activate() const
   if (!client_ekf_trigger_->service_is_ready()) {
     throw component_interface_utils::ServiceUnready("EKF triggering service is not ready");
   }
-  if (!client_ndt_trigger_->service_is_ready()) {
+  if (ndt_enabled_ && !client_ndt_trigger_->service_is_ready()) {
     throw component_interface_utils::ServiceUnready("NDT triggering service is not ready");
   }
 
   auto future_ekf = client_ekf_trigger_->async_send_request(req);
-  auto future_ndt = client_ndt_trigger_->async_send_request(req);
 
-  if (future_ekf.get()->success & future_ndt.get()->success) {
+  bool is_succeeded = false;
+  if(ndt_enabled_)
+  {
+    auto future_ndt = client_ndt_trigger_->async_send_request(req);
+    is_succeeded = (future_ekf.get()->success  && future_ndt.get()->success);
+  }
+  else
+  {
+    is_succeeded = future_ekf.get()->success;
+  }
+
+  if (is_succeeded) {
     RCLCPP_INFO(logger_, "Activation succeeded");
   } else {
     RCLCPP_INFO(logger_, "Activation failed");

diff --git a/localization/pose_initializer/src/pose_initializer/localization_trigger_module.hpp b/localization/pose_initializer/src/pose_initializer/localization_trigger_module.hpp
@@ -25,14 +25,16 @@ class LocalizationTriggerModule
   using SetBool = std_srvs::srv::SetBool;
 
 public:
-  explicit LocalizationTriggerModule(rclcpp::Node * node);
+  explicit LocalizationTriggerModule(rclcpp::Node * node, bool ndt_enabled);
   void deactivate() const;
   void activate() const;
 
 private:
   rclcpp::Logger logger_;
   rclcpp::Client<SetBool>::SharedPtr client_ekf_trigger_;
   rclcpp::Client<SetBool>::SharedPtr client_ndt_trigger_;
+
+  bool ndt_enabled_;
 };
 
 #endif  // POSE_INITIALIZER__LOCALIZATION_TRIGGER_MODULE_HPP_
diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp
@@ -34,12 +34,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");
 
+  bool ndt_enabled = declare_parameter<bool>("ndt_enabled");
+
+  localization_trigger_ = std::make_unique<LocalizationTriggerModule>(this, ndt_enabled);
+
   if (declare_parameter<bool>("gnss_enabled")) {
     gnss_ = std::make_unique<GnssModule>(this);
   }
-  if (declare_parameter<bool>("ndt_enabled")) {
+  if (ndt_enabled) {
     ndt_ = std::make_unique<NdtModule>(this);
-    localization_trigger_ = std::make_unique<LocalizationTriggerModule>(this);
   }
   if (declare_parameter<bool>("stop_check_enabled")) {
     // Add 1.0 sec margin for twist buffer.
@@ -79,8 +82,9 @@ void PoseInitializer::on_initialize(
     auto pose = req->pose.empty() ? get_gnss_pose() : req->pose.front();
     if (ndt_) {
       pose = ndt_->align_pose(pose);
+      pose.pose.covariance = output_pose_covariance_;
     }
-    pose.pose.covariance = output_pose_covariance_;
+
     pub_reset_->publish(pose);
     if (localization_trigger_) {
       localization_trigger_->activate();
@@ -97,7 +101,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped PoseInitializer::get_gnss_pose()
 {
   if (gnss_) {
     PoseWithCovarianceStamped pose = gnss_->get_pose();
-    pose.pose.covariance = gnss_particle_covariance_;
+    if (ndt_) pose.pose.covariance = gnss_particle_covariance_;
     return pose;
   }
   throw ServiceException(